pragma Warnings (Off);
with Ada.Numerics.Aux; use Ada.Numerics.Aux;
pragma Warnings (On);

package body Trackball is

   package Ana renames Ada.Numerics.Aux;

   TRACKBALLSIZE : constant Float := 0.8;
   --  This size should really be based on the distance from the center of
   --  rotation to the point on the object underneath the mouse.  That point
   --  would then track the mouse as closely as possible.  This is a simple
   --  example, though, so that is left as an Exercise for the Programmer.

   procedure Vsub (Src1, Src2 : Vector; Dst : out Vector) is
   begin
      Dst := (Src1 (0) - Src2 (0),
              Src1 (1) - Src2 (1),
              Src1 (2) - Src2 (2));
   end Vsub;

   procedure Vcross (V1, V2 : Vector; Cross : out Vector) is
   begin
      Cross := (V1 (1) * V2 (2) - V1 (2) * V2 (1),
                V1 (2) * V2 (0) - V1 (0) * V2 (2),
                V1 (0) * V2 (1) - V1 (1) * V2 (0));
   end Vcross;

   function Vlength (V : Vector) return Float is
   begin
      return Float (Sqrt (Ana.Double (V (0) * V (0)
                                      + V (1) * V (1)
                                      + V (2) * V (2))));
   end Vlength;

   procedure Vscale (V : in out Vector; Div : Float) is
   begin
      V := (V (0) * Div,
            V (1) * Div,
            V (2) * Div);
   end Vscale;

   procedure Vnormal (V : in out Vector) is
   begin
      Vscale (V, 1.0 / Vlength (V));
   end Vnormal;

   function Vdot (V1, V2 : Vector) return Float is
   begin
      return V1 (0) * V2 (0) + V1 (1) * V2 (1) + V1 (2) * V2 (2);
   end Vdot;

   procedure Vadd (Src1, Src2 : Vector; Dst : out Vector) is
   begin
      Dst := (Src1 (0) + Src2 (0),
              Src1 (1) + Src2 (1),
              Src1 (2) + Src2 (2));
   end Vadd;

   --  Quaternions always obey: a^2 + b^2 + c^2 + d^2 = 1.0 If they don't add
   --  up to 1.0, dividing by their magnitued will renormalize them.
   --
   --  Note: See the following for more information on quaternions:
   --
   --  Shoemake, K., Animating rotation with quaternion curves, Computer
   --    Graphics 19, No 3 (Proc. SIGGRAPH'85), 245-254, 1985.
   --  Pletinckx, D., Quaternion calculus as a basic tool in computer
   --    graphics, The Visual Computer 5, 2-13, 1989.

   procedure Normalize_Quat (Q : in out Quaternion) is
      Mag : Float;

   begin
      Mag := Q (0) * Q (0) + Q (1) * Q (1) + Q (2) * Q (2) + Q (3) * Q (3);
      Q (0) := Q (0) / Mag;
      Q (1) := Q (1) / Mag;
      Q (2) := Q (2) / Mag;
      Q (3) := Q (3) / Mag;
   end Normalize_Quat;

   -----------------------
   -- Project_To_Sphere --
   -----------------------
   --  Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet if
   --  we are away from the center of the sphere.

   function Project_To_Sphere (R, X, Y : Float) return Float is
      D, T : Float;
   begin
      D := Float (Sqrt (Ana.Double (X * X + Y * Y)));
      if D < R * 0.70710678118654752440 then  --  inside sphere
         return Float (Sqrt (Ana.Double (R * R - D * D)));
      else   --  on hyperbola
         T := R / 1.41421356237309504880;
         return T * T / D;
      end if;
   end Project_To_Sphere;

   ---------------
   -- Trackball --
   ---------------

   --  Ok, simulate a track-ball.  Project the points onto the virtual
   --  trackball, then figure out the axis of rotation, which is the cross
   --  product of P1 P2 and O P1 (O is the center of the ball, 0,0,0) Note:
   --  This is a deformed trackball-- is a trackball in the center, but is
   --  deformed into a hyperbolic sheet of rotation away from the center.
   --  This particular function was chosen after trying out several
   --  variations.

   --  It is assumed that the arguments to this routine are in the range (-1.0
   --  ... 1.0)

   procedure Trackball (Q : out Quaternion;
                        P1x, P1y, P2x, P2y : Float)
   is
      A   : Vector;  --  Axis of rotation
      Phi : Float;   --  How much to rotate about axis
      P1, P2, D : Vector;
      T   : Float;

   begin

      if P1x = P2x and then P1y = P2y then
         Q := (0.0, 0.0, 0.0, 1.0);
         return;
      end if;

      --  First, figure out z-coordinates for project of P1 and P2 to
      --  deformed sphere

      P1 := (P1x, P1y, Project_To_Sphere (TRACKBALLSIZE, P1x, P1y));
      P2 := (P2x, P2y, Project_To_Sphere (TRACKBALLSIZE, P2x, P2y));

      Vcross (P2, P1, Cross => A);

      --  Figure out how much to rotate around that axis
      Vsub (P1, P2, Dst => D);
      T := Vlength (D) / (2.0 * TRACKBALLSIZE);

      --  Avoid problem with out-of-control values
      if T > 1.0 then
         T := 1.0;
      elsif T < -1.0 then
         T := -1.0;
      end if;

      Phi := 2.0 * Float (Asin (Ana.Double (T)));

      Axis_To_Quat (A, Phi, Q);
   end Trackball;

   ---------------
   -- Add_Quats --
   ---------------

   --  Given two rotations, e1 and e2, expressed as quaternion rotations,
   --  figure out the equivalent single rotation and stuff it into dest.
   --  This routine also normalizes the result every RENORMCOUNT times it is
   --  called, to keep error from creeping in.
   --  NOTE: This routine is written so that q1 or q2 may be the same as dest
   --  (or each other).

   Count : Integer := 0;
   RENORMCOUNT : constant Integer := 97;

   procedure Add_Quats (Q1, Q2 : Quaternion;
                        Dest   : out Quaternion)
   is
      Qv1 : constant Vector := Vector (Q1 (0 .. 2));
      Qv2 : constant Vector := Vector (Q2 (0 .. 2));
      T1, T2, T3, Tf : Vector;

   begin
      T1 := Qv1;
      Vscale (T1, Q2 (3));

      T2 := Qv2;
      Vscale (T1, Q1 (3));

      Vcross (Qv1, Qv2, Cross => T3);
      Vadd (T1, T2, Tf);
      Vadd (T3, Tf, Tf);
      Dest := (Tf (0), Tf (1), Tf (2),
               Q1 (3) * Q2 (3) - Vdot (Qv1, Qv2));

      Count := Count + 1;
      if Count > RENORMCOUNT then
         Count := 0;
         Normalize_Quat (Dest);
      end if;
   end Add_Quats;

   ---------------------
   -- Build_Rotmatrix --
   ---------------------
   --  Build a rotation matrix, given a quaternion rotation.

   procedure Build_Rotmatrix (M : out Matrix; Q : Quaternion)
   is
   begin
      M (0, 0) := 1.0 - 2.0 * (Q (1) * Q (1) + Q (2) * Q (2));
      M (0, 1) := 2.0 * (q(0) * q(1) - q(2) * q(3));
      M (0, 2) := 2.0 * (q(2) * q(0) + q(1) * q(3));
      M (0, 3) := 0.0;

      M (1, 0) := 2.0 * (q(0) * q(1) + q(2) * q(3));
      M (1, 1) := 1.0 - 2.0 * (q(2) * q(2) + q(0) * q(0));
      M (1, 2) := 2.0 * (q(1) * q(2) - q(0) * q(3));
      M (1, 3) := 0.0;

      M (2, 0) := 2.0 * (q(2) * q(0) - q(1) * q(3));
      M (2, 1) := 2.0 * (q(1) * q(2) + q(0) * q(3));
      M (2, 2) := 1.0 - 2.0 * (q(1) * q(1) + q(0) * q(0));
      M (2, 3) := 0.0;

      M (3, 0) := 0.0;
      M (3, 1) := 0.0;
      M (3, 2) := 0.0;
      M (3, 3) := 1.0;
   end Build_Rotmatrix;

   ------------------
   -- Axis_To_Quat --
   ------------------

   procedure Axis_To_Quat (A : Vector; Phi : Float; Q : out Quaternion) is
      V : Vector;
   begin
      V := A;
      Vnormal (V);
      Vscale (V, Float (Sin (Ana.Double (Phi / 2.0))));
      Q := (V (0), V (1), V (2),
            Float (Cos (Ana.Double (Phi / 2.0))));
   end Axis_To_Quat;

end Trackball;
