MODULE PZGeo3;

(* Last edited on 1999-08-15 09:55:25 by hcgl *)

IMPORT Math, Wr, Thread, Fmt;
IMPORT LR3;

FROM Math IMPORT sqrt;
FROM Stdio IMPORT stderr;

TYPE
  LONGS = ARRAY OF LONG;
 
PROCEDURE LinearInterpolate(
    t: LONG; 
    a: LONG; READONLY pa: Point; 
    b: LONG; READONLY pb: Point;
  ): Point =
  VAR Ca, Cb: LONG;
  BEGIN 
    IF (b - a) # 0.0d0 THEN
      Ca := (b - t) / (b - a);
      Cb := 1.00d0 - Ca
    ELSE
      Ca := 0.5d0; Cb := 0.5d0
    END;
    RETURN Point{
      Ca * pa[0] + Cb * pb[0],
      Ca * pa[1] + Cb * pb[1],
      Ca * pa[2] + Cb * pb[2]
    }
  END LinearInterpolate;
  
PROCEDURE HermiteInterpolate(
    t: LONG; 
    a: LONG; READONLY pa: Point; READONLY va: Point; 
    b: LONG; READONLY pb: Point; READONLY vb: Point;  
    VAR p: Point;
    VAR v: Point;
  ) =
  BEGIN
    <* ASSERT a <= b *>
    
    (* 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.0d0 THEN
      (* Interval is degenerate: *)
      p := LR3.Mix(0.5d0, pa, 0.5d0, pb);
      v := LR3.Mix(0.5d0, va, 0.5d0, vb)
    ELSE
      WITH
        tab = b - a,

        tax = t - a,
        txb = b - t,

        rax = tax/tab,
        rxb = txb/tab,

        Cpa = rxb * rxb * (3.0d0 * rax + rxb),
        Dpa = - 6.0d0 * rax * rxb / tab, 

        Cva = + rax * rxb * txb,
        Dva = + rxb * (rxb - 2.0d0 * rax), 

        Cvb = - rax * rxb * tax,
        Dvb = + rax * (rax - 2.0d0 * rxb), 

        Cpb = rax * rax * (rax + 3.0d0 * rxb),
        Dpb = + 6.0d0 * rax * rxb / tab

      DO
        p := Point{
          Cpa*pa[0] + Cva*va[0] + Cvb*vb[0] + Cpb*pb[0],
          Cpa*pa[1] + Cva*va[1] + Cvb*vb[1] + Cpb*pb[1],
          Cpa*pa[2] + Cva*va[2] + Cvb*vb[2] + Cpb*pb[2]
        };
        v := Point{
          Dpa*pa[0] + Dva*va[0] + Dvb*vb[0] + Dpb*pb[0],
          Dpa*pa[1] + Dva*va[1] + Dvb*vb[1] + Dpb*pb[1],
          Dpa*pa[2] + Dva*va[2] + Dvb*vb[2] + Dpb*pb[2]
        }
      END
    END;
  END HermiteInterpolate;

PROCEDURE EstimateVelocityQ(
    a: LONG; READONLY pa: Point; 
    b: LONG; READONLY pb: Point; 
    c: LONG; READONLY pc: Point;
  ): Point =
  BEGIN
    <* ASSERT a <= b AND b <= c *>
    
    (* Test for degenerate intervals. *)
    (* If intervals are degenerate, use linear interpolation, just to return *)
    (* something (but it will be almost surely wrong, and discontinuous). *)
    WITH eps = FLOAT(1.0e-7,LONG)*(c - a) DO
      IF eps = 0.0d0 THEN
        (* Whole interval is degenerate: *)
        RETURN Point{0.0d0, 0.0d0, 0.0d0}
      ELSIF (b-a) < eps THEN
        (* Left interval is degenerate; *)
        RETURN LR3.Scale(1.0d0/(c-b), LR3.Sub(pc, pb));
      ELSIF (c-b) < eps THEN
        (* Right interval is degenerate; *)
        RETURN LR3.Scale(1.0d0/(b-a), LR3.Sub(pb, pa));
      ELSE
        WITH
          fac = 1.0d0/(c - a),
          fab = 1.0d0/(b - a),
          fbc = 1.0d0/(c - b),

          Ca = fac - fab,
          Cb = fab - fbc,
          Cc = fbc - fac
        DO
          RETURN Point{
            Ca*pa[0] + Cb*pb[0] + Cc*pc[0],
            Ca*pa[1] + Cb*pb[1] + Cc*pc[1],
            Ca*pa[2] + Cb*pb[2] + Cc*pc[2]
          }
        END    
      END
    END;
  END EstimateVelocityQ;

PROCEDURE EstimateVelocityL(
    a: LONG; READONLY pa: Point; 
    b: LONG; <*UNUSED*> READONLY pb: Point;
    c: LONG; READONLY pc: Point;
  ): Point =
  BEGIN
    <* ASSERT a <= b AND b <= c *>
    (* Test for degenerate intervals: *)
    (* If whole interval is degenerate, return 0 (which is almost surely wrong). *)
    IF c-a = 0.0d0 THEN
      RETURN Point{0.0d0, 0.0d0, 0.0d0}
    ELSE
      WITH
        Ca = 1.0d0/(c - a)
      DO
        RETURN Point{
          Ca*(pc[0] - pa[0]),
          Ca*(pc[1] - pa[1]),
          Ca*(pc[2] - pa[2])
        }
      END
    END
  END EstimateVelocityL;

PROCEDURE EstimateVelocityC(
    a: LONG; READONLY pa: Point; 
    b: LONG; READONLY pb: Point; 
    c: LONG; READONLY pc: Point;
  ): Point =
  BEGIN
    <* ASSERT a <= b AND b <= c *>
    (* Test for degenerate intervals: *)
    (* If whole interval is degenerate, return 0 (which is almost surely wrong). *)
    IF c - a = 0.0d0 THEN
      (* Whole interval is degenerate: *)
      RETURN Point{0.0d0, 0.0d0, 0.0d0}
    ELSE
      WITH
        tab = b - a,
        tbc = c - b,
        den = tab*tab + tbc*tbc,
        
        Ca = - tab/den,
        Cb = (tab - tbc)/den,
        Cc = + tbc/den
      DO
        <* ASSERT den > 0.0d0 *>
        RETURN Point{
          Ca*pa[0] + Cb*pb[0] + Cc*pc[0],
          Ca*pa[1] + Cb*pb[1] + Cc*pc[1],
          Ca*pa[2] + Cb*pb[2] + Cc*pc[2]
        }
      END
    END
  END EstimateVelocityC;


PROCEDURE HermiteCurveLength(
    a: LONG; READONLY pa: Point; READONLY va: Point; 
    b: LONG; READONLY pb: Point; READONLY vb: Point;  
  ): LONG =
  BEGIN
    (* Shift time axis so that midpoint of "b" and "c" is zero: *)
    WITH m = 0.5d0*(a+b) DO
      a := a - m;
      b := b - m;
    END;
    (* In case of degenerate interval, assume straight line: *)
    <* ASSERT a <= b *>
    IF b-a = 0.0d0 THEN
      RETURN LR3.Dist(pa, pb)
    ELSE
      WITH
        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.
        *)       

        dxdu_2 = 0.750d0*(pa[0]-pb[0]) + 0.375d0*(va[0]+vb[0])*ab,
        dydu_2 = 0.750d0*(pa[1]-pb[1]) + 0.375d0*(va[1]+vb[1])*ab,
        dzdu_2 = 0.750d0*(pa[2]-pb[2]) + 0.375d0*(va[2]+vb[2])*ab,

        dxdu_1 = 0.250d0*(vb[0] - va[0])*ab,
        dydu_1 = 0.250d0*(vb[1] - va[1])*ab,
        dzdu_1 = 0.250d0*(vb[2] - va[2])*ab,

        dxdu_0 = 0.750d0*(pb[0]-pa[0]) - 0.125d0*(va[0]+vb[0])*ab,
        dydu_0 = 0.750d0*(pb[1]-pa[1]) - 0.125d0*(va[1]+vb[1])*ab,
        dzdu_0 = 0.750d0*(pb[2]-pa[2]) - 0.125d0*(va[2]+vb[2])*ab,

        (* Computes the speed squared "|dh/du|^2", as a function of "u". *)
        (* We only need the even powers: *)
        
        s_4 = dxdu_2*dxdu_2 + dydu_2*dydu_2 + dzdu_2*dzdu_2,
        s_2 = 2.0d0 * (dxdu_2*dxdu_0 + dydu_2*dydu_0 + dzdu_2*dzdu_0) +
             (dxdu_1*dxdu_1 + dydu_1*dydu_1 + dzdu_1*dzdu_1),
        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" *)
        r_0 = sqrt(s_0 + 1.0d-50),
        r_2 = 0.5d0 * s_2/r_0,
        r_4 = 0.5d0 * s_4/r_0,

        (* Integrates the speed from -1 to +1 to get the length: *)
        length = 2.0d0*(r_4/5.0d0 + r_2/3.0d0 + r_0)
      DO
        RETURN length
      END
    END
  END HermiteCurveLength;
  
