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

  Created by J. Stolfi, with contributions by
  M. C. Carrard. *)

IMPORT R3Rep, RN, Math, Wr, Thread, Random, GaussRandom;

PROCEDURE Zero(): T =
  BEGIN 
    RETURN T{0.0, 0.0, 0.0}
  END Zero;
    
PROCEDURE All(x: LONGREAL): T =
  BEGIN
    WITH xx = FLOAT(x, REAL) DO 
      RETURN T{xx, xx, xx}
    END
  END All;

PROCEDURE Axis(i: [0..N-1]): T =
  VAR r: T := T{0.0, 0.0, 0.0};
  BEGIN
    r[i] := 1.0;
    RETURN r
  END Axis;

PROCEDURE Get(READONLY a: T; i: [0..N-1]): LONGREAL =
  BEGIN
    RETURN FLOAT(a[i], LONGREAL)
  END Get;
    
PROCEDURE Set(VAR a: T; i: [0..N-1]; x: LONGREAL) =
  BEGIN
    a[i] := FLOAT(x, ElemT)
  END Set;

PROCEDURE Add(READONLY a, b: T): T =
  BEGIN
    RETURN T{a[0] + b[0], a[1] + b[1], a[2] + b[2]}
  END Add;

PROCEDURE Sub(READONLY a, b: T): T =
  BEGIN
    RETURN T{a[0] - b[0], a[1] - b[1], a[2] - b[2]}
  END Sub;

PROCEDURE Neg(READONLY a: T): T =
  BEGIN
    RETURN T{- a[0], - a[1], - a[2]}
  END Neg;

PROCEDURE Scale(s: LONGREAL; READONLY a: T): T = 
  BEGIN
    WITH
      a0 = s * FLOAT(a[0], LONGREAL),
      a1 = s * FLOAT(a[1], LONGREAL),
      a2 = s * FLOAT(a[2], LONGREAL)
    DO
      RETURN T{FLOAT(a0), FLOAT(a1), FLOAT(a2)}
    END
  END Scale;

PROCEDURE Weigh(READONLY w: LongRealT; READONLY a: T): T =
  BEGIN
    WITH
      c0 = FLOAT(a[0], LONGREAL) * w[0],
      c1 = FLOAT(a[1], LONGREAL) * w[1],
      c2 = FLOAT(a[2], LONGREAL) * w[2]
    DO
      RETURN T{FLOAT(c0), FLOAT(c1), FLOAT(c2)}
    END
  END Weigh;

PROCEDURE Mix(s: LONGREAL; READONLY a: T; t: LONGREAL; READONLY b: T): T = 
  BEGIN
    WITH
      c0 = s * FLOAT(a[0], LONGREAL) + t * FLOAT(b[0], LONGREAL),
      c1 = s * FLOAT(a[1], LONGREAL) + t * FLOAT(b[1], LONGREAL),
      c2 = s * FLOAT(a[2], LONGREAL) + t * FLOAT(b[2], LONGREAL)
    DO
      RETURN T{FLOAT(c0), FLOAT(c1), FLOAT(c2)}
    END
  END Mix;

