nichkara/lace
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add initial prototype.

Browse Source
This commit is contained in:
Rod Kay
2022-07-31 17:34:54 +10:00
commit 54a53b2ac0
1421 changed files with 358874 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

253
1-base/math/source/generic/pure/algebra/any_math-any_algebra-any_linear-any_d2.adb Normal file
View File

@@ -0,0 +1,253 @@
package body any_Math.any_Algebra.any_linear.any_d2
is
-----------
-- Vector_2
--
function Angle_between_pre_Norm (U, V : in Vector_2) return Radians
is
use Functions, Vectors;
Val : Real := U * V; -- Dot product.
begin
if val < -1.0 then val := -1.0; -- Clamp to avoid rounding errors. arcCos will
elsif val > 1.0 then val := 1.0; -- fail with values outside this range.
end if;
return arcCos (Val);
end Angle_between_pre_Norm;
function Midpoint (From, To : in Vector_2) return Vector_2
is
begin
return [(From (1) + To (1)) * 0.5,
(From (2) + To (2)) * 0.5];
end Midpoint;
function Distance (From, To : in Vector_2) return Real
is
begin
return abs (From - To);
end Distance;
function Interpolated (From, To : in Vector_2;
Percent : in unit_Percentage) return Vector_2
is
P : constant Real := to_Real (Percent);
S : constant Real := 1.0 - P;
begin
return [S * From (1) + P * To (1),
S * From (2) + P * To (2)];
end Interpolated;
-------------
-- Matrix_2x2
--
function to_Matrix (Row_1, Row_2 : in Vector_2) return Matrix_2x2
is
begin
return [[Row_1 (1), Row_1 (2)],
[Row_2 (1), Row_2 (2)]];
end to_Matrix;
function to_rotation_Matrix (Angle : in Radians ) return Matrix_2x2
is
use Functions;
begin
return [[ cos (Angle), sin (Angle)],
[-sin (Angle), cos (Angle)]];
end to_rotation_Matrix;
function up_Direction (Self : in Matrix_2x2) return Vector_2
is
begin
return Normalised (Row (Self, 2));
end up_Direction;
function right_Direction (Self : in Matrix_2x2) return Vector_2
is
begin
return Normalised (Row (Self, 1));
end right_Direction;
function to_rotation_Transform (Rotation : in Matrix_2x2) return Matrix_3x3
is
begin
return [[Rotation (1, 1), Rotation (1, 2), 0.0],
[Rotation (2, 1), Rotation (2, 2), 0.0],
[ 0.0, 0.0, 1.0]];
end to_rotation_Transform;
-------------
-- Transform
--
function to_Transform_2d (From : in Matrix_3x3) return Transform_2d
is
begin
return (Rotation => get_Rotation (From),
Translation => get_Translation (From));
end to_Transform_2d;
function to_Transform (From : in Transform_2d) return Matrix_3x3
is
begin
return to_rotation_Transform (From.Rotation) * to_translation_Transform (From.Translation);
end to_Transform;
function to_translation_Transform (Translation : Vector_2) return Matrix_3x3
is
begin
return [[ 1.0, 0.0, 0.0],
[ 0.0, 1.0, 0.0],
[Translation (1), Translation (2), 1.0]];
end to_translation_Transform;
function to_rotation_Transform (Angle : in Radians) return Matrix_3x3
is
use Functions;
begin
return [[ cos (Angle), sin (Angle), 0.0],
[-sin (Angle), cos (Angle), 0.0],
[ 0.0, 0.0, 1.0]];
end to_rotation_Transform;
function to_scale_Transform (Scale : in Vector_2) return Matrix_3x3
is
begin
return [[Scale (1), 0.0, 0.0],
[ 0.0, Scale (2), 0.0],
[ 0.0, 0.0, 1.0]];
end to_scale_Transform;
function to_Transform (Rotation : in Matrix_2x2;
Translation : in Vector_2) return Matrix_3x3
is
begin
return [[Rotation (1, 1), Rotation (1, 2), 0.0],
[Rotation (2, 1), Rotation (2, 2), 0.0],
[Translation (1), Translation (2), 1.0]];
end to_Transform;
function to_Transform_2d (Rotation : in Radians;
Translation : in Vector_2) return Transform_2d
is
begin
return (to_rotation_Matrix (Rotation),
Translation);
end to_Transform_2d;
function "*" (Left : in Vector_2; Right : in Transform_2d) return Vector_2
is
Pad : constant Vector_3 := [Left (1), Left (2), 1.0];
Result : constant Vector_3 := Pad * to_Transform (Right);
begin
return Vector_2 (Result (1 .. 2));
end "*";
function "*" (Left : in Vector_2; Right : in Matrix_3x3) return Vector_2
is
use Vectors;
Result : constant Vector := Vector (Left & 1.0) * Matrix (Right);
begin
return Vector_2 (Result (1 .. 2));
end "*";
function Invert (Transform : in Transform_2d) return Transform_2d
is
inverse_Rotation : constant Matrix_2x2 := Transpose (Transform.Rotation);
begin
return (Translation => inverse_Rotation * (-Transform.Translation),
Rotation => inverse_Rotation);
end Invert;
function inverse_Transform (Transform : in Transform_2d; Vector : in Vector_2) return Vector_2
is
V : constant Vector_2 := Vector - Transform.Translation;
begin
return Transpose (Transform.Rotation) * V;
end inverse_Transform;
function get_Rotation (Transform : in Matrix_3x3) return Matrix_2x2
is
begin
return [[Transform (1, 1), Transform (1, 2)],
[Transform (2, 1), Transform (2, 2)]];
end get_Rotation;
procedure set_Rotation (Transform : in out Matrix_3x3; To : in Matrix_2x2)
is
begin
Transform (1, 1) := To (1, 1);
Transform (1, 2) := To (1, 2);
Transform (1, 3) := 0.0;
Transform (2, 1) := To (2, 1);
Transform (2, 2) := To (2, 2);
Transform (2, 3) := 0.0;
end set_Rotation;
function get_Translation (Transform : in Matrix_3x3) return Vector_2
is
begin
return [Transform (3, 1),
Transform (3, 2)];
end get_Translation;
procedure set_Translation (Transform : in out Matrix_3x3; To : in Vector_2)
is
begin
Transform (3, 1) := To (1);
Transform (3, 2) := To (2);
Transform (3, 3) := 1.0;
end set_Translation;
end any_Math.any_Algebra.any_linear.any_d2;

