MODULE HI3;
(* 
  Created in Feb. 19, 1997 BY Marcus Vinicius A. Andrade
  Based on H3.pas by J. Stolfi.    *)

IMPORT MPInt, I3, I4, I4Extras, I6, LR3, Math;

PROCEDURE IsInvalidPoint(READONLY p: Point) : BOOLEAN =
  BEGIN
    RETURN I4.IsAllZero(p.c)
  END IsInvalidPoint;

PROCEDURE FromCartesian(READONLY c: LR3.T): Point =
  BEGIN
    RETURN Point{c := I4.T{MPInt.DecDig[1], 
                           MPInt.Trunc(c[0]), 
                           MPInt.Trunc(c[1]), 
                           MPInt.Trunc(c[2])}}
  END FromCartesian;
    
PROCEDURE ToCartesian(READONLY p: Point): LR3.T =
  BEGIN
    WITH w = MPInt.Float(p.c[0]) DO
      <* ASSERT w # 0.0d0 *>
      RETURN LR3.T{MPInt.Float(p.c[1])/w, 
                   MPInt.Float(p.c[2])/w, 
                   MPInt.Float(p.c[3])/w}
    END
  END ToCartesian;

PROCEDURE Side(READONLY p: Point; READONLY Q: Plane): Sign =
  BEGIN
    RETURN MPInt.GetSign(I4.Dot(p.c, Q.f))
  END Side;       

PROCEDURE Orient(READONLY p, q, r, s: Point): Sign =
  BEGIN
    RETURN MPInt.GetSign(I4Extras.Det(p.c, q.c, r.c, s.c))
  END Orient;
  
PROCEDURE IsInvalidLine(READONLY l: Line) : BOOLEAN =
  BEGIN
    RETURN I6.IsAllZero(l.k)
  END IsInvalidLine;

PROCEDURE LineFromTwoPoints(READONLY p, q: Point): Line =
  BEGIN
    RETURN Line{
      k := I6.Reduce(I6.T{
        MPInt.Sub(MPInt.Mult(p.c[2],q.c[3]),MPInt.Mult(p.c[3],q.c[2])),
        MPInt.Sub(MPInt.Mult(p.c[3],q.c[1]),MPInt.Mult(p.c[1],q.c[3])),
        MPInt.Sub(MPInt.Mult(p.c[0],q.c[3]),MPInt.Mult(p.c[3],q.c[0])),
        MPInt.Sub(MPInt.Mult(p.c[1],q.c[2]),MPInt.Mult(p.c[2],q.c[1])),
        MPInt.Sub(MPInt.Mult(p.c[2],q.c[0]),MPInt.Mult(p.c[0],q.c[2])),
        MPInt.Sub(MPInt.Mult(p.c[0],q.c[1]),MPInt.Mult(p.c[1],q.c[0]))
        }
      )
    }
  END LineFromTwoPoints;
  
PROCEDURE PlaneFromThreePoints(READONLY p, q, r: Point): Plane =
  BEGIN
    RETURN Plane{f := I4Extras.Cross(p.c, q.c, r.c)}
  END PlaneFromThreePoints;   

PROCEDURE PlaneFromLineAndPoint(READONLY n: Line; READONLY p: Point): Plane =
  BEGIN
    WITH
      n0p1 = MPInt.Mult(n.k[0],p.c[1]),
      n1p2 = MPInt.Mult(n.k[1],p.c[2]),
      n3p3 = MPInt.Mult(n.k[3],p.c[3]),
      n0p0 = MPInt.Mult(n.k[0],p.c[0]),
      n2p2 = MPInt.Mult(n.k[2],p.c[2]),
      n4p3 = MPInt.Mult(n.k[4],p.c[3]),
      n1p0 = MPInt.Mult(n.k[1],p.c[0]),
      n2p1 = MPInt.Mult(n.k[2],p.c[1]),
      n5p3 = MPInt.Mult(n.k[5],p.c[3]),
      n3p0 = MPInt.Mult(n.k[3],p.c[0]),
      n4p1 = MPInt.Mult(n.k[4],p.c[1]),
      n5p2 = MPInt.Mult(n.k[5],p.c[2])
    DO
      RETURN Plane{
        f := I4.Neg(
               I4.Reduce(I4.T{MPInt.Add(MPInt.Add(n0p1,n1p2),n3p3),
                              MPInt.Add(MPInt.Sub(n2p2,n0p0),n4p3),
                              MPInt.Sub(n5p3,MPInt.Add(n1p0,n2p1)),
                              MPInt.Neg(MPInt.Add(n3p0,MPInt.Add(n4p1,n5p2)))
                             }
                        )
             )
      }
    END
  END PlaneFromLineAndPoint;

