(* Copyright (C) 1992, Digital Equipment Corporation                         *)
(* All rights reserved.                                                      *)
(* See the file COPYRIGHT for a full description.                            *)
(*                                                                           *)
(* Created on Sep 15 1988 by Jorge Stolfi                      *)
(* Last modified on Tue Jun 16 18:29:57 PDT 1992 by muller     *)
(*      modified on Fri Nov 22 20:20:23 PST 1991 by stolfi     *)
(*      modified on Wed Jan  3 21:52:03 1990 by harrison       *)

MODULE R4x4;

IMPORT R4;

(********************************************************************)
(*                                                                  *)
(* WARNING: DO NOT EDIT THIS FILE.  IT WAS GENERATED MECHANICALLY.  *)
(* See the  Makefile for more details.                              *)
(*                                                                  *)
(********************************************************************)

(* TO DO : Use double precision for all dot/wedge products. *)

(* TO DO : Write a better Inv, Det; write Adjoint, Cofactors *)

PROCEDURE FromRows(READONLY p0, p1, p2, p3: R4.T): Matrix =
  VAR A: Matrix;
  BEGIN
    FOR i := 0 TO 3 DO
      A[0,i] := p0[i];
      A[1,i] := p1[i];

      A[2,i] := p2[i];

      A[3,i] := p3[i];

    END;
    RETURN A
  END FromRows;

PROCEDURE Equal (READONLY A, B: Matrix): BOOLEAN =
  BEGIN
    FOR i := 0 TO 3 DO
      FOR j := 0 TO 3 DO
        IF A[i,j] # B[i,j] THEN RETURN FALSE END;
      END
    END;
    RETURN TRUE;
  END Equal;

PROCEDURE Zero (): Matrix =
  VAR A: Matrix;
  BEGIN
    FOR i := 0 TO 3 DO
      FOR j := 0 TO 3 DO
        A[i,j] := 0.0
      END
    END;
    RETURN A
  END Zero;

PROCEDURE Identity (): Matrix =
  VAR A: Matrix;
  BEGIN
    FOR i := 0 TO 3 DO
      FOR j := 0 TO 3 DO
        IF i = j THEN A[i,j] := 1.0 ELSE A[i,j] := 0.0 END
      END
    END;
    RETURN A
  END Identity;

PROCEDURE Scale (alpha: REAL; READONLY A: Matrix): Matrix =
  VAR B: Matrix;
  BEGIN
    FOR i := 0 TO 3 DO
      FOR j := 0 TO 3 DO
        B[i,j] := alpha * A[i,j]
      END
    END;
    RETURN B
  END Scale;

PROCEDURE Add (READONLY A, B: Matrix): Matrix =
  VAR C: Matrix;
  BEGIN
    FOR i := 0 TO 3 DO
      FOR j := 0 TO 3 DO
        C[i,j] := A[i,j] + B[i,j]
      END
    END;
    RETURN C
  END Add;

PROCEDURE Sub (READONLY A, B: Matrix): Matrix =
  VAR C: Matrix;
  BEGIN
    FOR i := 0 TO 3 DO
      FOR j := 0 TO 3 DO
        C[i,j] := A[i,j] - B[i,j]
      END
    END;
    RETURN C
  END Sub;

PROCEDURE Minus (READONLY A: Matrix): Matrix =
  VAR B: Matrix;
  BEGIN
    FOR i := 0 TO 3 DO
      FOR j := 0 TO 3 DO
        B[i,j] := - A[i,j]
      END
    END;
    RETURN B
  END Minus;