55
1-base/math/source/generic/pure/algebra/any_math-any_algebra-any_linear-any_d2.ads Normal file
View File

@@ -0,0 +1,55 @@
generic
package any_Math.any_Algebra.any_linear.any_d2
is
pragma Pure;
-----------
-- Vector_2
--
function Interpolated (From, To : in Vector_2; Percent : in unit_Percentage) return Vector_2;
function Distance (From, To : in Vector_2) return Real;
function Midpoint (From, To : in Vector_2) return Vector_2;
function Angle_between_pre_Norm (U, V : in Vector_2) return Radians;
--
-- Given that the vectors 'U' and 'V' are already normalized, returns a positive angle between 0 and 180 degrees.
-------------
-- Matrix_2x2
--
function to_Matrix (Row_1,
Row_2 : in Vector_2) return Matrix_2x2;
function to_rotation_Matrix (Angle : in Radians) return Matrix_2x2;
function up_Direction (Self : in Matrix_2x2) return Vector_2;
function right_Direction (Self : in Matrix_2x2) return Vector_2;
------------
-- Transform
--
function to_Transform (Rotation : in Matrix_2x2;
Translation : in Vector_2) return Matrix_3x3;
function to_Transform (From : in Transform_2d) return Matrix_3x3;
function to_translation_Transform (Translation : in Vector_2) return Matrix_3x3;
function to_rotation_Transform (Rotation : in Matrix_2x2) return Matrix_3x3;
function to_rotation_Transform (Angle : in Radians ) return Matrix_3x3;
function to_scale_Transform (Scale : in Vector_2) return Matrix_3x3;
function to_Transform_2d (From : in Matrix_3x3) return Transform_2d;
function to_Transform_2d (Rotation : in Radians;
Translation : in Vector_2) return Transform_2d;
function "*" (Left : in Vector_2; Right : in Transform_2d) return Vector_2;
function "*" (Left : in Vector_2; Right : in Matrix_3x3) return Vector_2;
function Invert (Transform : in Transform_2d) return Transform_2d;
function inverse_Transform (Transform : in Transform_2d; Vector : in Vector_2) return Vector_2;
function get_Rotation (Transform : in Matrix_3x3) return Matrix_2x2;
procedure set_Rotation (Transform : in out Matrix_3x3; To : in Matrix_2x2);
function get_Translation (Transform : in Matrix_3x3) return Vector_2;
procedure set_Translation (Transform : in out Matrix_3x3; To : in Vector_2);
end any_Math.any_Algebra.any_linear.any_d2;

