/* See frb_geo.h */
/* Last edited on 2005-01-16 14:20:40 by stolfi */
/* Copyright © 2005 Universidade Estadual de Campinas. See note at end of file. */

#include <frb_geo.h>
#include <frb_types.h>

#include <math.h>
#include <r3.h>
#include <r4x4.h>
#include <stdio.h>

void frb_geo_debug ( char *msg, double_vec_t *x );

r3_t frb_linear_interpolate
  ( double t, 
    double a, 
    r3_t *pa, 
    double b, 
    r3_t *pb 
  )
  { 
    double Ca, Cb;
    if ((b - a) != 0.0)
      { Ca = (b - t) / (b - a);  Cb = 1.00 - Ca; }
    else
      { Ca = 0.5; Cb = 0.5; }
    return (r3_t){{
      Ca * pa->c[0] + Cb * pb->c[0],
      Ca * pa->c[1] + Cb * pb->c[1],
      Ca * pa->c[2] + Cb * pb->c[2]
    }};
  }

void frb_hermite_interpolate
  ( double t, 
    double a, 
    r3_t *pa, 
    r3_t *va, 
    double b, 
    r3_t *pb, 
    r3_t *vb,  
    r3_t *p,
    r3_t *v 
  )
  {
    affirm(a <= b, "arg order");
    /* 
      Test for degenerate intervals: If the interval has zero width,
      return the mean of the positions (which is an OK result) and the
      mean of the velocities (which is almost certainly wrong).
    */
    if (b - a == 0.0)
      { /* Interval is degenerate: */
        r3_mix(0.5, pa, 0.5, pb, p);
        r3_mix(0.5, va, 0.5, vb, v);
      }
    else
      {
        double tab = b - a;

        double tax = t - a;
        double txb = b - t;

        double rax = tax/tab;
        double rxb = txb/tab;

        double Cpa = rxb * rxb * (3.0 * rax + rxb);
        double Dpa = - 6.0 * rax * rxb / tab;

        double Cva = + rax * rxb * txb;
        double Dva = + rxb * (rxb - 2.0 * rax);

        double Cvb = - rax * rxb * tax;
        double Dvb = + rax * (rax - 2.0 * rxb);

        double Cpb = rax * rax * (rax + 3.0 * rxb);
        double Dpb = + 6.0 * rax * rxb / tab;

        *p = (r3_t){{
          Cpa*pa->c[0] + Cva*va->c[0] + Cvb*vb->c[0] + Cpb*pb->c[0],
          Cpa*pa->c[1] + Cva*va->c[1] + Cvb*vb->c[1] + Cpb*pb->c[1],
          Cpa*pa->c[2] + Cva*va->c[2] + Cvb*vb->c[2] + Cpb*pb->c[2]
        }};
        *v = (r3_t){{
          Dpa*pa->c[0] + Dva*va->c[0] + Dvb*vb->c[0] + Dpb*pb->c[0],
          Dpa*pa->c[1] + Dva*va->c[1] + Dvb*vb->c[1] + Dpb*pb->c[1],
          Dpa*pa->c[2] + Dva*va->c[2] + Dvb*vb->c[2] + Dpb*pb->c[2]
        }};
      }
  }

r3_t  frb_estimate_velocity_Q
  ( double a, 
    r3_t *pa, 
    double b, 
    r3_t *pb, 
    double c, 
    r3_t *pc 
  )
  {
    affirm((a <= b) && (b <= c), "arg order");
    
    /* Test for degenerate intervals: If intervals are degenerate, use
      linear interpolation, just to return something (but it will be
      almost surely wrong, and discontinuous).
    */
    double eps = 1.0e-7*(c - a);
    if (eps == 0.0)
      { /* Whole interval is degenerate: */
        return (r3_t){{0.0, 0.0, 0.0}};
      }
    else if ((b-a) < eps)
      { /* Left interval is degenerate; */
        r3_t pbc; r3_sub(pc, pb, &pbc);
        r3_scale(1.0/(c-b), &pbc, &pbc);
        return pbc;
      }
    else if ((c-b) < eps)
      { /* Right interval is degenerate; */
        r3_t pab; r3_sub(pb, pa, &pab);
        r3_scale(1.0/(b-a), &pab, &pab);
        return pab;
      }
    else
      { double fac = 1.0/(c - a);
        double fab = 1.0/(b - a);
        double fbc = 1.0/(c - b);

        double Ca = fac - fab;
        double Cb = fab - fbc;
        double Cc = fbc - fac;
        return (r3_t){{
          Ca*pa->c[0] + Cb*pb->c[0] + Cc*pc->c[0],
          Ca*pa->c[1] + Cb*pb->c[1] + Cc*pc->c[1],
          Ca*pa->c[2] + Cb*pb->c[2] + Cc*pc->c[2]
        }};
      }
  }

