MODULE PZGeo4;

(* Last edited on 03-03-2000 by lozada *)

IMPORT LR4;
 
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],
      Ca * pa[3] + Cb * pb[3]
    }
  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 := LR4.Mix(0.5d0, pa, 0.5d0, pb);
      v := LR4.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],
          Cpa*pa[3] + Cva*va[3] + Cvb*vb[3] + Cpb*pb[3]
        };
        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],
          Dpa*pa[3] + Dva*va[3] + Dvb*vb[3] + Dpb*pb[3]
        }
      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, 0.0d0}
      ELSIF (b-a) < eps THEN
        (* Left interval is degenerate; *)
        RETURN LR4.Scale(1.0d0/(c-b), LR4.Sub(pc, pb));
      ELSIF (c-b) < eps THEN
        (* Right interval is degenerate; *)
        RETURN LR4.Scale(1.0d0/(b-a), LR4.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],
            Ca*pa[3] + Cb*pb[3] + Cc*pc[3]
          }
        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, 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]),
          Ca*(pc[3] - pa[3])
        }
      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, 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],
          Ca*pa[3] + Cb*pb[3] + Cc*pc[3]
        }
      END
    END
  END EstimateVelocityC;

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;


PROCEDURE CubicBezier(
    t: LONG;
    READONLY Pabc: Point;
    READONLY Pbcd: Point;
    READONLY Pcde: Point;
    READONLY Pdef: Point;
  ): Point =
  BEGIN
    WITH
      Ptbc = LinearInterpolate(t, 1.0d0, Pabc, 1.0d0, Pbcd),
      Ptcd = LinearInterpolate(t, 1.0d0, Pbcd, 1.0d0, Pcde),
      Ptde = LinearInterpolate(t, 1.0d0, Pcde, 1.0d0, Pdef),
      Pttc = LinearInterpolate(t, 1.0d0, Ptbc, 1.0d0, Ptcd),
      Pttd = LinearInterpolate(t, 1.0d0, Ptcd, 1.0d0, Ptde),
      Pttt = LinearInterpolate(t, 1.0d0, Pttc, 1.0d0, Pttd)
    DO
      RETURN Pttt 
    END
  END CubicBezier;

BEGIN
END PZGeo4.