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

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

FROM Math IMPORT sqrt;
FROM LR3 IMPORT Dir;
FROM Stdio IMPORT stderr;
FROM PZTypes IMPORT LONG, LONGS, NAT;

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;

PROCEDURE AvgSegDist(READONLY a1, a2, b1, b2: Point): LONG =
  BEGIN
    RETURN sqrt(AvgSegDistSqr(a1, a2, b1, b2))
  END AvgSegDist;

PROCEDURE AvgSegDistSqr(READONLY a1, a2, b1, b2: Point): 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: Point): 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: Point): 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: NAT := 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: Point): Point =
  VAR r: Point;
  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: Point): Point =
  VAR o: Point;
  BEGIN 
    FOR i := 0 TO 2 DO  
      o[i] := (p[i] + q[i]) / 2.0d0;
    END;
    RETURN o  
  END SegMid;
 
BEGIN
END PZGeo.
(*
  Copyright © 2001 Universidade Estadual de Campinas (UNICAMP).
  Authors: Helena C. G. Leitão and Jorge Stolfi.
  
  This file can be freely distributed, used, and modified, provided
  that this copyright and authorship notice is preserved, and that any
  modified versions are clearly marked as such.
  
  This software has NO WARRANTY of correctness or applicability for
  any purpose. Neither the authors nor their employers chall be held
  responsible for any losses or damages that may result from its use.
*)