r3_t  frb_estimate_velocity_L
  ( double a, 
    r3_t *pa, 
    double b, 
    r3_t *pb,
    double c, 
    r3_t *pc 
  )
  { affirm((a <= b) && (b <= c), "arg order");
    /* Test for degenerate intervals: If whole interval is degenerate,
      return 0 (which is almost surely wrong). */
    if (c-a == 0.0)
      { return (r3_t){{0.0, 0.0, 0.0}}; }
    else
      { double Ca = 1.0/(c - a);
        return (r3_t){{
          Ca*(pc->c[0] - pa->c[0]),
          Ca*(pc->c[1] - pa->c[1]),
          Ca*(pc->c[2] - pa->c[2])
        }};
      }
  }

r3_t  frb_estimate_velocity_C
  ( double a, 
    r3_t *pa, 
    double b, 
    r3_t *pb, 
    double c, 
    r3_t *pc 
  )
  { affirm((a <= b) && (b <= c), "arg order");
    /* Test for degenerate intervals: */
    /* If whole interval is degenerate, return 0 (which is almost surely wrong). */
    if (c - a == 0.0)
      { /* Whole interval is degenerate: */
        return (r3_t){{0.0, 0.0, 0.0}};
      }
    else
      { double tab = b - a;
        double tbc = c - b;
        double den = tab*tab + tbc*tbc;
        
        double Ca = - tab/den;
        double Cb = (tab - tbc)/den;
        double Cc = + tbc/den;
        
        affirm(den > 0.0, "bug");
        return (r3_t){{
          Ca*pa->c[0] + Cb*pb->c[0] + Cc*pc->c[0],
          Ca*pa->c[1] + Cb*pb->c[1] + Cc*pc->c[1],
          Ca*pa->c[2] + Cb*pb->c[2] + Cc*pc->c[2]
        }};
      }
  }

