unit H3;

  { Oriented projective geometry in three dimensions. }
  { Last edited by J. Stolfi on 93-04-20. }
{$N+}
interface

uses R3, R4, R4x4;

  type
    Point = record c: R4.T end;  { c[0..3] are coords [w,x,y,z]. }
    Plane = record c: R4.T end;  { c[0..3] are coeffs <W,X,Y,Z>. }
    PMap =  record dir: R4x4.T; inv: R4x4.T end;
    Sign = -1 .. +1;

  procedure to_cartesian(p: Point; var res: R3.T);
    { Converts p to Cartesian coordinates }

  procedure mk_point(w,x,y,z: real; var res: Point);
    { Sets res := [w,x,y,z]. }

  procedure mk_plane(W,X,Y,Z: real; var Res: Plane);
    { Sets res := <W,X,Y,Z>. }

  function test(p: Point; Q: Plane): Sign;
    { Returns sign of point p relative to plane Q:
        +1 in positive halfspace,
        0 on the plane,
        -1 on negative halfspace.
      Beware of rounding errors! }

  function dst(frm, tto: Point): double;

  procedure dir(frm, tto: Point; var res: R3.T);

  procedure join(p, q, r: Point; var Res: Plane);
    { Sets res to the Plane through p, q, and r. }

  procedure meet(P, Q, R: Plane; var res: Point);
    { Sets res to the Point common to P, Q, and R. }

  procedure map_point(p: Point; m: PMap; var res: Point);
    { Applies projective map m to Point p, result is res. }

  procedure map_plane(P: Plane; m: PMap; var Res: Plane);
    { Applies projective map m toPlane P, result is res. }

  procedure comp_map(m, n: PMap; var res: PMap);
    { Sets res to composition of m and n, applied in that order. }

  procedure persp_map(
      obs: Point;   { SceneSys coords of ImageSys (0,0,d) (observer). }
      foc: Point;   { SceneSys coords of ImageSys (0,0,0) (image focus). }
      upp: Point;   { A SceneSys point to project on ImageSys Z axis. }
      var res: PMap { The complete map. }
    );
    { Computes a perpective transformation with given viewing
      parameters. }

  procedure print_point(var f: TEXT; p: Point);
    { Prints the coords of p to the given file. }

  procedure print_map(var f: TEXT; m: PMap);
    { Prints the matrix of the map m to the given file. }

implementation