1027
1-base/math/source/generic/pure/algebra/any_math-any_algebra-any_linear-any_d3.adb Normal file
View File

File diff suppressed because it is too large Load Diff

226
1-base/math/source/generic/pure/algebra/any_math-any_algebra-any_linear-any_d3.ads Normal file
View File

@@ -0,0 +1,226 @@
generic
package any_math.any_Algebra.any_linear.any_d3
is
pragma Pure;
------------
--- Vector_3
--
function Distance (From, To : in Vector_3) return Real;
function Midpoint (From, To : in Vector_3) return Vector_3;
function Interpolated (From, To : in Vector_3;
Percent : in unit_Percentage) return Vector_3;
function Angle_between_pre_Norm (U, V : in Vector_3) return Radians;
--
-- Given that the vectors 'U' and 'V' are already normalized, returns a positive angle between 0 and 180 degrees.
function Angle (Point_1,
Point_2,
Point_3 : in Vector_3) return Radians;
--
-- Returns the angle between the vector Point_1 to Point_2 and the vector Point_3 to Point_2.
--------------
--- Matrix_3x3
--
z_Up_to_y_Up : constant Matrix_3x3; -- Provides a rotation which may be multiplied
y_Up_to_z_Up : constant Matrix_3x3; -- by a vector to change co-ordinate systems.
function to_Matrix (Row_1,
Row_2,
Row_3 : in Vector_3) return Matrix_3x3;
function forward_Direction (Matrix : in Matrix_3x3) return Vector_3;
function up_Direction (Matrix : in Matrix_3x3) return Vector_3;
function right_Direction (Matrix : in Matrix_3x3) return Vector_3;
procedure re_Orthonormalise (Matrix : in out Matrix_3x3);
-------------
--- Rotations
--
function x_Rotation_from (Angle : in Radians) return Matrix_3x3;
function y_Rotation_from (Angle : in Radians) return Matrix_3x3;
function z_Rotation_from (Angle : in Radians) return Matrix_3x3;
function xyz_Rotation (x_Angle,
y_Angle,
z_Angle : in Real) return Matrix_3x3;
function xyz_Rotation (Angles : in Vector_3) return Matrix_3x3;
function to_Rotation (Axis : in Vector_3;
Angle : in Real) return Matrix_3x3;
function to_Rotation (Axis_x,
Axis_y,
Axis_z : in Real;
Rotation : in Radians) return Matrix_3x3;
--
-- Returns a rotation matrix describing a given rotation about an axis.
-- (TODO: Make this obsolescent and use the vector Axis version instead.)
---------
--- Euler
--
type Euler is new Vector_3;
--
-- 1: Roll
-- 2: Pitch
-- 3: Yaw
function to_Rotation (Angles : in Euler) return Matrix_3x3;
--
-- The euler angles are used to produce a rotation matrix. The euler
-- angles are applied in ZYX order. That is, a vector is first rotated
-- about X, then Y and then Z.
-----------
--- General
--
function Look_at (Eye,
Center,
Up : in Vector_3) return Matrix_4x4;
function to_Viewport_Transform (Origin,
Extent : in Vector_2) return Matrix_4x4;
function to_Perspective (FoVy : in Degrees;
Aspect,
zNear,
zFar : in Real) return Matrix_4x4;
-------------
--- Transform
--
function to_Translation_Matrix (Translation : in Vector_3) return Matrix_4x4;
function to_Transform (Matrix : in Matrix_4x4) return Transform_3d;
function "*" (Left : in Transform_3d; Right : in Vector_3) return Vector_3;
function "*" (Left : in Vector_3; Right : in Transform_3d) return Vector_3;
function "*" (Left : in Transform_3d; Right : in Transform_3d) return Transform_3d;
function "*" (Left : in Vector_3; Right : in Matrix_4x4) return Vector_3;
function Invert (Transform : in Transform_3d) return Transform_3d;
function inverse_Transform (Transform : in Transform_3d; Vector : in Vector_3) return Vector_3;
--------------
--- Quaternion
--
procedure set_from_Matrix_3x3 (Quat : out Quaternion; Matrix : in Matrix_3x3);
function to_Quaternion (Matrix : in Matrix_3x3) return Quaternion;
function to_Matrix (Quat : in Quaternion) return Matrix_3x3;
function Norm (Quat : in Quaternion) return Real;
function Versor (Quat : in Quaternion) return Quaternion; -- Produces the unit quaternion of Quat.
function Farthest (Quat : in Quaternion; qd : in Quaternion) return Quaternion; -- TODO: Document this.
function Invert (Quat : in Quaternion) return Quaternion;
function Angle (Quat : in Quaternion) return Radians;
function Axis (Quat : in Quaternion) return Vector_3;
function "*" (Left, Right : in Quaternion) return Real; -- Dot product.
function "*" (Left, Right : in Quaternion) return Quaternion; -- Cross product.
function "+" (Left, Right : in Quaternion) return Quaternion;
function "-" (Left, Right : in Quaternion) return Quaternion;
function "-" (Quat : in Quaternion) return Quaternion;
function "*" (Left : in Quaternion; Right : in Vector_3) return Quaternion;
function "*" (Left : in Vector_3; Right : in Quaternion) return Quaternion;
function "*" (Left : in Quaternion; Right : in Real) return Quaternion;
function Interpolated (From,
To : in Quaternion;
Percent : in unit_Percentage) return Quaternion;
--
-- Return the quaternion which is the result of spherical linear interpolation (Slerp) between Initial and Final.
-- Percent is the ratio between 'From' and 'To' to interpolate.
-- If Percent = 0.0 the result is Initial.
-- If Percent = 100.0 the result is Final.
-- Interpolates assuming constant velocity.
------------
--- Vector_4
--
function "/" (Left, Right : in Vector_4) return Vector_4;
function max_Axis (Vector : in Vector_4) return Integer;
function closest_Axis (Vector : in Vector_4) return Integer;
function to_transform_Matrix (Transform : in Transform_3d) return Matrix_4x4;
function to_transform_Matrix (Rotation : in Matrix_3x3;
Translation : in Vector_3) return Matrix_4x4;
function to_rotate_Matrix (Rotation : in Matrix_3x3) return Matrix_4x4;
function to_translate_Matrix (Translation : in Vector_3) return Matrix_4x4;
function to_scale_Matrix (Scale : in Vector_3) return Matrix_4x4;
----------------------
--- Transform Matrices
--
function get_Rotation (Transform : in Matrix_4x4) return Matrix_3x3;
procedure set_Rotation (Transform : in out Matrix_4x4; To : in Matrix_3x3);
function get_Translation (Transform : in Matrix_4x4) return Vector_3;
procedure set_Translation (Transform : in out Matrix_4x4; To : in Vector_3);
function inverse_Rotation (Rotation : in Matrix_3x3) return Matrix_3x3;
function inverse_Transform (Transform : in Matrix_4x4) return Matrix_4x4;
--------------
--- un-Project
--
type Rectangle is
record
Min : Integers (1 .. 2); -- Bottom left corner.
Max : Integers (1 .. 2); -- Upper right corner.
end record;
function unProject (From : in Vector_3;
Model : in Matrix_4x4;
Projection : in Matrix_4x4;
Viewport : in Rectangle) return Vector_3;
--
-- Maps the 'From' window space coordinates into object space coordinates using Model, Projection and Viewport.
private
z_Up_to_y_Up : constant Matrix_3x3 := [[1.0, 0.0, 0.0],
[0.0, 0.0, 1.0],
[0.0, -1.0, 0.0]];
y_Up_to_z_Up : constant Matrix_3x3 := [[1.0, 0.0, 0.0],
[0.0, 0.0, -1.0],
[0.0, 1.0, 0.0]];
pragma Inline ("+");
pragma Inline ("-");
pragma Inline ("*");
end any_math.any_Algebra.any_linear.any_d3;