double  frb_hermite_curve_length
  ( double a, 
    r3_t *pa, 
    r3_t *va, 
    double b, 
    r3_t *pb, 
    r3_t *vb 
  )
  {
    /* Shift time axis so that midpoint of {b} and {c} is zero: */
    { double m = 0.5*(a+b);
      a = a - m;
      b = b - m;
    }
    /* In case of degenerate interval, assume straight line: */
    affirm(a <= b, "arg order");
    if (b-a == 0.0)
      { return r3_dist(pa, pb); }
    else
      {
        double ab = b - a;

        /* Compute the derivative of the Hermite interpolant.
        
          Let the Hermite interpolant be {h(u) = h3*u^3 + ... + h0};
          where {u = 2*(t - a)/(b-a)-1} ranges from -1 to +1.  
          Note that {dh/du(-1) = va*(b-a)/2}, and {dh/du(1) = vb*(b-a)/2}.
          Denoting {b-a} by {ab}, the polynomial {h} is then
          
           | { h(u) = 
           |
           |     + pa*(+u^3 - 3*u + 2)/4
           |     + pb*(-u^3 + 3*u + 2)/4
           |     + va*ab*(u^3 - u^2 - u + 1)/8
           |     + vb*ab*(u^3 + u^2 - u - 1)/8
           |   = 
           |     + (1/4*(pa-pb) + 1/8*(va+vb)*ab)*u^3
           |     + ((vb-va)*ab/8)*u^2
           |     + (3/4*(pb-pa) - 1/8*(va+vb)*ab)*u
           |     + (1/2*(pa+pb) + 1/8*(va-vb)*ab
           | }

           Its derivative with respect to {u} is then

           | { dh/du(u) = 
           |     + (3/4*(pa-pb)+3/8*(va+vb)*ab) * u^2
           |     + (1/4*(vb-va)*ab) * u
           |     + (3/4*(pb-pa) - 1/8*(va+vb)*ab)
           | }
           
           Note that the curve depends only on the products
           {va*ab} and {vb*ab}, not on {va}, {vb}, or {ab}
           separately.
        */       

        double dxdu_2 = 0.750*(pa->c[0]-pb->c[0]) + 0.375*(va->c[0]+vb->c[0])*ab;
        double dydu_2 = 0.750*(pa->c[1]-pb->c[1]) + 0.375*(va->c[1]+vb->c[1])*ab;
        double dzdu_2 = 0.750*(pa->c[2]-pb->c[2]) + 0.375*(va->c[2]+vb->c[2])*ab;

        double dxdu_1 = 0.250*(vb->c[0] - va->c[0])*ab;
        double dydu_1 = 0.250*(vb->c[1] - va->c[1])*ab;
        double dzdu_1 = 0.250*(vb->c[2] - va->c[2])*ab;

        double dxdu_0 = 0.750*(pb->c[0]-pa->c[0]) - 0.125*(va->c[0]+vb->c[0])*ab;
        double dydu_0 = 0.750*(pb->c[1]-pa->c[1]) - 0.125*(va->c[1]+vb->c[1])*ab;
        double dzdu_0 = 0.750*(pb->c[2]-pa->c[2]) - 0.125*(va->c[2]+vb->c[2])*ab;

        /* Computes the speed squared {|dh/du|^2}, as a function of
          {u}. We only need the even powers: */
        
        double s_4 = dxdu_2*dxdu_2 + dydu_2*dydu_2 + dzdu_2*dzdu_2;
        double s_2 = 2.0 * (dxdu_2*dxdu_0 + dydu_2*dydu_0 + dzdu_2*dzdu_0) +
             (dxdu_1*dxdu_1 + dydu_1*dydu_1 + dzdu_1*dzdu_1);
        double s_0 = dxdu_0*dxdu_0 + dydu_0*dydu_0 + dzdu_0*dzdu_0;

        /* Computes the approximate square root of the polynomial
          Assumes the constant term {s0} dominates, so 
          {sqrt(U + s_0) ~ sqrt(s_0) + U/sqrt(s_0)/2} */
          
        double r_0 = sqrt(s_0 + 1.0e-50);
        double r_2 = 0.5 * s_2/r_0;
        double r_4 = 0.5 * s_4/r_0;

        /* Integrates the speed from -1 to +1 to get the length: */
        double length = 2.0*(r_4/5.0 + r_2/3.0 + r_0);
        return length;
      }
  }

void frb_geo_debug ( char *msg, double_vec_t *x )
  {
    fprintf(stderr, msg);
    int i;
    for (i = 0; i < x->nel; i++)
      { fprintf(stderr, " %12.8f", x->el[i]); }
    fprintf(stderr, "\n");
  }

r3_t frb_B_spline_approximation
  ( double t,
    double a, 
    double b, 
    r3_t *Pabc,
    double c, 
    r3_t *Pbcd,
    double d, 
    r3_t *Pcde,
    double e, 
    r3_t *Pdef,
    double f 
  )
  {
    r3_t Ptbc = frb_linear_interpolate(t, a, Pabc, d, Pbcd);
    r3_t Ptcd = frb_linear_interpolate(t, b, Pbcd, e, Pcde);
    r3_t Ptde = frb_linear_interpolate(t, c, Pcde, f, Pdef);
    r3_t Pttc = frb_linear_interpolate(t, b, &Ptbc, d, &Ptcd);
    r3_t Pttd = frb_linear_interpolate(t, c, &Ptcd, e, &Ptde);
    r3_t Pttt = frb_linear_interpolate(t, c, &Pttc, d, &Pttd);
    return Pttt;
  }

double frb_avg_seg_dist
  ( r3_t *a1,
    r3_t *a2,
    r3_t *b1,
    r3_t *b2 
  )
  {
    return sqrt(frb_avg_seg_dist_sqr(a1, a2, b1, b2));
  }

