1. -- 
  2. -- Jan & Uwe R. Zimmer, Australia, July 2011 
  3. -- 
  4.  
  5. with Ada.Numerics; use Ada.Numerics; 
  6.  
  7. package body Rotations is 
  8.  
  9.    use Real_Elementary_Functions; 
  10.  
  11.    function To_Radiants (A : Degrees) return Radiants is ((A / 180.0) * Pi); 
  12.  
  13.    -- 
  14.  
  15.    function To_Degrees (A : Radiants) return Degrees is ((A / Pi) * 180.0); 
  16.  
  17.    -- 
  18.  
  19.    function Zero_Rotation return Quaternion_Rotation is (To_Rotation (0.0, 0.0, 0.0)); 
  20.  
  21.    -- 
  22.  
  23.    function Inverse (Quad : Quaternion_Rotation) return Quaternion_Rotation is (Conj (Quad)); 
  24.  
  25.    -- 
  26.  
  27.    function To_Quaternion (Input_Vector : Vector_3D) return Quaternion_Rotation is 
  28.      (w => 0.0, 
  29.       x => Input_Vector (x), 
  30.       y => Input_Vector (y), 
  31.       z => Input_Vector (z)); 
  32.  
  33.    -- 
  34.  
  35.    function To_Rotation (Axis : Vector_3D; Rotation_Angle : Radiants) return Quaternion_Rotation is 
  36.  
  37.       Sin_Rotation_Half : constant Real := Sin (Rotation_Angle / 2.0); 
  38.       Cos_Rotation_Half : constant Real := Cos (Rotation_Angle / 2.0); 
  39.  
  40.    begin 
  41.       return (w => Cos_Rotation_Half, 
  42.               x => Sin_Rotation_Half * Axis (x), 
  43.               y => Sin_Rotation_Half * Axis (y), 
  44.               z => Sin_Rotation_Half * Axis (z) 
  45.              ); 
  46.    end To_Rotation; 
  47.  
  48.    -- 
  49.  
  50.    function To_Rotation (Roll_Angle, Pitch_Angle, Yaw_Angle : Radiants) return Quaternion_Rotation is 
  51.  
  52.       Sin_R : constant Real := Sin (Roll_Angle  / 2.0); Cos_R : constant Real := Cos (Roll_Angle  / 2.0); 
  53.       Sin_P : constant Real := Sin (Pitch_Angle / 2.0); Cos_P : constant Real := Cos (Pitch_Angle / 2.0); 
  54.       Sin_Y : constant Real := Sin (Yaw_Angle   / 2.0); Cos_Y : constant Real := Cos (Yaw_Angle   / 2.0); 
  55.  
  56.    begin 
  57.       return (w => Cos_R * Cos_P * Cos_Y - Sin_R * Sin_P * Sin_Y, 
  58.               x => Cos_R * Sin_P * Sin_Y + Sin_R * Cos_P * Cos_Y, 
  59.               y => Cos_R * Cos_P * Sin_Y + Sin_R * Sin_P * Cos_Y, 
  60.               z => Cos_R * Sin_P * Cos_Y - Sin_R * Cos_P * Sin_Y 
  61.              ); 
  62.    end To_Rotation; 
  63.  
  64.    -- 
  65.  
  66.    function To_Rotation (Matrix : Matrix_3D) return Quaternion_Rotation is 
  67.  
  68.       m : Matrix_3D renames Matrix; 
  69.  
  70.       q : Quaternion_Rotation := 
  71.         (w => Sqrt (Real'Max (0.0, 1.0 + m (1, 1) + m (2, 2) + m (3, 3))) / 2.0, 
  72.          x => Sqrt (Real'Max (0.0, 1.0 + m (1, 1) - m (2, 2) - m (3, 3))) / 2.0, 
  73.          y => Sqrt (Real'Max (0.0, 1.0 - m (1, 1) + m (2, 2) - m (3, 3))) / 2.0, 
  74.          z => Sqrt (Real'Max (0.0, 1.0 - m (1, 1) - m (2, 2) + m (3, 3))) / 2.0); 
  75.  
  76.       q_max : constant Real := Real'Max (q.w, Real'Max (q.x, Real'Max (q.y, q.z))); 
  77.  
  78.       function Copy_Sign (Value, Sign : Real) return Real is 
  79.  
  80.       begin 
  81.          if Sign >= 0.0 then 
  82.             return +abs (Value); 
  83.          else 
  84.             return -abs (Value); 
  85.          end if; 
  86.       end Copy_Sign; 
  87.  
  88.    begin 
  89.       if q.w = q_max then 
  90.          q.x := Copy_Sign (q.x, (m (3, 2) - m (2, 3))); 
  91.          q.y := Copy_Sign (q.y, (m (1, 3) - m (3, 1))); 
  92.          q.z := Copy_Sign (q.z, (m (2, 1) - m (1, 2))); 
  93.       elsif q.x = q_max then 
  94.          q.w := Copy_Sign (q.w, (m (3, 2) - m (2, 3))); 
  95.          q.y := Copy_Sign (q.y, (m (2, 1) + m (1, 2))); 
  96.          q.z := Copy_Sign (q.z, (m (1, 3) + m (3, 1))); 
  97.       elsif q.y = q_max then 
  98.          q.w := Copy_Sign (q.w, (m (1, 3) - m (3, 1))); 
  99.          q.x := Copy_Sign (q.x, (m (2, 1) + m (1, 2))); 
  100.          q.z := Copy_Sign (q.z, (m (3, 2) + m (2, 3))); 
  101.       elsif q.z = q_max then 
  102.          q.w := Copy_Sign (q.w, (m (2, 1) - m (1, 2))); 
  103.          q.x := Copy_Sign (q.x, (m (3, 1) + m (1, 3))); 
  104.          q.y := Copy_Sign (q.y, (m (3, 2) + m (2, 3))); 
  105.       end if; 
  106.  
  107.       return Unit (q); 
  108.    end To_Rotation; 
  109.  
  110.    -- 
  111.  
  112.    function To_Rotation (Matrix : Matrix_4D) return Quaternion_Rotation is 
  113.  
  114.       m : Matrix_4D renames Matrix; 
  115.  
  116.       q : Quaternion_Rotation := 
  117.         (w => Sqrt (Real'Max (0.0, 1.0 + m (1, 1) + m (2, 2) + m (3, 3))) / 2.0, 
  118.          x => Sqrt (Real'Max (0.0, 1.0 + m (1, 1) - m (2, 2) - m (3, 3))) / 2.0, 
  119.          y => Sqrt (Real'Max (0.0, 1.0 - m (1, 1) + m (2, 2) - m (3, 3))) / 2.0, 
  120.          z => Sqrt (Real'Max (0.0, 1.0 - m (1, 1) - m (2, 2) + m (3, 3))) / 2.0); 
  121.  
  122.       q_max : constant Real := Real'Max (q.w, Real'Max (q.x, Real'Max (q.y, q.z))); 
  123.  
  124.       function Copy_Sign (Value, Sign : Real) return Real is 
  125.  
  126.       begin 
  127.          if Sign >= 0.0 then 
  128.             return +abs (Value); 
  129.          else 
  130.             return -abs (Value); 
  131.          end if; 
  132.       end Copy_Sign; 
  133.  
  134.    begin 
  135.       if q.w = q_max then 
  136.          q.x := Copy_Sign (q.x, (m (3, 2) - m (2, 3))); 
  137.          q.y := Copy_Sign (q.y, (m (1, 3) - m (3, 1))); 
  138.          q.z := Copy_Sign (q.z, (m (2, 1) - m (1, 2))); 
  139.       elsif q.x = q_max then 
  140.          q.w := Copy_Sign (q.x, (m (3, 2) - m (2, 3))); 
  141.          q.y := Copy_Sign (q.y, (m (2, 1) + m (1, 2))); 
  142.          q.z := Copy_Sign (q.z, (m (1, 3) + m (3, 1))); 
  143.       elsif q.y = q_max then 
  144.          q.w := Copy_Sign (q.x, (m (1, 3) - m (3, 1))); 
  145.          q.x := Copy_Sign (q.y, (m (2, 1) + m (1, 2))); 
  146.          q.z := Copy_Sign (q.z, (m (3, 2) + m (2, 3))); 
  147.       elsif q.z = q_max then 
  148.          q.w := Copy_Sign (q.x, (m (2, 1) - m (1, 2))); 
  149.          q.x := Copy_Sign (q.y, (m (3, 1) + m (1, 3))); 
  150.          q.y := Copy_Sign (q.z, (m (3, 2) + m (2, 3))); 
  151.       end if; 
  152.  
  153.       return Unit (q); 
  154.    end To_Rotation; 
  155.  
  156.    -- 
  157.  
  158.    function To_Vector (Quat : Quaternion_Rotation) return Vector_3D is 
  159.      (x => Quat.x, 
  160.       y => Quat.y, 
  161.       z => Quat.z); 
  162.  
  163.    -- 
  164.  
  165.    function Rotate (Current_Rotation, Additional_Rotation : Quaternion_Rotation) return Quaternion_Rotation is 
  166.  
  167.    begin 
  168.       return Additional_Rotation * Current_Rotation; 
  169.    end Rotate; 
  170.  
  171.    function Unit_Vector (Axis : Vector) return Vector is 
  172.  
  173.       Axis_Length : constant Real      := Sqrt (Axis (x)**2 + Axis (y)**2 + Axis (z)**2); 
  174.       Unit_Axis   : constant Vector_3D := (Axis (x) / Axis_Length, 
  175.                                            Axis (y) / Axis_Length, 
  176.                                            Axis (z) / Axis_Length); 
  177.  
  178.    begin 
  179.       return Unit_Axis; 
  180.    end Unit_Vector; 
  181.  
  182.    function Rotate (Current_Rotation : Quaternion_Rotation; Rotation_Axis : Vector; Rotation_Angle : Radiants) return Quaternion_Rotation is 
  183.  
  184.       Rotation_Q : constant Quaternion_Rotation := To_Rotation (Unit_Vector (Rotation_Axis), Rotation_Angle); 
  185.  
  186.    begin 
  187.       return Rotate (Current_Rotation, Rotation_Q); 
  188.    end Rotate; 
  189.  
  190.    -- 
  191.  
  192.    function Rotate    (Current_Vector, Rotation_Axis : Vector_3D; 
  193.                        Rotation_Angle     : Radiants) return Vector_3D is 
  194.  
  195.       Rotation_Q : constant Quaternion_Rotation := To_Rotation (Unit_Vector (Rotation_Axis), Rotation_Angle); 
  196.  
  197.    begin 
  198.       return To_Vector (Conj (Rotation_Q) * To_Quaternion (Current_Vector) * Rotation_Q); 
  199. --        return To_Vector (Rotation_Q * To_Quaternion (Current_Vector) * Conj (Rotation_Q)); 
  200.    end Rotate; 
  201.  
  202.    -- 
  203.  
  204.    function Rotate (Current_Vector : Vector; Apply_Rotation : Quaternion_Rotation) return Vector is 
  205.  
  206.    begin 
  207.       return To_Vector (Conj (Apply_Rotation) * To_Quaternion (Current_Vector) * Apply_Rotation); 
  208. --        return To_Vector (Apply_Rotation * To_Quaternion (Current_Vector) * Conj (Apply_Rotation)); 
  209.    end Rotate; 
  210.  
  211.    -- 
  212.  
  213.    function To_Matrix_3D (Quad : Quaternion_Rotation) return Matrix_3D is 
  214.  
  215.       x  :          Real renames Quad.x; 
  216.       y  :          Real renames Quad.y; 
  217.       z  :          Real renames Quad.z; 
  218.       w  :          Real renames Quad.w; 
  219.       x2 : constant Real := x * x; 
  220.       y2 : constant Real := y * y; 
  221.       z2 : constant Real := z * z; 
  222.       xw : constant Real := x * w; 
  223.       yw : constant Real := y * w; 
  224.       zw : constant Real := z * w; 
  225.       xy : constant Real := x * y; 
  226.       xz : constant Real := x * z; 
  227.       yz : constant Real := y * z; 
  228.  
  229.       Matrix : constant Matrix_3D := 
  230.         ((1.0 - 2.0 * (y2 + z2),       2.0 * (xy - zw),       2.0 * (xz + yw)), 
  231.                (2.0 * (xy + zw), 1.0 - 2.0 * (x2 + z2),       2.0 * (yz - xw)), 
  232.                (2.0 * (xz - yw),       2.0 * (yz + xw), 1.0 - 2.0 * (x2 + y2))); 
  233.  
  234.    begin 
  235.       return Matrix; 
  236.    end To_Matrix_3D; 
  237.  
  238.    -- 
  239.  
  240.    function To_Matrix_3D_OpenGL (Quad : Quaternion_Rotation) return Matrix_3D is 
  241.  
  242.       x  : constant Real := Quad.z; 
  243.       y  : constant Real := Quad.y; 
  244.       z  : constant Real := Quad.x; 
  245.       w  : constant Real := Quad.w; 
  246.       x2 : constant Real := x * x; 
  247.       y2 : constant Real := y * y; 
  248.       z2 : constant Real := z * z; 
  249.       xw : constant Real := x * w; 
  250.       yw : constant Real := y * w; 
  251.       zw : constant Real := z * w; 
  252.       xy : constant Real := x * y; 
  253.       xz : constant Real := x * z; 
  254.       yz : constant Real := y * z; 
  255.  
  256.       Matrix : constant Matrix_3D := 
  257.         ((1.0 - 2.0 * (y2 + z2),       2.0 * (xy - zw),       2.0 * (xz + yw)), 
  258.                (2.0 * (xy + zw), 1.0 - 2.0 * (x2 + z2),       2.0 * (yz - xw)), 
  259.                (2.0 * (xz - yw),       2.0 * (yz + xw), 1.0 - 2.0 * (x2 + y2))); 
  260.  
  261.    begin 
  262.       return Matrix; 
  263.    end To_Matrix_3D_OpenGL; 
  264.  
  265.    -- 
  266.  
  267.    function To_Matrix_4D (Quad : Quaternion_Rotation) return Matrix_4D is 
  268.  
  269.       x  :          Real renames Quad.x; 
  270.       y  :          Real renames Quad.y; 
  271.       z  :          Real renames Quad.z; 
  272.       w  :          Real renames Quad.w; 
  273.       x2 : constant Real := x * x; 
  274.       y2 : constant Real := y * y; 
  275.       z2 : constant Real := z * z; 
  276.       xw : constant Real := x * w; 
  277.       yw : constant Real := y * w; 
  278.       zw : constant Real := z * w; 
  279.       xy : constant Real := x * y; 
  280.       xz : constant Real := x * z; 
  281.       yz : constant Real := y * z; 
  282.  
  283.       Matrix : constant Matrix_4D := 
  284.         ((1.0 - 2.0 * (y2 + z2),       2.0 * (xy - zw),       2.0 * (xz + yw), 0.0), 
  285.                (2.0 * (xy + zw), 1.0 - 2.0 * (x2 + z2),       2.0 * (yz - xw), 0.0), 
  286.                (2.0 * (xz - yw),       2.0 * (yz + xw), 1.0 - 2.0 * (x2 + y2), 0.0), 
  287.                            (0.0,                   0.0,                   0.0, 1.0)); 
  288.  
  289.    begin 
  290.       return Matrix; 
  291.    end To_Matrix_4D; 
  292.  
  293.    -- 
  294.  
  295.    function Roll (Quad : Quaternion_Rotation) return Radiants is 
  296.  
  297.       Pole : constant Real := Real'Rounding (1_000_000.0 * (Quad.x * Quad.y + Quad.z * Quad.w)) / 1_000_000.0; 
  298.  
  299.    begin 
  300.       return (if Pole = 0.5 or else Pole = -0.5 then 
  301.                  0.0 
  302.               else 
  303.                  Arctan (2.0 * Quad.w * Quad.x - 2.0 * Quad.y * Quad.z, 1.0 - 2.0 * (Quad.x ** 2) - 2.0 * (Quad.z ** 2))); 
  304.    end Roll; 
  305.  
  306.    -- 
  307.  
  308.    function Pitch (Quad : Quaternion_Rotation) return Radiants is 
  309.      (Arcsin (Real'Max (-1.0, Real'Min (1.0, 2.0 * Quad.x * Quad.y + 2.0 * Quad.w * Quad.z)))); 
  310.  
  311.    -- 
  312.  
  313.    function Yaw (Quad : Quaternion_Rotation) return Radiants is 
  314.  
  315.       Pole : constant Real := Real'Rounding (1_000_000.0 * (Quad.x * Quad.y + Quad.z * Quad.w)) / 1_000_000.0; 
  316.  
  317.    begin 
  318.       return (if Pole = 0.5 then 
  319.                  +2.0 * Arctan (Quad.x, Quad.w) 
  320.               elsif Pole = -0.5 then 
  321.                  -2.0 * Arctan (Quad.x, Quad.w) 
  322.               else 
  323.                  Arctan (2.0 * Quad.w * Quad.y - 2.0 * Quad.x * Quad.z, 1.0 - 2.0 * (Quad.y ** 2) - 2.0 * (Quad.z **2))); 
  324.    end Yaw; 
  325.  
  326.    -- 
  327.  
  328.    function Roll  (Matrix : Matrix_3D) return Radiants is 
  329.      (if Matrix (2, 1) = 1.0 or else Matrix (2, 1) = -1.0 then 
  330.          0.0 
  331.       else 
  332.          Arctan (-Matrix (2, 3), Matrix (2, 2))); 
  333.  
  334.    -- 
  335.  
  336.    function Pitch (Matrix : Matrix_3D) return Radiants is (Arcsin (Matrix (2, 1))); 
  337.  
  338.    -- 
  339.  
  340.    function Yaw   (Matrix : Matrix_3D) return Radiants is 
  341.      (if Matrix (2, 1) = 1.0 or else Matrix (2, 1) = -1.0 then 
  342.          Arctan (Matrix (1, 2), Matrix (3, 3)) 
  343.       else 
  344.          Arctan (-Matrix (3, 1), Matrix (1, 1))); 
  345.  
  346.    -- 
  347.  
  348.    function To_Matrix_3D_OpenGL (Roll_Angle, Pitch_Angle, Yaw_Angle : Radiants; 
  349.                                  Order        : Rotation_Order := RPY; 
  350.                                  Column_First : Boolean        := True) return Matrix_3D is 
  351.  
  352.       use Matrices_3D; 
  353.  
  354.       cx : constant Real := Cos (Pitch_Angle); 
  355.       sx : constant Real := Sin (Pitch_Angle); 
  356.  
  357.       cy : constant Real := Cos (Yaw_Angle); 
  358.       sy : constant Real := Sin (Yaw_Angle); 
  359.  
  360.       cz : constant Real := Cos (Roll_Angle); 
  361.       sz : constant Real := Sin (Roll_Angle); 
  362.  
  363.       Mx : constant Matrix_3D := ((1.0, 0.0, 0.0), 
  364.                                   (0.0,  cx, -sx), 
  365.                                   (0.0,  sx,  cx)); 
  366.  
  367.       My : constant Matrix_3D := ((cy,  0.0,  sy), 
  368.                                   (0.0, 1.0, 0.0), 
  369.                                   (-sy, 0.0,  cy)); 
  370.  
  371.       Mz : constant Matrix_3D := ((cz,  -sz, 0.0), 
  372.                                   (sz,   cz, 0.0), 
  373.                                   (0.0, 0.0, 1.0)); 
  374.  
  375.       Mrot : Matrix_3D; 
  376.  
  377.       function Column_First_Mirror (Matrix : Matrix_3D) return Matrix_3D is 
  378.  
  379.          Mirrored_Matrix : Matrix_3D; 
  380.  
  381.       begin 
  382.          for i in Matrix'Range (1) loop 
  383.             for j in Matrix'Range (2) loop 
  384.                Mirrored_Matrix (j, i) := Matrix (i, j); 
  385.             end loop; 
  386.          end loop; 
  387.          return Mirrored_Matrix; 
  388.       end Column_First_Mirror; 
  389.  
  390.    begin 
  391.       case Order is 
  392.          when RPY => Mrot := Mz * Mx * My; 
  393.          when RYP => Mrot := Mz * My * Mx; 
  394.          when PRY => Mrot := Mx * Mz * My; 
  395.          when PYR => Mrot := Mx * My * Mz; 
  396.          when YRP => Mrot := My * Mz * Mx; 
  397.          when YPR => Mrot := My * Mx * Mz; 
  398.       end case; 
  399.  
  400.       if Column_First then 
  401.          return Column_First_Mirror (Mrot); 
  402.       else 
  403.          return Mrot; 
  404.       end if; 
  405.    end To_Matrix_3D_OpenGL; 
  406.  
  407.    -- 
  408.  
  409.    function To_Matrix_3D (Roll_Angle, Pitch_Angle, Yaw_Angle : Radiants) return Matrix_3D is 
  410.  
  411.       cx : constant Real := Cos (Roll_Angle); 
  412.       sx : constant Real := Sin (Roll_Angle); 
  413.  
  414.       cy : constant Real := Cos (Pitch_Angle); 
  415.       sy : constant Real := Sin (Pitch_Angle); 
  416.  
  417.       cz : constant Real := Cos (Yaw_Angle); 
  418.       sz : constant Real := Sin (Yaw_Angle); 
  419.  
  420.       Mx : constant Matrix_3D := ((1.0, 0.0, 0.0), 
  421.                                   (0.0,  cx, -sx), 
  422.                                   (0.0,  sx,  cx)); 
  423.  
  424.       My : constant Matrix_3D := ((cy,  0.0,  sy), 
  425.                                   (0.0, 1.0, 0.0), 
  426.                                   (-sy, 0.0,  cy)); 
  427.  
  428.       Mz : constant Matrix_3D := ((cz,   sz, 0.0), 
  429.                                   (-sz,  cz, 0.0), 
  430.                                   (0.0, 0.0, 1.0)); 
  431.  
  432.       use Matrices_3D; 
  433.  
  434.    begin 
  435.       return Mx * Mz * My; 
  436.    end To_Matrix_3D; 
  437.  
  438.    -- 
  439.  
  440. end Rotations;