uses Crt;

  procedure to_cartesian(p: Point; var res: R3.T);
    var k: integer;
    begin
      for k := 1 to 3 do res[k-1] := p.c[k]/p.c[0];
    end;

  procedure errmsg(s: string);
    begin
      writeln(s);
      halt(1)
    end;

  procedure mk_point(w,x,y,z: real; var res: Point);
    begin
      res.c[0] := w;
      res.c[1] := x;
      res.c[2] := y;
      res.c[3] := z;
    end;

  procedure mk_plane(W,X,Y,Z: real; var Res: Plane);
    begin
      res.c[0] := W;
      res.c[1] := X;
      res.c[2] := Y;
      res.c[3] := Z;
    end;

  function test(p: Point; Q: Plane): Sign;
    const epsilon=1.0E-10;
    var i: integer;
        s: double;
    begin
      s := 0.0;
      for i := 1 to 3 do s := s + (p.c[i]/p.c[0]) * Q.c[i]; s:=s + Q.c[0];
      if abs(s)/sqrt(Q.c[1]*Q.c[1] + Q.c[2]*Q.c[2] + Q.c[3]*Q.c[3]) < epsilon
      then test:=0
      else if s > 0.0
           then test := +1
           else test := -1
    end;

  procedure join(p, q, r: Point; var Res: Plane);
    begin
      R4.cross(p.c, q.c, r.c,  Res.c)
    end;

  procedure meet(P, Q, R: Plane; var res: Point);
    begin
      R4.cross(P.c, Q.c, R.c,  res.c)
    end;

  procedure map_point(p: Point; m: PMap; var res: Point);
    begin
      R4x4.map_row(p.c, m.dir, res.c)
    end;

  procedure map_plane(P: Plane; m: PMap; var Res: Plane);
    begin
      R4x4.map_col(m.inv, P.c, Res.c)
    end;

  procedure comp_map(m, n: PMap; var res: PMap);
    begin
      R4x4.mul(m.dir, n.dir, res.dir);
      R4x4.mul(n.inv, m.inv, res.inv);
    end;

  procedure dir(frm, tto: Point; var res: R3.T);
    var i: integer;
        d, dd, vi, fw, tw: double;
    begin
      dd := 0.0;
      fw := frm.c[0];
      tw := tto.c[0];
      for i := 1 to 3 do
        begin
          vi := fw*tto.c[i] - tw*frm.c[i];
          res[i-1] := vi;
          dd := dd + vi*vi
        end;
      d := sqrt(dd);
      for i := 0 to 2 do
        res[i] := res[i]/d
    end;

  function dst(frm, tto: Point): double;
    var i: integer;
        dd, vi, fw, tw: double;
    begin
      dd := 0.0;
      fw := frm.c[0];
      tw := tto.c[0];
      for i := 1 to 3 do
        begin
          vi := fw*tto.c[i] - tw*frm.c[i];
          dd := dd + vi*vi
        end;
      dst := abs(sqrt(dd)/(fw*tw));
    end;

  procedure persp_map(
      obs: Point;
      foc: Point;
      upp: Point;
      var res: PMap
    );
    var r, s, t, s1: R3.T;
        i, j: integer;
        d, uno: double;
        mt, mr, mc, mrt: R4x4.T;
    begin
      if foc.c[0] <= 0.0 then
        errmsg('H3.persp_map: bad focus point (foc)');
      if obs.c[0] < 0.0 then
        errmsg('H3.persp_map: bad observer position (obs)');
      if upp.c[0] < 0.0 then
        errmsg('H3.persp_map: bad zenith reference (upp)');
      { Compute translation matrix: }
      for i := 0 to 3 do
        begin
          for j := 0 to 3 do
            if i = j then
              mt[i,j] := foc.c[0]
            else if i = 0 then
              mt[i,j] := - foc.c[j]
            else
              mt[i,j] := 0.0;
        end;
      { Compute rotation matrix: }
      dir(foc, obs, t);
      dir(foc, upp, s1);
      R3.orthize(s1, t, s);
      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');
      R3.reduce(s);
      R3.cross(s, t, r);
      mr[0,0] := 1.0;
      for i := 1 to 3 do
        begin
          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: }
      R4x4.mul(mt, mr, mrt);
      { If obs is finite, add conical projection step: }
      if obs.c[0] = 0.0 then
        res.dir := mrt
      else
        begin
          d := dst(obs, foc);
          if abs(d) > 1.0 then
            begin uno := -1.0/d; d := 1.0 end
          else
            begin 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;
          mc[3,0] := uno;
          R4x4.mul(mrt, mc, res.dir)
        end;
      { Compute adjoint: }
      R4x4.adj(res.dir, res.inv);
    end;

  procedure print_map(var f: TEXT; m: PMap);
    var mp: R4x4.T;
    begin
      writeln;
      writeln(f, '--- dir --------------------------------------------------');
      R4x4.print(f, m.dir);
      writeln(f, '--- inv --------------------------------------------------');
      R4x4.print(f, m.inv);
      writeln(f, '--- dir*inv ----------------------------------------------');
      R4x4.mul(m.dir, m.inv, mp);
      R4x4.print(f, mp);
      writeln(f, '----------------------------------------------------------');
      writeln(f);
    end;

  procedure print_point(var f: TEXT; p: Point);
    var i: integer;
    begin
      write(f, '[ ');
      for i := 0 to 3 do write(f, p.c[i]:16:8);
      write(f, ' ]');
    end;

end.