In all modules: add Get, Set

In all matrix implementations: Add PlaneRotation

PROCEDURE PlaneRotation(angle: LONGREAL; READONLY u, v: VRep.T): T;
  (*
    Returns an orthonormal matrix "R" that rotates "u" towards "v"
    by "angle" radians, while keeping the orthogonal complement
    of "u" and "v" fixed. 
    
    More precisely, let "w[2],.. w[N-1]" be any basis for the 
    orthogonal complement of "u" and "v".  If the coordinates of 
    a vector "p" in the basis "u, v, w[2],.. w[N-1]"
    are "x[0], x[1], ..., x[N-1]", then the coordinates of 
    "MapRow(p, R)" in that basis will be 
      "c*x[0]-s*x[1], s*x[0]-c*x[1], x[2],.. x[N-1]"
    Thus, for example, "Rotation(Pi/2, u, v)" will take "u" to "v",
    "v" to "-u", and leave any vector orthogonal to them unchanged. 
    
    The result is a "NaN" matrix if "u" and "v" are linearly dependent. *)

PROCEDURE PlaneRotation(angle: LONGREAL; READONLY u, v: VRep.T): T =
  VAR m: T;
      fu, fv: ARRAY [0..N-1] OF LONGREAL;
      tuu, tuv, tvv: LONGREAL := 0.0d0;
  BEGIN
    FOR i := 0 TO N-1 DO 
      WITH 
        fui = FLOAT(u[i], LONGREAL), 
        fvi = FLOAT(v[i], LONGREAL)
      DO
        fu[i] := fui; fv[i] := fvi;
        tuu := tuu + fui*fui;
        tuv := tuv + fui*fvi;
        tvv := tvv + fvi*fvi
      END
    END;
    WITH
      invdett = 1.0d0/(tuu*tvv - tuv*tuv),
      c = Math.cos(angle) - 1.0d0,
      s = Math.sin(Angle),
      auu = invdett * (+ c*tvv - s*tuv),
      auv = invdett * (- s*tvv - c*tuv),
      avu = invdett * (+ s*tuu - c*tuv),
      avv = invdett * (+ c*tuu + s*tuv)
    DO
      FOR i := 0 TO N-1 DO
        WITH mi = m[i], fui = fu[i], fvi = fv[i] DO
          FOR j := 0 TO N-1 DO
            WITH 
              fuj = fu[j], 
              fvj = fv[j],
              fmij = (fui*auu + fvi*auv)*fuj + (fui*avu + fvi*avv)*fvj
            DO
              IF i = j THEN 
                m[i,j] := FLOAT(fmij + 1.0d0)
              ELSE
                m[i,j] := fmij
              END
            END
          END
        END
      END
    END;
    RETURN m
  END PlaneRotation;
                                                       


In matrix interfaces: Get row, get column, SET row, SET column. 

(* In Vector2Extras: *)

PROCEDURE Sin(READONLY a, b: T): LONGREAL;
  (*
    The sine of the counterclockwise angle from "a" to "b". *)
    
PROCEDURE Sin(READONLY a, b: T): LONGREAL =
  BEGIN 
    WITH 
      a0 = FLOAT(a[0], LONGREAL),
      a1 = FLOAT(a[1], LONGREAL),
      
      b0 = FLOAT(b[0], LONGREAL),
      b1 = FLOAT(b[1], LONGREAL),

      t = a0*b1 - a1*b0,
      sa = a0*a0 + a1*a1,
      sb = b0*b0 + b1*b1
    DO
      RETURN t/(Math.sqrt(sa)*Math.sqrt(sb))
    END
  END Sin;
    
