MODULE R3x3 EXPORTS R3x3, R3x3Extras;
(*
Hand-optimized implementation of the interfaces "R3x3" and "R3x3Extras".
Created by J. Stolfi, with contributions by M. C. Carrard. *)
IMPORT R3Rep, R3x3Rep, R3Extras, RMxN, Wr, Thread, Math;
TYPE RowT = ARRAY [0..N-1] OF ElemT;
PROCEDURE Null(): T =
BEGIN
RETURN T{RowT{0.0, ..}, ..}
END Null;
PROCEDURE Identity(): T =
BEGIN
RETURN T{
RowT{1.0, 0.0, 0.0},
RowT{0.0, 1.0, 0.0},
RowT{0.0, 0.0, 1.0}
}
END Identity;
PROCEDURE ToReal(READONLY m: T): RealT =
BEGIN
RETURN m
END ToReal;
PROCEDURE ToLongReal(READONLY m: T): LongRealT =
VAR lm: LongRealT;
BEGIN
FOR i := 0 TO N-1 DO
FOR j := 0 TO N-1 DO
lm[i,j] := FLOAT(m[i,j], LONGREAL)
END
END;
RETURN lm
END ToLongReal;
PROCEDURE FromReal(READONLY c: RealT): T =
BEGIN
RETURN c
END FromReal;
PROCEDURE FromLongReal(READONLY c: LongRealT): 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 FromLongReal;
PROCEDURE Get(READONLY m: T; i, j: [0..N-1]): LONGREAL =
BEGIN
RETURN FLOAT(m[i, j], LONGREAL)
END Get;
PROCEDURE Set(VAR m: T; i, j: [0..N-1]; x: LONGREAL) =
BEGIN
m[i, j] := FLOAT(x, ElemT)
END Set;
PROCEDURE MapRow(READONLY a: R3Rep.T; READONLY m: T): R3Rep.T =
VAR r: R3Rep.T;
BEGIN
WITH
a0 = FLOAT(a[0], LONGREAL),
a1 = FLOAT(a[1], LONGREAL),
a2 = FLOAT(a[2], LONGREAL)
DO
WITH
m00 = FLOAT(m[0,0], LONGREAL),
m10 = FLOAT(m[1,0], LONGREAL),
m20 = FLOAT(m[2,0], LONGREAL)
DO
r[0] := FLOAT(a0*m00 + a1*m10 + a2*m20)
END;
WITH
m01 = FLOAT(m[0,1], LONGREAL),
m11 = FLOAT(m[1,1], LONGREAL),
m21 = FLOAT(m[2,1], LONGREAL)
DO
r[1] := FLOAT(a0*m01 + a1*m11 + a2*m21)
END;
WITH
m02 = FLOAT(m[0,2], LONGREAL),
m12 = FLOAT(m[1,2], LONGREAL),
m22 = FLOAT(m[2,2], LONGREAL)
DO
r[2] := FLOAT(a0*m02 + a1*m12 + a2*m22)
END
END;
RETURN r
END MapRow;
PROCEDURE MapCol(READONLY m: T; READONLY a: R3Rep.T): R3Rep.T =
VAR r: R3Rep.T;
BEGIN
WITH
a0 = FLOAT(a[0], LONGREAL),
a1 = FLOAT(a[1], LONGREAL),
a2 = FLOAT(a[2], LONGREAL)
DO
WITH
m00 = FLOAT(m[0,0], LONGREAL),
m01 = FLOAT(m[0,1], LONGREAL),
m02 = FLOAT(m[0,2], LONGREAL)
DO
r[0] := FLOAT(m00*a0 + m01*a1 + m02*a2)
END;
WITH
m10 = FLOAT(m[1,0], LONGREAL),
m11 = FLOAT(m[1,1], LONGREAL),
m12 = FLOAT(m[1,2], LONGREAL)
DO
r[1] := FLOAT(m10*a0 + m11*a1 + m12*a2)
END;
WITH
m20 = FLOAT(m[2,0], LONGREAL),
m21 = FLOAT(m[2,1], LONGREAL),
m22 = FLOAT(m[2,2], LONGREAL)
DO
r[2] := FLOAT(m20*a0 + m21*a1 + m22*a2)
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 = FLOAT(m[0,0], LONGREAL),
m01 = FLOAT(m[0,1], LONGREAL),
m02 = FLOAT(m[0,2], LONGREAL),
m10 = FLOAT(m[1,0], LONGREAL),
m11 = FLOAT(m[1,1], LONGREAL),
m12 = FLOAT(m[1,2], LONGREAL),
m20 = FLOAT(m[2,0], LONGREAL),
m21 = FLOAT(m[2,1], LONGREAL),
m22 = FLOAT(m[2,2], LONGREAL),
m01_01 = m00 * m11 - m01 * m10,
m01_02 = m00 * m12 - m02 * m10,
m01_12 = m01 * m12 - m02 * m11,
m02_01 = m00 * m21 - m01 * m20,
m02_02 = m00 * m22 - m02 * m20,
m02_12 = m01 * m22 - m02 * m21,
m12_01 = m10 * m21 - m11 * m20,
m12_02 = m10 * m22 - m12 * m20,
m12_12 = m11 * m22 - m12 * m21
DO
RETURN T{
R3Rep.T{FLOAT(+m12_12), FLOAT(-m02_12), FLOAT(+m01_12)},
R3Rep.T{FLOAT(-m12_02), FLOAT(+m02_02), FLOAT(-m01_02)},
R3Rep.T{FLOAT(+m12_01), FLOAT(-m02_01), FLOAT(+m01_01)}
}
END;
END Adj;
PROCEDURE Inv(READONLY m: T): T =
BEGIN
WITH
m00 = FLOAT(m[0,0], LONGREAL),
m01 = FLOAT(m[0,1], LONGREAL),
m02 = FLOAT(m[0,2], LONGREAL),
m10 = FLOAT(m[1,0], LONGREAL),
m11 = FLOAT(m[1,1], LONGREAL),
m12 = FLOAT(m[1,2], LONGREAL),
m20 = FLOAT(m[2,0], LONGREAL),
m21 = FLOAT(m[2,1], LONGREAL),
m22 = FLOAT(m[2,2], LONGREAL),
m01_01 = m00 * m11 - m01 * m10,
m01_02 = m00 * m12 - m02 * m10,
m01_12 = m01 * m12 - m02 * m11,
m02_01 = m00 * m21 - m01 * m20,
m02_02 = m00 * m22 - m02 * m20,
m02_12 = m01 * m22 - m02 * m21,
m12_01 = m10 * m21 - m11 * m20,
m12_02 = m10 * m22 - m12 * m20,
m12_12 = m11 * m22 - m12 * m21,
d = + m01_01 * m22 - m01_02 * m21 + m01_12 * m20
DO
RETURN T{
R3Rep.T{FLOAT(+ m12_12/d), FLOAT(- m02_12/d), FLOAT(+ m01_12/d)},
R3Rep.T{FLOAT(- m12_02/d), FLOAT(+ m02_02/d), FLOAT(- m01_02/d)},
R3Rep.T{FLOAT(+ m12_01/d), FLOAT(- m02_01/d), FLOAT(+ m01_01/d)}
}
END
END Inv;
PROCEDURE Det(READONLY m: T): LONGREAL =
BEGIN
RETURN R3Extras.Det(m[0], m[1], m[2])
END Det;
PROCEDURE Print(
wr: Wr.T;
READONLY m: T;
olp := "( "; osep := "\n "; orp := " )";
ilp := "("; isep := " "; irp := ")";
fmt: PROCEDURE (x: ElemT): TEXT := R3x3Rep.DefaultElemFmt;
align: BOOLEAN := TRUE;
) RAISES {Wr.Failure, Thread.Alerted} =
BEGIN
RMxN.Print(wr, m, olp, osep, orp, ilp, isep, irp, fmt, align)
END Print;
PROCEDURE ToText(READONLY m: T): TEXT =
BEGIN
RETURN RMxN.ToText(m)
END ToText;
(* R3x3Extras *)
PROCEDURE Rotation(
angle: LONGREAL;
READONLY axis: R3Rep.T;
scale: LONGREAL := 1.0d0;
): T =
BEGIN
WITH
ct = scale * Math.cos(angle),
st = scale * Math.sin(angle),
rt = scale - ct,
ax = FLOAT(axis[0], LONGREAL),
ay = FLOAT(axis[1], LONGREAL),
az = FLOAT(axis[2], LONGREAL),
rtax = rt*ax,
rtay = rt*ay,
rtaz = rt*az,
rtaxx = rtax * ax,
rtayy = rtay * ay,
rtazz = rtaz * az,
rtaxy = rtax * ay,
rtaxz = rtax * az,
rtayz = rtay * az,
ctd = ct * (ax*ax + ay*ay + az*az),
stax = st * ax,
stay = st * ay,
staz = st * az
DO
RETURN T{
R3Rep.T{FLOAT(rtaxx + ctd), FLOAT(rtaxy + staz), FLOAT(rtaxz - stay)},
R3Rep.T{FLOAT(rtaxy - staz), FLOAT(rtayy + ctd), FLOAT(rtayz + stax)},
R3Rep.T{FLOAT(rtaxz + stay), FLOAT(rtayz - stax), FLOAT(rtazz + ctd) }
}
END
END Rotation;
BEGIN
END R3x3.