MODULE H3;

    (*
        Oriented projective geometry in three dimensions

        Author: Jorge Stolfi

        Last edited: M. Carrard - 18/04/1993 - Transcricao para modula-3
    *)

IMPORT R3, R4, R4x4, Stdio, Wr, Math;

CONST Epslon = 0.0000001;

(* Output error messages *)
PROCEDURE errmsg( s: TEXT ) =
BEGIN
   Wr.PutText( Stdio.stderr, s );
END errmsg;

(* Set the point coords *)
PROCEDURE mk_point( w,x,y,z: REAL ): Point =
VAR p: Point;
BEGIN
   p.c[0] := w;
   p.c[1] := x;
   p.c[2] := y;
   p.c[3] := z;
   RETURN p;
END mk_point;

(* Set the plane coeffs *)
PROCEDURE mk_plane( W,X,Y,Z: REAL ): Plane =
VAR p: Plane;
BEGIN
   p.c[0] := W;
   p.c[1] := X;
   p.c[2] := Y;
   p.c[3] := Z;
   RETURN p;
END mk_plane;

(* Return the sign of point p relative to plane Q *)
PROCEDURE test( p: Point; Q: Plane ): Sign =
VAR s: REAL;
BEGIN
   s := 0.0;
   FOR i := 0 TO 3 DO s := s + p.c[i] * Q.c[i]; END;
   IF s > Epslon THEN 
      RETURN 1;
   ELSIF s < -Epslon THEN 
          RETURN -1;
       ELSE
          RETURN 0;
       END;
END test;       

(* Find de plane through p, q, and r *)
PROCEDURE join( p,q,r: Point ): Plane =
VAR P: Plane;
BEGIN
    P.c := R4.cross( p.c, q.c, r.c );
    RETURN P;
END join;   

(* Find the common point to P, Q, and R *)
PROCEDURE meet( P,Q,R: Plane ): Point =
VAR p: Point;
BEGIN
    p.c := R4.cross( P.c, Q.c, R.c );
    RETURN p;
END meet;        

(* Applies the projective map m to point p *)
PROCEDURE map_point( p: Point; m: PMap ): Point =
VAR q: Point;
BEGIN
    q.c := R4x4.map_row( p.c, m.dir );
    RETURN q;
END map_point;    
        
(* Applies the projective map m to plane P *)
PROCEDURE map_plane( P: Plane; m: PMap ): Plane =
VAR Q: Plane;
BEGIN
    Q.c := R4x4.map_col( m.inv, P.c );
    RETURN Q;
END map_plane;
    
(* eturn the composition of m and n, applied in that order *)
PROCEDURE comp_map( m,n: PMap ): PMap =
VAR pm: PMap;
BEGIN
    pm.dir := R4x4.mul( m.dir, n.dir );
    pm.inv := R4x4.mul( m.inv, n.inv );
    RETURN pm;
END comp_map;    

(* *)
PROCEDURE dir( frm,tto: Point ): R3.T =
VAR r: R3.T;
    d, dd, vi, fw, tw: REAL;
BEGIN
    dd := 0.0;
    fw := frm.c[0];
    tw := tto.c[0];
    FOR i:= 1 TO 3 DO
       vi := fw * tto.c[i] - tw * frm.c[i];
       r[i-1] := vi;
       dd := dd + vi * vi;
    END;
    d := FLOAT( Math.sqrt( FLOAT( dd, LONGREAL) ) );
    FOR i:= 0 TO 2 DO r[i] := r[i] / d; END;
    RETURN r;
END dir;    
    
(* *)
PROCEDURE dst( frm,tto: Point ): REAL =
VAR r: REAL;
    dd, vi, fw, tw: REAL;
BEGIN
    dd := 0.0;
    fw := frm.c[0];
    tw := tto.c[0];
    FOR i:= 1 TO 3 DO
       vi := fw * tto.c[i] - tw * frm.c[i];
       dd := dd + vi * vi;
    END; 
    r := FLOAT( Math.sqrt( FLOAT( dd, LONGREAL ) ) ) / fw / tw;
    RETURN ABS(r);
END dst;

PROCEDURE persp_map( obs, foc, upp: Point ): PMap =
VAR res: PMap;
    r, s, t, s1: R3.T;
    d, uno: REAL;
    mt, mr, mc, mrt: R4x4.T;
BEGIN
    IF foc.c[0] <= 0.0 THEN
        errmsg( "H3.persp_map: bad focus point (foc)." );
    END;
    IF obs.c[0] < 0.0 THEN
        errmsg( "H3.persp_map: bad observer position (obs)." );
    END;
    IF upp.c[0] < 0.0 THEN
        errmsg( "H3.persp_map: bad zenith reference (upp)." );
    END;
    (* 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 );
    s1 := dir( foc, upp );
    s := R3.orthize( s1, t );
    IF s[0] = 0.0 AND s[1] = 0.0 AND s[2] = 0.0 THEN
       errmsg( "H3.persp_map: zenith (upp) and observer (obs) are aligned." );
    END;
    s := R3.reduce( s );
    r := 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: *)
    mrt := R4x4.mul( mt, mr );
    (* If obs is finite, add conical projection step: *)
    IF obs.c[0] = 0.0 THEN
        res.dir := mrt;
    ELSE
        d := dst( obs, 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;
        res.dir := R4x4.mul( mrt, mc );
    END;
    (* Compute adjoint: *)
    res.inv := R4x4.adj( res.dir );
    RETURN res;
END persp_map;    

BEGIN 
END H3.