MODULE RT3;

(* 
  Created 93-05-18 by Marcos C. Carrard.
  Based on H3.pas by J. Stolfi.
  Last edited by stolfi *)

IMPORT R3, R4, R4x4, Math;

PROCEDURE FromCartesian(READONLY c: R3.T): Point =
  BEGIN
    RETURN Point{c := R4.T{1.0, c[0], c[1], c[2]}}
  END FromCartesian;
    
PROCEDURE ToCartesian(READONLY p: Point): R3.T =
  BEGIN
    WITH w = p.c[0] DO
      <* ASSERT w # 0.0 *>
      RETURN R3.T{p.c[1]/w, p.c[2]/w, p.c[3]/w}
    END
  END ToCartesian;

PROCEDURE Test(READONLY p: Point; READONLY Q: Plane): Sign =
  BEGIN
    WITH s = R4.Dot(p.c, Q.f) DO
      IF s > 0.0D0 THEN 
        RETURN 1
      ELSIF s < 0.0D0 THEN 
        RETURN -1
      ELSE
        RETURN 0
      END
    END
  END Test;       

PROCEDURE Join(READONLY p, q, r: Point): Plane =
  BEGIN
    RETURN Plane{f := R4.Cross(p.c, q.c, r.c)}
  END Join;   

PROCEDURE Meet(READONLY P, Q, R: Plane): Point =
  BEGIN
    RETURN Point{c := R4.Cross(P.f, Q.f, R.f)}
  END Meet;        

PROCEDURE MapPoint(READONLY p: Point; READONLY m: PMap): Point =
  BEGIN
    RETURN Point{c := R4x4.MapRow(p.c, m.dir)}
  END MapPoint;
        
PROCEDURE MapPlane(READONLY P: Plane; READONLY m: PMap): Plane =
  BEGIN
    RETURN Plane{f := R4x4.MapCol(m.inv, P.f)}
  END MapPlane;
    
PROCEDURE CompMap(READONLY m, n: PMap): PMap =
  BEGIN
    RETURN PMap{
      dir := R4x4.Mul(m.dir, n.dir),
      inv := R4x4.Mul(n.inv, m.inv)
    }
  END CompMap;    

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

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

PROCEDURE PerspMap(READONLY obs, foc, upp: Point): PMap =
  VAR m: PMap;
      r, s, t: R3.T;
      d, uno: REAL;
      mt, mr, mc, mrt: R4x4.T;
  BEGIN
    <* ASSERT foc.c[0] > 0.0 *>  (* Focus must be finite and hither *)
    <* ASSERT obs.c[0] >= 0.0 *> (* Observer must be hither or infinite *)
    <* ASSERT upp.c[0] >= 0.0 *> (* Zenith must be hither or infinite *)

    (* Compute translation matrix *)
    FOR i := 0 TO 3 DO
      FOR j := 0 TO 3 DO
        IF i = j THEN mt[i,j] := foc.c[0];
        ELSIF i = 0 THEN mt[i,j] := -foc.c[j];
        ELSE mt[i,j] := 0.0;
        END;
      END;
    END;

    (* Compute rotation matrix *)
    t := Dir(foc, obs);
    s := R3.Orthize(Dir(foc, upp), t);
    (* Zenith reference point (upp) must not be on imagesys Z axis: *)
    <* ASSERT NOT R3.Length(s) < 0.00001 *> 
    s := R3.Reduce(s);
    r := R3.Reduce(R3.Cross(s, t));
    
    mr[0,0] := 1.0;
    FOR i := 1 TO 3 DO
        mr[0,i] := 0.0;
        mr[i,0] := 0.0;
        mr[i,1] := r[i-1];
        mr[i,2] := s[i-1];
        mr[i,3] := t[i-1];
    END;

    (* Compose the two matrices: *)
    mrt := R4x4.Mul(mt, mr);

    IF obs.c[0] = 0.0 THEN
      (* Observer is at infinity; cilindrical projection. *)
      m.dir := mrt;
    ELSE
      (* Observer is finite; add conical projection step. *)
      d := R3.Dist(ToCartesian(obs), ToCartesian(foc));
      IF ABS(d) > 1.0 THEN
        uno := -1.0/d;
        d := 1.0;
      ELSE
        uno := -1.0;
      END;
      FOR i := 0 TO 3 DO
        FOR j := 0 TO 3 DO
          IF i = j THEN mc[i,j] := d ELSE mc[i,j] := 0.0 END;
        END;
      END;
      mc[3,0] := uno;
      m.dir := R4x4.Mul(mrt, mc);
    END;
    
    (* Compute inverse matrix: *)
    m.inv := R4x4.Inv(m.dir);
    RETURN m;
  END PerspMap;

BEGIN 
END RT3.