<*UNUSED*>
PROCEDURE Debug(msg: TEXT; READONLY x: LONGS) =
  <* FATAL Wr.Failure, Thread.Alerted *>
  BEGIN
    Wr.PutText(stderr, msg);
    FOR i := 0 TO LAST(x) DO
      Wr.PutText(stderr, " ");
      Wr.PutText(stderr, 
        Fmt.Pad(Fmt.LongReal(x[i], prec := 8, style := Fmt.Style.Fix), 12)
      )
    END;
    Wr.PutText(stderr, "\n");
  END Debug;

PROCEDURE BSplineApproximation(
    t: LONG;
    a: LONG; 
    b: LONG; READONLY Pabc: Point;
    c: LONG; READONLY Pbcd: Point;
    d: LONG; READONLY Pcde: Point;
    e: LONG; READONLY Pdef: Point;
    f: LONG;
  ): Point =
  BEGIN
    WITH
      Ptbc = LinearInterpolate(t, a, Pabc, d, Pbcd),
      Ptcd = LinearInterpolate(t, b, Pbcd, e, Pcde),
      Ptde = LinearInterpolate(t, c, Pcde, f, Pdef),
      Pttc = LinearInterpolate(t, b, Ptbc, d, Ptcd),
      Pttd = LinearInterpolate(t, c, Ptcd, e, Ptde),
      Pttt = LinearInterpolate(t, c, Pttc, d, Pttd)
    DO
      RETURN Pttt 
    END
  END BSplineApproximation;


BEGIN
END PZGeo3.