with ada.Characters.latin_1; package body any_Math.any_Algebra.any_linear is ----------- --- Vectors -- function Norm_squared (Self : in Vector) return Real is Norm_2 : Real := 0.0; begin for Each in Self'Range loop Norm_2 := Norm_2 + Self (Each) * Self (Each); end loop; return Norm_2; end Norm_squared; procedure normalise (Self : in out Vector) is use Vectors; inverse_Norm : constant Real := 1.0 / abs Self; begin for Each in Self'Range loop Self (Each) := Self (Each) * inverse_Norm; end loop; end normalise; function Normalised (Self : in Vector) return Vector is Result : Vector := Self; begin normalise (Result); return Result; end Normalised; procedure normalise (Self : in out Vector_2) is inverse_Norm : constant Real := 1.0 / abs Self; begin Self := Self * inverse_Norm; end normalise; function Normalised (Self : in Vector_2) return Vector_2 is inverse_Norm : constant Real := 1.0 / abs Self; begin return Self * inverse_Norm; end Normalised; procedure normalise (Self : in out Vector_3) is inverse_Norm : constant Real := 1.0 / abs Self; begin Self := Self * inverse_Norm; end normalise; function Normalised (Self : in Vector_3) return Vector_3 is inverse_Norm : constant Real := 1.0 / abs Self; begin return Self * inverse_Norm; end Normalised; function Min (Left, Right : in Vector) return Vector is Min : Vector (Left'Range); begin pragma Assert (Left'Length = Right'Length); for Each in Min'Range loop Min (Each) := Real'Min (Left (Each), Right (Each)); end loop; return Min; end Min; function Max (Left, Right : in Vector) return Vector is Max : Vector (Left'Range); begin pragma Assert (Left'Length = Right'Length); for Each in Max'Range loop Max (Each) := Real'Max (Left (Each), Right (Each)); end loop; return Max; end Max; function scaled (Self : in Vector; By : in Vector) return Vector is Result : Vector (Self'Range); begin for Each in Result'Range loop Result (Each) := Self (Each) * By (Each); end loop; return Result; end scaled; ------------ --- Matrices -- function to_Matrix (Row_1, Row_2, Row_3 : in Vector_3) return Matrix_3x3 is begin return ((Row_1 (1), Row_1 (2), Row_1 (3)), (Row_2 (1), Row_2 (2), Row_2 (3)), (Row_3 (1), Row_3 (2), Row_3 (3))); end to_Matrix; function Min (Self : in Matrix) return Real is Min : Real := Real'Last; begin for each_Row in Self'Range (1) loop for each_Col in Self'Range (2) loop Min := Real'Min (Min, Self (each_Row, each_Col)); end loop; end loop; return Min; end Min; function Max (Self : in Matrix) return Real is Max : Real := Real'First; begin for each_Row in Self'Range (1) loop for each_Col in Self'Range (2) loop Max := Real'Max (Max, Self (each_Row, each_Col)); end loop; end loop; return Max; end Max; function Image (Self : in Matrix) return String is Image : String (1 .. 1024 * 1024); -- Handles one megabyte image, excess is truncated. Count : Standard.Natural := 0; procedure add (Text : in String) is begin Image (Count + 1 .. Count + text'Length) := Text; Count := Count + text'Length; end add; begin add ("("); for Row in self'Range (1) loop add ((1 => ada.Characters.latin_1.LF)); if Row /= self'First (1) then add (", "); end if; for Col in self'Range (2) loop if Col /= self'First (2) then add (", "); end if; add (Real'Image (Self (Row, Col))); end loop; end loop; add (")"); return Image (1 .. Count); exception when others => return Image (1 .. Count); end Image; function is_Square (Self : in Matrix) return Boolean is begin return Self'Length (1) = Self'Length (2); end is_Square; function sub_Matrix (Self : in Matrix; start_Row, end_Row : in Index; start_Col, end_Col : in Index) return Matrix is sub_Matrix : Matrix (1 .. end_Row - start_Row + 1, 1 .. end_Col - start_Col + 1); begin for each_Row in sub_Matrix'Range (1) loop for each_Col in sub_Matrix'Range (2) loop sub_Matrix (each_Row, each_Col) := Self (each_Row + start_Row - 1, each_Col + start_Col - 1); end loop; end loop; return sub_Matrix; end sub_Matrix; function Identity (Size : in Index := 3) return Matrix is Result : Matrix (1 .. Size, 1 .. Size); begin for Row in 1 .. Size loop for Col in 1 .. Size loop if Row = Col then Result (Row, Col) := 1.0; else Result (Row, Col) := 0.0; end if; end loop; end loop; return Result; end Identity; procedure invert (Self : in out Matrix) is use Vectors; begin Self := Inverse (Self); end invert; --------------- --- Quaternions -- function to_Quaternion (axis_X, axis_Y, axis_Z : in Real; Angle : in Real) return Quaternion is Result : Quaternion; L : Real := axis_X * axis_X + axis_Y * axis_Y + axis_Z * axis_Z; begin if L > 0.0 then declare use Functions; half_Angle : constant Real := Angle * 0.5; begin Result.R := Cos (half_Angle); L := Sin (half_Angle) * (1.0 / SqRt (L)); Result.V (1) := axis_X * L; Result.V (2) := axis_Y * L; Result.V (3) := axis_Z * L; end; else Result.R := L; Result.V (1) := 0.0; Result.V (2) := 0.0; Result.V (3) := 0.0; end if; return Result; end to_Quaternion; function to_Quaternion (Axis : in Vector_3; Angle : in Real) return Quaternion is Result : Quaternion; L : Real := Axis * Axis; begin if L > 0.0 then declare use Functions; half_Angle : constant Real := Angle * 0.5; begin Result.R := Cos (half_Angle); L := Sin (half_Angle) * (1.0 / SqRt (L)); Result.V := Axis * L; end; else Result.R := L; Result.V := (0.0, 0.0, 0.0); end if; return Result; end to_Quaternion; function "*" (Self : in Quaternion; By : in Quaternion) return Quaternion is x : constant := 1; y : constant := 2; z : constant := 3; A : Quaternion renames Self; B : Quaternion renames By; AtBt : constant Real := A.R * B.R; AxBx : constant Real := A.V (x) * B.V (x); AyBy : constant Real := A.V (y) * B.V (y); AzBz : constant Real := A.V (z) * B.V (z); AtBx : constant Real := A.R * B.V (x); AxBt : constant Real := A.V (x) * B.R; AyBz : constant Real := A.V (y) * B.V (z); AzBy : constant Real := A.V (z) * B.V (y); AtBy : constant Real := A.R * B.V (y); AxBz : constant Real := A.V (x) * B.V (z); AyBt : constant Real := A.V (y) * B.R; AzBx : constant Real := A.V (z) * B.V (x); AtBz : constant Real := A.R * B.V (z); AxBy : constant Real := A.V (x) * B.V (y); AyBx : constant Real := A.V (y) * B.V (x); AzBt : constant Real := A.V (z) * B.R; begin return (R => AtBt - AxBx - AyBy - AzBz, V => (AtBx + AxBt + AyBz - AzBy, AtBy - AxBz + AyBt + AzBx, AtBz + AxBy - AyBx + AzBt)); end "*"; function Unit (Self : in Quaternion) return Quaternion is begin return to_Quaternion ( to_Vector (Self) / abs to_Vector (Self)); end Unit; function infinitesimal_Rotation_from (Self : in Quaternion; angular_Velocity : in Vector_3) return Quaternion is i_Rotation : Quaternion; begin i_Rotation.R := 0.5 * (- angular_Velocity (1) * Self.V (1) - angular_Velocity (2) * Self.V (2) - angular_Velocity (3) * Self.V (3)); i_Rotation.V (1) := 0.5 * ( angular_Velocity (1) * Self.R + angular_Velocity (2) * Self.V (3) - angular_Velocity (3) * Self.V (2)); i_Rotation.V (2) := 0.5 * (- angular_Velocity (1) * Self.V (3) + angular_Velocity (2) * Self.R + angular_Velocity (3) * Self.V (1)); i_Rotation.V (3) := 0.5 * ( angular_Velocity (1) * Self.V (2) - angular_Velocity (2) * Self.V (1) + angular_Velocity (3) * Self.R); return i_Rotation; end infinitesimal_Rotation_from; function euler_Angles (Self : in Quaternion) return Vector_3 -- 'Self' can be a non-normalised quaternion. is use Functions; w : Real renames Self.R; x : Real renames Self.V (1); y : Real renames Self.V (2); z : Real renames Self.V (3); the_Angles : Vector_3; Bank : Real renames the_Angles (1); Heading : Real renames the_Angles (2); Attitude : Real renames the_Angles (3); sqw : constant Real := w * w; sqx : constant Real := x * x; sqy : constant Real := y * y; sqz : constant Real := z * z; unit : constant Real := sqx + sqy + sqz + sqw; -- If normalised then is 1.0 else is a correction factor. test : constant Real := x * y + z * w; begin if test > 0.499 * unit then -- Singularity at north pole. Heading := 2.0 * arcTan (x, w); Attitude := Pi / 2.0; Bank := 0.0; return the_Angles; end if; if test < -0.499 * unit then -- Singularity at south pole. Heading := -2.0 * arcTan (x, w); Attitude := -Pi / 2.0; Bank := 0.0; return the_Angles; end if; Heading := arcTan (2.0 * y * w - 2.0 * x * z, sqx - sqy - sqz + sqw); Bank := arcTan (2.0 * x * w - 2.0 * y * z, -sqx + sqy - sqz + sqw); Attitude := arcSin (2.0 * test / unit); return the_Angles; end euler_Angles; function to_Quaternion (Self : in Matrix_3x3) return Quaternion is use Functions; TR : Real; S : Real; Result : Quaternion; begin TR := Self (1, 1) + Self (2, 2) + Self (3, 3); if TR >= 0.0 then S := SqRt (TR + 1.0); Result.R := 0.5 * S; S := 0.5 * (1.0 / S); Result.V (1) := (Self (3, 2) - Self (2, 3)) * S; Result.V (2) := (Self (1, 3) - Self (3, 1)) * S; Result.V (3) := (Self (2, 1) - Self (1, 2)) * S; return Result; end if; -- Otherwise, find the largest diagonal element and apply the appropriate case. -- declare function case_1_Result return Quaternion is begin S := SqRt (Self (1, 1) - (Self (2, 2) + Self (3, 3)) + 1.0); Result.V (1) := 0.5 * S; S := 0.5 * (1.0 / S); Result.V (2) := (Self (1, 2) + Self (2, 1)) * S; Result.V (3) := (Self (3, 1) + Self (1, 3)) * S; Result.R := (Self (3, 2) - Self (2, 3)) * S; return Result; end case_1_Result; function case_2_Result return Quaternion is begin S := SqRt (Self (2, 2) - (Self (3, 3) + Self (1, 1)) + 1.0); Result.V (2) := 0.5 * S; S := 0.5 * (1.0 / S); Result.V (3) := (Self (2, 3) + Self (3, 2)) * S; Result.V (1) := (Self (1, 2) + Self (2, 1)) * S; Result.R := (Self (1, 3) - Self (3, 1)) * S; return Result; end case_2_Result; function case_3_Result return Quaternion is begin S := SqRt (Self (3, 3) - (Self (1, 1) + Self (2, 2)) + 1.0); Result.V (3) := 0.5 * S; S := 0.5 * (1.0 / S); Result.V (1) := (Self (3, 1) + Self (1, 3)) * S; Result.V (2) := (Self (2, 3) + Self (3, 2)) * S; Result.R := (Self (2, 1) - Self (1, 2)) * S; return Result; end case_3_Result; pragma Inline (case_1_Result); pragma Inline (case_2_Result); pragma Inline (case_3_Result); begin if Self (2, 2) > Self (1, 1) then if Self (3, 3) > Self (2, 2) then return case_3_Result; end if; return case_2_Result; end if; if Self (3, 3) > Self (1, 1) then return case_3_Result; end if; return case_1_Result; end; end to_Quaternion; function Conjugate (Self : in Quaternion) return Quaternion is begin return (Self.R, -Self.V); end conjugate; procedure normalise (Self : in out Quaternion) is begin Self := Normalised (Self); end normalise; function Normalised (Self : in Quaternion) return Quaternion is begin return to_Quaternion (Vector_4 (Normalised (Vector (to_Vector (Self))))); end Normalised; end any_Math.any_Algebra.any_linear;