PROCEDURE Map (READONLY p: R4.T; READONLY A: Matrix): R4.T =
  VAR rr: R4.T;

  BEGIN

    rr[0] := FLOAT(
        FLOAT(p[0], LONGREAL)*FLOAT(A[0][0], LONGREAL)
      + FLOAT(p[1], LONGREAL)*FLOAT(A[1][0], LONGREAL)
      + FLOAT(p[2], LONGREAL)*FLOAT(A[2][0], LONGREAL)
      + FLOAT(p[3], LONGREAL)*FLOAT(A[3][0], LONGREAL)
    );
    rr[1] := FLOAT(
        FLOAT(p[0], LONGREAL)*FLOAT(A[0][1], LONGREAL)
      + FLOAT(p[1], LONGREAL)*FLOAT(A[1][1], LONGREAL)
      + FLOAT(p[2], LONGREAL)*FLOAT(A[2][1], LONGREAL)
      + FLOAT(p[3], LONGREAL)*FLOAT(A[3][1], LONGREAL)
    );
    rr[2] := FLOAT(
        FLOAT(p[0], LONGREAL)*FLOAT(A[0][2], LONGREAL)
      + FLOAT(p[1], LONGREAL)*FLOAT(A[1][2], LONGREAL)
      + FLOAT(p[2], LONGREAL)*FLOAT(A[2][2], LONGREAL)
      + FLOAT(p[3], LONGREAL)*FLOAT(A[3][2], LONGREAL)
    );
    rr[3] := FLOAT(
        FLOAT(p[0], LONGREAL)*FLOAT(A[0][3], LONGREAL)
      + FLOAT(p[1], LONGREAL)*FLOAT(A[1][3], LONGREAL)
      + FLOAT(p[2], LONGREAL)*FLOAT(A[2][3], LONGREAL)
      + FLOAT(p[3], LONGREAL)*FLOAT(A[3][3], LONGREAL)
    );

    RETURN rr
  END Map;

PROCEDURE Transpose (READONLY A: Matrix): Matrix =
  VAR B: Matrix;
  BEGIN
    FOR i := 0 TO 3 DO
      FOR j := 0 TO 3 DO
        B[i,j] := A[j,i]
      END
    END;
    RETURN B
  END Transpose;

PROCEDURE TrMap (READONLY A: Matrix; READONLY p: R4.T): R4.T =
  VAR rr: R4.T;

  BEGIN

    rr[0] := FLOAT(
        FLOAT(A[0][0], LONGREAL)*FLOAT(p[0], LONGREAL)
      + FLOAT(A[0][1], LONGREAL)*FLOAT(p[1], LONGREAL)
      + FLOAT(A[0][2], LONGREAL)*FLOAT(p[2], LONGREAL)
      + FLOAT(A[0][3], LONGREAL)*FLOAT(p[3], LONGREAL)
    );
    rr[1] := FLOAT(
        FLOAT(A[1][0], LONGREAL)*FLOAT(p[0], LONGREAL)
      + FLOAT(A[1][1], LONGREAL)*FLOAT(p[1], LONGREAL)
      + FLOAT(A[1][2], LONGREAL)*FLOAT(p[2], LONGREAL)
      + FLOAT(A[1][3], LONGREAL)*FLOAT(p[3], LONGREAL)
    );
    rr[2] := FLOAT(
        FLOAT(A[2][0], LONGREAL)*FLOAT(p[0], LONGREAL)
      + FLOAT(A[2][1], LONGREAL)*FLOAT(p[1], LONGREAL)
      + FLOAT(A[2][2], LONGREAL)*FLOAT(p[2], LONGREAL)
      + FLOAT(A[2][3], LONGREAL)*FLOAT(p[3], LONGREAL)
    );
    rr[3] := FLOAT(
        FLOAT(A[3][0], LONGREAL)*FLOAT(p[0], LONGREAL)
      + FLOAT(A[3][1], LONGREAL)*FLOAT(p[1], LONGREAL)
      + FLOAT(A[3][2], LONGREAL)*FLOAT(p[2], LONGREAL)
      + FLOAT(A[3][3], LONGREAL)*FLOAT(p[3], LONGREAL)
    );

    RETURN rr
  END TrMap;

PROCEDURE Row(READONLY A: Matrix; i: Axis): R4.T =
  VAR r: R4.T;
  BEGIN

    FOR j := 0 TO 3 DO r[j] := A[i,j] END;

    RETURN r
  END Row;

PROCEDURE Col(READONLY A: Matrix; j: Axis): R4.T =
  VAR r: R4.T;
  BEGIN

    FOR i := 0 TO 3 DO r[i] := A[i,j] END;

    RETURN r
  END Col;

