MODULE LR4x4 EXPORTS LR4x4, LR4x4Extras;
(*
Hand-optimized implementation OF the "LR3x3" interface.
Created by J. Stolfi with contributions by Marcos C. Carrard
and R. L. W. Liesenfeld. *)
IMPORT LR4Rep, LR4x4Rep, LR4Extras, LRMxN, Wr, Thread;
TYPE RowT = ARRAY [0..N-1] OF ElemT;
PROCEDURE Null(): T =
BEGIN
RETURN T{RowT{0.0d0, ..}, ..}
END Null;
PROCEDURE Identity(): T =
BEGIN
RETURN T{
RowT{1.0d0, 0.0d0, 0.0d0, 0.0d0},
RowT{0.0d0, 1.0d0, 0.0d0, 0.0d0},
RowT{0.0d0, 0.0d0, 1.0d0, 0.0d0},
RowT{0.0d0, 0.0d0, 0.0d0, 1.0d0}
}
END Identity;
PROCEDURE ToReal(READONLY m: T): RealT =
VAR rm: RealT;
BEGIN
FOR i := 0 TO N-1 DO
FOR j := 0 TO N-1 DO
rm[i,j] := FLOAT(m[i,j], REAL)
END
END;
RETURN rm
END ToReal;
PROCEDURE ToLongReal(READONLY m: T): LongRealT =
BEGIN
RETURN m
END ToLongReal;
PROCEDURE FromReal(READONLY c: RealT): T =
VAR m: T;
BEGIN
FOR i := 0 TO N-1 DO
FOR j := 0 TO N-1 DO
m[i,j] := FLOAT(c[i,j], ElemT)
END
END;
RETURN m
END FromReal;
PROCEDURE FromLongReal(READONLY c: LongRealT): T =
BEGIN
RETURN c
END FromLongReal;
PROCEDURE Get(READONLY m: T; i, j: [0..N-1]): LONGREAL =
BEGIN
RETURN m[i, j]
END Get;
PROCEDURE Set(VAR m: T; i, j: [0..N-1]; x: LONGREAL) =
BEGIN
m[i, j] := x
END Set;
PROCEDURE MapRow(READONLY a: LR4Rep.T; READONLY m: T): LR4Rep.T =
VAR r: LR4Rep.T;
BEGIN
WITH
a0 = a[0], a1 = a[1], a2 = a[2], a3 = a[3]
DO
WITH m00 = m[0,0], m10 = m[1,0], m20 = m[2,0], m30 = m[3,0] DO
r[0] := a0*m00 + a1*m10 + a2*m20 + a3*m30
END;
WITH m01 = m[0,1], m11 = m[1,1], m21 = m[2,1], m31 = m[3,1] DO
r[1] := a0*m01 + a1*m11 + a2*m21 + a3*m31
END;
WITH m02 = m[0,2], m12 = m[1,2], m22 = m[2,2], m32 = m[3,2] DO
r[2] := a0*m02 + a1*m12 + a2*m22 + a3*m32
END;
WITH m03 = m[0,3], m13 = m[1,3], m23 = m[2,3], m33 = m[3,3] DO
r[3] := a0*m03 + a1*m13 + a2*m23 + a3*m33
END
END;
RETURN r
END MapRow;
PROCEDURE MapCol(READONLY m: T; READONLY a: LR4Rep.T): LR4Rep.T =
VAR r: LR4Rep.T;
BEGIN
WITH
a0 = a[0],
a1 = a[1],
a2 = a[2],
a3 = a[3]
DO
WITH m00 = m[0,0], m01 = m[0,1], m02 = m[0,2], m03 = m[0,3] DO
r[0] := m00*a0 + m01*a1 + m02*a2 + m03*a3
END;
WITH m10 = m[1,0], m11 = m[1,1], m12 = m[1,2], m13 = m[1,3] DO
r[1] := m10*a0 + m11*a1 + m12*a2 + m13*a3
END;
WITH m20 = m[2,0], m21 = m[2,1], m22 = m[2,2], m23 = m[2,3] DO
r[2] := m20*a0 + m21*a1 + m22*a2 + m23*a3
END;
WITH m30 = m[3,0], m31 = m[3,1], m32 = m[3,2], m33 = m[3,3] DO
r[3] := m30*a0 + m31*a1 + m32*a2 + m33*a3
END
END;
RETURN r
END MapCol;
PROCEDURE Mul(READONLY m, n: T): T =
VAR r: T;
BEGIN
FOR i := 0 TO N-1 DO
r[i] := MapRow(m[i], n)
END;
RETURN r
END Mul;
PROCEDURE Adj(READONLY m: T): T =
BEGIN
WITH
m00 = m[0,0], m01 = m[0,1], m02 = m[0,2], m03 = m[0,3],
m10 = m[1,0], m11 = m[1,1], m12 = m[1,2], m13 = m[1,3],
m20 = m[2,0], m21 = m[2,1], m22 = m[2,2], m23 = m[2,3],
m30 = m[3,0], m31 = m[3,1], m32 = m[3,2], m33 = m[3,3],
m01_01 = m00 * m11 - m01 * m10,
m01_02 = m00 * m12 - m02 * m10,
m01_12 = m01 * m12 - m02 * m11,
m01_03 = m00 * m13 - m03 * m10,
m01_13 = m01 * m13 - m03 * m11,
m01_23 = m02 * m13 - m03 * m12,
m23_01 = m20 * m31 - m21 * m30,
m23_02 = m20 * m32 - m22 * m30,
m23_12 = m21 * m32 - m22 * m31,
m23_03 = m20 * m33 - m23 * m30,
m23_13 = m21 * m33 - m23 * m31,
m23_23 = m22 * m33 - m23 * m32,
m012_012 = m01_01 * m22 - m01_02 * m21 + m01_12 * m20,
m012_013 = m01_01 * m23 - m01_03 * m21 + m01_13 * m20,
m012_023 = m01_02 * m23 - m01_03 * m22 + m01_23 * m20,
m012_123 = m01_12 * m23 - m01_13 * m22 + m01_23 * m21,
m013_012 = m01_01 * m32 - m01_02 * m31 + m01_12 * m30,
m013_013 = m01_01 * m33 - m01_03 * m31 + m01_13 * m30,
m013_023 = m01_02 * m33 - m01_03 * m32 + m01_23 * m30,
m013_123 = m01_12 * m33 - m01_13 * m32 + m01_23 * m31,
m023_012 = m00 * m23_12 - m01 * m23_02 + m02 * m23_01,
m023_013 = m00 * m23_13 - m01 * m23_03 + m03 * m23_01,
m023_023 = m00 * m23_23 - m02 * m23_03 + m03 * m23_02,
m023_123 = m01 * m23_23 - m02 * m23_13 + m03 * m23_12,
m123_012 = m10 * m23_12 - m11 * m23_02 + m12 * m23_01,
m123_013 = m10 * m23_13 - m11 * m23_03 + m13 * m23_01,
m123_023 = m10 * m23_23 - m12 * m23_03 + m13 * m23_02,
m123_123 = m11 * m23_23 - m12 * m23_13 + m13 * m23_12
DO
RETURN T{
LR4Rep.T{+ m123_123, - m023_123, + m013_123, - m012_123},
LR4Rep.T{- m123_023, + m023_023, - m013_023, + m012_023},
LR4Rep.T{+ m123_013, - m023_013, + m013_013, - m012_013},
LR4Rep.T{- m123_012, + m023_012, - m013_012, + m012_012}
}
END
END Adj;
PROCEDURE Inv(READONLY m: T): T =
BEGIN
WITH
m00 = m[0,0], m01 = m[0,1], m02 = m[0,2], m03 = m[0,3],
m10 = m[1,0], m11 = m[1,1], m12 = m[1,2], m13 = m[1,3],
m20 = m[2,0], m21 = m[2,1], m22 = m[2,2], m23 = m[2,3],
m30 = m[3,0], m31 = m[3,1], m32 = m[3,2], m33 = m[3,3],
m01_01 = m00 * m11 - m01 * m10,
m01_02 = m00 * m12 - m02 * m10,
m01_12 = m01 * m12 - m02 * m11,
m01_03 = m00 * m13 - m03 * m10,
m01_13 = m01 * m13 - m03 * m11,
m01_23 = m02 * m13 - m03 * m12,
m23_01 = m20 * m31 - m21 * m30,
m23_02 = m20 * m32 - m22 * m30,
m23_12 = m21 * m32 - m22 * m31,
m23_03 = m20 * m33 - m23 * m30,
m23_13 = m21 * m33 - m23 * m31,
m23_23 = m22 * m33 - m23 * m32,
m012_012 = m01_01 * m22 - m01_02 * m21 + m01_12 * m20,
m012_013 = m01_01 * m23 - m01_03 * m21 + m01_13 * m20,
m012_023 = m01_02 * m23 - m01_03 * m22 + m01_23 * m20,
m012_123 = m01_12 * m23 - m01_13 * m22 + m01_23 * m21,
m013_012 = m01_01 * m32 - m01_02 * m31 + m01_12 * m30,
m013_013 = m01_01 * m33 - m01_03 * m31 + m01_13 * m30,
m013_023 = m01_02 * m33 - m01_03 * m32 + m01_23 * m30,
m013_123 = m01_12 * m33 - m01_13 * m32 + m01_23 * m31,
m023_012 = m00 * m23_12 - m01 * m23_02 + m02 * m23_01,
m023_013 = m00 * m23_13 - m01 * m23_03 + m03 * m23_01,
m023_023 = m00 * m23_23 - m02 * m23_03 + m03 * m23_02,
m023_123 = m01 * m23_23 - m02 * m23_13 + m03 * m23_12,
m123_012 = m10 * m23_12 - m11 * m23_02 + m12 * m23_01,
m123_013 = m10 * m23_13 - m11 * m23_03 + m13 * m23_01,
m123_023 = m10 * m23_23 - m12 * m23_03 + m13 * m23_02,
m123_123 = m11 * m23_23 - m12 * m23_13 + m13 * m23_12,
d = m012_012 * m33 - m012_013 * m32 + m012_023 * m31 - m012_123 * m30
DO
RETURN T{
LR4Rep.T{+ m123_123/d, - m023_123/d, + m013_123/d, - m012_123/d},
LR4Rep.T{- m123_023/d, + m023_023/d, - m013_023/d, + m012_023/d},
LR4Rep.T{+ m123_013/d, - m023_013/d, + m013_013/d, - m012_013/d},
LR4Rep.T{- m123_012/d, + m023_012/d, - m013_012/d, + m012_012/d}
}
END
END Inv;
PROCEDURE Det(READONLY m: T): LONGREAL =
BEGIN
RETURN LR4Extras.Det(m[0], m[1], m[2], m[3])
END Det;
PROCEDURE Print(
wr: Wr.T;
READONLY m: T;
olp := "( "; osep := "\n "; orp := " )";
ilp := "("; isep := " "; irp := ")";
fmt: PROCEDURE (x: ElemT): TEXT := LR4x4Rep.DefaultElemFmt;
align: BOOLEAN := TRUE;
) RAISES {Wr.Failure, Thread.Alerted} =
BEGIN
LRMxN.Print(wr, m, olp, osep, orp, ilp, isep, irp, fmt, align)
END Print;
PROCEDURE ToText(READONLY m: T): TEXT =
BEGIN
RETURN LRMxN.ToText(m)
END ToText;
BEGIN
END LR4x4.