double frb_avg_seg_dist_sqr
  ( r3_t *a1,
    r3_t *a2,
    r3_t *b1,
    r3_t *b2 
  )
  {
    r3_t d1; r3_sub(a1, b1, &d1);
    r3_t d2; r3_sub(a2, b2, &d2);
    double d = ( 
      d1.c[0]*d1.c[0] + d1.c[0]*d2.c[0] + d2.c[0]*d2.c[0] +
      d1.c[1]*d1.c[1] + d1.c[1]*d2.c[1] + d2.c[1]*d2.c[1] +
      d1.c[2]*d1.c[2] + d1.c[2]*d2.c[2] + d2.c[2]*d2.c[2]
    )/3.0;
    return d;
  }

r4x4_t frb_translation_matrix ( r3_t *p ) 
  {
    r4x4_t m; 
    r4x4_ident(&m);
    m.c[0][1] = p->c[0];
    m.c[0][2] = p->c[1];
    m.c[0][3] = p->c[2];
    return m;
  }

r4x4_t frb_rotation_matrix ( r3_t *u, r3_t *v )
  {
    r4x4_t m; 
    r3_t e;
    double cos, sin;
    r4x4_ident(&m);
    cos = r3_dot(u, v);
    r3_cross(u,v, &e);
    sin = r3_norm(&e);
    r3_dir(&e, &e);
    affirm(abs(cos*cos + sin*sin - 1.0) < 1.0e-10, "degen matrix");
    if (abs(1.0 - abs(cos)) < 1.0e-10)
      { /* Vectors are practically collinear. */
        if (cos > 0.0) { return m; }
        /* Vectors are practically opposite; pick any {e} orthogonal to {u}. */
        /* Find smallest coordinate {u[k]}: */
        { 
          unsigned k = 0;
          if (abs(u->c[1]) < abs(u->c[k])) { k = 1; }
          if (abs(u->c[2]) < abs(u->c[k])) { k = 2; }
          e = (r3_t){{0.0, 0.0, 0.0}};
          e.c[k] = 1.0;;
        }
        r3_t ue; r3_cross(u,&e,&ue); r3_dir(&ue, &e);
        sin = 0.0;
      }
    double x = e.c[0], y = e.c[1], z = e.c[2];
    double xx = x*x, yy = y*y, zz = z*z;
    double xy = x*y, yz = y*z, zx = z*x;
    
    m.c[1][1] = xx + cos*(yy + zz);
    m.c[1][2] = (1.0 - cos)*xy + sin*z;
    m.c[1][3] = (1.0 - cos)*zx - sin*y;
    m.c[2][1] = (1.0 - cos)*xy - sin*z;
    m.c[2][2] = yy + cos*(xx + zz);
    m.c[2][3] = (1.0 - cos)*yz + sin*x;
    m.c[3][1] = (1.0 - cos)*zx + sin*y;
    m.c[3][2] = (1.0 - cos)*yz - sin*x;
    m.c[3][3] = zz + cos*(xx + yy);;
    return m;
  }

r3_t frb_seg_dir ( r3_t *p, r3_t *q )
  {
    r3_t r;
    r.c[0] = q->c[0] - p->c[0];
    r.c[1] = q->c[1] - p->c[1];
    r.c[2] = q->c[2] - p->c[2];
    r3_dir(&r, &r);
    return r;
  }

r3_t  frb_seg_mid ( r3_t *p, r3_t *q )
  { 
    r3_t o;
    int i;
    for (i = 0; i < 3; i++)
      { o.c[i] = (p->c[i] + q->c[i]) / 2.0; }
    return o;  
  }
/* COPYRIGHT AND AUTHORSHIP NOTICE

  Copyright © 2005 Universidade Estadual de Campinas (UNICAMP).
  Created by Helena C. G. Leitão and Jorge Stolfi in 1995--2005.
  
  This source file can be freely distributed, used, and modified,
  provided that this copyright and authorship notice is preserved in
  all copies, and that any modified versions of this file are clearly
  marked as such.
  
  This software has NO WARRANTY of correctness or applicability for
  any purpose. Neither the authors nor their employers shall be held
  responsible for any losses or damages that may result from its use.

  END OF NOTICE */