PROCEDURE Norm(READONLY a: T): LONGREAL =
  BEGIN
    WITH
      a0 = FLOAT(a[0], LONGREAL),
      a1 = FLOAT(a[1], LONGREAL),
      a2 = FLOAT(a[2], LONGREAL),
      (* This can't overflow, and is probably faster than "Math.hypot": *)
      norm = Math.sqrt(a0*a0 + a1*a1 + a2*a2)
    DO
      RETURN norm
    END
  END Norm;

PROCEDURE NormSqr(READONLY a: T): LONGREAL =
  BEGIN
    WITH
      a0 = FLOAT(a[0], LONGREAL),
      a1 = FLOAT(a[1], LONGREAL),
      a2 = FLOAT(a[2], LONGREAL),
      normSqr = a0*a0 + a1*a1 + a2*a2
    DO
      RETURN normSqr
    END
  END NormSqr;

PROCEDURE Dist(READONLY a, b: T): LONGREAL =
  BEGIN
    WITH
      t0 = FLOAT(a[0], LONGREAL) - FLOAT(b[0], LONGREAL),
      t1 = FLOAT(a[1], LONGREAL) - FLOAT(b[1], LONGREAL),
      t2 = FLOAT(a[2], LONGREAL) - FLOAT(b[2], LONGREAL),
      (* This can't overflow, and is probably faster than "Math.hypot": *)
      dist = Math.sqrt(t0*t0 + t1*t1 + t2*t2)
    DO
      RETURN dist
    END
  END Dist;
    
PROCEDURE DistSqr(READONLY a, b: T): LONGREAL =
  BEGIN
    WITH
      t0 = FLOAT(a[0], LONGREAL) - FLOAT(b[0], LONGREAL),
      t1 = FLOAT(a[1], LONGREAL) - FLOAT(b[1], LONGREAL),
      t2 = FLOAT(a[2], LONGREAL) - FLOAT(b[2], LONGREAL),
      distSqr = t0*t0 + t1*t1 + t2*t2
    DO
      RETURN distSqr
    END
  END DistSqr;

PROCEDURE Dir(READONLY a: T): T =
  BEGIN
    WITH
      a0 = FLOAT(a[0], LONGREAL),
      a1 = FLOAT(a[1], LONGREAL),
      a2 = FLOAT(a[2], LONGREAL),
      (* This can't overflow, and is probably faster than "Math.hypot": *)
      norm = Math.sqrt(a0*a0 + a1*a1 + a2*a2)
    DO
      RETURN T{FLOAT(a0/norm), FLOAT(a1/norm), FLOAT(a2/norm)}
    END
  END Dir; 

PROCEDURE LInfNorm(READONLY a: T): LONGREAL =
  BEGIN
    WITH 
      norm = MAX(MAX(ABS(a[0]), ABS(a[1])), ABS(a[2]))
    DO
      RETURN FLOAT(norm, LONGREAL)
    END
  END LInfNorm;  

PROCEDURE LInfDist(READONLY a, b: T): LONGREAL =
  BEGIN
    WITH 
      dist = MAX(ABS(a[0] - b[0]), MAX(ABS(a[1] - b[1]), ABS(a[2] - b[2])))
    DO
      RETURN FLOAT(dist, LONGREAL)
    END
  END LInfDist;  

PROCEDURE LInfDir(READONLY a: T): T =
  BEGIN
    WITH 
      norm = MAX(MAX(ABS(a[0]), ABS(a[1])), ABS(a[2]))
    DO
      RETURN T{a[0]/norm, a[1]/norm, a[2]/norm}
    END
  END LInfDir;  

PROCEDURE Dot(READONLY a, b: T): LONGREAL =
  BEGIN
    WITH
      a0 = FLOAT(a[0], LONGREAL),
      a1 = FLOAT(a[1], LONGREAL),
      a2 = FLOAT(a[2], LONGREAL),

      b0 = FLOAT(b[0], LONGREAL),
      b1 = FLOAT(b[1], LONGREAL),
      b2 = FLOAT(b[2], LONGREAL),

      dot = a0*b0 + a1*b1 + a2*b2
    DO
      RETURN dot
    END
  END Dot;

PROCEDURE Cos(READONLY a, b: T): LONGREAL =
  BEGIN
    WITH
      a0 = FLOAT(a[0], LONGREAL),
      a1 = FLOAT(a[1], LONGREAL),
      a2 = FLOAT(a[2], LONGREAL),
           
      b0 = FLOAT(b[0], LONGREAL),
      b1 = FLOAT(b[1], LONGREAL),
      b2 = FLOAT(b[2], LONGREAL),

      aa = a0*a0 + a1*a1 + a2*a2,
      bb = b0*b0 + b1*b1 + b2*b2,
      ab = a0*b0 + a1*b1 + a2*b2,
      cos = ab/(Math.sqrt(aa)*Math.sqrt(bb))
    DO
      RETURN cos
    END
  END Cos;

PROCEDURE Sin(READONLY a, b: T): LONGREAL =
  BEGIN 
    WITH 
      a0 = FLOAT(a[0], LONGREAL),
      a1 = FLOAT(a[1], LONGREAL),
      a2 = FLOAT(a[2], LONGREAL),
               
      b0 = FLOAT(b[0], LONGREAL),
      b1 = FLOAT(b[1], LONGREAL),
      b2 = FLOAT(b[2], LONGREAL),
      
      c0 = a1*b2 - a2*b1,
      c1 = a2*b1 - a1*b2,
      c2 = a0*b1 - a1*b0,

      aa = a0*a0 + a1*a1 + a2*a2,
      bb = b0*b0 + b1*b1 + b2*b2,
      cc = c0*c0 + c1*c1 + c2*c2,
      sin = Math.sqrt(cc/aa/bb)
    DO
      RETURN sin
    END
  END Sin;

PROCEDURE Det(READONLY a, b, c: T): LONGREAL =
  BEGIN
    WITH
      a_0 = FLOAT(a[0], LONGREAL),
      a_1 = FLOAT(a[1], LONGREAL),
      a_2 = FLOAT(a[2], LONGREAL),
      
      b_0 = FLOAT(b[0], LONGREAL),
      b_1 = FLOAT(b[1], LONGREAL),
      b_2 = FLOAT(b[2], LONGREAL),
      
      c_0 = FLOAT(c[0], LONGREAL),
      c_1 = FLOAT(c[1], LONGREAL),
      c_2 = FLOAT(c[2], LONGREAL),
      
      ab_01 = a_0 * b_1 - a_1 * b_0,
      ab_02 = a_0 * b_2 - a_2 * b_0,
      ab_12 = a_1 * b_2 - a_2 * b_1,

      t = + ab_01 * c_2 - ab_02 * c_1 + ab_12 * c_0
    DO
      RETURN t
    END
  END Det;  

PROCEDURE Cross(READONLY a, b: T): T =
  BEGIN
    WITH
      a0 = FLOAT(a[0], LONGREAL),
      a1 = FLOAT(a[1], LONGREAL),
      a2 = FLOAT(a[2], LONGREAL),

      b0 = FLOAT(b[0], LONGREAL),
      b1 = FLOAT(b[1], LONGREAL),
      b2 = FLOAT(b[2], LONGREAL),

      t01 = a0*b1 - a1*b0,
      t02 = a0*b2 - a2*b0,
      t12 = a1*b2 - a2*b1
    DO
      RETURN T{
        FLOAT(+ t12),
        FLOAT(- t02),
        FLOAT(+ t01)
      }
    END;
  END Cross;
  
PROCEDURE Project(READONLY a, u: T): T =
  BEGIN
    WITH
      a0 = FLOAT(a[0], LONGREAL),
      a1 = FLOAT(a[1], LONGREAL),
      a2 = FLOAT(a[2], LONGREAL),

      u0 = FLOAT(u[0], LONGREAL),
      u1 = FLOAT(u[1], LONGREAL),
      u2 = FLOAT(u[2], LONGREAL),

      sau = a0*u0 + a1*u1 + a2*u2
    DO
      IF sau = 0.0d0 THEN
        RETURN T{0.0, 0.0, 0.0}
      ELSE
        WITH
          suu = u0*u0 + u1*u1 + u2*u2,
          c = sau/suu
        DO
          RETURN T{FLOAT(c*u0), FLOAT(c*u1), FLOAT(c*u2)}
        END
      END
    END;
  END Project;
  
PROCEDURE Orthize(READONLY a, u: T): T =
  BEGIN
    WITH
      u0 = FLOAT(u[0], LONGREAL),
      u1 = FLOAT(u[1], LONGREAL),
      u2 = FLOAT(u[2], LONGREAL),

      suu = u0*u0 + u1*u1 + u2*u2
    DO
      IF suu = 0.0d0 THEN
        RETURN a
      ELSE
        WITH
          a0 = FLOAT(a[0], LONGREAL),
          a1 = FLOAT(a[1], LONGREAL),
          a2 = FLOAT(a[2], LONGREAL),

          sau = a0*u0 + a1*u1 + a2*u2
        DO
          IF sau = 0.0d0 THEN
            RETURN a
          ELSE
            WITH
              c = sau/suu
            DO
              RETURN T{FLOAT(a0-c*u0), FLOAT(a1-c*u1), FLOAT(a2-c*u2)}
            END
          END
        END
      END
    END;
  END Orthize;
  
PROCEDURE ToReal(READONLY a: T): RealT =
  BEGIN RETURN a END ToReal;
  
PROCEDURE FromReal(READONLY a: RealT): T =
  BEGIN RETURN a END FromReal;

PROCEDURE ToLongReal(READONLY a: T): LongRealT =
  BEGIN
    RETURN LongRealT{
      FLOAT(a[0], LONGREAL),
      FLOAT(a[1], LONGREAL),
      FLOAT(a[2], LONGREAL)
    }
  END ToLongReal;
  
PROCEDURE FromLongReal(READONLY a: LongRealT): T =
  BEGIN
    RETURN T{
      FLOAT(a[0], REAL),
      FLOAT(a[1], REAL),
      FLOAT(a[2], REAL)
    }
  END FromLongReal;

PROCEDURE URandom(rnd: Random.T; min, max: LONGREAL): T =
  VAR r: T;
  BEGIN
    FOR i := 0 TO N-1 DO
      r[i] := rnd.real(FLOAT(min), FLOAT(max))
    END;
    RETURN r
  END URandom;
    
PROCEDURE NRandom(rnd: Random.T; avg, dev: LONGREAL): T =
  VAR r: T;
  BEGIN
    FOR i := 0 TO N-1 DO
      r[i] := FLOAT(avg + dev * FLOAT(GaussRandom.Real(rnd), LONGREAL))
    END;
    RETURN r
  END NRandom;

PROCEDURE Print(
    wr: Wr.T;
    READONLY a: T;
    lp := "("; sep := " "; rp := ")";
    fmt: PROCEDURE (x: ElemT): TEXT := R3Rep.DefaultElemFmt;
  ) RAISES {Wr.Failure, Thread.Alerted} =
  BEGIN
    RN.Print(wr, a, lp, sep, rp, fmt)
  END Print;

PROCEDURE ToText(READONLY a: T): TEXT =
  BEGIN
    RETURN RN.ToText(a)
  END ToText;

BEGIN 
END R3.