MODULE PZGeo;

IMPORT Math, LR3, LR3Extras, LR4x4;

FROM Math IMPORT sqrt;
FROM LR3 IMPORT Dir;

IMPORT Wr, Thread, Fmt;
FROM Stdio IMPORT stderr;

PROCEDURE LinearInterpolate(
    t: LONG; 
    a: LONG; READONLY pa: T; 
    b: LONG; READONLY pb: T;
  ): T =
  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 T{
      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: T; READONLY va: T; 
    b: LONG; READONLY pb: T; READONLY vb: T;  
    VAR p: T;
    VAR v: T;
  ) =
  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 := T{
          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 := T{
          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: T; 
    b: LONG; READONLY pb: T; 
    c: LONG; READONLY pc: T;
  ): T =
  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 T{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 T{
            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: T; 
    b: LONG; <*UNUSED*> READONLY pb: T;
    c: LONG; READONLY pc: T;
  ): T =
  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 T{0.0d0, 0.0d0, 0.0d0}
    ELSE
      WITH
        Ca = 1.0d0/(c - a)
      DO
        RETURN T{
          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: T; 
    b: LONG; READONLY pb: T; 
    c: LONG; READONLY pc: T;
  ): T =
  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 T{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
        RETURN T{
          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: T; READONLY va: T; 
    b: LONG; READONLY pb: T; READONLY vb: T;  
  ): 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
        tab = b - a,

        (* Compute the derivative of the Hermite interpolant. *)
        (* 
          Let the Hermite interpolant be h(u) = h3*u^3 + ... + h0,
          where u = 2*(t - ta)/(tb-ta)-1.  
          Note that dh/du(-1) = va*(tb-ta)/2, and dh/du(1) = vb*(tb-ta)/2.
          The polynomial h is then
             h(u) = 

               + pa*(+u^3 - 3*u + 2)/4
               + pb*(-u^3 + 3*u + 2)/4
               + va*tab*(u^3 - u^2 - u + 1)/8
               + vb*tab*(u^3 + u^2 - u - 1)/8
             = 
               + (1/4*(pa-pb) + 1/8*(va+vb)*tab)*u^3
               + ((vb-va)*tab/8)*u^2
               + (3/4*(pb-pa) - 1/8*(va+vb)*tab)*u
               + (1/2*(pa+pb) + 1/8*(va-vb)*tab

           Its derivative with respect to "u" is then

             dh/du(u) = 
               + (3/4*(pa-pb)+3/8*(va+vb)*tab) * u^2
               + (1/4*(vb-va)*tab) * u
               + (3/4*(pb-pa) - 1/8*(va+vb)*tab)
        *)       

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

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

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

        (* 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 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: ARRAY OF LONG) =
  <* 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: T;
    c: LONG; READONLY Pbcd: T;
    d: LONG; READONLY Pcde: T;
    e: LONG; READONLY Pdef: T;
    f: LONG;
  ): T =
  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 AvgSegDist(READONLY a1, a2, b1, b2: T): LONG =
  BEGIN
    RETURN sqrt(AvgSegDistSqr(a1, a2, b1, b2))
  END AvgSegDist;

PROCEDURE AvgSegDistSqr(READONLY a1, a2, b1, b2: T): LONG =
  BEGIN
    WITH
      d1 = LR3.Sub(a1, b1),
      d2 = LR3.Sub(a2, b2),
      d = ( 
        d1[0]*d1[0] + d1[0]*d2[0] + d2[0]*d2[0] +
        d1[1]*d1[1] + d1[1]*d2[1] + d2[1]*d2[1] +
        d1[2]*d1[2] + d1[2]*d2[2] + d2[2]*d2[2]
      )/3.0d0    
    DO
      RETURN d
    END
  END AvgSegDistSqr;

PROCEDURE Translation(READONLY p: T): LR4x4.T = 
  VAR m : LR4x4.T; 
  BEGIN
    m :=  LR4x4.Identity();
    m[0, 1] := p[0];
    m[0, 2] := p[1];
    m[0, 3] := p[2];
    RETURN m;
  END Translation;

 
PROCEDURE Rotation(READONLY u, v: T): LR4x4.T =
  VAR m : LR4x4.T; 
      e: LR3.T;
      cos, sin: LONG;
  BEGIN
    m :=  LR4x4.Identity();
    cos := LR3.Dot(u,v);
    e := LR3Extras.Cross(u,v);
    sin := LR3.Norm(e);
    e := LR3.Dir(e);
    <* ASSERT ABS(cos*cos + sin*sin - 1.0d0) < 1.0d-10 *>
    IF ABS(1.0d0 - ABS(cos)) < 1.0d-10 THEN 
      (* Vectors are practically collinear. *)
      IF cos > 0.0d0 THEN RETURN m END;
      (* Vectors are practically opposite. *)
      (* Pick any "e" orthogonal to "u": *)
      (* Find smallest coordinate "u[k]": *)
      VAR k: CARDINAL := 0;
      BEGIN 
        IF ABS(u[1]) < ABS(u[k]) THEN k := 1 END;
        IF ABS(u[2]) < ABS(u[k]) THEN k := 2 END;
        e := LR3.T{0.0d0, 0.0d0, 0.0d0};
        e[k] := 1.0d0;
      END;
      e := LR3.Dir(LR3Extras.Cross(u,e));
      sin := 0.0d0;
    END;
    WITH
      x = e[0], y = e[1], z = e[2],
      xx = x*x, yy = y*y, zz = z*z,
      xy = x*y, yz = y*z, zx = z*x
    DO
      m[1,1] := xx + cos*(yy + zz);
      m[1,2] := (1.0d0 - cos)*xy + sin*z;
      m[1,3] := (1.0d0 - cos)*zx - sin*y;
      m[2,1] := (1.0d0 - cos)*xy - sin*z;
      m[2,2] := yy + cos*(xx + zz);
      m[2,3] := (1.0d0 - cos)*yz + sin*x;
      m[3,1] := (1.0d0 - cos)*zx + sin*y;
      m[3,2] := (1.0d0 - cos)*yz - sin*x;
      m[3,3] := zz + cos*(xx + yy);
    END;
    RETURN m;
  END Rotation;
 
PROCEDURE  SegDir(READONLY p, q: T): T =
  VAR r: T;
  BEGIN
    r[0] := q[0] - p[0];
    r[1] := q[1] - p[1];
    r[2] := q[2] - p[2];
    RETURN Dir(r);
  END SegDir;
 
 
PROCEDURE SegMid(READONLY p, q: T): T =
  VAR o: T;
  BEGIN 
    FOR i := 0 TO 2 DO  
      o[i] := (p[i] + q[i]) / 2.0d0;
    END;
    RETURN o  
  END SegMid;
 
BEGIN
END PZGeo.