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