PROCEDURE PlaneOrthogToPlaneThroughLine(READONLY p: Plane; READONLY l: Line): Plane=
  BEGIN
    RETURN PlaneFromLineAndPoint(l,SPolarOfPlane(p))  
  END PlaneOrthogToPlaneThroughLine;

PROCEDURE LineFromTwoPlanes(READONLY P, Q: Plane): Line =
  BEGIN
    RETURN Line{
      k := I6.Reduce(I6.T{
          MPInt.Sub(MPInt.Mult(P.f[0],Q.f[1]),MPInt.Mult(P.f[1],Q.f[0])),
          MPInt.Sub(MPInt.Mult(P.f[0],Q.f[2]),MPInt.Mult(P.f[2],Q.f[0])),
          MPInt.Sub(MPInt.Mult(P.f[1],Q.f[2]),MPInt.Mult(P.f[2],Q.f[1])),
          MPInt.Sub(MPInt.Mult(P.f[0],Q.f[3]),MPInt.Mult(P.f[3],Q.f[0])),
          MPInt.Sub(MPInt.Mult(P.f[1],Q.f[3]),MPInt.Mult(P.f[3],Q.f[1])),
          MPInt.Sub(MPInt.Mult(P.f[2],Q.f[3]),MPInt.Mult(P.f[3],Q.f[2]))
         }
        )
      }
  END LineFromTwoPlanes;
  
PROCEDURE TwoPlanesByLine(READONLY l: Line; VAR p,q: Plane) = 
  VAR i : CARDINAL;
  BEGIN
    IF NOT IsLineThroughOrigin(l) THEN 
      q:=PlaneFromLineAndPoint(l,Origin);
    ELSE
      WITH
        a = SPolarOfPoint(LineDir(l))
      DO
        i:=1;
        WHILE i<=3 AND NOT MPInt.IsZero(a.f[i]) DO 
          i:=i+1 
        END;
        IF i < 4 THEN (* the ith coefficient of "a" is zero *)
          q:=Plane{f:=I4.Axis(i)};
        ELSE           (* all coefficients of "a" are non zero *)
          q:=Plane{f:=I4.T{a.f[0],MPInt.Neg(a.f[2]),a.f[1],a.f[3]}}
        END
      END
    END;
    p:=PlaneOrthogToPlaneThroughLine(q,l);
  END TwoPlanesByLine;

PROCEDURE PointFromThreePlanes(READONLY P, Q, R: Plane): Point =
  BEGIN
    RETURN Point{c := I4Extras.Cross(Q.f, P.f, R.f)}
  END PointFromThreePlanes; 
  
PROCEDURE PointFromLineAndPlane(READONLY n: Line; P: Plane): Point =
  BEGIN
    WITH
      n2P3 = MPInt.Mult(n.k[2],P.f[3]),
      n4P2 = MPInt.Mult(n.k[4],P.f[2]),
      n5P1 = MPInt.Mult(n.k[5],P.f[1]),
      n1P3 = MPInt.Mult(n.k[1],P.f[3]),
      n3P2 = MPInt.Mult(n.k[3],P.f[2]),
      n5P0 = MPInt.Mult(n.k[5],P.f[0]),
      n0P3 = MPInt.Mult(n.k[0],P.f[3]),
      n3P1 = MPInt.Mult(n.k[3],P.f[1]),
      n4P0 = MPInt.Mult(n.k[4],P.f[0]),
      n0P2 = MPInt.Mult(n.k[0],P.f[2]),
      n1P1 = MPInt.Mult(n.k[1],P.f[1]),
      n2P0 = MPInt.Mult(n.k[2],P.f[0])
    DO
      RETURN Point{
        c := I4.Reduce(
                I4.T{MPInt.Sub(n4P2,MPInt.Add(n2P3,n5P1)),
                     MPInt.Add(n5P0,MPInt.Sub(n1P3,n3P2)),
                     MPInt.Sub(n3P1,MPInt.Add(n0P3,n4P0)),
                     MPInt.Add(n2P0,MPInt.Sub(n0P2,n1P1))
                    }
             )
      }
    END
  END PointFromLineAndPlane;

