/* See {PZGeo4.h} */
#include <PZGeo4.h>

/* Last edited on 2007-01-25 10:22:12 by stolfi */

#define PZGeo4_C_author \
  "Modified by L.A.P.Lozada on 2000-03-03."

#include <r4.h>

#include <assert.h>
 
r4_t PZGeo4_LinearInterpolate(double t, double a, r4_t *pa, double b, r4_t *pb)
  { double Ca, Cb;
    if ((b - a) != 0.0)
      { Ca = (b - t)/(b - a);  Cb = 1 - Ca; }
    else
      { Ca = 0.5; Cb = 0.5; }
    return (r4_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],
        Ca*pa->c[3] + Cb*pb->c[3]
      }};
  }

void PZGeo4_HermiteInterpolate
  ( double t, 
    double a, r4_t *pa, r4_t *va, 
    double b, r4_t *pb, r4_t *vb,  
    r4_t *p, r4_t *v
  )
  { assert(a <= b);
    
    /* 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: */
        r4_mix(0.5, pa, 0.5, pb, p);
        r4_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) = (r4_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],
            Cpa*pa->c[3] + Cva*va->c[3] + Cvb*vb->c[3] + Cpb*pb->c[3]
          }};
        (*v) = (r4_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],
            Dpa*pa->c[3] + Dva*va->c[3] + Dvb*vb->c[3] + Dpb*pb->c[3]
          }};
      }
  }

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

r4_t PZGeo4_CubicBezier
  ( double t,  
    r4_t *Pabc,
    r4_t *Pbcd,
    r4_t *Pcde,
    r4_t *Pdef
  )
  { r4_t Ptbc = PZGeo4_LinearInterpolate(t, 1.0,  Pabc, 1.0,  Pbcd);
    r4_t Ptcd = PZGeo4_LinearInterpolate(t, 1.0,  Pbcd, 1.0,  Pcde);
    r4_t Ptde = PZGeo4_LinearInterpolate(t, 1.0,  Pcde, 1.0,  Pdef);
    r4_t Pttc = PZGeo4_LinearInterpolate(t, 1.0, &Ptbc, 1.0, &Ptcd);
    r4_t Pttd = PZGeo4_LinearInterpolate(t, 1.0, &Ptcd, 1.0, &Ptde);
    r4_t Pttt = PZGeo4_LinearInterpolate(t, 1.0, &Pttc, 1.0, &Pttd);
    return Pttt;
  }