MODULE LR3x3 EXPORTS LR3x3, LR3x3Extras;
(*
  Hand-optimized implementation of the interfaces "LR3x3" and "LR3x3Extras". 

  Created by J. Stolfi with contributions by Marcos C. Carrard
  and R. L. W. Liesenfeld. *)
  
IMPORT LR3Rep, LR3x3Rep, LR3Extras, LRMxN, Wr, Thread, Math;

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},
      RowT{0.0d0, 1.0d0, 0.0d0},
      RowT{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: LR3Rep.T; READONLY m: T): LR3Rep.T =
  VAR r: LR3Rep.T;
  BEGIN
    WITH
      a0 = a[0],
      a1 = a[1],
      a2 = a[2]
    DO
      WITH  m00 = m[0,0], m10 = m[1,0], m20 = m[2,0]  DO
        r[0] := a0*m00 + a1*m10 + a2*m20
      END;
      WITH  m01 = m[0,1], m11 = m[1,1], m21 = m[2,1]  DO
        r[1] := a0*m01 + a1*m11 + a2*m21
      END;
      WITH  m02 = m[0,2], m12 = m[1,2], m22 = m[2,2]  DO
        r[2] := a0*m02 + a1*m12 + a2*m22
      END
    END;
    RETURN r
  END MapRow;

PROCEDURE MapCol(READONLY m: T; READONLY a: LR3Rep.T): LR3Rep.T =
  VAR r: LR3Rep.T;
  BEGIN
    WITH
      a0 = a[0],
      a1 = a[1],
      a2 = a[2]
    DO
      WITH  m00 = m[0,0], m01 = m[0,1], m02 = m[0,2]  DO
        r[0] := m00*a0 + m01*a1 + m02*a2
      END;
      WITH  m10 = m[1,0], m11 = m[1,1], m12 = m[1,2]  DO
        r[1] := m10*a0 + m11*a1 + m12*a2
      END;
      WITH  m20 = m[2,0], m21 = m[2,1], m22 = m[2,2]  DO
        r[2] := m20*a0 + m21*a1 + m22*a2
      END
    END;
    RETURN r
  END MapCol;

PROCEDURE Mul(READONLY m, n: T): T =
  BEGIN
    WITH
      m00 = m[0,0], m01 = m[0,1], m02 = m[0,2],
      m10 = m[1,0], m11 = m[1,1], m12 = m[1,2],
      m20 = m[2,0], m21 = m[2,1], m22 = m[2,2],

      n00 = n[0,0], n01 = n[0,1], n02 = n[0,2],
      n10 = n[1,0], n11 = n[1,1], n12 = n[1,2],
      n20 = n[2,0], n21 = n[2,1], n22 = n[2,2]
    DO
      RETURN T{
        LR3Rep.T{
          m00*n00 + m01*n10 + m02*n20, 
          m00*n01 + m01*n11 + m02*n21, 
          m00*n02 + m01*n12 + m02*n22
        },
        LR3Rep.T{
          m10*n00 + m11*n10 + m12*n20, 
          m10*n01 + m11*n11 + m12*n21, 
          m10*n02 + m11*n12 + m12*n22
        },
        LR3Rep.T{
          m20*n00 + m21*n10 + m22*n20, 
          m20*n01 + m21*n11 + m22*n21, 
          m20*n02 + m21*n12 + m22*n22
        }
      }
    END
  END Mul;
  
PROCEDURE Adj(READONLY m: T): T =
  BEGIN
    WITH
      m00 = m[0,0], m01 = m[0,1], m02 = m[0,2],
      m10 = m[1,0], m11 = m[1,1], m12 = m[1,2],
      m20 = m[2,0], m21 = m[2,1], m22 = m[2,2],
      
      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{
        LR3Rep.T{+ m12_12, - m02_12, + m01_12},
        LR3Rep.T{- m12_02, + m02_02, - m01_02},
        LR3Rep.T{+ m12_01, - m02_01, + m01_01}
      }
    END
  END Adj;

PROCEDURE Inv(READONLY m: T): T =
  BEGIN
    WITH
      m00 = m[0,0], m01 = m[0,1], m02 = m[0,2],
      m10 = m[1,0], m11 = m[1,1], m12 = m[1,2],
      m20 = m[2,0], m21 = m[2,1], m22 = m[2,2],
      
      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{
        LR3Rep.T{+ m12_12/d, - m02_12/d, + m01_12/d},
        LR3Rep.T{- m12_02/d, + m02_02/d, - m01_02/d},
        LR3Rep.T{+ m12_01/d, - m02_01/d, + m01_01/d}
      }
    END
  END Inv;

PROCEDURE Det(READONLY m: T): LONGREAL =
  BEGIN
    RETURN LR3Extras.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 := LR3x3Rep.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;

PROCEDURE Rotation(
    angle: LONGREAL;
    READONLY axis: LR3Rep.T; 
    scale: LONGREAL := 1.0d0;
  ): T =
  BEGIN
    WITH 
      ct = scale * Math.cos(angle),
      st = scale * Math.sin(angle),
      rt = scale - ct,
      ax = axis[0],
      ay = axis[1],
      az = axis[2],
      
      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{
        LR3Rep.T{rtaxx + ctd,  rtaxy + staz,  rtaxz - stay},        
        LR3Rep.T{rtaxy - staz, rtayy + ctd,   rtayz + stax},        
        LR3Rep.T{rtaxz + stay, rtayz - stax,  rtazz + ctd }
      }
    END
  END Rotation;

BEGIN 
END LR3x3.