PROCEDURE LineDir(READONLY n: Line): Point =
  BEGIN
    RETURN Point{c:=I4.T{MPInt.DecDig[0],n.k[5],MPInt.Neg(n.k[4]),n.k[2]}}
  END LineDir;

PROCEDURE PolarComplementOfPlane(READONLY P: Plane) : Point =
  BEGIN
    RETURN Point{c:=I4.T{P.f[0],P.f[1],P.f[2],P.f[3]}}
  END PolarComplementOfPlane;

PROCEDURE PolarComplementOfPoint(READONLY p: Point) : Plane =
  BEGIN
    RETURN Plane{f:=I4.T{p.c[0],p.c[1],p.c[2],p.c[3]}}
  END PolarComplementOfPoint;

PROCEDURE PolarComplementOfLine(READONLY n: Line): Line =
  BEGIN
    RETURN Line{k:=I6.T{n.k[5],MPInt.Neg(n.k[4]),n.k[3],
                        n.k[2],MPInt.Neg(n.k[1]),n.k[0]}}
  END PolarComplementOfLine;

PROCEDURE FrontMeetOfTwoLines(READONLY n,m: Line): Point =
  VAR k : MPInt.T;
      p : Point;
  BEGIN
    IF AreLinesConcurrent(n,m) THEN (* "n" meets "m" *)
      k := MPInt.Sub(MPInt.Mult(n.k[2],m.k[4]),MPInt.Mult(n.k[4],m.k[2]));
      IF MPInt.GetSign(k) # 0 THEN
        p.c[1]:=MPInt.Add(MPInt.Sub(MPInt.Mult(n.k[3],m.k[2]),
                                    MPInt.Mult(n.k[1],m.k[4])),
                          MPInt.Mult(n.k[5],m.k[0]));
        p.c[2]:=MPInt.Sub(MPInt.Mult(n.k[0],m.k[4]),MPInt.Mult(n.k[4],m.k[0]));
        p.c[3]:=MPInt.Sub(MPInt.Mult(n.k[2],m.k[0]),MPInt.Mult(n.k[0],m.k[2]))
      ELSE 
        k := MPInt.Sub(MPInt.Mult(n.k[2],m.k[5]),MPInt.Mult(n.k[5],m.k[2]));
        IF MPInt.GetSign(k) # 0 THEN
           p.c[1]:=MPInt.Sub(MPInt.Mult(n.k[5],m.k[1]),MPInt.Mult(n.k[1],m.k[5]));
           p.c[2]:=MPInt.Sub(MPInt.Add(MPInt.Mult(n.k[0],m.k[5]),
                                       MPInt.Mult(n.k[3],m.k[2])),
                             MPInt.Mult(n.k[4],m.k[1]));
           p.c[3]:=MPInt.Sub(MPInt.Mult(n.k[2],m.k[1]),MPInt.Mult(n.k[1],m.k[2]))
        ELSE
           k := MPInt.Sub(MPInt.Mult(n.k[4],m.k[5]),MPInt.Mult(n.k[5],m.k[4]));
           IF MPInt.GetSign(k) # 0 THEN
             p.c[1]:=MPInt.Sub(MPInt.Mult(n.k[5],m.k[3]),MPInt.Mult(n.k[3],m.k[5]));
             p.c[2]:=MPInt.Sub(MPInt.Mult(n.k[3],m.k[4]),MPInt.Mult(n.k[4],m.k[3]));
             p.c[3]:=MPInt.Add(MPInt.Sub(MPInt.Mult(n.k[0],m.k[5]),
                                         MPInt.Mult(n.k[1],m.k[4])),
                               MPInt.Mult(n.k[2],m.k[3]))
           END
        END
      END;
      p.c[0] := k;
      IF MPInt.GetSign(k) = -1  THEN 
        p:=Point{c:=I4.Neg(p.c)}
      END;
      RETURN p
    ELSE  (* "n" doesn't meet "m"  *)
      RETURN Point{c:=I4.All(MPInt.DecDig[0])}
    END
  END FrontMeetOfTwoLines;

PROCEDURE Normal(READONLY P: Plane): LR3.T =
  BEGIN
    WITH 
      nx = MPInt.Float(P.f[1]),
      ny = MPInt.Float(P.f[2]),
      nz = MPInt.Float(P.f[3]),
      
      length = FLOAT(Math.hypot(Math.hypot(nx, ny), nz),LONGREAL)
    DO
      RETURN LR3.T{nx/length, ny/length, nz/length}
    END;
  END Normal;    

PROCEDURE Dir(READONLY frm, tto: Point): LR3.T =
  BEGIN
    WITH 
      fw = MPInt.Float(frm.c[0]),
      tw = MPInt.Float(tto.c[0]),
      
      fx = MPInt.Float(frm.c[1]),
      tx = MPInt.Float(tto.c[1]),
      dx = fw * tx - tw * fx,
      
      fy = MPInt.Float(frm.c[2]),
      ty = MPInt.Float(tto.c[2]),
      dy = fw * ty - tw * fy,
      
      fz = MPInt.Float(frm.c[3]),
      tz = MPInt.Float(tto.c[3]),
      dz = fw * tz - tw * fz,
      
      length = FLOAT(Math.hypot(Math.hypot(dx, dy), dz),LONGREAL)
    DO
      RETURN LR3.T{dx/length, dy/length, dz/length}
    END;
  END Dir;  
  
PROCEDURE AntipPoint(READONLY a: Point): Point =
  BEGIN
    RETURN Point{c:=I4.Neg(a.c)}
  END AntipPoint;

PROCEDURE OppLine(READONLY l: Line): Line =
  BEGIN
    RETURN Line{k:=I6.Neg(l.k)}
  END OppLine;
    
PROCEDURE OppPlane(READONLY P: Plane): Plane =
  BEGIN
    RETURN Plane{f:=I4.Neg(P.f)}    
  END OppPlane;
    
PROCEDURE ReflectPlaneAcrossOrg(READONLY P: Plane): Plane = 
  BEGIN
    RETURN Plane{f:=I4.T{MPInt.Copy(P.f[0]),MPInt.Neg(P.f[1]),
                         MPInt.Neg(P.f[2]),MPInt.Neg(P.f[3])}}
  END ReflectPlaneAcrossOrg;
  
PROCEDURE MiddlePoint(READONLY l: Line) : Point =
  BEGIN
    WITH
      v   = I4.T{MPInt.DecDig[0],l.k[2],l.k[4],l.k[5]},
      C0b = I4.Dot(v,v),
      x   = MPInt.Neg(MPInt.Add(MPInt.Mult(l.k[1],l.k[2]),
                                  MPInt.Mult(l.k[3],l.k[4]))),
      y   = MPInt.Sub(MPInt.Mult(l.k[0],l.k[2]),MPInt.Mult(l.k[3],l.k[5])),
      z   = MPInt.Add(MPInt.Mult(l.k[0],l.k[4]),MPInt.Mult(l.k[1],l.k[5]))
    DO
      RETURN Point{c:=I4.T{C0b,x,y,z}}
    END
  END MiddlePoint;
  
  
PROCEDURE Dist(READONLY a, b: Point): LONGREAL =
  BEGIN
    WITH 
      aw = 1.0d0/MPInt.Float(a.c[0]),
      bw = 1.0d0/MPInt.Float(b.c[0]),
      
      ax = MPInt.Float(a.c[1]),
      bx = MPInt.Float(b.c[1]),
      dx = ax*aw - bx*bw,
      
      ay = MPInt.Float(a.c[2]),
      by = MPInt.Float(b.c[2]),
      dy = ay*aw - by*bw,
      
      az = MPInt.Float(a.c[3]),
      bz = MPInt.Float(b.c[3]),
      dz = az*aw - bz*bw
    DO
     RETURN Math.hypot(Math.hypot(dx, dy), dz)
    END;
  END Dist;
    
PROCEDURE DistSqr(READONLY a, b: Point): LONGREAL =
  BEGIN
    WITH 
      aw = 1.0d0/MPInt.Float(a.c[0]),
      bw = 1.0d0/MPInt.Float(b.c[0]),
      
      ax = MPInt.Float(a.c[1]),
      bx = MPInt.Float(b.c[1]),
      dx = ax*aw - bx*bw,
      
      ay = MPInt.Float(a.c[2]),
      by = MPInt.Float(b.c[2]),
      dy = ay*aw - by*bw,
      
      az = MPInt.Float(a.c[3]),
      bz = MPInt.Float(b.c[3]),
      dz = az*aw - bz*bw
    DO
      RETURN dx * dx + dy * dy + dz * dz
    END;
  END DistSqr;

PROCEDURE SideOfSphere(READONLY p : Point) : Sign =
  VAR s : I4.ElemT;
  BEGIN
    s := MPInt.Neg(MPInt.Mult(p.c[0],p.c[0]));
    FOR i:=1 TO 3 DO 
      s:=MPInt.Add(s,MPInt.Mult(p.c[i],p.c[i]))
    END;
    RETURN MPInt.GetSign(s)
  END SideOfSphere;

PROCEDURE SPolarOfPoint(READONLY p : Point) : Plane =
  BEGIN
    RETURN Plane{f:=I4.T{MPInt.Neg(p.c[0]),p.c[1],p.c[2],p.c[3]}}    
  END SPolarOfPoint;

PROCEDURE SPolarOfPlane(READONLY P : Plane) : Point =
  BEGIN
    RETURN Point{c:=I4.T{MPInt.Neg(P.f[0]),P.f[1],P.f[2],P.f[3]}}    
  END SPolarOfPlane;

PROCEDURE SPolarOfLine(READONLY l : Line) : Line = 
  BEGIN
    RETURN Line{k:=I6.T{MPInt.Neg(l.k[5]),l.k[4],l.k[3],
                        MPInt.Neg(l.k[2]),MPInt.Neg(l.k[1]),l.k[0]}}
  END SPolarOfLine;
  
PROCEDURE ArePlanesParallel(READONLY P,Q : Plane) : Sign = 
  BEGIN
    WITH
      p  = I4.T{MPInt.DecDig[0],P.f[1],P.f[2],P.f[3]},
      q  = I4.T{MPInt.DecDig[0],Q.f[1],Q.f[2],Q.f[3]}
    DO
      RETURN I4.Equal(p,q)
    END
  END ArePlanesParallel;

PROCEDURE IsLineThroughOrigin(READONLY l: Line) : BOOLEAN =
  BEGIN
    RETURN MPInt.IsZero(l.k[0]) AND 
           MPInt.IsZero(l.k[1]) AND 
           MPInt.IsZero(l.k[3])
  END IsLineThroughOrigin;

PROCEDURE IsLineTangent(READONLY l: Line) : BOOLEAN = 
  BEGIN 
    IF IsInvalidLine(l) THEN 
      RETURN FALSE
    ELSE 
      WITH 
        t   = l.k, 
        u   = I3.T{t[0],t[1],t[3]},
        v   = I3.T{t[2],t[4],t[5]},
        C0  = I3.Dot(u,u),
        C0b = I3.Dot(v,v),
        d   = MPInt.Sub(C0b,C0)
      DO
        RETURN MPInt.IsZero(d)
      END
    END
  END IsLineTangent;

PROCEDURE IsPointOnLine(READONLY p: Point; READONLY l: Line): BOOLEAN =
  BEGIN
    WITH
      eq0=MPInt.Sub(MPInt.Sub(MPInt.Mult(p.c[0],l.k[0]),MPInt.Mult(p.c[2],l.k[2])),
                    MPInt.Mult(p.c[3],l.k[4])),
      eq1=MPInt.Sub(MPInt.Sub(MPInt.Mult(p.c[3],l.k[5]),MPInt.Mult(p.c[1],l.k[2])),
                    MPInt.Mult(p.c[0],l.k[1])),
      eq2=MPInt.Add(MPInt.Add(MPInt.Mult(p.c[0],l.k[3]),MPInt.Mult(p.c[1],l.k[4])),
                    MPInt.Mult(p.c[2],l.k[5])),
      eq3=MPInt.Add(MPInt.Add(MPInt.Mult(p.c[1],l.k[0]),MPInt.Mult(p.c[2],l.k[1])),
                    MPInt.Mult(p.c[3],l.k[3]))
    DO
      RETURN (MPInt.IsZero(eq0) AND MPInt.IsZero(eq1) AND 
              MPInt.IsZero(eq2) AND MPInt.IsZero(eq3))
    END 
  END IsPointOnLine;

PROCEDURE IsLineOnPlane(READONLY l: Line; P: Plane) : BOOLEAN =
  BEGIN
    WITH
      p = PointFromLineAndPlane(l,P)
    DO
      RETURN I4.IsAllZero(p.c)
    END 
  END IsLineOnPlane;
  
PROCEDURE AreLinesCoplanar(READONLY l,m : Line) : BOOLEAN =
  BEGIN
    WITH
      s0 = MPInt.Add(MPInt.Sub(MPInt.Mult(l.k[0],m.k[5]),MPInt.Mult(l.k[1],m.k[4])),
                     MPInt.Mult(l.k[2],m.k[3])),
      s1 = MPInt.Add(MPInt.Sub(MPInt.Mult(l.k[3],m.k[2]),MPInt.Mult(l.k[4],m.k[1])),
                     MPInt.Mult(l.k[5],m.k[0])),
      s  = MPInt.Add(s0,s1)
    DO
      RETURN MPInt.IsZero(s)
    END
  END AreLinesCoplanar;
  
PROCEDURE AreLinesParallel(READONLY l,m : Line) : Sign =
  BEGIN
    WITH
      dl = LineDir(l),
      dm = LineDir(m)
    DO
      RETURN I4.Equal(dl.c,dm.c)
    END
  END AreLinesParallel;

PROCEDURE AreLinesConcurrent(READONLY l,m : Line) : BOOLEAN = 
  BEGIN
    RETURN AreLinesCoplanar(l,m) AND (AreLinesParallel(l,m) = 0)
  END AreLinesConcurrent;

PROCEDURE Ahead2PlanesAroundLine(READONLY A,B : Plane; READONLY dl: Point) : Sign RAISES {ValidationError} =
  VAR s: Sign;
  BEGIN
    WITH
      d = LineDir(LineFromTwoPlanes(A,B))
    DO 
      IF I4.IsAllZero(d.c) THEN 
         RETURN 0
      ELSE
        s := I4.Equal(dl.c,d.c);
        IF s=0 THEN 
           RAISE ValidationError("The line is not common to the three planes") 
        ELSE
          RETURN s
        END
      END
    END;
  END Ahead2PlanesAroundLine;

PROCEDURE CircOrdOf3PlanesAroundALine(READONLY P0,P1,P2: Plane; READONLY l: Line) : Sign RAISES {ValidationError} = 
  VAR dl          : Point;
  BEGIN
    dl:=LineDir(l);
    WITH
      s = Ahead2PlanesAroundLine(P0,P1,dl) + 
          Ahead2PlanesAroundLine(P1,P2,dl) + 
          Ahead2PlanesAroundLine(P2,P0,dl)
    DO
      IF s = 0  THEN RETURN 0
      ELSIF s > 0 THEN RETURN +1 
      ELSE RETURN -1 
      END  
    END
  END CircOrdOf3PlanesAroundALine;
  
PROCEDURE CircOrdOf3PointsOnALine(READONLY p,q,r: Point; READONLY l: Line) : Sign RAISES {ValidationError} =
  BEGIN
    TRY 
      WITH
        pd = PolarComplementOfPoint(p),
        qd = PolarComplementOfPoint(q),
        rd = PolarComplementOfPoint(r),
        ld = PolarComplementOfLine(l)
      DO
        RETURN CircOrdOf3PlanesAroundALine(pd,qd,rd,ld)
      END 
    EXCEPT 
    | ValidationError(t) => RAISE ValidationError("One of the points is not on the line") 
    END
  END CircOrdOf3PointsOnALine;

BEGIN 
  Origin:=Point{c:=I4.Axis(0)}
END HI3.