PROCEDURE Cof(m: T; ix, jx: CARDINAL): LONGREAL = VAR r, d: LONGREAL; t: R2x2.T; ii, jj: CARDINAL; BEGIN (* Extract minor *) ii := 0; FOR i := 0 TO 2 DO IF i # ix THEN jj := 0; FOR j := 0 TO 2 DO IF j # jx THEN t[ii,jj] := m[i,j]; INC(jj) END END; INC(ii) END END; (* Compute its determinant *) d := R2x2.Det(t); IF (ix + jx) MOD 2 = 1 THEN d := -d END; RETURN r END Cof; PROCEDURE AdjAux(READONLY m: T; adj, det: BOOLEAN; VAR a: T; VAR d: LONGREAL) = (* If "adj" is true, returns the adjoint of "m" in "a". If "det" is true, returns the determinant of "m" in "d". *) BEGIN WITH m_00 = FLOAT(m[0,0], LONGREAL), m_01 = FLOAT(m[0,1], LONGREAL), m_02 = FLOAT(m[0,2], LONGREAL), m_10 = FLOAT(m[1,0], LONGREAL), m_11 = FLOAT(m[1,1], LONGREAL), m_12 = FLOAT(m[1,2], LONGREAL), m_20 = FLOAT(m[2,0], LONGREAL), m_21 = FLOAT(m[2,1], LONGREAL), m_22 = FLOAT(m[2,2], LONGREAL), m_01_01 = m_00 * m_11 - m_01 * m_10, m_01_02 = m_00 * m_12 - m_02 * m_10, m_01_12 = m_01 * m_12 - m_02 * m_11 DO IF adj THEN WITH m_02_01 = m_00 * m_21 - m_01 * m_20, m_02_02 = m_00 * m_22 - m_02 * m_20, m_02_12 = m_01 * m_22 - m_02 * m_21, m_12_01 = m_10 * m_21 - m_11 * m_20, m_12_02 = m_10 * m_22 - m_12 * m_20, m_12_12 = m_11 * m_22 - m_12 * m_21 DO a[0,0] := FLOAT(+ m_12_12); a[1,0] := FLOAT(- m_12_02); a[2,0] := FLOAT(+ m_12_01); a[0,1] := FLOAT(- m_02_12); a[1,1] := FLOAT(+ m_02_02); a[2,1] := FLOAT(- m_02_01); a[0,2] := FLOAT(+ m_01_12); a[1,2] := FLOAT(- m_01_02); a[2,2] := FLOAT(+ m_01_01); END; END; IF det THEN d := + m_01_01 * m_22 - m_01_02 * m_21 + m_01_12 * m_20 END; END END AdjAux; (* *) PROCEDURE dst( frm,tto: Point ): REAL = VAR r: REAL; dd, vi, fw, tw: REAL; BEGIN dd := 0.0; fw := frm.c[0]; tw := tto.c[0]; FOR i:= 1 TO 3 DO vi := fw * tto.c[i] - tw * frm.c[i]; dd := dd + vi * vi; END; r := FLOAT( Math.sqrt( FLOAT( dd, LONGREAL ) ) ) / fw / tw; RETURN ABS(r); END dst; PROCEDURE Dist(READONLY a, b: Point): REAL = BEGIN WITH aw = FLOAT(a.c[0], LONGREAL), bw = FLOAT(b.c[0], LONGREAL), ax = FLOAT(a.c[1], LONGREAL) / aw, bx = FLOAT(b.c[1], LONGREAL) / bw, dx = ax - bx, ay = FLOAT(a.c[2], LONGREAL) / aw, by = FLOAT(b.c[2], LONGREAL) / bw, dy = ay - by, az = FLOAT(a.c[3], LONGREAL) / aw, bz = FLOAT(b.c[3], LONGREAL) / bw, dz = az - bz, length = Math.sqrt(dx * dx + dy * dy + dz * dz) DO RETURN FLOAT(length) END; END Dist; (* Version of R4.T with "FOR" loops: *) PROCEDURE Add(x, y: T): T = VAR r: T; BEGIN FOR i := 0 TO 3 DO r[i] := x[i] + y[i] END; RETURN r END Add; PROCEDURE Sub(x, y: T): T = VAR r: T; BEGIN FOR i := 0 TO 3 DO r[i] := x[i] - y[i] END; RETURN r END Sub; PROCEDURE Neg(x: T): T = VAR r: T; BEGIN FOR i := 0 TO 3 DO r[i] := - x[i] END; RETURN r END Neg; PROCEDURE Scale(x: T; s: REAL): T = VAR r: T; BEGIN FOR i := 0 TO 3 DO r[i] := x[i] * s END; RETURN r END Scale; PROCEDURE ReduceInf(x: T): T = VAR m: REAL; r: T; BEGIN m := 0.0; FOR i := 0 TO 3 DO IF ABS(x[i]) > m THEN m := ABS(x[i]) END; END; FOR i := 0 TO 3 DO r[i] := x[i] / m END; RETURN r END ReduceInf; PROCEDURE Reduce(x: T): T = VAR r: T; m: LONGREAL; BEGIN m := 0.0D0; FOR i:= 0 TO 3 DO WITH xi = FLOAT(x[i], LONGREAL) DO m := m + xi * xi END END; WITH sm = FLOAT(Math.sqrt(m)) DO FOR i:= 0 TO 3 DO r[i] := x[i] / m END END; RETURN r END Reduce; PROCEDURE Dot(x, y: T): LONGREAL = VAR m: LONGREAL; BEGIN FOR i:= 0 TO 3 DO WITH xi = FLOAT(x[i], LONGREAL), yi = FLOAT(x[i], LONGREAL) DO m := m + xi * yi END END; RETURN m END Dot; PROCEDURE Cross(x, y: T): T = VAR r: T; BEGIN WITH x0 = FLOAT(x[0], LONGREAL), x1 = FLOAT(x[1], LONGREAL), x2 = FLOAT(x[2], LONGREAL), x3 = FLOAT(x[3], LONGREAL), y0 = FLOAT(y[0], LONGREAL), y1 = FLOAT(y[1], LONGREAL), y2 = FLOAT(y[2], LONGREAL), y3 = FLOAT(y[3], LONGREAL), d01 = x0*y1 - x1*y0, d02 = x0*y2 - x2*y0, d12 = x1*y2 - x2*y1, d03 = x0*y3 - x3*y0, d13 = x1*y3 - x3*y1, d23 = x2*y3 - x3*y2 DO r[0] := FLOAT(- d12*z[3] + d13*z[2] - d23*z[1]); r[1] := FLOAT(+ d02*z[3] - d03*z[2] + d23*z[0]); r[2] := FLOAT(- d01*z[3] + d03*z[1] - d13*z[0]); r[3] := FLOAT(+ d01*z[2] - d02*z[1] + d12*z[0]); END; RETURN r END Cross; PROCEDURE Orthize(x, u: T): T = VAR r: T; xi, ui, sxu, suu, c: LONGREAL; BEGIN suu := 0.0D0; sxu := 0.0D0; FOR i := 0 TO 3 DO WITH xi = FLOAT(x[i], LONGREAL), ui = FLOAT(u[i], LONGREAL) DO suu := suu + ui*ui; sxu := sxu + xi*ui; END END; c := sxu/suu; FOR i := 0 TO 3 DO WITH xi = FLOAT(x[i], LONGREAL), ui = FLOAT(u[i], LONGREAL) DO r[i] := FLOAT(xi - c*ui); END; RETURN r END Orthize; (* End FOR version *) PROCEDURE Cof(READONLY m: T; ix, jx: CARDINAL): LONGREAL; (* Returns the cofactor of element "[ix,jx]" in matrix "m". *) PROCEDURE Cof(m: T; ix, jx: CARDINAL): LONGREAL = VAR t: R3x3.T; ii, jj: CARDINAL; BEGIN (* Extract minor *) ii := 0; FOR i := 0 TO 3 DO IF i # ix THEN jj := 0; FOR j := 0 TO 3 DO IF j # jx THEN t[ii,jj] := m[i,j]; INC(jj) END END; INC(ii) END END; (* Compute its determinant *) WITH d = R3x3.Det(t) DO IF (ix + jx) MOD 2 = 1 THEN RETURN FLOAT(-d) ELSE RETURN FLOAT(d) END END END Cof; PROCEDURE Det(READONLY m: T): LONGREAL = BEGIN WITH m_00 = FLOAT(m[0,0], LONGREAL), m_01 = FLOAT(m[0,1], LONGREAL), m_02 = FLOAT(m[0,2], LONGREAL), m_10 = FLOAT(m[1,0], LONGREAL), m_11 = FLOAT(m[1,1], LONGREAL), m_12 = FLOAT(m[1,2], LONGREAL), m_20 = FLOAT(m[2,0], LONGREAL), m_21 = FLOAT(m[2,1], LONGREAL), m_22 = FLOAT(m[2,2], LONGREAL), m_01_01 = m_00 * m_11 - m_01 * m_10, m_01_02 = m_00 * m_12 - m_02 * m_10, m_01_12 = m_01 * m_12 - m_02 * m_11, d = + m_01_01 * m_22 - m_01_02 * m_21 + m_01_12 * m_20 DO RETURN d END END Det; PROCEDURE Det(READONLY m: T): LONGREAL = BEGIN WITH m_00 = FLOAT(m[0,0], LONGREAL), m_01 = FLOAT(m[0,1], LONGREAL), m_02 = FLOAT(m[0,2], LONGREAL), m_03 = FLOAT(m[0,3], LONGREAL), m_10 = FLOAT(m[1,0], LONGREAL), m_11 = FLOAT(m[1,1], LONGREAL), m_12 = FLOAT(m[1,2], LONGREAL), m_13 = FLOAT(m[1,3], LONGREAL), m_20 = FLOAT(m[2,0], LONGREAL), m_21 = FLOAT(m[2,1], LONGREAL), m_22 = FLOAT(m[2,2], LONGREAL), m_23 = FLOAT(m[2,3], LONGREAL), m_30 = FLOAT(m[3,0], LONGREAL), m_31 = FLOAT(m[3,1], LONGREAL), m_32 = FLOAT(m[3,2], LONGREAL), m_33 = FLOAT(m[3,3], LONGREAL), m_01_01 = m_00 * m_11 - m_01 * m_10, m_01_02 = m_00 * m_12 - m_02 * m_10, m_01_12 = m_01 * m_12 - m_02 * m_11, m_01_03 = m_00 * m_13 - m_03 * m_10, m_01_13 = m_01 * m_13 - m_03 * m_11, m_01_23 = m_02 * m_13 - m_03 * m_12, m_23_01 = m_20 * m_31 - m_21 * m_30, m_23_02 = m_20 * m_32 - m_22 * m_30, m_23_12 = m_21 * m_32 - m_22 * m_31, m_23_03 = m_20 * m_33 - m_23 * m_30, m_23_13 = m_21 * m_33 - m_23 * m_31, m_23_23 = m_22 * m_33 - m_23 * m_32, d = + m_01_01 * m_23_23 - m_01_02 * m_23_13 + m_01_12 * m_23_03 + m_01_03 * m_23_12 - m_01_13 * m_23_02 + m_01_23 * m_23_01 DO RETURN d END END Det; PROCEDURE Triangularize(VAR a: ARRAY OF T) = BEGIN FOR i := 0 TO MIN(LAST(a)-1, N-1) DO (* Elements "a[r][s]" with "r>=i" and "si" *) WITH ai = a[i], aii = ai[i] DO FOR k := i+1 TO LAST(a) DO WITH ak = a[k], aki = ak[i] DO (* Swap rows "i" and "k" if needed: *) IF ABS(aki) > ABS(aii) THEN FOR r := i TO N-1 DO WITH akr = ak[r], air = ai[r] DO VAR t := akr; BEGIN akr := -air; air := t END; END END END; (* Subtract from row "k" a multiple of row "i" that cancels "a[k,i]": *) IF aki # Zero THEN WITH s = FLOAT(aki, LONGREAL)/FLOAT(aii, LONGREAL) DO aki := Zero; FOR r := i+1 TO N-1 DO WITH akr = ak[r], fakr = FLOAT(akr, LONGREAL) fair = FLOAT(ai[r], LONGREAL), DO akr := FLOAT(fakr - s*fair, Elem.T) END END END END; END END; (* FOR k*) END END; END Triangularize; PROCEDURE Conjugate(READONLY a: ARRAY OF T): REF ARRAY OF T; (* Returns an array of vectors "r", in the linear span of the rows of "a", such that "Dot(a[i], r[j])" is 0 if "i # j", and 1 if "i = j". Defined only if the rows of "a" are linearly independent. *) PROCEDURE Adjoint(READONLY m: ARRAY OF T): REF ARRAY OF T; (* Returns an array of vectors "r" such that "Dot(m[i], r[j])" is zero if "i # j", otherwise it is the measure of the paralellotope "m". *) PROCEDURE Triangularize(VAR a: ARRAY OF T); (* Reduces "m" to upper triangular form by and elementary row operations (adding a multiple of one row to another row, or swapping two rows and negating one of them). *) PROCEDURE GaussRandom.Extended(rnd: Random.T): EXTENDED = (* !!! TEMPORARY HACK !!! *) CONST Sqrt3: EXTENDED := 1.7320508075688772935274463415x0; VAR s: EXTENDED := 0.0x0; BEGIN FOR i := 0 TO 5 DO s := s + rnd.extended(-Sqrt3, +Sqrt3); END; RETURN s END GaussRandom.Extended; PROCEDURE Print( wr: Wr.T; READONLY a: T; lp := "("; sep := " "; rp := ")" fmt: PROCEDURE (x: ElemT): TEXT := VRep.DefaultElemFmt; ) = <* FATAL Wr.Failure, Thread.Alerted *> BEGIN Wr.PutText(wr, lp); FOR i := 0 TO N-1 DO IF i # 0 THEN Wr.PutText(wr, sep) END; Wr.PutText(wr, Fmt.Pad(Fmt.Real(a[i], style := style, prec := prec), pad)); END; Wr.PutText(wr, rp); END Print; PROCEDURE ToText(READONLY ??: T): TEXT = BEGIN RETURN OpsN.ToText(??) END ToText; PROCEDURE Project(READONLY a, u: T): T = BEGIN WITH a0 = FLOAT(a[0], LONGREAL), a1 = FLOAT(a[1], LONGREAL), u0 = FLOAT(u[0], LONGREAL), u1 = FLOAT(u[1], LONGREAL), sau = a0*u0 + a1*u1 DO IF sau = 0.0d0 THEN RETURN T{0.0, 0.0} ELSE WITH suu = u0*u0 + u1*u1, c = sau/suu DO RETURN T{FLOAT(c*u0), FLOAT(c*u1)} END END END; END Project; PROCEDURE Orthize(READONLY a, u: T): T = BEGIN WITH u0 = FLOAT(u[0], LONGREAL), u1 = FLOAT(u[1], LONGREAL), suu = u0*u0 + u1*u1 DO IF suu = 0.0d0 THEN RETURN a ELSE WITH a0 = FLOAT(a[0], LONGREAL), a1 = FLOAT(a[1], LONGREAL), sau = a0*u0 + a1*u1 DO IF sau = 0.0d0 THEN RETURN T{0.0, 0.0} ELSE WITH c = sau/suu DO RETURN T{FLOAT(a0-c*u0), FLOAT(a1-c*u1)} END END END END END; END Orthize; PROCEDURE Det(READONLY a: Basis): LONGREAL = VAR m: Basis := a; r: LONGREAL := 1.0d0; BEGIN Triangularize(m); FOR i := 0 TO N-1 DO r := r * FLOAT(m[i,i], LONGREAL) IF r = 0.0d0 THEN RETURN r END END; RETURN r END Det; PROCEDURE Cross(READONLY a: SubBasis): T; VAR r: T; m: SubBasis := a; BEGIN Triangularize(m); <* ASSERT FALSE *> RETURN r END Cross; PROCEDURE Det(READONLY m: ARRAY OF T): LONGREAL; (* Returns the determinant of the matrix whose rows are "m[0..N-1]", where "N = Dim(m[i])" for all "i". Requires "NUMBER(m) = N". *) PROCEDURE Cross(READONLY m: ARRAY OF T): T; (* Returns the `cross product' of the vectors "m[0..N-2]", where "N = Dim(m[i])" for all "i". Requires "NUMBER(m) = N-1". The cross product is defined as a vector "r" such that | "Dot(r, b) = Det({m[0],.. m[N-2], b})" for all "b". *) PROCEDURE Print(wr: Wr.T; READONLY a: T) = <* FATAL Wr.Failure, Thread.Alerted *> BEGIN Wr.PutText(wr, "["); FOR i := 0 TO 3 DO IF i # 0 THEN Wr.PutChar(wr, ' ') END; Wr.PutText(wr, Fmt.Real(a[i])); END; Wr.PutText(wr, "]"); END Print; PROCEDURE ToText(READONLY ??: T): TEXT = BEGIN RETURN OpsN.ToText(??) END ToText; PROCEDURE Print( wr: Wr.T; READONLY a: T; lp := "("; sep := " "; rp := ")" fmt: PROCEDURE (x: ElemT): TEXT := VRep.DefaultElemFmt; ) = <* FATAL Wr.Failure, Thread.Alerted *> BEGIN Wr.PutText(wr, lp); FOR i := 0 TO N-1 DO IF i # 0 THEN Wr.PutText(wr, sep) END; Wr.PutText(wr, Fmt.Pad(Fmt.Real(a[i], style := style, prec := prec), pad)); END; Wr.PutText(wr, rp); END Print PROCEDURE Adj(READONLY m: T): T = BEGIN RETURN VRep.Adj(m) END Adj; r[0,0] := FLOAT(+ m123_123); r[1,0] := FLOAT(- m123_023); r[2,0] := FLOAT(+ m123_013); r[3,0] := FLOAT(- m123_012); r[0,1] := FLOAT(- m023_123); r[1,1] := FLOAT(+ m023_023); r[2,1] := FLOAT(- m023_013); r[3,1] := FLOAT(+ m023_012); r[0,2] := FLOAT(+ m013_123); r[1,2] := FLOAT(- m013_023); r[2,2] := FLOAT(+ m013_013); r[3,2] := FLOAT(- m013_012); r[0,3] := FLOAT(- m012_123); r[1,3] := FLOAT(+ m012_023); r[2,3] := FLOAT(- m012_013); r[3,3] := FLOAT(+ m012_012); r[0,0] := FLOAT(+ m123_123 / d); r[1,0] := FLOAT(- m123_023 / d); r[2,0] := FLOAT(+ m123_013 / d); r[3,0] := FLOAT(- m123_012 / d); r[0,1] := FLOAT(- m023_123 / d); r[1,1] := FLOAT(+ m023_023 / d); r[2,1] := FLOAT(- m023_013 / d); r[3,1] := FLOAT(+ m023_012 / d); r[0,2] := FLOAT(+ m013_123 / d); r[1,2] := FLOAT(- m013_023 / d); r[2,2] := FLOAT(+ m013_013 / d); r[3,2] := FLOAT(- m013_012 / d); r[0,3] := FLOAT(- m012_123 / d); r[1,3] := FLOAT(+ m012_023 / d); r[2,3] := FLOAT(- m012_013 / d); r[3,3] := FLOAT(+ m012_012 / d); (* Rotation 3x3: *) WITH ct = Math.cos(angle), st = Math.sin(angle), rt = 1.0d0 - ct, ax = axis[0], ay = axis[1], az = axis[2], axx = ax * ax, ayy = ay * ay, azz = az * az, axy = ax * ay, axz = ax * az, ayz = ay * az DO RETURN T{ LR3.T{ axx + ct * (ayy + azz), rt * axy + st * az, rt * axz - st * ay }, LR3.T{ rt * axy - st * az, ayy + ct * (axx + azz), rt * ayz + st * ax }, LR3.T{ rt * axz + st * ay, rt * ayz - st * ax, azz + ct * (axx + ayy) } } END