589
1-base/math/source/generic/pure/algebra/any_math-any_algebra-any_linear.adb Normal file
View File

@@ -0,0 +1,589 @@
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;

103
1-base/math/source/generic/pure/algebra/any_math-any_algebra-any_linear.ads Normal file
View File

@@ -0,0 +1,103 @@
generic
package any_Math.any_Algebra.any_linear
is
pragma Pure;
----------
-- Vector
--
function Norm_squared (Self : in Vector) return Real; -- Length squared.
function Normalised (Self : in Vector) return Vector;
procedure Normalise (Self : in out Vector);
function Normalised (Self : in Vector_2) return Vector_2;
procedure Normalise (Self : in out Vector_2);
function Normalised (Self : in Vector_3) return Vector_3;
procedure Normalise (Self : in out Vector_3);
function Min (Left, Right : in Vector) return Vector;
function Max (Left, Right : in Vector) return Vector;
function Scaled (Self : in Vector; By : in Vector) return Vector;
----------
-- Matrix
--
function to_Matrix (Row_1,
Row_2,
Row_3 : in Vector_3) return Matrix_3x3;
function Identity (Size : in Index := 3) return Matrix;
function Min (Self : in Matrix) return Real;
function Max (Self : in Matrix) return Real;
function Image (Self : in Matrix) return String;
procedure invert (Self : in out Matrix);
function is_Square (Self : in Matrix) return Boolean;
function sub_Matrix (Self : in Matrix;
start_Row, end_Row : in Index;
start_Col, end_Col : in Index) return Matrix;
---------------
-- Quaternion
--
function to_Quaternion (axis_X,
axis_Y,
axis_Z : in Real;
Angle : in Real) return Quaternion;
--
-- Returns a quaternion defined by a rotation about an axis.
-- (TODO: rid this and use Vector_3 version instead.)
function to_Quaternion (Axis : in Vector_3;
Angle : in Real) return Quaternion;
--
-- Returns a quaternion defined by a rotation about an axis.
function to_Quaternion (Self : in Matrix_3x3) return Quaternion;
function "*" (Self : in Quaternion; By : in Quaternion) return Quaternion;
--
-- Grassmann product.
function Unit (Self : in Quaternion) return Quaternion;
function Conjugate (Self : in Quaternion) return Quaternion;
--
-- (TODO: only for unit quaternions.)
function euler_Angles (Self : in Quaternion) return Vector_3;
function infinitesimal_Rotation_from
(Self : in Quaternion; angular_Velocity : in Vector_3) return Quaternion;
--
-- An infinitesimal rotation may be multiplied by a duration and then added to the original attitude
-- to produce the attitude at the given time.
function Normalised (Self : in Quaternion) return Quaternion;
procedure normalise (Self : in out Quaternion);
private
pragma Inline ("*");
pragma Inline_Always (Norm_squared);
pragma Inline_Always (Normalise);
end any_Math.any_Algebra.any_linear;

7
1-base/math/source/generic/pure/algebra/any_math-any_algebra.ads Normal file
View File

@@ -0,0 +1,7 @@
generic
package any_Math.any_Algebra
is
pragma Pure;
end any_Math.any_Algebra;