PROCEDURE Diagonal(READONLY A: Matrix): R4.T =
  VAR r: R4.T;
  BEGIN

    FOR i := 0 TO 3 DO r[i] := A[i,i] END;

    RETURN r
  END Diagonal;

PROCEDURE Mul (READONLY A, B: Matrix): Matrix =
  VAR MN: Matrix;
      s: LONGREAL;
  BEGIN
    FOR i := 0 TO 3 DO
      FOR j := 0 TO 3 DO
        s := 0.0D0;
        FOR t := 0 TO 3 DO
          s := s + FLOAT(A[i,t], LONGREAL) * FLOAT(B[t,j], LONGREAL)
        END;
        MN [i,j] := FLOAT(s)
      END;
    END;
    RETURN MN
  END Mul;

PROCEDURE Triangularize(VAR A, B: Matrix; useB: BOOLEAN) =
  VAR ipiv: NAT;
      abspiv, piv, c, t: REAL;
  BEGIN
    FOR i := 0 TO 2 DO 
      (* Find pivot row *)
      abspiv := ABS(A[i,i]); ipiv := i;
      FOR k := i + 1 TO 3 DO
        c := ABS(A[k,i]);
        IF c > abspiv THEN abspiv := c; ipiv := k END
      END;
      (* Permute equations to bring pivot to A[i,i] *)
      IF ipiv # i THEN
        FOR j := i TO 3 DO
          t := A[i,j]; A[i,j] := A[ipiv,j]; A[ipiv,j] := - t
        END;
        IF useB THEN
          FOR j := 0 TO 3 DO
            t := B[i,j]; B[i,j] := B[ipiv,j]; B[ipiv,j] := - t
          END;
        END;
      END;
      (* Eliminate variable i from equations i + 1..3 *)
      IF abspiv > 0.0 THEN
        piv := A[i,i];
        FOR k := i + 1 TO 3 DO
          c := A[k,i]/piv;
          IF c # 0.0 THEN
            A[k,i] := 0.0;
            FOR j := i + 1 TO 3 DO
              A[k,j] := A[k,j] - c * A[i,j]
            END;
            IF useB THEN
              FOR j := 0 TO 3 DO
                B[k,j] := B[k,j] - c * B[i,j]
              END;
            END;
          END;
        END;
      END;
    END;
  END Triangularize;

PROCEDURE Inv(READONLY M: Matrix): Matrix =
  VAR A, B: Matrix;
      t, diag: REAL;
  BEGIN
    (* !!! Needs improvement !!! *)
    A := M;
    B := Identity();
    Triangularize(A,B, TRUE);
    (* Now A is upper triangular; compute B := Inverse(A)*B *)
    FOR i := 3 TO 0 BY -1 DO
      diag := A[i,i];
      FOR j := 0 TO 3 DO
        (* Back-substitute known variables: *)
        t := B[i,j];
        FOR k := i + 1 TO 3 DO
          t := t - A[i,k] * B[k,j]
        END;
        (* Divide by diagonal element, checking for overflow: *)
        B[i,j] := t/diag
      END;
    END;
    RETURN B
  END Inv;

PROCEDURE Det (READONLY A: Matrix): REAL =
  (* !!!!! Should use double precision !!!!! *)
  BEGIN

    RETURN
      + (A[0,0]*A[1,1] - A[0,1]*A[1,0]) * (A[2,2]*A[3,3] - A[2,3]*A[3,2])
      - (A[0,0]*A[1,2] - A[0,2]*A[1,0]) * (A[2,1]*A[3,3] - A[2,3]*A[3,1])
      + (A[0,0]*A[1,3] - A[0,3]*A[1,0]) * (A[2,1]*A[3,2] - A[2,2]*A[3,1])
      + (A[0,1]*A[1,2] - A[0,2]*A[1,1]) * (A[2,0]*A[3,3] - A[2,3]*A[3,0])
      - (A[0,1]*A[1,3] - A[0,3]*A[1,1]) * (A[2,0]*A[3,2] - A[2,2]*A[3,0])
      + (A[0,2]*A[1,3] - A[0,3]*A[1,2]) * (A[2,0]*A[3,1] - A[2,1]*A[3,0])

  END Det;

BEGIN
END R4x4.