(* from Stolfi's original "R2.m3" *******************************)

PROCEDURE Cos (READONLY x, y: T): REAL =
  VAR xy, xx, yy: LONGREAL;
      tx, ty, mx, my: REAL;
  BEGIN
    (* Compute rescaling factors to avoid spurious overflow: *)

    mx := MAX(ABS(x[0]), ABS(x[1]));
    my := MAX(ABS(y[0]), ABS(y[1]));

    (* Now collect dot product and lengths (rescaled): *)

    tx := x[0]/mx; ty := y[0]/my;
    xx := FLOAT(tx*tx, LONGREAL); yy := FLOAT(ty*ty, LONGREAL);
    xy := FLOAT(tx, LONGREAL)*FLOAT(ty, LONGREAL);

    tx := x[1]/mx; ty := y[1]/my;
    xx := xx + FLOAT(tx*tx, LONGREAL); yy := yy + FLOAT(ty*ty, LONGREAL);
    xy := xy + FLOAT(tx, LONGREAL) * FLOAT(ty, LONGREAL);

    xx := 1.0d0 / Math.sqrt(FLOAT(xx*yy, LONGREAL));
    xx := xx*xy;
    RETURN FLOAT(xx)
  END Cos;

PROCEDURE Equal(READONLY x, y: T): BOOLEAN =
  BEGIN
    RETURN (x[0] = y[0])
      AND  (x[1] = y[1])

  END Equal;

PROCEDURE IsZero(READONLY x: T): BOOLEAN =
  BEGIN
    RETURN (x[0] = 0.0)
      AND  (x[1] = 0.0)

  END IsZero;

PROCEDURE Shift(READONLY x: T; delta: REAL): T =
  VAR rr: T;
  BEGIN
    rr[0] := x[0] + delta;
    rr[1] := x[1] + delta;

    RETURN rr
  END Shift;

PROCEDURE FMap(READONLY x: T; F: Function): T =
  VAR rr: T;
  BEGIN
    rr[0] := F(x[0]);
    rr[1] := F(x[1]);

    RETURN rr
  END FMap;

PROCEDURE Sum (READONLY x: T): REAL =
  VAR dd: LONGREAL;
  BEGIN
    dd :=
        FLOAT(x[0], LONGREAL)
      + FLOAT(x[1], LONGREAL)

      ;
    RETURN FLOAT(dd)
  END Sum;

PROCEDURE Max(READONLY x: T): REAL =
  BEGIN

    RETURN MAX(x[0], x[1])

  END Max;

PROCEDURE MaxAbsAxis(READONLY x: T): Axis =
  VAR
    a: Axis;
  BEGIN
    IF ABS(x[0]) > ABS(x[1]) THEN a := 0 ELSE a := 1 END;

    RETURN a;
  END MaxAbsAxis;

PROCEDURE Min(READONLY x: T): REAL =
  BEGIN

    RETURN MIN(x[0], x[1])

  END Min;

PROCEDURE RelDist(READONLY x, y: T; eps: REAL := 1.0e-37): REAL =
  VAR u, v: REAL;
      s, m: REAL;
  BEGIN
    s := 0.0;
    FOR i := 0 TO 1 DO
      u := x[i]; v := y[i];
      m := MAX(MAX(ABS(u), ABS(v)), eps);
      s := MAX(ABS(u/m - v/m) - eps/m, s);
    END;
    RETURN s
  END RelDist;

PROCEDURE ToText(READONLY x: T): TEXT =
BEGIN
  RETURN
    "(" & Fmt.Real(x[0]) &
    " " & Fmt.Real(x[1]) &

    ")";
END ToText;

(*************************** Fom Mark Njork's "Matrix4.m3" *)

PROCEDURE Translate (READONLY M : T; x, y, z : REAL) : T =
   BEGIN

 <* ASSERT M[3][0] = 0.0 *> <* ASSERT M[3][1] = 0.0 *> <* ASSERT M[3][2] = 0.0 *> <* ASSERT
 M[3][3] = 1.0 *> 

     RETURN T {Row{M[0][0], M[0][1], M[0][2], M[0][3] + x},
               Row{M[1][0], M[1][1], M[1][2], M[1][3] + y},
               Row{M[2][0], M[2][1], M[2][2], M[2][3] + z},
               Row{0.0, 0.0, 0.0, 1.0}};
  END Translate;

PROCEDURE Scale (READONLY M : T; x, y, z : REAL) : T =
  VAR
    N := T {Row {  x, 0.0, 0.0, 0.0},
            Row {0.0,   y, 0.0, 0.0},
            Row {0.0, 0.0,   z, 0.0},
            Row {0.0, 0.0, 0.0, 1.0}};
  BEGIN
    RETURN Multiply (N, M);
  END Scale;

PROCEDURE Transpose (READONLY M : T) : T =
  BEGIN
    RETURN T {Row {M[0][0], M[1][0], M[2][0], M[3][0]},
              Row {M[0][1], M[1][1], M[2][1], M[3][1]},
              Row {M[0][2], M[1][2], M[2][2], M[3][2]},
              Row {M[0][3], M[1][3], M[2][3], M[3][3]}};
  END Transpose;

PROCEDURE TransformUnitCube (p, a, b, c : Point3.T) : T =
  BEGIN
    RETURN T {Row {a.x - p.x, b.x - p.x, c.x - p.x, p.x},
              Row {a.y - p.y, b.y - p.y, c.y - p.y, p.y},
              Row {a.z - p.z, b.z - p.z, c.z - p.z, p.z},
              Row {      0.0,       0.0,       0.0, 1.0}};
  END TransformUnitCube;

PROCEDURE Equal (READONLY A, B : T) : BOOLEAN =
  CONST
    eps = 0.0005;
  BEGIN
    FOR i := 0 TO 3 DO
      FOR j := 0 TO 3 DO
        IF ABS (A[i][j] - B[i][j]) > eps THEN
          RETURN FALSE;
        END;
      END;
    END;
    RETURN TRUE;
  END Equal;

Basic Assertions: (1) M has been created by combining rotations, translations, and uniform(!) scalings. (2) s > 0 
PROCEDURE Decompose ((* in *)  M : T;
                     (* out *) VAR tx, ty, tz, s, angX, angY, angZ : REAL) =
  VAR
    a, b, c: Point3.T;
  BEGIN
    <* ASSERT M[3][0] = 0.0 *>
    <* ASSERT M[3][1] = 0.0 *>
    <* ASSERT M[3][2] = 0.0 *>
    <* ASSERT M[3][3] = 1.0 *>

    (* separate the translation component *)
    tx := M[0][3];
    ty := M[1][3];
    tz := M[2][3];

    (* remove the translation component from M *)
    M[0][3] := 0.0;
    M[1][3] := 0.0;
    M[2][3] := 0.0;

    (* We assumed uniform scaling, which makes it very easy to determine s. *)
    WITH p0 = Point3.T {1.0, 0.0, 0.0},
         p1 = TransformPoint3 (M, p0) DO
      s := Point3.Length (p1);
    END;

    (* Also, for a uniform scaling S, SM = MS for any matrix M.
       So, we can remove S easily. *)
    FOR i := 0 TO 2 DO
      FOR j := 0 TO 2 DO
        M[i][j] := M[i][j] / s;
      END;
    END;

    (* Take three orthogonal unit vectors *)
    a := Point3.T {1.0, 0.0, 0.0};
    b := Point3.T {0.0, 1.0, 0.0};
    c := Point3.T {0.0, 0.0, 1.0};

    (* Apply M to them *)
    a := TransformPoint3 (M, a);
    b := TransformPoint3 (M, b);
    c := TransformPoint3 (M, c);

    (* We want to rotate vector "a" around the z axis such that it falls into
       the x-z plane. So, we need to find the angle "angZ" between the
       projection of "a" onto the x-y plane and the z axis. *)
    IF a.y = 0.0 THEN
      (* If "a.y" = 0, then "a" is already in the x-z plane. *)
      angZ := 0.0;
    ELSE
      (* a.y # 0, hence Length ( (a.x, 0, a.y) ) > 0 *)
      angZ := - Mth.asin (a.y / Point3.Length (Point3.T {a.x, a.y, 0.0}));
    END;
    IF a.x < 0.0 THEN
      angZ := Math.Pi - angZ;
    END;
    WITH N = RotateZ (Id, angZ) DO
      a := TransformPoint3 (N, a);
      b := TransformPoint3 (N, b);
      c := TransformPoint3 (N, c);
    END;

    (* We want to rotate vector "a" around the y axis such that it falls onto
       the x axis. So, we need to find the angle "angY" between "a" and the
       x axis. Note that the previous rotation moved "a" into the x-z plane,
       hence "a.y" is 0, hence we do not need to project "a" onto any plane.
       Also, note that "a" is guaranteed to have a positive length. *)
    angY := Mth.asin (a.z / Point3.Length (a));
    IF a.x < 0.0 THEN
      angY := Math.Pi - angY;
    END;
    WITH N = RotateY (Id, angY) DO
      a := TransformPoint3 (N, a);
      b := TransformPoint3 (N, b);
      c := TransformPoint3 (N, c);
    END;

    (* At this point, "a" should be lying on the positive half of x axis,
       and "b" and "c" should both be lying in the y-z plane. We want to
       rotate "b" around the x axis so that it lies on the positive half
       of the y axis. *)
    angX := - Mth.asin (b.z / Point3.Length (b));
    IF b.y < 0.0 THEN
      angX := Math.Pi - angX;
    END;
    WITH N = RotateX (Id, angX) DO
      a := TransformPoint3 (N, a);
      b := TransformPoint3 (N, b);
      c := TransformPoint3 (N, c);
    END;

    angX := - angX;
    angY := - angY;
    angZ := - angZ;
  END Decompose;

Basic Assertions: (1) M has been created by combining rotations, translations, and uniform(!) scalings. (2) s > 0 
PROCEDURE Decomp (M : T; VAR tx, ty, tz, s : REAL) : T RAISES {Error} =
  BEGIN
    <* ASSERT M[3][0] = 0.0 *>
    <* ASSERT M[3][1] = 0.0 *>
    <* ASSERT M[3][2] = 0.0 *>
    <* ASSERT M[3][3] = 1.0 *>

    (* separate the translation component *)
    tx := M[0][3];
    ty := M[1][3];
    tz := M[2][3];

    (* remove the translation component from M *)
    M[0][3] := 0.0;
    M[1][3] := 0.0;
    M[2][3] := 0.0;

    (* We assumed uniform scaling, which makes it very easy to determine s. *)
    WITH p0 = Point3.T {1.0, 0.0, 0.0},
         p1 = TransformPoint3 (M, p0) DO
      s := Point3.Length (p1);
    END;

    (* Also, for a uniform scaling S, SM = MS for any matrix M.
       So, we can remove S easily. *)
    FOR i := 0 TO 2 DO
      FOR j := 0 TO 2 DO
        M[i][j] := M[i][j] / s;
      END;
    END;

    IF NOT Orthonormal (M) THEN
      RAISE Error;
    ELSE
      RETURN M;
    END;
  END Decomp;

 (* From "point3.m3" by Mark Najork *****************************)
 
 PROCEDURE OrthoVector (n : T) : T =
  (* We are looking for a unit vector "m" that is orthogonal to "n". So, we
     have the following two equations to start with:
     (1) "m" orthogonal "n", so "DotProduct(m,n)" = 0,
         so m.x * n.x + m.y * n.y + m.z * n.z = 0
     (2) "m" is a unit vector, so sqrt(m.x^2 + m.y^2 + m.z^2) = 1
     So we have 3 unknowns (m.x, m.y, and m.z) and 3 equations, leaving us
     with one degree of freedom.
     If n.x # 0, and we set m.z to 0, we can solve the system to:
        m.xx = 1 / sqrt(1 + (n.x^2 / n.y^2))
        m.y = - (n.x / n.y) / sqrt(1 + (n.x^2 / n.y^2))
     The cases for n.y # 0 and n.z = 0 are similar.
     Passing n = Origin is a fatal error.
  *)
  BEGIN
    IF n.x # 0.0 THEN
      WITH p = n.y / n.x, sub = 1.0 / Mth.sqrt (1.0 + p * p) DO
        RETURN T {-p * sub, sub, 0.0};
      END;
    ELSIF n.y # 0.0 THEN
      WITH p = n.x / n.y, sub = 1.0 / Mth.sqrt (1.0 + p * p) DO
        RETURN T {sub, -p * sub, 0.0};
      END;
    ELSIF n.z # 0.0 THEN
      WITH p = n.x / n.z, sub = 1.0 / Mth.sqrt (1.0 + p * p) DO
        RETURN T {sub, 0.0, -p * sub};
      END;
    ELSE
      Process.Crash ("Fatal Error: called OrthoVector(Origin) \n");
      RETURN Origin;      (* ... only to suppress compiler warnings *)
    END;
  END OrthoVector;

(* Eigenvectors/eigenvalues/SVD *)

PROCEDURE Det(READONLY m: ARRAY [0..N-1] OF T): LONGREAL;
  (*
    Returns the determinant of the matrix whose rows are "m[0..N-1]". *)

PROCEDURE Cross(READONLY m: ARRAY [0..N-2] OF T): T;
  (*
    Returns the `cross product' of the vectors "m[0]" through "m[N-2]",
    defined as a vector "r" such that 
|     "Dot(r, b) = Det(Tuple{m[0],.. m[N-2], b})" 
    for all "b". *)

PROCEDURE Det(READONLY m: ARRAY [0..N-1] OF T): LONGREAL =
  VAR t: ARRAY [0..N-1] OF T := m;
      r: LONGREAL := 1.0d0;
  BEGIN
    VecN.Triangularize(t, total := FALSE);
    FOR i := 0 TO N-1 DO
      r := r * FLOAT(t[i,i], LONGREAL);
      IF r = 0.0d0 THEN RETURN r END
    END;
    RETURN r
  END Det;

PROCEDURE Cross(READONLY m: ARRAY [0..N-2] OF T): T =
  VAR t: ARRAY [0..N-2] OF T := m;
      fr: LongRealT;
      r: T;
      s: LONGREAL;
  BEGIN
    VecN.Triangularize(t, total := FALSE);
    fr[N-1] := 1.0d0;
    FOR i := N-2 TO 0 BY -1 DO 
      (* Compute "r[i]" so that "r" is orthogonal to "t[i]": *)
      WITH 
        ti = t[i], 
        ftii = FLOAT(ti[i], LONGREAL)
      DO
        s := 0.0d0;
        FOR j := i+1 TO N-1 DO
          WITH frj = fr[j], ftij = FLOAT(ti[j], LONGREAL) DO 
            s := s + frj*ftij;
            frj := frj*ftii
          END;
          fr[i] := -s;
        END
      END
    END;
    FOR i := 0 TO N-1 DO r[i] := FLOAT(fr[i], ElemT) END;
    RETURN r
  END Cross;