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.