Squashed 'Assets/NanoBrain/LinearAlgebra/' content from commit 15c08f2

git-subtree-dir: Assets/NanoBrain/LinearAlgebra
git-subtree-split: 15c08f215655988682ecc6207c2783fa047b65e3

.editorconfig

@@ -0,0 +1,19 @@
+# EditorConfig is awesome: https://EditorConfig.org
+
+# top-most EditorConfig file
+root = true
+
+[*]
+indent_style = space
+indent_size = 4
+end_of_line = crlf
+charset = utf-8
+trim_trailing_whitespace = false
+insert_final_newline = false
+max_line_length = 80
+
+[*.cs]
+csharp_new_line_before_open_brace = none
+# Suppress warnings everywhere
+dotnet_diagnostic.IDE1006.severity = none
+dotnet_diagnostic.IDE0130.severity = none

.gitea/workflows/unit_tests.yaml

@@ -0,0 +1,37 @@
+name: Build and Run C# Unit Tests
+
+on:
+  push:
+    branches:
+      - '**'
+  pull_request:
+    branches:
+      - '**'
+
+jobs:
+  build-and-test:
+    runs-on: ubuntu-latest
+
+    steps:
+      - name: Checkout code
+        uses: actions/checkout@v2
+
+      - name: Setup .NET
+        uses: actions/setup-dotnet@v1
+        with:
+          dotnet-version: '8.0.x'  # Specify the .NET SDK version
+
+      - name: Check Current Directory
+        run: pwd  # Logs the current working directory
+
+      - name: List Files
+        run: ls -la  # Lists all files in the current directory
+
+      - name: Restore Dependencies
+        run: dotnet restore ./LinearAlgebra-csharp.sln  # Restore NuGet packages
+
+      - name: Build the Project
+        run: dotnet build ./LinearAlgebra-csharp.sln --configuration Release  # Build the C# project
+
+      - name: Run Unit Tests
+        run: dotnet test ./test/LinearAlgebra_Test.csproj --configuration Release  # Execute unit tests

.gitignore

@@ -0,0 +1,5 @@
+DoxyGen/DoxyWarnLogfile.txt
+.vscode/settings.json
+**bin
+**obj
+**.meta

LinearAlgebra-csharp.sln

@@ -0,0 +1,30 @@
+Microsoft Visual Studio Solution File, Format Version 12.00
+# Visual Studio Version 17
+VisualStudioVersion = 17.5.2.0
+MinimumVisualStudioVersion = 10.0.40219.1
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "LinearAlgebra", "src\LinearAlgebra.csproj", "{ECB58727-0354-924D-AE7B-22F6B21097EB}"
+EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "LinearAlgebra_Test", "test\LinearAlgebra_Test.csproj", "{715BB399-5FC4-2AC9-3757-177CA0C80774}"
+EndProject
+Global
+	GlobalSection(SolutionConfigurationPlatforms) = preSolution
+		Debug|Any CPU = Debug|Any CPU
+		Release|Any CPU = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(ProjectConfigurationPlatforms) = postSolution
+		{ECB58727-0354-924D-AE7B-22F6B21097EB}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{ECB58727-0354-924D-AE7B-22F6B21097EB}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{ECB58727-0354-924D-AE7B-22F6B21097EB}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{ECB58727-0354-924D-AE7B-22F6B21097EB}.Release|Any CPU.Build.0 = Release|Any CPU
+		{715BB399-5FC4-2AC9-3757-177CA0C80774}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{715BB399-5FC4-2AC9-3757-177CA0C80774}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{715BB399-5FC4-2AC9-3757-177CA0C80774}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{715BB399-5FC4-2AC9-3757-177CA0C80774}.Release|Any CPU.Build.0 = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(SolutionProperties) = preSolution
+		HideSolutionNode = FALSE
+	EndGlobalSection
+	GlobalSection(ExtensibilityGlobals) = postSolution
+		SolutionGuid = {E93B8294-87D4-4887-83B7-182A623D5833}
+	EndGlobalSection
+EndGlobal

src/Angle.cs

@@ -0,0 +1,332 @@
+using System;
+
+namespace LinearAlgebra {
+
+    public struct AngleFloat {
+        public const float Rad2Deg = 360.0f / ((float)Math.PI * 2); //0.0174532924F;
+        public const float Deg2Rad = (float)Math.PI * 2 / 360.0f; //57.29578F;
+
+        private AngleFloat(float degrees) {
+            this.value = degrees;
+        }
+        private readonly float value;
+
+        public static AngleFloat Degrees(float degrees) {
+            // Reduce it to (-180..180]
+            if (float.IsFinite(degrees)) {
+                while (degrees < -180)
+                    degrees += 360;
+                while (degrees >= 180)
+                    degrees -= 360;
+            }
+            return new AngleFloat(degrees);
+        }
+
+        public static AngleFloat Radians(float radians) {
+            // Reduce it to (-pi..pi]
+            if (float.IsFinite(radians)) {
+                while (radians <= -Math.PI)
+                    radians += 2 * (float)Math.PI;
+                while (radians > Math.PI)
+                    radians -= 2 * (float)Math.PI;
+            }
+
+            return new AngleFloat(radians * Rad2Deg);
+
+        }
+
+        public static AngleFloat Revolutions(float revolutions) {
+            // reduce it to (-0.5 .. 0.5]
+            if (float.IsFinite(revolutions)) {
+                // Get the integer part
+                int integerPart = (int)revolutions;
+
+                // Get the decimal part
+                revolutions -= integerPart;
+                if (revolutions < -0.5)
+                    revolutions += 1;
+                if (revolutions >= 0.5)
+                    revolutions -= 1;
+            }
+            return new AngleFloat(revolutions * 360);
+        }
+
+        public float inDegrees {
+            get { return this.value; }
+        }
+
+        public float inRadians {
+            get { return this.value * Deg2Rad; }
+        }
+
+        public float inRevolutions {
+            get { return this.value / 360.0f; }
+        }
+
+        public static readonly AngleFloat zero = Degrees(0);
+        public static readonly AngleFloat deg90 = Degrees(90);
+        public static readonly AngleFloat deg180 = Degrees(180);
+
+        /// <summary>
+        /// Get the sign of the angle
+        /// </summary>
+        /// <param name="a">The angle</param>
+        /// <returns>-1 when the angle is negative, 1 when it is positive and 0 in all other cases</returns>
+        public static int Sign(AngleFloat a) {
+            if (a.value < 0)
+                return -1;
+            if (a.value > 0)
+                return 1;
+            return 0;
+        }
+
+        /// <summary>
+        /// Returns the magnitude of the angle
+        /// </summary>
+        /// <param name="a">The angle</param>
+        /// <returns>The positive magnitude of the angle</returns>
+        /// Negative values are negated to get a positive result
+        public static AngleFloat Abs(AngleFloat a) {
+            if (Sign(a) < 0)
+                return -a;
+            else
+                return a;
+        }
+
+        /// <summary>
+        /// Tests the equality of two angles
+        /// </summary>
+        /// <param name="a1"></param>
+        /// <param name="a2"></param>
+        /// <returns>True when the angles are equal, false otherwise</returns>
+        /// <remarks>The equality is determine within the limits of precision of a float</remarks>
+        public static bool operator ==(AngleFloat a1, AngleFloat a2) {
+            return a1.value == a2.value;
+        }
+
+        /// <summary>
+        /// Tests the inequality of two angles
+        /// </summary>
+        /// <param name="a1"></param>
+        /// <param name="a2"></param>
+        /// <returns>True when the angles are not equal, false otherwise</returns>
+        /// <remarks>The equality is determine within the limits of precision of a float</remarks>
+        public static bool operator !=(AngleFloat a1, AngleFloat a2) {
+            return a1.value != a2.value;
+        }
+
+        /// <summary>
+        /// Tests if the first angle is greater than the second
+        /// </summary>
+        /// <param name="a1"></param>
+        /// <param name="a2"></param>
+        /// <returns>True when a1 is greater than a2, False otherwise</returns>
+        public static bool operator >(AngleFloat a1, AngleFloat a2) {
+            return a1.value > a2.value;
+        }
+
+        /// <summary>
+        /// Tests if the first angle is greater than or equal to the second
+        /// </summary>
+        /// <param name="a1"></param>
+        /// <param name="a2"></param>
+        /// <returns>True when a1 is greater than or equal to a2, False otherwise</returns>
+        public static bool operator >=(AngleFloat a1, AngleFloat a2) {
+            return a1.value >= a2.value;
+        }
+
+        /// <summary>
+        /// Tests if the first angle is less than the second
+        /// </summary>
+        /// <param name="a1"></param>
+        /// <param name="a2"></param>
+        /// <returns>True when a1 is less than a2, False otherwise</returns>
+        public static bool operator <(AngleFloat a1, AngleFloat a2) {
+            return a1.value < a2.value;
+        }
+
+        /// <summary>
+        /// Tests if the first angle is less than or equal to the second
+        /// </summary>
+        /// <param name="a1"></param>
+        /// <param name="a2"></param>
+        /// <returns>True when a1 is less than or equal to a2, False otherwise</returns>
+        public static bool operator <=(AngleFloat a1, AngleFloat a2) {
+            return a1.value <= a2.value;
+        }
+
+        /// <summary>
+        /// Negate the angle
+        /// </summary>
+        /// <param name="a">The angle</param>
+        /// <returns>The negated angle</returns>
+        /// The negation of -180 is still -180 because the range is (-180..180]
+        public static AngleFloat operator -(AngleFloat a) {
+            AngleFloat r = new(-a.value);
+            return r;
+        }
+
+        /// <summary>
+        /// Subtract two angles
+        /// </summary>
+        /// <param name="a1">Angle 1</param>
+        /// <param name="a2">Angle 2</param>
+        /// <returns>The result of the subtraction</returns>
+        public static AngleFloat operator -(AngleFloat a1, AngleFloat a2) {
+            AngleFloat r = new(a1.value - a2.value);
+            return r;
+        }
+        /// <summary>
+        /// Add two angles
+        /// </summary>
+        /// <param name="a1">Angle 1</param>
+        /// <param name="a2">Angle 2</param>
+        /// <returns>The result of the addition</returns>
+        public static AngleFloat operator +(AngleFloat a1, AngleFloat a2) {
+            AngleFloat r = new(a1.value + a2.value);
+            return r;
+        }
+
+        /// <summary>
+        /// Multiplies the angle
+        /// </summary>
+        /// <param name="a">The angle to multiply</param>
+        /// <param name="factor">The factor by which the angle is multiplied</param>
+        /// <returns>The multiplied angle</returns>
+        public static AngleFloat operator *(AngleFloat a, float factor) {
+            return Degrees(a.inDegrees * factor);
+        }
+        public static AngleFloat operator *(float factor, AngleFloat a) {
+            return Degrees(factor * a.inDegrees);
+        }
+
+        /// <summary>
+        /// Clamp the angle between the given min and max values
+        /// </summary>
+        /// <param name="angle">The angle to clamp</param>
+        /// <param name="min">The minimum angle</param>
+        /// <param name="max">The maximum angle</param>
+        /// <returns>The clamped angle</returns>
+        /// Angles are normalized
+        public static float Clamp(AngleFloat angle, AngleFloat min, AngleFloat max) {
+            return Float.Clamp(angle.inDegrees, min.inDegrees, max.inDegrees);
+        }
+
+        /// @brief Calculates the cosine of an angle
+        /// @param angle The given angle
+        /// @return The cosine of the angle
+        public static float Cos(AngleFloat angle) {
+            return MathF.Cos(angle.inRadians);
+        }
+        /// @brief Calculates the sine of an angle
+        /// @param angle The given angle
+        /// @return The sine of the angle
+        public static float Sin(AngleFloat angle) {
+            return MathF.Sin(angle.inRadians);
+        }
+        /// @brief Calculates the tangent of an angle
+        /// @param angle The given angle
+        /// @return The tangent of the angle
+        public static float Tan(AngleFloat angle) {
+            return MathF.Tan(angle.inRadians);
+        }
+
+        /// @brief Calculates the arc cosine angle
+        /// @param f The value
+        /// @return The arc cosine for the given value
+        public static AngleFloat Acos(float f) {
+            return Radians(MathF.Acos(f));
+        }
+        /// @brief Calculates the arc sine angle
+        /// @param f The value
+        /// @return The arc sine for the given value
+        public static AngleFloat Asin(float f) {
+            return Radians(MathF.Asin(f));
+        }
+        /// @brief Calculates the arc tangent angle
+        /// @param f The value
+        /// @return The arc tangent for the given value
+        public static AngleFloat Atan(float f) {
+            return Radians(MathF.Atan(f));
+        }
+        /// @brief Calculates the tangent for the given values
+        /// @param y The vertical value
+        /// @param x The horizontal value
+        /// @return The tanget for the given values
+        /// Uses the y and x signs to compute the quadrant
+        public static AngleFloat Atan2(float y, float x) {
+            return Radians(MathF.Atan2(y, x));
+        }
+
+        /// <summary>
+        /// Rotate from one angle to the other with a maximum degrees
+        /// </summary>
+        /// <param name="fromAngle">Starting angle</param>
+        /// <param name="toAngle">Target angle</param>
+        /// <param name="maxAngle">Maximum angle to rotate</param>
+        /// <returns>The resulting angle</returns>
+        /// This function is compatible with radian and degrees angles
+        public static AngleFloat MoveTowards(AngleFloat fromAngle, AngleFloat toAngle, float maxDegrees) {
+            maxDegrees = Math.Max(0, maxDegrees);  // filter out negative distances
+            AngleFloat d = toAngle - fromAngle;
+            float dDegrees = Abs(d).inDegrees;
+            d = Degrees(Float.Clamp(dDegrees, 0, maxDegrees));
+            if (Sign(d) < 0)
+                d = -d;
+            return fromAngle + d;
+        }
+    }
+
+
+    /// <summary>
+    /// %Angle utilities
+    /// </summary>
+    public static class Angles {
+        public const float pi = 3.1415927410125732421875F;
+        // public static float Rad2Deg = 360.0f / ((float)Math.PI * 2);
+        // public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f;
+
+
+        /// <summary>
+        /// Determine the angle difference, result is a normalized angle
+        /// </summary>
+        /// <param name="a">First first angle</param>
+        /// <param name="b">The second angle</param>
+        /// <returns>the angle between the two angles</returns>
+        /// Angle values should be degrees
+        public static float Difference(float a, float b) {
+            float r = Normalize(b - a);
+            return r;
+        }
+
+        /// <summary>
+        /// Normalize an angle to the range -180 < angle <= 180
+        /// </summary>
+        /// <param name="angle">The angle to normalize</param>
+        /// <returns>The normalized angle in interval (-180..180] </returns>
+        /// Angle values should be in degrees
+        public static float Normalize(float angle) {
+            if (float.IsInfinity(angle))
+                return angle;
+
+            while (angle <= -180) angle += 360;
+            while (angle > 180) angle -= 360;
+            return angle;
+        }
+
+        /// <summary>
+        /// Map interval of angles between vectors [0..Pi] to interval [0..1]
+        /// </summary>
+        /// <param name="v1">The first vector</param>
+        /// <param name="v2">The second vector</param>
+        /// <returns>The resulting factor in interval [0..1]</returns>
+        /// Vectors a and b must be normalized
+        /// \deprecated Please use Vector2.ToFactor instead.
+        // [Obsolete("Please use Vector2.ToFactor instead.")]
+        // public static float ToFactor(Vector2Float v1, Vector2Float v2) {
+        //     return (1 - Vector2Float.Dot(v1, v2)) / 2;
+        // }
+
+    }
+
+}

src/Decomposition.cs

@@ -0,0 +1,287 @@
+using System;
+namespace LinearAlgebra {
+    class QR {
+        // QR Decomposition of a matrix A
+        public static (Matrix2 Q, Matrix2 R) Decomposition(Matrix2 A) {
+            int nRows = A.nRows;
+            int nCols = A.nCols;
+
+            float[,] Q = new float[nRows, nCols];
+            float[,] R = new float[nCols, nCols];
+
+            // Perform Gram-Schmidt orthogonalization
+            for (uint colIx = 0; colIx < nCols; colIx++) {
+
+                // Step 1: v = column(ix) of A
+                float[] v = new float[nRows];
+                for (int rowIx = 0; rowIx < nRows; rowIx++)
+                    v[rowIx] = A.data[rowIx, colIx];
+
+                // Step 2: Subtract projections of v onto previous q's (orthogonalize)
+                for (uint colIx2 = 0; colIx2 < colIx; colIx2++) {
+                    float dotProd = 0;
+                    for (int i = 0; i < nRows; i++)
+                        dotProd += Q[i, colIx2] * v[i];
+                    for (int i = 0; i < nRows; i++)
+                        v[i] -= dotProd * Q[i, colIx2];
+                }
+
+                // Step 3: Normalize v to get column(ix) of Q
+                float norm = 0;
+                for (int rowIx = 0; rowIx < nRows; rowIx++)
+                    norm += v[rowIx] * v[rowIx];
+                norm = (float)Math.Sqrt(norm);
+
+                for (int rowIx = 0; rowIx < nRows; rowIx++)
+                    Q[rowIx, colIx] = v[rowIx] / norm;
+
+                // Store the coefficients of R
+                for (int colIx2 = 0; colIx2 <= colIx; colIx2++) {
+                    R[colIx2, colIx] = 0;
+                    for (int k = 0; k < nRows; k++)
+                        R[colIx2, colIx] += Q[k, colIx2] * A.data[k, colIx];
+                }
+            }
+            return (new Matrix2(Q), new Matrix2(R));
+        }
+
+        // Reduced QR Decomposition of a matrix A
+        public static (Matrix2 Q, Matrix2 R) ReducedDecomposition(Matrix2 A) {
+            int nRows = A.nRows;
+            int nCols = A.nCols;
+
+            float[,] Q = new float[nRows, nCols];
+            float[,] R = new float[nCols, nCols];
+
+            // Perform Gram-Schmidt orthogonalization
+            for (int colIx = 0; colIx < nCols; colIx++) {
+
+                // Step 1: v = column(colIx) of A
+                float[] columnIx = new float[nRows];
+                bool isZeroColumn = true;
+                for (int rowIx = 0; rowIx < nRows; rowIx++) {
+                    columnIx[rowIx] = A.data[rowIx, colIx];
+                    if (columnIx[rowIx] != 0)
+                        isZeroColumn = false;
+                }
+                if (isZeroColumn) {
+                    for (int rowIx = 0; rowIx < nRows; rowIx++)
+                        Q[rowIx, colIx] = 0;
+                    // Set corresponding R element to 0
+                    R[colIx, colIx] = 0;
+
+                    Console.WriteLine($"zero column {colIx}");
+
+                    continue;
+                }
+
+                // Step 2: Subtract projections of v onto previous q's (orthogonalize)
+                for (int colIx2 = 0; colIx2 < colIx; colIx2++) {
+                    // Compute the dot product of v and column(colIx2) of Q
+                    float dotProduct = 0;
+                    for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
+                        dotProduct += columnIx[rowIx2] * Q[rowIx2, colIx2];
+                    // Subtract the projection from v
+                    for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
+                        columnIx[rowIx2] -= dotProduct * Q[rowIx2, colIx2];
+                }
+
+                // Step 3: Normalize v to get column(colIx) of Q
+                float norm = 0;
+                for (int rowIx = 0; rowIx < nRows; rowIx++)
+                    norm += columnIx[rowIx] * columnIx[rowIx];
+                if (norm == 0)
+                    throw new Exception("invalid value");
+
+                norm = (float)Math.Sqrt(norm);
+
+                for (int rowIx = 0; rowIx < nRows; rowIx++)
+                    Q[rowIx, colIx] = columnIx[rowIx] / norm;
+
+                // Here is where it deviates from the Full QR Decomposition !
+
+                // Step 4: Compute the row(colIx) of R
+                for (int colIx2 = colIx; colIx2 < nCols; colIx2++) {
+                    float dotProduct = 0;
+                    for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
+                        dotProduct += Q[rowIx2, colIx] * A.data[rowIx2, colIx2];
+                    R[colIx, colIx2] = dotProduct;
+                }
+            }
+            if (!float.IsFinite(R[0, 0]))
+                throw new Exception("invalid value");
+
+            return (new Matrix2(Q), new Matrix2(R));
+        }
+    }
+
+    class SVD {
+        // According to ChatGPT, Mathnet uses  Golub-Reinsch SVD algorithm
+        // 1. Bidiagonalization: The input matrix AA is reduced to a bidiagonal form using Golub-Kahan bidiagonalization.
+        // This process involves applying a sequence of Householder reflections to AA to create a bidiagonal matrix.
+        // This step reduces the complexity by making the matrix simpler while retaining the essential structure needed for SVD.
+        //
+        // 2. Diagonalization: Once the matrix is in bidiagonal form,
+        // the singular values are computed using an iterative process
+        // (typically involving QR factorization or Jacobi rotations) until convergence.
+        // This process diagonalizes the bidiagonal matrix and allows extraction of the singular values.
+        //
+        // 3. Computing UU and VTVT: After obtaining the singular values,
+        // the left singular vectors UU and right singular vectors VTVT are computed
+        // using the accumulated transformations (such as Householder reflections) from the bidiagonalization step.
+
+        // Bidiagnolizations through Householder transformations
+        public static (Matrix2 U1, Matrix2 B, Matrix2 V1) Bidiagonalization(Matrix2 A) {
+            int m = A.nRows; // Rows of A
+            int n = A.nCols; // Columns of A
+            float[,] U1 = new float[m, m]; // Left orthogonal matrix
+            float[,] V1 = new float[n, n]; // Right orthogonal matrix
+            float[,] B = A.Clone().data; // Copy A to B for transformation
+
+            // Initialize U1 and V1 as identity matrices
+            for (int i = 0; i < m; i++)
+                U1[i, i] = 1;
+            for (int i = 0; i < n; i++)
+                V1[i, i] = 1;
+
+            // Perform Householder reflections to create a bidiagonal matrix B
+            for (int j = 0; j < n; j++) {
+                // Step 1: Construct the Householder vector y
+                float[] y = new float[m - j];
+                for (int i = j; i < m; i++)
+                    y[i - j] = B[i, j];
+
+                // Step 2: Compute the norm and scalar alpha
+                float norm = 0;
+                for (int i = 0; i < y.Length; i++)
+                    norm += y[i] * y[i];
+                norm = (float)Math.Sqrt(norm);
+
+                if (B[j, j] > 0)
+                    norm = -norm;
+
+                float alpha = (float)Math.Sqrt(0.5 * (norm * (norm - B[j, j])));
+                float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j])));
+
+                // Step 3: Apply the reflection to zero out below diagonal
+                for (int k = j; k < n; k++) {
+                    float dot = 0;
+                    for (int i = j; i < m; i++)
+                        dot += y[i - j] * B[i, k];
+                    dot /= r;
+
+                    for (int i = j; i < m; i++)
+                        B[i, k] -= 2 * dot * y[i - j];
+                }
+
+                // Step 4: Update U1 with the Householder reflection (U1 * Householder)
+                for (int i = j; i < m; i++)
+                    U1[i, j] = y[i - j] / alpha;
+
+                // Step 5: Update V1 (storing the Householder vector y)
+                // Correct indexing: we only need to store part of y in V1 from index j to n
+                for (int i = j; i < n; i++)
+                    V1[j, i] = B[j, i];
+
+                // Repeat steps for further columns if necessary
+            }
+            return (new Matrix2(U1), new Matrix2(B), new Matrix2(V1));
+        }
+
+        public static Matrix2 Bidiagonalize(Matrix2 A) {
+            int m = A.nRows; // Rows of A
+            int n = A.nCols; // Columns of A
+            float[,] B = A.Clone().data; // Copy A to B for transformation
+
+            // Perform Householder reflections to create a bidiagonal matrix B
+            for (int j = 0; j < n; j++) {
+                // Step 1: Construct the Householder vector y
+                float[] y = new float[m - j];
+                for (int i = j; i < m; i++)
+                    y[i - j] = B[i, j];
+
+                // Step 2: Compute the norm and scalar alpha
+                float norm = 0;
+                for (int i = 0; i < y.Length; i++)
+                    norm += y[i] * y[i];
+                norm = (float)Math.Sqrt(norm);
+
+                if (B[j, j] > 0)
+                    norm = -norm;
+
+                float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j])));
+
+                // Step 3: Apply the reflection to zero out below diagonal
+                for (int k = j; k < n; k++) {
+                    float dot = 0;
+                    for (int i = j; i < m; i++)
+                        dot += y[i - j] * B[i, k];
+                    dot /= r;
+
+                    for (int i = j; i < m; i++)
+                        B[i, k] -= 2 * dot * y[i - j];
+                }
+
+                // Repeat steps for further columns if necessary
+            }
+            return new Matrix2(B);
+        }
+
+        // QR Iteration for diagonalization of a bidiagonal matrix B
+        public static (Matrix1 singularValues, Matrix2 U, Matrix2 Vt) QRIteration(Matrix2 B) {
+            int m = B.nRows;
+            int n = B.nCols;
+
+            Matrix2 U = new(m, m); // Left singular vectors (U)
+            Matrix2 Vt = new(n, n); // Right singular vectors (V^T)
+            float[] singularValues = new float[Math.Min(m, n)]; // Singular values
+
+            // Initialize U and Vt as identity matrices
+            for (int i = 0; i < m; i++)
+                U.data[i, i] = 1;
+            for (int i = 0; i < n; i++)
+                Vt.data[i, i] = 1;
+
+            // Perform QR iterations
+            float tolerance = 1e-7f; //1e-12f; for double
+            bool converged = false;
+            while (!converged) {
+                // Perform QR decomposition on the matrix B
+                (Matrix2 Q, Matrix2 R) = QR.Decomposition(B);
+
+                // Update B to be the product Q * R //R * Q
+                B = R * Q;
+
+                // Accumulate the transformations in U and Vt
+                U *= Q;
+                Vt *= R;
+
+                // Check convergence by looking at the off-diagonal elements of B
+                converged = true;
+                for (int i = 0; i < m - 1; i++) {
+                    for (int j = i + 1; j < n; j++) {
+                        if (Math.Abs(B.data[i, j]) > tolerance) {
+                            converged = false;
+                            break;
+                        }
+                    }
+                }
+            }
+
+            // Extract singular values (diagonal elements of B)
+            for (int i = 0; i < Math.Min(m, n); i++)
+                singularValues[i] = B.data[i, i];
+
+            return (new Matrix1(singularValues), U, Vt);
+        }
+
+        public static (Matrix2 U, Matrix1 S, Matrix2 Vt) Decomposition(Matrix2 A) {
+            if (A.nRows != A.nCols)
+                throw new ArgumentException("SVD: matrix A has to be square.");
+
+            Matrix2 B = Bidiagonalize(A);
+            (Matrix1 S, Matrix2 U, Matrix2 Vt) = QRIteration(B);
+            return (U, S, Vt);
+        }
+    }
+}

src/Direction.cs

@@ -0,0 +1,181 @@
+using System;
+#if UNITY_5_3_OR_NEWER
+using Vector3Float = UnityEngine.Vector3;
+#endif
+
+namespace LinearAlgebra {
+
+    /// <summary>
+    /// A direction in 3D space
+    /// </summary>
+    /// A direction is represented using two angles:
+    /// * The horizontal angle ranging from -180 (inclusive) to 180 (exclusive)
+    /// degrees which is a rotation in the horizontal plane
+    /// * A vertical angle ranging from -90 (inclusive) to 90 (exclusive) degrees
+    /// which is the rotation in the up/down direction applied after the horizontal
+    /// rotation has been applied.
+    /// The angles are automatically normalized to stay within the abovenmentioned
+    /// ranges.
+    public struct Direction {
+        /// @brief horizontal angle, range = (-180..180] degrees
+        public AngleFloat horizontal;
+        /// @brief vertical angle, range in degrees = (-90..90] degrees
+        public AngleFloat vertical;
+
+        /// <summary>
+        /// Create a new direction
+        /// </summary>
+        /// <param name="horizontal">The horizontal angle</param>
+        /// <param name="vertical">The vertical angle</param>
+        /// <remarks>The direction will be normalized automatically
+        /// to ensure the angles are within the allowed ranges</remarks>
+        public Direction(AngleFloat horizontal, AngleFloat vertical) {
+            this.horizontal = horizontal;
+            this.vertical = vertical;
+            this.Normalize();
+        }
+
+        /// <summary>
+        /// Create a direction using angle values in degrees
+        /// </summary>
+        /// <param name="horizontal">The horizontal angle in degrees</param>
+        /// <param name="vertical">The vertical angle in degrees</param>
+        /// <returns>The direction</returns>
+        /// <remarks>The direction will be normalized automatically
+        /// to ensure the angles are within the allowed ranges</remarks>
+        public static Direction Degrees(float horizontal, float vertical) {
+            Direction d = new() {
+                horizontal = AngleFloat.Degrees(horizontal),
+                vertical = AngleFloat.Degrees(vertical)
+            };
+            d.Normalize();
+            return d;
+        }
+        /// <summary>
+        /// Create a direction using angle values in radians
+        /// </summary>
+        /// <param name="horizontal">The horizontal angle in radians</param>
+        /// <param name="vertical">The vertical angle in radians</param>
+        /// <returns>The direction</returns>
+        public static Direction Radians(float horizontal, float vertical) {
+            Direction d = new() {
+                horizontal = AngleFloat.Radians(horizontal),
+                vertical = AngleFloat.Radians(vertical)
+            };
+            d.Normalize();
+            return d;
+        }
+
+        /// <summary>
+        /// A forward direction with zero for both angles
+        /// </summary>
+        public readonly static Direction forward = Degrees(0, 0);
+        /// <summary>
+        /// A backward direction with horizontal angle -180 and zero vertical
+        /// angle
+        /// </summary>
+        public readonly static Direction backward = Degrees(-180, 0);
+        /// <summary>
+        /// A upward direction with zero horizontal angle and vertical angle 90
+        /// </summary>
+        public readonly static Direction up = Degrees(0, 90);
+        /// <summary>
+        /// A downward direction with zero horizontal angle and vertical angle
+        /// -90
+        /// </summary>
+        public readonly static Direction down = Degrees(0, -90);
+        /// <summary>
+        /// A left-pointing direction with horizontal angle -90 and zero
+        /// vertical angle
+        /// </summary>
+        public readonly static Direction left = Degrees(-90, 0);
+        /// <summary>
+        /// A right-pointing direction with horizontal angle 90 and zero
+        /// vertical angle
+        /// </summary>
+        public readonly static Direction right = Degrees(90, 0);
+
+        private void Normalize() {
+            if (this.vertical > AngleFloat.deg90 || this.vertical < -AngleFloat.deg90) {
+                this.horizontal += AngleFloat.deg180;
+                this.vertical = AngleFloat.deg180 - this.vertical;
+            }
+        }
+
+#if !UNITY_5_3_OR_NEWER
+        /// <summary>
+        /// Convert the direction into a carthesian vector
+        /// </summary>
+        /// <returns>The carthesian vector corresponding to this direction.</returns>
+        public Vector3Float ToVector3Float() {
+            Quaternion q = Quaternion.Euler(90 - this.vertical.inDegrees, this.horizontal.inDegrees, 0);
+            Vector3Float v = q * Vector3Float.forward;
+            return v;
+        }
+#else        
+        /// <summary>
+        /// Convert the direction into a carthesian vector
+        /// </summary>
+        /// <returns>The carthesian vector corresponding to this direction.</returns>
+        public UnityEngine.Vector3 ToVector3() {
+            UnityEngine.Quaternion q = UnityEngine.Quaternion.Euler(90 - this.vertical.inDegrees, this.horizontal.inDegrees, 0);
+            UnityEngine.Vector3 v = q * UnityEngine.Vector3.forward;
+            return v;
+        }
+#endif        
+
+        /// <summary>
+        /// Convert a carthesian vector into a direction
+        /// </summary>
+        /// <param name="v">The carthesian vector</param>
+        /// <returns>The direction</returns>
+        /// <remarks>Information about the length of the carthesian vector is not
+        /// included in this transformation</remarks>
+        public static Direction FromVector3(Vector3Float v) {
+            AngleFloat horizontal = AngleFloat.Atan2(v.horizontal, v.depth);
+            AngleFloat vertical = AngleFloat.deg90 - AngleFloat.Acos(v.vertical);
+            Direction d = new(horizontal, vertical);
+            return d;
+        }
+
+        /// <summary>
+        /// Tests the equality of two directions
+        /// </summary>
+        /// <param name="d1"></param>
+        /// <param name="d2"></param>
+        /// <returns>True when the direction angles are equal, false otherwise.</returns>
+        public static bool operator ==(Direction d1, Direction d2) {
+            bool horizontalEq = d1.horizontal == d2.horizontal;
+            bool verticalEq = d1.vertical == d2.vertical;
+            return horizontalEq && verticalEq;
+        }
+
+        /// <summary>
+        /// Tests the inequality of two directions
+        /// </summary>
+        /// <param name="d1"></param>
+        /// <param name="d2"></param>
+        /// <returns>True when the direction angles are not equal, false otherwise.</returns>
+        public static bool operator !=(Direction d1, Direction d2) {
+            bool horizontalNEq = d1.horizontal != d2.horizontal;
+            bool verticalNEq = d1.vertical != d2.vertical;
+            return horizontalNEq || verticalNEq;
+        }
+
+        public override readonly bool Equals(object obj) {
+            if (obj is not Direction d)
+                return false;
+
+            bool horizontalEq = this.horizontal == d.horizontal;
+            bool verticalEq = this.vertical == d.vertical;
+            return horizontalEq && verticalEq;
+        }
+
+
+        public override readonly int GetHashCode() {
+            return HashCode.Combine(horizontal, vertical);
+        }
+
+    }
+
+}

src/Float.cs

@@ -0,0 +1,41 @@
+namespace LinearAlgebra {
+
+    /// <summary>
+    /// Float number utilities
+    /// </summary>
+    public class Float {
+        /// <summary>
+        /// The precision of float numbers
+        /// </summary>
+        public const float epsilon = 1E-05f;
+        /// <summary>
+        /// The square of the float number precision
+        /// </summary>
+        public const float sqrEpsilon = 1e-10f;
+
+        /// <summary>
+        /// Clamp the value between the given minimum and maximum values
+        /// </summary>
+        /// <param name="f">The value to clamp</param>
+        /// <param name="min">The minimum value</param>
+        /// <param name="max">The maximum value</param>
+        /// <returns>The clamped value</returns>
+        public static float Clamp(float f, float min, float max) {
+            if (f < min)
+                return min;
+            if (f > max)
+                return max;
+            return f;
+        }
+
+        /// <summary>
+        /// Clamp the value between to the interval [0..1]
+        /// </summary>
+        /// <param name="f">The value to clamp</param>
+        /// <returns>The clamped value</returns>
+        public static float Clamp01(float f) {
+            return Clamp(f, 0, 1);
+        }
+    }
+
+}

src/LinearAlgebra.csproj

@@ -0,0 +1,14 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <GenerateAssemblyInfo>false</GenerateAssemblyInfo>
+    <GenerateTargetFrameworkAttribute>false</GenerateTargetFrameworkAttribute> 
+    <TargetFramework>net8.0</TargetFramework>
+  </PropertyGroup>
+
+  <ItemGroup>
+    <PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.13.0" />
+    <PackageReference Include="NUnit" Version="3.13.2" />
+  </ItemGroup>
+
+</Project>

src/Matrix.cs

@@ -0,0 +1,689 @@
+using System;
+#if UNITY_5_3_OR_NEWER
+using Vector3Float = UnityEngine.Vector3;
+using Vector2Float = UnityEngine.Vector2;
+using Quaternion = UnityEngine.Quaternion;
+#endif
+
+namespace LinearAlgebra {
+
+    public readonly struct Slice
+    {
+        public int start { get; }
+        public int stop { get; }
+        public Slice(int start, int stop)
+        {
+            this.start = start;
+            this.stop = stop;
+        }
+    }
+
+    public class Matrix2 {
+        public float[,] data { get; }
+
+        public int nRows => data.GetLength(0);
+        public int nCols => data.GetLength(1);
+
+        public Matrix2(int nRows, int nCols)
+        {
+            this.data = new float[nRows, nCols];
+        }
+        public Matrix2(float[,] data) {
+            this.data = data;
+        }
+
+        public Matrix2 Clone() {
+            float[,] data = new float[this.nRows, nCols];
+            for (int rowIx = 0; rowIx < this.nRows; rowIx++) {
+                for (int colIx = 0; colIx < this.nCols; colIx++)
+                    data[rowIx, colIx] = this.data[rowIx, colIx];
+            }
+            return new Matrix2(data);
+        }
+
+        public static Matrix2 Zero(int nRows, int nCols)
+        {
+            return new Matrix2(nRows, nCols);
+        }
+
+        public static Matrix2 FromVector3(Vector3Float v) {
+            float[,] result = new float[3, 1];
+            result[0, 0] = v.horizontal;
+            result[1, 0] = v.vertical;
+            result[2, 0] = v.depth;
+            return new Matrix2(result);
+        }
+
+        public static Matrix2 Identity(int size)
+        {
+            return Diagonal(1, size);
+        }
+        public static Matrix2 Identity(int nRows, int nCols)
+        {
+            Matrix2 m = Zero(nRows, nCols);
+            m.FillDiagonal(1);
+            return m;
+        }
+
+        public static Matrix2 Diagonal(Matrix1 v) {
+            float[,] resultData = new float[v.size, v.size];
+            for (int ix = 0; ix < v.size; ix++)
+                resultData[ix, ix] = v.data[ix];
+            return new Matrix2(resultData);
+        }
+        public static Matrix2 Diagonal(float f, int size)
+        {
+            float[,] resultData = new float[size, size];
+            for (int ix = 0; ix < size; ix++)
+                resultData[ix, ix] = f;
+            return new Matrix2(resultData);
+        }
+        public void FillDiagonal(Matrix1 v)
+        {
+            int n = (int)Math.Min(Math.Min(this.nRows, this.nCols), v.size);
+            for (int ix = 0; ix < n; ix++)
+                this.data[ix, ix] = v.data[ix];
+        }
+        public void FillDiagonal(float f)
+        {
+            int n = Math.Min(this.nRows, this.nCols);
+            for (int ix = 0; ix < n; ix++)
+                this.data[ix, ix] = f;
+        }
+
+        public static Matrix2 SkewMatrix(Vector3Float v) {
+            float[,] result = new float[3, 3] {
+            {0, -v.depth, v.vertical},
+            {v.depth, 0, -v.horizontal},
+            {-v.vertical, v.horizontal, 0}
+        };
+            return new Matrix2(result);
+        }
+
+        public Matrix1 GetRow(int rowIx) {
+            float[] row = new float[this.nCols];
+            for (int colIx = 0; colIx < this.nCols; colIx++) {
+                row[colIx] = this.data[rowIx, colIx];
+            }
+            return new Matrix1(row);
+        }
+
+#if UNITY_5_3_OR_NEWER
+    public UnityEngine.Vector3 GetRow3(int rowIx) {
+        int cols = this.nCols;
+        UnityEngine.Vector3 row = new() {
+            x = this.data[rowIx, 0],
+            y = this.data[rowIx, 1],
+            z = this.data[rowIx, 2]
+        };
+        return row;
+    }
+#endif
+        public void SetRow(int rowIx, Matrix1 v) {
+            for (uint ix = 0; ix < v.size; ix++)
+                this.data[rowIx, ix] = v.data[ix];
+        }
+        public void SetRow3(int rowIx, Vector3Float v) {
+            this.data[rowIx, 0] = v.horizontal;
+            this.data[rowIx, 1] = v.vertical;
+            this.data[rowIx, 2] = v.depth;
+        }
+
+        public void SwapRows(int row1, int row2) {
+            for (uint ix = 0; ix < this.nCols; ix++) {
+                float temp = this.data[row1, ix];
+                this.data[row1, ix] = this.data[row2, ix];
+                this.data[row2, ix] = temp;
+            }
+        }
+
+        public Matrix1 GetColumn(int colIx)
+        {
+            float[] column = new float[this.nRows];
+            for (int i = 0; i < this.nRows; i++) {
+                column[i] = this.data[i, colIx];
+            }
+            return new Matrix1(column);
+        }
+        public void SetColumn(int colIx, Matrix1 v) {
+            for (uint ix = 0; ix < v.size; ix++)
+                this.data[ix, colIx] = v.data[ix];
+        }
+        public void SetColumn(int colIx, Vector3Float v) {
+            this.data[0, colIx] = v.horizontal;
+            this.data[1, colIx] = v.vertical;
+            this.data[2, colIx] = v.depth;
+        }
+
+        public static bool AllClose(Matrix2 A, Matrix2 B, float atol = 1e-08f) {
+            for (int i = 0; i < A.nRows; i++) {
+                for (int j = 0; j < A.nCols; j++) {
+                    float d = MathF.Abs(A.data[i, j] - B.data[i, j]);
+                    if (d > atol)
+                        return false;
+                }
+            }
+            return true;
+        }
+
+        public Matrix2 Transpose() {
+            float[,] resultData = new float[this.nCols, this.nRows];
+            for (uint rowIx = 0; rowIx < this.nRows; rowIx++) {
+                for (uint colIx = 0; colIx < this.nCols; colIx++)
+                    resultData[colIx, rowIx] = this.data[rowIx, colIx];
+            }
+            return new Matrix2(resultData);
+            // double checked code
+        }
+        public Matrix2 transposed {
+            get => Transpose();
+        }
+
+        public static Matrix2 operator -(Matrix2 m) {
+            float[,] result = new float[m.nRows, m.nCols];
+
+            for (int i = 0; i < m.nRows; i++) {
+                for (int j = 0; j < m.nCols; j++)
+                    result[i, j] = -m.data[i, j];
+            }
+            return new Matrix2(result);
+        }
+
+        public static Matrix2 operator -(Matrix2 A, Matrix2 B) {
+            if (A.nRows != B.nRows || A.nCols != B.nCols)
+                throw new System.ArgumentException("Size of A must match size of B.");
+
+            float[,] result = new float[A.nRows, B.nCols];
+
+            for (int i = 0; i < A.nRows; i++) {
+                for (int j = 0; j < A.nCols; j++)
+                    result[i, j] = A.data[i, j] - B.data[i, j];
+            }
+            return new Matrix2(result);
+        }
+
+        public static Matrix2 operator +(Matrix2 A, Matrix2 B) {
+            if (A.nRows != B.nRows || A.nCols != B.nCols)
+                throw new System.ArgumentException("Size of A must match size of B.");
+
+            float[,] result = new float[A.nRows, B.nCols];
+
+            for (int i = 0; i < A.nRows; i++) {
+                for (int j = 0; j < A.nCols; j++)
+                    result[i, j] = A.data[i, j] + B.data[i, j];
+            }
+            return new Matrix2(result);
+        }
+
+        public static Matrix2 operator *(Matrix2 A, Matrix2 B) {
+            if (A.nCols != B.nRows)
+                throw new System.ArgumentException("Number of columns in A must match number of rows in B.");
+
+            float[,] result = new float[A.nRows, B.nCols];
+
+            for (int i = 0; i < A.nRows; i++) {
+                for (int j = 0; j < B.nCols; j++) {
+                    float sum = 0.0f;
+                    for (int k = 0; k < A.nCols; k++)
+                        sum += A.data[i, k] * B.data[k, j];
+
+                    result[i, j] = sum;
+                }
+            }
+
+            return new Matrix2(result);
+            // double checked code
+        }
+
+        public static Matrix1 operator *(Matrix2 A, Matrix1 v) {
+            float[] result = new float[A.nRows];
+
+            for (int i = 0; i < A.nRows; i++) {
+                for (int j = 0; j < A.nCols; j++) {
+                    result[i] += A.data[i, j] * v.data[j];
+                }
+            }
+
+            return new Matrix1(result);
+        }
+
+        public static Vector3Float operator *(Matrix2 A, Vector3Float v) {
+            return new Vector3Float(
+                A.data[0, 0] * v.horizontal + A.data[0, 1] * v.vertical + A.data[0, 2] * v.depth,
+                A.data[1, 0] * v.horizontal + A.data[1, 1] * v.vertical + A.data[1, 2] * v.depth,
+                A.data[2, 0] * v.horizontal + A.data[2, 1] * v.vertical + A.data[2, 2] * v.depth
+            );
+        }
+
+        public static Matrix2 operator *(Matrix2 A, float s) {
+            float[,] result = new float[A.nRows, A.nCols];
+
+            for (int i = 0; i < A.nRows; i++) {
+                for (int j = 0; j < A.nCols; j++)
+                    result[i, j] = A.data[i, j] * s;
+            }
+
+            return new Matrix2(result);
+        }
+        public static Matrix2 operator *(float s, Matrix2 A) {
+            return A * s;
+        }
+
+        public static Matrix2 operator /(Matrix2 A, float s) {
+            float[,] result = new float[A.nRows, A.nCols];
+
+            for (int i = 0; i < A.nRows; i++) {
+                for (int j = 0; j < A.nCols; j++)
+                    result[i, j] = A.data[i, j] / s;
+            }
+
+            return new Matrix2(result);
+        }
+        public static Matrix2 operator /(float s, Matrix2 A) {
+            float[,] result = new float[A.nRows, A.nCols];
+
+            for (int i = 0; i < A.nRows; i++) {
+                for (int j = 0; j < A.nCols; j++)
+                    result[i, j] = s / A.data[i, j];
+            }
+
+            return new Matrix2(result);
+        }
+
+        public Matrix2 GetRows(Slice slice) {
+            return GetRows(slice.start, slice.stop);
+        }
+        public Matrix2 GetRows(int from, int to) {
+            if (from < 0 || to >= this.nRows)
+                throw new System.ArgumentException("Slice index out of range.");
+
+            float[,] result = new float[to - from, this.nCols];
+            int resultRowIx = 0;
+            for (int rowIx = from; rowIx < to; rowIx++) {
+                for (int colIx = 0; colIx < this.nCols; colIx++)
+                    result[resultRowIx, colIx] = this.data[rowIx, colIx];
+                
+                resultRowIx++;
+            }
+
+            return new Matrix2(result);
+        }
+
+        public Matrix2 Slice(Slice slice)
+        {
+            return Slice(slice.start, slice.stop);
+        }
+        public Matrix2 Slice(int from, int to)
+        {
+            if (from < 0 || to >= this.nRows)
+                throw new System.ArgumentException("Slice index out of range.");
+
+            float[,] result = new float[to - from, this.nCols];
+            int resultRowIx = 0;
+            for (int rowIx = from; rowIx < to; rowIx++)
+            {
+                for (int colIx = 0; colIx < this.nCols; colIx++)
+                {
+                    result[resultRowIx, colIx] = this.data[rowIx, colIx];
+                }
+                resultRowIx++;
+            }
+
+            return new Matrix2(result);
+        }
+        public Matrix2 Slice(Slice rowRange, Slice colRange) {
+            return Slice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop));
+        }
+        public Matrix2 Slice((int start, int stop) rowRange, (int start, int stop) colRange)
+        {
+            float[,] result = new float[rowRange.stop - rowRange.start, colRange.stop - colRange.start];
+
+            int resultRowIx = 0;
+            int resultColIx = 0;
+            for (int i = rowRange.start; i < rowRange.stop; i++)
+            {
+                for (int j = colRange.start; j < colRange.stop; j++)
+                    result[resultRowIx, resultColIx] = this.data[i, j];
+            }
+            return new Matrix2(result);
+        }
+
+        public void UpdateSlice(Slice slice, Matrix2 m) {
+            UpdateSlice((slice.start, slice.stop), m);
+        }
+        public void UpdateSlice((int start, int stop) slice, Matrix2 m) {
+            // if (slice.start == slice.stop)
+            //     Console.WriteLine("WARNING: no data is updates when start equals stop in a slice!");
+            int mRowIx = 0;
+            for (int rowIx = slice.start; rowIx < slice.stop; rowIx++, mRowIx++) {
+                for (int colIx = 0; colIx < this.nCols; colIx++)
+                    this.data[rowIx, colIx] = m.data[mRowIx, colIx];
+            }
+        }
+
+        public void UpdateSlice(Slice rowRange, Slice colRange, Matrix2 m)
+        {
+            UpdateSlice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop), m);
+        }
+        public void UpdateSlice((int start, int stop) rowRange, (int start, int stop) colRange, Matrix2 m)
+        {
+            for (int i = rowRange.start; i < rowRange.stop; i++)
+            {
+                for (int j = colRange.start; j < colRange.stop; j++)
+                    this.data[i, j] = m.data[i - rowRange.start, j - colRange.start];
+            }
+        }
+
+        public Matrix2 Inverse() {
+            Matrix2 A = this;
+            // unchecked
+            int n = A.nRows;
+
+            // Create an identity matrix of the same size as the original matrix
+            float[,] augmentedMatrix = new float[n, 2 * n];
+            for (int i = 0; i < n; i++) {
+                for (int j = 0; j < n; j++) {
+                    augmentedMatrix[i, j] = A.data[i, j];
+                    augmentedMatrix[i, j + n] = (i == j) ? 1 : 0;  // Identity matrix
+                }
+            }
+
+            // Perform Gaussian elimination
+            for (int i = 0; i < n; i++) {
+                // Find the pivot row
+                float pivot = augmentedMatrix[i, i];
+                if (Math.Abs(pivot) < 1e-10) // Check for singular matrix
+                    throw new InvalidOperationException("Matrix is singular and cannot be inverted.");
+
+                // Normalize the pivot row
+                for (int j = 0; j < 2 * n; j++)
+                    augmentedMatrix[i, j] /= pivot;
+
+                // Eliminate the column below the pivot
+                for (int j = i + 1; j < n; j++) {
+                    float factor = augmentedMatrix[j, i];
+                    for (int k = 0; k < 2 * n; k++)
+                        augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k];
+                }
+            }
+
+            // Back substitution
+            for (int i = n - 1; i >= 0; i--)
+            {
+                // Eliminate the column above the pivot
+                for (int j = i - 1; j >= 0; j--)
+                {
+                    float factor = augmentedMatrix[j, i];
+                    for (int k = 0; k < 2 * n; k++)
+                        augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k];
+                }
+            }
+
+            // Extract the inverse matrix from the augmented matrix
+            float[,] inverse = new float[n, n];
+            for (int i = 0; i < n; i++) {
+                for (int j = 0; j < n; j++)
+                    inverse[i, j] = augmentedMatrix[i, j + n];
+            }
+
+            return new Matrix2(inverse);
+        }
+
+        public float Determinant()
+        {
+            int n = this.nRows;
+            if (n != this.nCols)
+                throw new System.ArgumentException("Matrix must be square.");
+
+            if (n == 1)
+                return this.data[0, 0]; // Base case for 1x1 matrix
+
+            if (n == 2) // Base case for 2x2 matrix
+                return this.data[0, 0] * this.data[1, 1] - this.data[0, 1] * this.data[1, 0];
+
+            float det = 0;
+            for (int col = 0; col < n; col++)
+                det += (col % 2 == 0 ? 1 : -1) * this.data[0, col] * this.Minor(0, col).Determinant();
+
+            return det;
+        }
+
+        // Helper function to compute the minor of a matrix
+        private Matrix2 Minor(int rowToRemove, int colToRemove)
+        {
+            int n = this.nRows;
+            float[,] minor = new float[n - 1, n - 1];
+
+            int r = 0, c = 0;
+            for (int i = 0; i < n; i++) {
+                if (i == rowToRemove) continue;
+
+                c = 0;
+                for (int j = 0; j < n; j++) {
+                    if (j == colToRemove) continue;
+
+                    minor[r, c] = this.data[i, j];
+                    c++;
+                }
+                r++;
+            }
+
+            return new Matrix2(minor);
+        }
+
+        public static Matrix2 DeleteRows(Matrix2 A, Slice rowRange) {
+            float[,] result = new float[A.nRows - (rowRange.stop - rowRange.start), A.nCols];
+
+            int resultRowIx = 0;
+            for (int i = 0; i < A.nRows; i++) {
+                if (i >= rowRange.start && i < rowRange.stop)
+                    continue;
+
+                for (int j = 0; j < A.nCols; j++)
+                    result[resultRowIx, j] = A.data[i, j];
+                    
+                resultRowIx++;
+            }
+            return new Matrix2(result);
+        }
+
+        internal static Matrix2 DeleteColumns(Matrix2 A, Slice colRange) {
+            float[,] result = new float[A.nRows, A.nCols - (colRange.stop - colRange.start)];
+
+            for (int i = 0; i < A.nRows; i++) {
+                int resultColIx = 0;
+                for (int j = 0; j < A.nCols; j++) {
+                    if (j >= colRange.start && j < colRange.stop)
+                        continue;
+
+                    result[i, resultColIx++] = A.data[i, j];
+                }
+            }
+            return new Matrix2(result);
+        }    
+    }
+
+    public class Matrix1
+    {
+        public float[] data { get; }
+
+        public int size => data.GetLength(0);
+
+        public Matrix1(int size)
+        {
+            this.data = new float[size];
+        }
+
+        public Matrix1(float[] data) {
+            this.data = data;
+        }
+
+        public static Matrix1 Zero(int size)
+        {
+            return new Matrix1(size);
+        }
+
+        public static Matrix1 FromVector2(Vector2Float v) {
+            float[] result = new float[2];
+            result[0] = v.horizontal;
+            result[1] = v.vertical;
+            return new Matrix1(result);
+        }
+
+        public static Matrix1 FromVector3(Vector3Float v) {
+            float[] result = new float[3];
+            result[0] = v.horizontal;
+            result[1] = v.vertical;
+            result[2] = v.depth;
+            return new Matrix1(result);
+        }
+
+#if UNITY_5_3_OR_NEWER
+    public static Matrix1 FromQuaternion(Quaternion q) {
+        float[] result = new float[4];
+        result[0] = q.x;
+        result[1] = q.y;
+        result[2] = q.z;
+        result[3] = q.w;
+        return new Matrix1(result);
+    }
+#endif
+
+        public Vector2Float vector2 {
+            get {
+                if (this.size != 2)
+                    throw new System.ArgumentException("Matrix1 must be of size 2");
+                return new Vector2Float(this.data[0], this.data[1]);
+            }
+        }
+        public Vector3Float vector3 {
+            get {
+                if (this.size != 3)
+                    throw new System.ArgumentException("Matrix1 must be of size 3");
+                return new Vector3Float(this.data[0], this.data[1], this.data[2]);
+            }
+        }
+
+#if UNITY_5_3_OR_NEWER
+    public Quaternion quaternion {
+        get {
+            if (this.size != 4)
+                throw new System.ArgumentException("Matrix1 must be of size 4");
+            return new Quaternion(this.data[0], this.data[1], this.data[2], this.data[3]);
+        }
+    }
+#endif
+
+        public Matrix1 Clone() {
+            float[] data = new float[this.size];
+            for (int rowIx = 0; rowIx < this.size; rowIx++)
+                data[rowIx] = this.data[rowIx];
+            return new Matrix1(data);
+        }
+
+
+        public float magnitude {
+            get {
+                float sum = 0;
+                foreach (var elm in data)
+                    sum += elm;
+                return sum / data.Length;
+            }
+        }
+        public static Matrix1 operator +(Matrix1 A, Matrix1 B) {
+            if (A.size != B.size)
+                throw new System.ArgumentException("Size of A must match size of B.");
+
+            float[] result = new float[A.size];
+
+            for (int i = 0; i < A.size; i++) {
+                result[i] = A.data[i] + B.data[i];
+            }
+            return new Matrix1(result);
+        }
+
+        public Matrix2 Transpose() {
+            float[,] r = new float[1, this.size];
+            for (uint colIx = 0; colIx < this.size; colIx++)
+                r[1, colIx] = this.data[colIx];
+
+            return new Matrix2(r);
+        }
+
+        public static float Dot(Matrix1 a, Matrix1 b) {
+            if (a.size != b.size)
+                throw new System.ArgumentException("Vectors must be of the same length.");
+
+            float result = 0.0f;
+            for (int i = 0; i < a.size; i++) {
+                result += a.data[i] * b.data[i];
+            }
+            return result;
+        }
+
+        public static Matrix1 operator -(Matrix1 A, Matrix1 B) {
+            if (A.size != B.size)
+                throw new System.ArgumentException("Size of A must match size of B.");
+
+            float[] result = new float[A.size];
+
+            for (int i = 0; i < A.size; i++) {
+                result[i] = A.data[i] - B.data[i];
+            }
+            return new Matrix1(result);
+        }
+
+        public static Matrix1 operator *(Matrix1 A, float f)
+        {
+            float[] result = new float[A.size];
+
+            for (int i = 0; i < A.size; i++)
+                result[i] += A.data[i] * f;
+
+            return new Matrix1(result);
+        }
+        public static Matrix1 operator *(float f, Matrix1 A) {
+            return A * f;
+        }
+
+        public static Matrix1 operator /(Matrix1 A, float f) {
+            float[] result = new float[A.size];
+
+            for (int i = 0; i < A.size; i++)
+                result[i] = A.data[i] / f;
+
+            return new Matrix1(result);
+        }
+        public static Matrix1 operator /(float f, Matrix1 A) {
+            float[] result = new float[A.size];
+
+            for (int i = 0; i < A.size; i++)
+                result[i] = f / A.data[i];
+
+            return new Matrix1(result);
+        }
+
+        public Matrix1 Slice(Slice range)
+        {
+            return Slice(range.start, range.stop);
+        }
+        public Matrix1 Slice(int from, int to)
+        {
+            if (from < 0 || to >= this.size)
+                throw new System.ArgumentException("Slice index out of range.");
+
+            float[] result = new float[to - from];
+            int resultIx = 0;
+            for (int ix = from; ix < to; ix++)
+                result[resultIx++] = this.data[ix];
+
+            return new Matrix1(result);
+        }
+        public void UpdateSlice(Slice slice, Matrix1 v) {
+            int vIx = 0;
+            for (int ix = slice.start; ix < slice.stop; ix++, vIx++)
+                this.data[ix] = v.data[vIx];
+        }
+    }
+
+}

src/Quat32.cs

@@ -0,0 +1,87 @@
+using System;
+
+namespace LinearAlgebra {
+    public class Quat32 {
+        public float x;
+        public float y;
+        public float z;
+        public float w;
+
+        public Quat32() {
+            this.x = 0;
+            this.y = 0;
+            this.z = 0;
+            this.w = 1;
+        }
+
+        public Quat32(float x, float y, float z, float w) {
+            this.x = x;
+            this.y = y;
+            this.z = z;
+            this.w = w;
+        }
+
+        public static Quat32 FromSwingTwist(SwingTwist s) {
+            Quat32 q32 = Quat32.Euler(-s.swing.vertical.inDegrees, s.swing.horizontal.inDegrees, s.twist.inDegrees);
+            return q32;
+        }
+
+        public static Quat32 Euler(float yaw, float pitch, float roll) {
+            float rollOver2 = roll * AngleFloat.Deg2Rad * 0.5f;
+            float sinRollOver2 = (float)Math.Sin((float)rollOver2);
+            float cosRollOver2 = (float)Math.Cos((float)rollOver2);
+            float pitchOver2 = pitch * 0.5f;
+            float sinPitchOver2 = (float)Math.Sin((float)pitchOver2);
+            float cosPitchOver2 = (float)Math.Cos((float)pitchOver2);
+            float yawOver2 = yaw * 0.5f;
+            float sinYawOver2 = (float)Math.Sin((float)yawOver2);
+            float cosYawOver2 = (float)Math.Cos((float)yawOver2);
+            Quat32 result = new Quat32() {
+                w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
+                       sinYawOver2 * sinPitchOver2 * sinRollOver2,
+                x = sinYawOver2 * cosPitchOver2 * cosRollOver2 +
+                       cosYawOver2 * sinPitchOver2 * sinRollOver2,
+                y = cosYawOver2 * sinPitchOver2 * cosRollOver2 -
+                       sinYawOver2 * cosPitchOver2 * sinRollOver2,
+                z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
+                       sinYawOver2 * sinPitchOver2 * cosRollOver2
+            };
+            return result;
+        }
+
+        public void ToAngles(out float right, out float up, out float forward) {
+            float test = this.x * this.y + this.z * this.w;
+            if (test > 0.499f) {  // singularity at north pole
+                right = 0;
+                up = 2 * (float)Math.Atan2(this.x, this.w) * AngleFloat.Rad2Deg;
+                forward = 90;
+                return;
+                //return Vector3(0, 2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, 90);
+            }
+            else if (test < -0.499f) {  // singularity at south pole
+                right = 0;
+                up = -2 * (float)Math.Atan2(this.x, this.w) * AngleFloat.Rad2Deg;
+                forward = -90;
+                return;
+                //return Vector3(0, -2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, -90);
+            }
+            else {
+                float sqx = this.x * this.x;
+                float sqy = this.y * this.y;
+                float sqz = this.z * this.z;
+
+                right = (float)Math.Atan2(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * AngleFloat.Rad2Deg;
+                up = (float)Math.Atan2(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * AngleFloat.Rad2Deg;
+                forward = (float)Math.Asin(2 * test) * AngleFloat.Rad2Deg;
+                return;
+                // return Vector3(
+                //     atan2f(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) *
+                //         Rad2Deg,
+                //     atan2f(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) *
+                //         Rad2Deg,
+                //     asinf(2 * test) * Angle.Rad2Deg);
+            }
+        }
+
+    }
+}

src/Quaternion.cs

@@ -0,0 +1,608 @@
+using System;
+#if UNITY_5_3_OR_NEWER
+using Quaternion = UnityEngine.Quaternion;
+#endif
+
+namespace LinearAlgebra {
+
+#if UNITY_5_3_OR_NEWER
+    public class QuaternionExtensions {
+        public static Quaternion Reflect(Quaternion q) {
+            return new(-q.x, -q.y, -q.z, q.w);
+        }
+
+        public static Matrix2 ToRotationMatrix(Quaternion q) {
+            float w = q.x, x = q.y, y = q.z, z = q.w;
+
+            float[,] result = new float[,]
+            {
+                { 1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y) },
+                { 2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x) },
+                { 2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y) }
+            };
+            return new Matrix2(result);
+        }
+
+        public static Quaternion FromRotationMatrix(Matrix2 m) {
+            float trace = m.data[0, 0] + m.data[1, 1] + m.data[2, 2];
+            float w, x, y, z;
+
+            if (trace > 0) {
+                float s = 0.5f / (float)Math.Sqrt(trace + 1.0f);
+                w = 0.25f / s;
+                x = (m.data[2, 1] - m.data[1, 2]) * s;
+                y = (m.data[0, 2] - m.data[2, 0]) * s;
+                z = (m.data[1, 0] - m.data[0, 1]) * s;
+            }
+            else {
+                if (m.data[0, 0] > m.data[1, 1] && m.data[0, 0] > m.data[2, 2]) {
+                    float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[0, 0] - m.data[1, 1] - m.data[2, 2]);
+                    w = (m.data[2, 1] - m.data[1, 2]) / s;
+                    x = 0.25f * s;
+                    y = (m.data[0, 1] + m.data[1, 0]) / s;
+                    z = (m.data[0, 2] + m.data[2, 0]) / s;
+                }
+                else if (m.data[1, 1] > m.data[2, 2]) {
+                    float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[1, 1] - m.data[0, 0] - m.data[2, 2]);
+                    w = (m.data[0, 2] - m.data[2, 0]) / s;
+                    x = (m.data[0, 1] + m.data[1, 0]) / s;
+                    y = 0.25f * s;
+                    z = (m.data[1, 2] + m.data[2, 1]) / s;
+                }
+                else {
+                    float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[2, 2] - m.data[0, 0] - m.data[1, 1]);
+                    w = (m.data[1, 0] - m.data[0, 1]) / s;
+                    x = (m.data[0, 2] + m.data[2, 0]) / s;
+                    y = (m.data[1, 2] + m.data[2, 1]) / s;
+                    z = 0.25f * s;
+                }
+            }
+
+            return new Quaternion(x, y, z, w);
+        }
+    }
+#else
+    public struct Quaternion {
+        public float x;
+        public float y;
+        public float z;
+        public float w;
+
+        /// <summary>
+        /// create a new quaternion with the given values
+        /// </summary>
+        /// <param name="x">x component</param>
+        /// <param name="y">y component</param>
+        /// <param name="z">z component</param>
+        /// <param name="w">w component</param>
+        public Quaternion(float x, float y, float z, float w) {
+            this.x = x;
+            this.y = y;
+            this.z = z;
+            this.w = w;
+        }
+
+        /// <summary>
+        /// An identity quaternion
+        /// </summary>
+        public static readonly Quaternion identity = new(0, 0, 0, 1);
+
+        private readonly Vector3Float xyz => new(x, y, z);
+
+        private readonly float magnitude => MathF.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w);
+
+        private readonly float sqrMagnitude => x * x + y * y + z * z + w * w;
+
+        /// <summary>
+        /// Convert to unit quaternion
+        /// </summary>
+        /// This will preserve the orientation,
+        /// but ensures that it is a unit quaternion.
+        public readonly Quaternion normalized {
+            get {
+                float length = this.magnitude;
+                Quaternion q = new(this.x / length, this.y / length, this.z / length, this.w / length);
+                return q;
+            }
+        }
+
+        /// <summary>
+        /// Convert to unity quaternion
+        /// </summary>
+        /// <param name="q">The quaternion to convert</param>
+        /// <returns>A unit quaternion</returns>
+        /// This will preserve the orientation,
+        /// but ensures that it is a unit quaternion.
+        public static Quaternion Normalize(Quaternion q) {
+            return q.normalized;
+        }
+
+        /// <summary>
+        /// Convert to euler angles
+        /// </summary>
+        /// <param name="q">The quaternion to convert</param>
+        /// <returns>A vector containing euler angles</returns>
+        /// The euler angles performed in the order: Z, X, Y
+        public static Vector3Float ToAngles(Quaternion q) {
+            // Extract Euler angles in Unity order (X = pitch, Y = yaw, Z = roll), returned in degrees as (x,pitch),(y,yaw),(z,roll)
+            // Handle singularities/gimbal lock
+            float test = 2f * (q.w * q.x - q.y * q.z);
+            // clamp
+            if (test >= 1f) test = 1f;
+            if (test <= -1f) test = -1f;
+
+            float pitch = MathF.Asin(test); // X
+
+            float roll = MathF.Atan2(2f * (q.w * q.z + q.x * q.y),
+                                     1f - 2f * (q.x * q.x + q.z * q.z)); // Z
+
+            float yaw = MathF.Atan2(2f * (q.w * q.y + q.x * q.z),
+                                    1f - 2f * (q.y * q.y + q.x * q.x)); // Y
+
+            const float rad2deg = 180f / MathF.PI;
+            return new Vector3Float(pitch * rad2deg, yaw * rad2deg, roll * rad2deg);
+            // float test = q.x * q.y + q.z * q.w;
+            // if (test > 0.499f)  // singularity at north pole
+            //     return new Vector3Float(0, 2 * MathF.Atan2(q.x, q.w) * AngleFloat.Rad2Deg, 90);
+
+            // else if (test < -0.499f)  // singularity at south pole
+            //     return new Vector3Float(0, -2 * MathF.Atan2(q.x, q.w) * AngleFloat.Rad2Deg, -90);
+
+            // else {
+            //     float sqx = q.x * q.x;
+            //     float sqy = q.y * q.y;
+            //     float sqz = q.z * q.z;
+
+            //     return new Vector3Float(
+            //         MathF.Atan2(2 * q.x * q.w - 2 * q.y * q.z, 1 - 2 * sqx - 2 * sqz) *
+            //             AngleFloat.Rad2Deg,
+            //         MathF.Atan2(2 * q.y * q.w - 2 * q.x * q.z, 1 - 2 * sqy - 2 * sqz) *
+            //             AngleFloat.Rad2Deg,
+            //         MathF.Asin(2 * test) * AngleFloat.Rad2Deg);
+            // }
+        }
+
+        /// <summary>
+        /// Create a rotation from euler angles
+        /// </summary>
+        /// <param name="x">The angle around the right axis</param>
+        /// <param name="y">The angle around the upward axis</param>
+        /// <param name="z">The angle around the forward axis</param>
+        /// <returns>The resulting quaternion</returns>
+        /// Rotation are appied in the order Z, X, Y.
+        public static Quaternion Euler(float x, float y, float z) {
+            return Quaternion.Euler(new Vector3Float(x, y, z));
+        }
+        /// <summary>
+        /// Create a rotation from a vector containing euler angles
+        /// </summary>
+        /// <param name="eulerAngles">Vector with the euler angles</param>
+        /// <returns>The resulting quaternion</returns>
+        /// Rotation are appied in the order Z, X, Y.
+        public static Quaternion Euler(Vector3Float angles) {
+            //     return Quaternion.FromEulerRad(angles * AngleFloat.Deg2Rad);
+            // }
+            // private static Quaternion FromEulerRad(Vector3Float euler) {
+            Vector3Float euler = angles * AngleFloat.Deg2Rad;
+            // euler.x = pitch, euler.y = yaw, euler.z = roll (radians)
+            float cx = MathF.Cos(euler.horizontal * 0.5f);
+            float sx = MathF.Sin(euler.horizontal * 0.5f);
+            float cy = MathF.Cos(euler.vertical * 0.5f);
+            float sy = MathF.Sin(euler.vertical * 0.5f);
+            float cz = MathF.Cos(euler.depth * 0.5f);
+            float sz = MathF.Sin(euler.depth * 0.5f);
+
+            // Unity uses intrinsic Z, then X, then Y -> q = Qy * Qx * Qz
+            Quaternion q;
+            q.w = cy * cx * cz + sy * sx * sz;
+            q.x = cy * sx * cz + sy * cx * sz;
+            q.y = sy * cx * cz - cy * sx * sz;
+            q.z = cy * cx * sz - sy * sx * cz;
+            return q;
+            // float yaw = euler.horizontal;
+            // float pitch = euler.vertical;
+            // float roll = euler.depth;
+            // float rollOver2 = roll * 0.5f;
+            // float sinRollOver2 = MathF.Sin(rollOver2);
+            // float cosRollOver2 = MathF.Cos(rollOver2);
+            // float pitchOver2 = pitch * 0.5f;
+            // float sinPitchOver2 = MathF.Sin(pitchOver2);
+            // float cosPitchOver2 = MathF.Cos(pitchOver2);
+            // float yawOver2 = yaw * 0.5f;
+            // float sinYawOver2 = MathF.Sin(yawOver2);
+            // float cosYawOver2 = MathF.Cos(yawOver2);
+            // Quaternion result;
+            // result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
+            //            sinYawOver2 * sinPitchOver2 * sinRollOver2;
+            // result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 +
+            //            cosYawOver2 * sinPitchOver2 * sinRollOver2;
+            // result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 -
+            //            sinYawOver2 * cosPitchOver2 * sinRollOver2;
+            // result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
+            //            sinYawOver2 * sinPitchOver2 * cosRollOver2;
+            // return result;
+        }
+
+        /// <summary>
+        /// Multiply two quaternions
+        /// </summary>
+        /// <param name="q1"></param>
+        /// <param name="q2"></param>
+        /// <returns>The resulting rotation</returns>
+        public static Quaternion operator *(Quaternion q1, Quaternion q2) {
+            return new Quaternion(
+                q1.x * q2.w + q1.y * q2.z - q1.z * q2.y + q1.w * q2.x,
+                -q1.x * q2.z + q1.y * q2.w + q1.z * q2.x + q1.w * q2.y,
+                q1.x * q2.y - q1.y * q2.x + q1.z * q2.w + q1.w * q2.z,
+                -q1.x * q2.x - q1.y * q2.y - q1.z * q2.z + q1.w * q2.w);
+        }
+
+        /// <summary>
+        /// Rotate a vector using this quaterion
+        /// </summary>
+        /// <param name="q">The rotation</param>
+        /// <param name="v">The vector to rotate</param>
+        /// <returns>The rotated vector</returns>
+        public static Vector3Float operator *(Quaternion q, Vector3Float v) {
+            float num = q.x * 2;
+            float num2 = q.y * 2;
+            float num3 = q.z * 2;
+            float num4 = q.x * num;
+            float num5 = q.y * num2;
+            float num6 = q.z * num3;
+            float num7 = q.x * num2;
+            float num8 = q.x * num3;
+            float num9 = q.y * num3;
+            float num10 = q.w * num;
+            float num11 = q.w * num2;
+            float num12 = q.w * num3;
+
+            float px = v.horizontal;
+            float py = v.vertical;
+            float pz = v.depth;
+            float rx =
+                (1 - (num5 + num6)) * px + (num7 - num12) * py + (num8 + num11) * pz;
+            float ry =
+                (num7 + num12) * px + (1 - (num4 + num6)) * py + (num9 - num10) * pz;
+            float rz =
+                (num8 - num11) * px + (num9 + num10) * py + (1 - (num4 + num5)) * pz;
+            Vector3Float result = new(rx, ry, rz);
+            return result;
+        }
+
+        /// <summary>
+        /// The inverse of quaterion
+        /// </summary>
+        /// <param name="quaternion">The quaternion for which the inverse is
+        /// needed</param> <returns>The inverted quaternion</returns>
+        public static Quaternion Inverse(Quaternion q) {
+            float n = MathF.Sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
+            return new Quaternion(-q.x / n, -q.y / n, -q.z / n, q.w / n);
+        }
+
+        /// <summary>
+        /// A rotation which looks in the given direction
+        /// </summary>
+        /// <param name="forward">The look direction</param>
+        /// <param name="upwards">The up direction</param>
+        /// <returns>The look rotation</returns>
+        public static Quaternion LookRotation(Vector3Float forward, Vector3Float up) {
+            Vector3Float nForward = forward.normalized;
+            Vector3Float nRight = Vector3Float.Normalize(Vector3Float.Cross(up, nForward));
+            Vector3Float nUp = Vector3Float.Cross(nForward, nRight);
+            float m00 = nRight.horizontal;      // x;
+            float m01 = nRight.vertical;         // y;
+            float m02 = nRight.depth;    // z;
+            float m10 = nUp.horizontal;         // x;
+            float m11 = nUp.vertical;            // y;
+            float m12 = nUp.depth;       // z;
+            float m20 = nForward.horizontal;    // x;
+            float m21 = nForward.vertical;       // y;
+            float m22 = nForward.depth;  // z;
+
+            float num8 = (m00 + m11) + m22;
+            float x, y, z, w;
+            if (num8 > 0) {
+                float num = MathF.Sqrt(num8 + 1);
+                w = num * 0.5f;
+                num = 0.5f / num;
+                x = (m12 - m21) * num;
+                y = (m20 - m02) * num;
+                z = (m01 - m10) * num;
+                return new Quaternion(x, y, z, w);
+            }
+            if ((m00 >= m11) && (m00 >= m22)) {
+                float num7 = MathF.Sqrt(((1 + m00) - m11) - m22);
+                float num4 = 0.5F / num7;
+                x = 0.5f * num7;
+                y = (m01 + m10) * num4;
+                z = (m02 + m20) * num4;
+                w = (m12 - m21) * num4;
+                return new Quaternion(x, y, z, w);
+            }
+            if (m11 > m22) {
+                float num6 = MathF.Sqrt(((1 + m11) - m00) - m22);
+                float num3 = 0.5F / num6;
+                x = (m10 + m01) * num3;
+                y = 0.5F * num6;
+                z = (m21 + m12) * num3;
+                w = (m20 - m02) * num3;
+                return new Quaternion(x, y, z, w);
+            }
+            float num5 = MathF.Sqrt(((1 + m22) - m00) - m11);
+            float num2 = 0.5F / num5;
+            x = (m20 + m02) * num2;
+            y = (m21 + m12) * num2;
+            z = 0.5F * num5;
+            w = (m01 - m10) * num2;
+            return new Quaternion(x, y, z, w);
+        }
+
+        /// <summary>
+        /// Creates a quaternion with the given forward direction with up =
+        /// Vector3::up
+        /// </summary>
+        /// <param name="forward">The look direction</param>
+        /// <returns>The rotation for this direction</returns>
+        /// For the rotation, Vector::up is used for the up direction.
+        /// Note: if the forward direction == Vector3::up, the result is
+        /// Quaternion::identity
+        public static Quaternion LookRotation(Vector3Float forward) {
+            Vector3Float up = new(0, 1, 0);
+            return LookRotation(forward, up);
+        }
+
+        /// <summary>
+        /// Calculat the rotation from on vector to another
+        /// </summary>
+        /// <param name="fromDirection">The from direction</param>
+        /// <param name="toDirection">The to direction</param>
+        /// <returns>The rotation from the first to the second vector</returns>
+        public static Quaternion FromToRotation(Vector3Float fromDirection, Vector3Float toDirection) {
+            Vector3Float axis = Vector3Float.Cross(fromDirection, toDirection);
+            axis = axis.normalized;
+            AngleFloat angle = Vector3Float.SignedAngle(fromDirection, toDirection, axis);
+            Quaternion rotation = AngleAxis(angle, axis);
+            return rotation;
+
+        }
+
+        /// <summary>
+        /// Rotate form one orientation to anther with a maximum amount of degrees
+        /// </summary>
+        /// <param name="from">The from rotation</param>
+        /// <param name="to">The destination rotation</param>
+        /// <param name="maxDegreesDelta">The maximum amount of degrees to
+        /// rotate</param> <returns>The possibly limited rotation</returns>
+        public static Quaternion RotateTowards(Quaternion from, Quaternion to,
+                                  float maxDegreesDelta) {
+            float num = Quaternion.UnsignedAngle(from, to);
+            if (num == 0) {
+                return to;
+            }
+            float t = MathF.Min(1, maxDegreesDelta / num);
+            return SlerpUnclamped(from, to, t);
+
+        }
+
+        /// <summary>
+        /// Convert an angle/axis representation to a quaternion
+        /// </summary>
+        /// <param name="angle">The angle</param>
+        /// <param name="axis">The axis</param>
+        /// <returns>The resulting quaternion</returns>
+        public static Quaternion AngleAxis(AngleFloat angle, Vector3Float axis) {
+            if (axis.sqrMagnitude == 0.0f)
+                return Quaternion.identity;
+
+            float radians = angle.inRadians;
+            radians *= 0.5f;
+
+            Vector3Float axis2 = axis * MathF.Sin(radians);
+            float x = axis2.horizontal;    // x;
+            float y = axis2.vertical;       // y;
+            float z = axis2.depth;  // z;
+            float w = MathF.Cos(radians);
+
+            return new Quaternion(x, y, z, w).normalized;
+        }
+        /// <summary>
+        /// Convert this quaternion to angle/axis representation
+        /// </summary>
+        /// <param name="angle">A pointer to the angle for the result</param>
+        /// <param name="axis">A pointer to the axis for the result</param>
+        public readonly void ToAngleAxis(out AngleFloat angle, out Vector3Float axis) {
+            Quaternion q1 = (MathF.Abs(this.w) > 1.0f) ? this.normalized : this;
+            angle = AngleFloat.Radians(2.0f * MathF.Acos(q1.w));  // angle
+            float den = MathF.Sqrt(1.0F - q1.w * q1.w);
+            if (den > 0.0001f) {
+                axis = Vector3Float.Normalize(q1.xyz / den);
+            }
+            else {
+                // This occurs when the angle is zero.
+                // Not a problem: just set an arbitrary normalized axis.
+                axis = Vector3Float.right;
+            }
+        }
+
+        /// <summary>
+        /// Get the angle between two orientations
+        /// </summary>
+        /// <param name="orientation1">The first orientation</param>
+        /// <param name="orientation2">The second orientation</param>
+        /// <returns>The smallest angle in degrees between the two
+        /// orientations</returns>
+        public static float UnsignedAngle(Quaternion q1, Quaternion q2) {
+            // float f = Dot(q1, q2);
+            // return MathF.Acos(MathF.Min(MathF.Abs(f), 1)) * 2 * AngleFloat.Rad2Deg;
+
+            float dot = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
+            dot = MathF.Min(MathF.Max(dot, -1f), 1f);
+            return 2f * MathF.Acos(MathF.Abs(dot)) * (180f / MathF.PI);
+        }
+
+        /// <summary>
+        /// Sherical lerp between two rotations
+        /// </summary>
+        /// <param name="rotation1">The first rotation</param>
+        /// <param name="rotation2">The second rotation</param>
+        /// <param name="factor">The factor between 0 and 1.</param>
+        /// <returns>The resulting rotation</returns>
+        /// A factor 0 returns rotation1, factor1 returns rotation2.
+        public static Quaternion Slerp(Quaternion a,
+                          Quaternion b, float t) {
+            if (t > 1)
+                t = 1;
+            if (t < 0)
+                t = 0;
+            return SlerpUnclamped(a, b, t);
+        }
+
+        /// <summary>
+        /// Unclamped sherical lerp between two rotations
+        /// </summary>
+        /// <param name="rotation1">The first rotation</param>
+        /// <param name="rotation2">The second rotation</param>
+        /// <param name="factor">The factor</param>
+        /// <returns>The resulting rotation</returns>
+        /// A factor 0 returns rotation1, factor1 returns rotation2.
+        /// Values outside the 0..1 range will result in extrapolated rotations
+        public static Quaternion SlerpUnclamped(Quaternion a,
+                                   Quaternion b, float t) {
+            // if either input is zero, return the other.
+            if (a.sqrMagnitude == 0.0f) {
+                if (b.sqrMagnitude == 0.0f) {
+                    return identity;
+                }
+                return b;
+            }
+            else if (b.sqrMagnitude == 0.0f) {
+                return a;
+            }
+
+            Vector3Float axyz = a.xyz;
+            Vector3Float bxyz = b.xyz;
+            float cosHalfAngle = a.w * b.w + Vector3Float.Dot(axyz, bxyz);
+
+            Quaternion b2 = b;
+            if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) {
+                // angle = 0.0f, so just return one input.
+                return a;
+            }
+            else if (cosHalfAngle < 0.0f) {
+                b2.x = -b.x;
+                b2.y = -b.y;
+                b2.z = -b.z;
+                b2.w = -b.w;
+                cosHalfAngle = -cosHalfAngle;
+            }
+
+            float blendA;
+            float blendB;
+            if (cosHalfAngle < 0.99f) {
+                // do proper slerp for big angles
+                float halfAngle = MathF.Acos(cosHalfAngle);
+                float sinHalfAngle = MathF.Sin(halfAngle);
+                float oneOverSinHalfAngle = 1.0F / sinHalfAngle;
+                blendA = MathF.Sin(halfAngle * (1.0F - t)) * oneOverSinHalfAngle;
+                blendB = MathF.Sin(halfAngle * t) * oneOverSinHalfAngle;
+            }
+            else {
+                // do lerp if angle is really small.
+                blendA = 1.0f - t;
+                blendB = t;
+            }
+            Vector3Float v = axyz * blendA + b2.xyz * blendB;
+            Quaternion result =
+               new(v.horizontal, v.vertical, v.depth, blendA * a.w + blendB * b2.w);
+            if (result.sqrMagnitude > 0.0f)
+                return result.normalized;
+            else
+                return Quaternion.identity;
+        }
+
+        /// <summary>
+        /// Convert this quaternion to angle/axis representation
+        /// </summary>
+        /// <param name="angle">A pointer to the angle for the result</param>
+        /// <param name="axis">A pointer to the axis for the result</param>
+        public readonly void ToAngleAxis(out float angle, out Vector3Float axis) {
+            ToAxisAngleRad(this, out axis, out angle);
+            angle *= AngleFloat.Rad2Deg;
+        }
+        private static void ToAxisAngleRad(Quaternion q,
+                                out Vector3Float axis,
+                                out float angle) {
+            Quaternion q1 = (MathF.Abs(q.w) > 1.0f) ? Quaternion.Normalize(q) : q;
+            angle = 2.0f * MathF.Acos(q1.w);  // angle
+            float den = MathF.Sqrt(1.0F - q1.w * q1.w);
+            if (den > 0.0001f) {
+                axis = (q1.xyz / den).normalized;
+            }
+            else {
+                // This occurs when the angle is zero.
+                // Not a problem: just set an arbitrary normalized axis.
+                axis = new Vector3Float(1, 0, 0);
+            }
+        }
+
+        /// <summary>
+        /// Returns the angle of around the give axis for a rotation
+        /// </summary>
+        /// <param name="axis">The axis around which the angle should be
+        /// computed</param> <param name="rotation">The source rotation</param>
+        /// <returns>The signed angle around the axis</returns>
+        public static float GetAngleAround(Vector3Float axis, Quaternion rotation) {
+            Quaternion secondaryRotation = GetRotationAround(axis, rotation);
+            secondaryRotation.ToAngleAxis(out float rotationAngle, out Vector3Float rotationAxis);
+
+            // Do the axis point in opposite directions?
+            if (Vector3Float.Dot(axis, rotationAxis) < 0)
+                rotationAngle = -rotationAngle;
+
+            return rotationAngle;
+        }
+
+        /// <summary>
+        /// Returns the rotation limited around the given axis
+        /// </summary>
+        /// <param name="axis">The axis which which the rotation should be
+        /// limited</param> <param name="rotation">The source rotation</param>
+        /// <returns>The rotation around the given axis</returns>
+        public static Quaternion GetRotationAround(Vector3Float axis, Quaternion rotation) {
+            Vector3Float ra = new(rotation.x, rotation.y, rotation.z);  // rotation axis
+            Vector3Float p = Vector3Float.Project(
+                ra, axis);  // return projection ra on to axis  (parallel component)
+            Quaternion twist = new(p.horizontal, p.vertical, p.depth, rotation.w);
+            twist = Normalize(twist);
+            return twist;
+
+        }
+
+        /// <summary>
+        /// Swing-twist decomposition of a rotation
+        /// </summary>
+        /// <param name="axis">The base direction for the decomposition</param>
+        /// <param name="q">The source rotation</param>
+        /// <param name="swing">A pointer to the quaternion for the swing
+        /// result</param> <param name="twist">A pointer to the quaternion for the
+        /// twist result</param>
+        static void GetSwingTwist(Vector3Float axis, Quaternion q,
+                                  out Quaternion swing, out Quaternion twist) {
+            twist = GetRotationAround(axis, q);
+            swing = q * Inverse(twist);
+        }
+
+        /// <summary>
+        /// Calculate the dot product of two quaternions
+        /// </summary>
+        /// <param name="rotation1">The first rotation</param>
+        /// <param name="rotation2">The second rotation</param>
+        /// <returns></returns>
+        public static float Dot(Quaternion q1, Quaternion q2) {
+            return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
+        }
+    }
+#endif
+
+}

src/Spherical.cs

@@ -0,0 +1,185 @@
+using System;
+#if UNITY_5_3_OR_NEWER
+using Vector3 = UnityEngine.Vector3;
+#endif
+
+namespace LinearAlgebra
+{
+    /// <summary>
+    /// A spherical vector
+    /// </summary>
+    /// <remark>This is a struct such that it is a value type and cannot be null 
+    public struct Spherical
+    {
+        /// <summary>
+        /// Create a spherical vector
+        /// </summary>
+        /// <param name="distance">The distance in meters</param>
+        /// <param name="direction">The direction of the vector</param>
+        public Spherical(float distance, Direction direction)
+        {
+            this.distance = distance;
+            this.direction = direction;
+        }
+
+        /// <summary>
+        /// Create spherical vector. All given angles are in degrees
+        /// </summary>
+        /// <param name="distance">The distance in meters</param>
+        /// <param name="horizontal">The horizontal angle in degrees</param>
+        /// <param name="vertical">The vertical angle in degrees</param>
+        /// <returns></returns>
+        public static Spherical Degrees(float distance, float horizontal, float vertical)
+        {
+            Direction direction = Direction.Degrees(horizontal, vertical);
+            Spherical s = new(distance, direction);
+            return s;
+        }
+
+        public static Spherical Radians(float distance, float horizontal, float vertical)
+        {
+            Direction direction = Direction.Radians(horizontal, vertical);
+            Spherical s = new(distance, direction);
+            return s;
+        }
+
+        /// <summary>
+        /// The distance in meters
+        /// </summary>
+        /// @remark The distance should never be negative
+        public float distance;
+        /// <summary>
+        /// The direction of the vector
+        /// </summary>
+        public Direction direction;
+
+        /// <summary>
+        /// A spherical vector with zero degree angles and distance
+        /// </summary>
+        public readonly static Spherical zero = new(0, Direction.forward);
+        /// <summary>
+        /// A normalized forward-oriented vector
+        /// </summary>
+        public readonly static Spherical forward = new(1, Direction.forward);
+
+
+        // public static Spherical FromVector3Float(Vector3Float v) {
+        //     float distance = v.magnitude;
+        //     if (distance == 0.0f)
+        //         return Spherical.zero;
+        //     else {
+        //         float verticalAngle = (float)((Angle.pi / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg);
+        //         float horizontalAngle = (float)Math.Atan2(v.x, v.z) * Angle.Rad2Deg;
+        //         return Spherical.Degrees(distance, horizontalAngle, verticalAngle);
+        //     }
+        // }
+
+        public static Spherical FromVector3Float(Vector3Float v)
+        {
+            float distance = v.magnitude;
+            if (distance == 0.0f)
+                return Spherical.zero;
+            else
+            {
+                float verticalAngle = (float)(Math.PI / 2 - Math.Acos(v.vertical / distance)) * AngleFloat.Rad2Deg;
+                float horizontalAngle = (float)Math.Atan2(v.horizontal, v.depth) * AngleFloat.Rad2Deg;
+                return Degrees(distance, horizontalAngle, verticalAngle);
+            }
+        }
+
+        // public Vector3Float ToVector3Float() {
+        //     float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad;
+        //     float horizontalRad = this.direction.horizontal * Angle.Deg2Rad;
+        //     float cosVertical = (float)Math.Cos(verticalRad);
+        //     float sinVertical = (float)Math.Sin(verticalRad);
+        //     float cosHorizontal = (float)Math.Cos(horizontalRad);
+        //     float sinHorizontal = (float)Math.Sin(horizontalRad);
+
+        //     float x = this.distance * sinVertical * sinHorizontal;
+        //     float y = this.distance * cosVertical;
+        //     float z = this.distance * sinVertical * cosHorizontal;
+
+        //     Vector3Float v = new(x, y, z);
+        //     return v;
+        // }
+
+        public Vector3Float ToVector3Float()
+        {
+            float verticalRad = (AngleFloat.deg90 - this.direction.vertical).inRadians;
+            float horizontalRad = this.direction.horizontal.inRadians;
+            float cosVertical = (float)Math.Cos(verticalRad);
+            float sinVertical = (float)Math.Sin(verticalRad);
+            float cosHorizontal = (float)Math.Cos(horizontalRad);
+            float sinHorizontal = (float)Math.Sin(horizontalRad);
+
+            float x = this.distance * sinVertical * sinHorizontal;
+            float y = this.distance * cosVertical;
+            float z = this.distance * sinVertical * cosHorizontal;
+
+            Vector3Float v = new(x, y, z);
+            return v;
+        }
+#if UNITY_5_3_OR_NEWER
+            public static Spherical FromVector3(Vector3 v)
+        {
+            float distance = v.magnitude;
+            if (distance == 0.0f)
+                return Spherical.zero;
+            else
+            {
+                float verticalAngle = (float)(Math.PI / 2 - Math.Acos(v.y / distance)) * AngleFloat.Rad2Deg;
+                float horizontalAngle = (float)Math.Atan2(v.x, v.z) * AngleFloat.Rad2Deg;
+                return Degrees(distance, horizontalAngle, verticalAngle);
+            }
+        }
+
+        public Vector3 ToVector3()
+        {
+            float verticalRad = (AngleFloat.deg90 - this.direction.vertical).inRadians;
+            float horizontalRad = this.direction.horizontal.inRadians;
+            float cosVertical = (float)Math.Cos(verticalRad);
+            float sinVertical = (float)Math.Sin(verticalRad);
+            float cosHorizontal = (float)Math.Cos(horizontalRad);
+            float sinHorizontal = (float)Math.Sin(horizontalRad);
+
+            float x = this.distance * sinVertical * sinHorizontal;
+            float y = this.distance * cosVertical;
+            float z = this.distance * sinVertical * cosHorizontal;
+
+            Vector3 v = new(x, y, z);
+            return v;
+        }
+#endif        
+
+        public float magnitude
+        {
+            get
+            {
+                return this.distance;
+            }
+        }
+        public Spherical normalized
+        {
+            get
+            {
+                Spherical r = new()
+                {
+                    distance = 1,
+                    direction = this.direction
+                };
+                return r;
+            }
+        }
+
+        public static Spherical operator +(Spherical s1, Spherical s2)
+        {
+            // let's do it the easy way...
+            Vector3Float v1 = s1.ToVector3Float();
+            Vector3Float v2 = s2.ToVector3Float();
+            Vector3Float v = v1 + v2;
+            Spherical r = FromVector3Float(v);
+            return r;
+        }
+
+    }
+}

src/SwingTwist.cs

@@ -0,0 +1,116 @@
+#if !UNITY_5_3_OR_NEWER
+using UnityEngine;
+#endif
+
+namespace LinearAlgebra {
+
+    /// <summary>
+    /// An orientation using swing and twist angles
+    /// </summary>
+    /// <param name="swing">The swing rotation</param>
+    /// <param name="twist">The twist rotation</param>
+    public struct SwingTwist {
+        public Direction swing;
+        public AngleFloat twist;
+
+        public SwingTwist(Direction swing, AngleFloat twist) {
+            this.swing = swing;
+            this.twist = twist;
+        }
+
+        /// <summary>
+        /// Create a swing/twist rotation using angles in degrees
+        /// </summary>
+        /// <param name="horizontalSwing">The swing angle in the horizontal plane in degrees</param>
+        /// <param name="verticalSwing">The swing angle in the vertical plan in degrees</param>
+        /// <param name="twist">The twist angle in degrees</param>
+        /// <returns>The swing/twist rotation</returns>
+        public static SwingTwist Degrees(float horizontalSwing, float verticalSwing, float twist) {
+            Direction swing = Direction.Degrees(horizontalSwing, verticalSwing);
+            AngleFloat twistAngle = AngleFloat.Degrees(twist);
+            SwingTwist s = new(swing, twistAngle);
+            return s;
+        }
+
+        /// <summary>
+        /// Create a swing/twist rotation using angles in degrees
+        /// </summary>
+        /// <param name="horizontalSwing">The swing angle in the horizontal plane in degrees</param>
+        /// <param name="verticalSwing">The swing angle in the vertical plan in degrees</param>
+        /// <param name="twist">The twist angle in degrees</param>
+        /// <returns>The swing/twist rotation</returns>
+        public static SwingTwist Radians(float horizontalSwing, float verticalSwing, float twist) {
+            Direction swing = Direction.Radians(horizontalSwing, verticalSwing);
+            AngleFloat twistAngle = AngleFloat.Radians(twist);
+            SwingTwist s = new(swing, twistAngle);
+            return s;
+        }
+
+#if !UNITY_5_3_OR_NEWER
+        /// <summary>
+        /// A zero angle rotation
+        /// </summary>
+        public static readonly SwingTwist zero = Degrees(0, 0, 0);
+
+        public Spherical ToAngleAxis() {
+            LinearAlgebra.Quaternion q = this.ToQuaternion();
+            q.ToAngleAxis(out float angle, out Vector3Float axis);
+            Direction direction = Direction.FromVector3(axis);
+
+            Spherical r = new(angle, direction);
+            return r;
+        }
+
+        public static SwingTwist FromAngleAxis(Spherical r) {
+            Vector3Float vectorAxis = r.direction.ToVector3();
+            LinearAlgebra.Quaternion q = LinearAlgebra.Quaternion.AngleAxis(AngleFloat.Degrees(r.distance), vectorAxis);
+            return FromQuaternion(q);
+        }
+
+        /// <summary>
+        /// Convert a quaternion in a swing/twist rotation
+        /// </summary>
+        /// <param name="q">The quaternion to convert</param>
+        /// <returns>The swing/twist rotation</returns>
+        public static SwingTwist FromQuaternion(LinearAlgebra.Quaternion q) {
+            Vector3Float v = LinearAlgebra.Quaternion.ToAngles(q);
+            SwingTwist r = Degrees(v.vertical, v.horizontal, v.depth);
+            return r;
+        }
+
+        public LinearAlgebra.Quaternion ToQuaternion() {
+            LinearAlgebra.Quaternion q = LinearAlgebra.Quaternion.Euler(this.swing.vertical.inDegrees,
+                                       this.swing.horizontal.inDegrees,
+                                       this.twist.inDegrees);
+            return q;
+
+        }
+
+        public static SwingTwist FromQuat32(Quat32 q32) {
+            q32.ToAngles(out float right, out float up, out float forward);
+            SwingTwist r = Degrees(up, right, forward);
+            return r;
+        }
+#endif
+#if UNITY_5_3_OR_NEWER
+        /// <summary>
+        /// Convert a quaternion in a swing/twist rotation
+        /// </summary>
+        /// <param name="q">The quaternion to convert</param>
+        /// <returns>The swing/twist rotation</returns>
+        public static SwingTwist FromQuaternion(UnityEngine.Quaternion q) {
+            UnityEngine.Vector3 angles = q.eulerAngles;
+            SwingTwist r = Degrees(angles.y, -angles.x, -angles.z);
+            return r;
+        }
+
+        public UnityEngine.Quaternion ToUnityQuaternion() {
+            UnityEngine.Quaternion q = UnityEngine.Quaternion.Euler(this.swing.vertical.inDegrees,
+                                   this.swing.horizontal.inDegrees,
+                                   this.twist.inDegrees);
+            return q;
+        }
+#endif        
+    }
+
+}

src/Vector2Float.cs

@@ -0,0 +1,483 @@
+using System;
+using System.Numerics;
+
+namespace LinearAlgebra {
+
+    /*
+    public struct Vector2Int {
+        public int horizontal;
+        public int vertical;
+
+        public Vector2Int(int horizontal, int vertical) {
+            this.horizontal = horizontal;
+            this.vertical = vertical;
+        }
+
+        /// <summary>
+        /// A vector with zero for all axis
+        /// </summary>
+        public static readonly Vector2Int zero = new(0, 0);
+        /// <summary>
+        /// A vector with values (1, 1)
+        /// </summary>
+        public static readonly Vector2Int one = new(1, 1);
+        /// <summary>
+        /// A vector with values (0, 1)
+        /// </summary>
+        public static readonly Vector2Int up = new(0, 1);
+        /// <summary>
+        /// A vector with values (0, -1)
+        /// </summary>
+        public static readonly Vector2Int down = new(0, -1);
+        /// <summary>
+        /// A vector with values (0, 1)
+        /// </summary>
+        public static readonly Vector2Int forward = new(0, 1);
+        /// <summary>
+        /// A vector with values (0, -1)
+        /// </summary>
+        public static readonly Vector2Int back = new(0, -1);
+        /// <summary>
+        /// A vector3 with values (-1, 0)
+        /// </summary>
+        public static readonly Vector2Int left = new(-1, 0);
+        /// <summary>
+        /// A vector with values (1, 0)
+        /// </summary>
+        public static readonly Vector2Int right = new(1, 0);
+
+        /// <summary>
+        /// Tests if the vector has equal values as the given vector
+        /// </summary>
+        /// <param name="v1">The vector to compare to</param>
+        /// <returns><em>true</em> if the vector values are equal</returns>
+        public readonly bool Equals(Vector2Int v) => this.horizontal == v.horizontal && vertical == v.vertical;
+
+        /// <summary>
+        /// Tests if the vector is equal to the given object
+        /// </summary>
+        /// <param name="obj">The object to compare to</param>
+        /// <returns><em>false</em> when the object is not a Vector2 or does not have equal values</returns>
+        public override readonly bool Equals(object obj) {
+            if (obj is not Vector2Int v)
+                return false;
+
+            return (this.horizontal == v.horizontal && this.vertical == v.vertical);
+        }
+
+        /// <summary>
+        /// Tests if the two vectors have equal values
+        /// </summary>
+        /// <param name="v1">The first vector</param>
+        /// <param name="v2">The second vector</param>
+        /// <returns><em>true</em>when the vectors have equal values</returns>
+        /// Note that this uses a Float equality check which cannot be not exact in all cases.
+        /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon
+        /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon
+        public static bool operator ==(Vector2Int v1, Vector2Int v2) {
+            return (v1.horizontal == v2.horizontal && v1.vertical == v2.vertical);
+        }
+        /// <summary>
+        /// Tests if two vectors have different values
+        /// </summary>
+        /// <param name="v1">The first vector</param>
+        /// <param name="v2">The second vector</param>
+        /// <returns><em>true</em>when the vectors have different values</returns>
+        /// Note that this uses a Float equality check which cannot be not exact in all case.
+        /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon.
+        /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon
+        public static bool operator !=(Vector2Int v1, Vector2Int v2) {
+            return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical);
+        }
+        public readonly float magnitude {
+            get {
+                int h = this.horizontal;
+                int v = this.vertical;
+                return MathF.Sqrt(h * h + v * v);
+            }
+        }
+
+        public static float MagnitudeOf(Vector2Int v) {
+            return v.magnitude;
+        }
+
+        public static Vector2Int operator -(Vector2Int v1, Vector2Int v2) {
+            return new Vector2Int(v1.horizontal - v2.horizontal, v1.vertical - v2.vertical);
+        }
+        public static Vector2Int operator +(Vector2Int v1, Vector2Int v2) {
+            return new Vector2Int(v1.horizontal + v2.horizontal, v1.vertical + v2.vertical);
+        }
+
+        public static float Distance(Vector2Int v1, Vector2Int v2) {
+            return (v1 - v2).magnitude;
+        }
+    }
+
+    public struct Vector2Float {
+        public float horizontal;
+        public float vertical;
+
+        public Vector2Float(float horizontal, float vertical) {
+            this.horizontal = horizontal;
+            this.vertical = vertical;
+        }
+
+        public readonly float magnitude {
+            get {
+                float h = this.horizontal;
+                float v = this.vertical;
+                return MathF.Sqrt(h * h + v * v);
+            }
+        }
+
+        public static Vector2Float operator -(Vector2Float v1, Vector2Float v2) {
+            return new Vector2Float(v1.horizontal - v2.horizontal, v1.vertical - v2.vertical);
+        }
+
+        public static float Distance(Vector2Float v1, Vector2Float v2) {
+            return (v1 - v2).magnitude;
+        }
+    }
+    */
+
+    /// <summary>
+    /// 2-dimensional vectors
+    /// </summary>
+    public struct Vector2Float {
+
+        /// <summary>
+        /// The right axis of the vector
+        /// </summary>
+        public float horizontal; // left/right
+        /// <summary>
+        /// The upward/forward axis of the vector
+        /// </summary>
+        public float vertical; // forward/backward
+                               // directions are to be inline with Vector3 as much as possible...
+
+        /// <summary>
+        /// Create a new 2-dimensional vector
+        /// </summary>
+        /// <param name="x">x axis value</param>
+        /// <param name="y">y axis value</param>
+        public Vector2Float(float x, float y) {
+            this.horizontal = x;
+            this.vertical = y;
+        }
+
+        /// <summary>
+        /// Convert a Vector2Int into a Vector2Float
+        /// </summary>
+        /// <param name="v">The Vector2Int</param>
+        public Vector2Float(Vector2Int v) {
+            this.horizontal = v.horizontal;
+            this.vertical = v.vertical;
+        }
+
+        /// <summary>
+        /// A vector with zero for all axis
+        /// </summary>
+        public static readonly Vector2Float zero = new Vector2Float(0, 0);
+        /// <summary>
+        /// A vector with values (1, 1)
+        /// </summary>
+        public static readonly Vector2Float one = new Vector2Float(1, 1);
+        /// <summary>
+        /// A vector with values (0, 1)
+        /// </summary>
+        public static readonly Vector2Float up = new Vector2Float(0, 1);
+        /// <summary>
+        /// A vector with values (0, -1)
+        /// </summary>
+        public static readonly Vector2Float down = new Vector2Float(0, -1);
+        /// <summary>
+        /// A vector with values (0, 1)
+        /// </summary>
+        public static readonly Vector2Float forward = new Vector2Float(0, 1);
+        /// <summary>
+        /// A vector with values (0, -1)
+        /// </summary>
+        public static readonly Vector2Float back = new Vector2Float(0, -1);
+        /// <summary>
+        /// A vector3 with values (-1, 0)
+        /// </summary>
+        public static readonly Vector2Float left = new Vector2Float(-1, 0);
+        /// <summary>
+        /// A vector with values (1, 0)
+        /// </summary>
+        public static readonly Vector2Float right = new Vector2Float(1, 0);
+
+        /// <summary>
+        /// The squared length of this vector
+        /// </summary>
+        /// <returns>The squared length</returns>
+        /// The squared length is computationally simpler than the real length.
+        /// Think of Pythagoras A^2 + B^2 = C^2.
+        /// This leaves out the calculation of the squared root of C.
+        public readonly float sqrMagnitude => horizontal * horizontal + vertical * vertical;
+        public static float SqrMagnitudeOf(Vector2Float v) {
+            return v.sqrMagnitude;
+        }
+
+        /// <summary>
+        /// The length of this vector
+        /// </summary>
+        /// <returns>The length of this vector</returns>
+        public readonly float magnitude => MathF.Sqrt(horizontal * horizontal + vertical * vertical);
+        public static float MagnitudeOf(Vector2Float v) {
+            return v.magnitude;
+        }
+
+        /// <summary>
+        /// Convert the vector to a length of a 1
+        /// </summary>
+        /// <returns>The vector with length 1</returns>
+        public Vector2Float normalized {
+            get {
+                float l = magnitude;
+                Vector2Float v = zero;
+                if (l > Float.epsilon)
+                    v = this / l;
+                return v;
+            }
+        }
+        public static Vector2Float Normalize(Vector2Float v) {
+            return v.normalized;
+        }
+
+        /// <summary>
+        /// Add two vectors
+        /// </summary>
+        /// <param name="v1">The first vector</param>
+        /// <param name="v2">The second vector</param>
+        /// <returns>The result of adding the two vectors</returns>
+        public static Vector2Float operator +(Vector2Float v1, Vector2Float v2) {
+            Vector2Float v = new Vector2Float(v1.horizontal + v2.horizontal, v1.vertical + v2.vertical);
+            return v;
+        }
+
+        /// <summary>
+        /// Subtract two vectors
+        /// </summary>
+        /// <param name="v1">The first vector</param>
+        /// <param name="v2">The second vector</param>
+        /// <returns>The result of adding the two vectors</returns>
+        public static Vector2Float operator -(Vector2Float v1, Vector2Float v2) {
+            Vector2Float v = new Vector2Float(v1.horizontal - v2.horizontal, v1.vertical - v2.vertical);
+            return v;
+        }
+
+        /// <summary>
+        /// Negate the vector
+        /// </summary>
+        /// <param name="v1">The vector to negate</param>
+        /// <returns>The negated vector</returns>
+        /// This will result in a vector pointing in the opposite direction
+        public static Vector2Float operator -(Vector2Float v1) {
+            Vector2Float v = new Vector2Float(-v1.horizontal, -v1.vertical);
+            return v;
+        }
+
+        /// <summary>
+        /// Scale a vector uniformly down
+        /// </summary>
+        /// <param name="v">The vector to scale</param>
+        /// <param name="f">The scaling factor</param>
+        /// <returns>The scaled vector</returns>
+        /// Each component of the vector will be devided by the same factor.
+        public static Vector2Float operator /(Vector2Float v, float f) {
+            Vector2Float r = new(v.horizontal / f, v.vertical / f);
+            return r;
+        }
+
+
+        /// <summary>
+        /// Scale a vector uniformly up
+        /// </summary>
+        /// <param name="v1">The vector to scale</param>
+        /// <param name="f">The scaling factor</param>
+        /// <returns>The scaled vector</returns>
+        /// Each component of the vector will be multipled with the same factor.
+        public static Vector2Float operator *(Vector2Float v1, float f) {
+            Vector2Float v = new Vector2Float(v1.horizontal * f, v1.vertical * f);
+            return v;
+        }
+
+        /// <summary>
+        /// Scale a vector uniformly up
+        /// </summary>
+        /// <param name="f">The scaling factor</param>
+        /// <param name="v1">The vector to scale</param>
+        /// <returns>The scaled vector</returns>
+        /// Each component of the vector will be multipled with the same factor.
+        public static Vector2Float operator *(float f, Vector2Float v1) {
+            Vector2Float v = new Vector2Float(f * v1.horizontal, f * v1.vertical);
+            return v;
+        }
+
+        /// @brief Scale the vector using another vector
+        /// @param v1 The vector to scale
+        /// @param v2 A vector with the scaling factors
+        /// @return The scaled vector
+        /// @remark Each component of the vector v1 will be multiplied with the
+        /// matching component from the scaling vector v2.
+        public static Vector2Float Scale(Vector2Float v1, Vector2Float v2) {
+            return new Vector2Float(v1.horizontal * v2.horizontal, v1.vertical * v2.vertical);
+        }
+
+        /*
+        /// <summary>
+        /// Tests if the vector has equal values as the given vector
+        /// </summary>
+        /// <param name="v1">The vector to compare to</param>
+        /// <returns><em>true</em> if the vector values are equal</returns>
+        public bool Equals(Vector2Float v1) => horizontal == v1.horizontal && vertical == v1.vertical;
+
+        /// <summary>
+        /// Tests if the vector is equal to the given object
+        /// </summary>
+        /// <param name="obj">The object to compare to</param>
+        /// <returns><em>false</em> when the object is not a Vector2 or does not have equal values</returns>
+        public override bool Equals(object obj) {
+            if (!(obj is Vector2Float v))
+                return false;
+
+            return (horizontal == v.horizontal && vertical == v.vertical);
+        }
+        */
+
+        /// <summary>
+        /// Tests if the two vectors have equal values
+        /// </summary>
+        /// <param name="v1">The first vector</param>
+        /// <param name="v2">The second vector</param>
+        /// <returns><em>true</em>when the vectors have equal values</returns>
+        /// Note that this uses a Float equality check which cannot be not exact in all cases.
+        /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon
+        /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon
+        public static bool operator ==(Vector2Float v1, Vector2Float v2) {
+            return (v1.horizontal == v2.horizontal && v1.vertical == v2.vertical);
+        }
+
+        /// <summary>
+        /// Tests if two vectors have different values
+        /// </summary>
+        /// <param name="v1">The first vector</param>
+        /// <param name="v2">The second vector</param>
+        /// <returns><em>true</em>when the vectors have different values</returns>
+        /// Note that this uses a Float equality check which cannot be not exact in all case.
+        /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon.
+        /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon
+        public static bool operator !=(Vector2Float v1, Vector2Float v2) {
+            return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical);
+        }
+
+        /*
+        /// <summary>
+        /// Get an hash code for the vector
+        /// </summary>
+        /// <returns>The hash code</returns>
+        public override int GetHashCode() {
+            return (horizontal, vertical).GetHashCode();
+        }
+        */
+
+        /// <summary>
+        /// Get the distance between two vectors
+        /// </summary>
+        /// <param name="v1">The first vector</param>
+        /// <param name="v2">The second vector</param>
+        /// <returns>The distance between the two vectors</returns>
+        public static float Distance(Vector2Float v1, Vector2Float v2) {
+            float x = v1.horizontal - v2.horizontal;
+            float y = v1.vertical - v2.vertical;
+            float d = (float)Math.Sqrt(x * x + y * y);
+            return d;
+        }
+
+        /// <summary>
+        /// The dot product of two vectors
+        /// </summary>
+        /// <param name="v1">The first vector</param>
+        /// <param name="v2">The second vector</param>
+        /// <returns>The dot product of the two vectors</returns>
+        public static float Dot(Vector2Float v1, Vector2Float v2) {
+            return v1.horizontal * v2.horizontal + v1.vertical * v2.vertical;
+        }
+
+        /// <summary>
+        /// Calculate the signed angle between two vectors.
+        /// </summary>
+        /// <param name="from">The starting vector</param>
+        /// <param name="to">The ending vector</param>
+        /// <param name="axis">The axis to rotate around</param>
+        /// <returns>The signed angle in degrees</returns>
+        public static float SignedAngle(Vector2Float from, Vector2Float to) {
+            //float sign = Math.Sign(v1.y * v2.x - v1.x * v2.y);
+            //return Vector2.Angle(v1, v2) * sign;
+
+            float sqrMagFrom = from.sqrMagnitude;
+            float sqrMagTo = to.sqrMagnitude;
+
+            if (sqrMagFrom == 0 || sqrMagTo == 0)
+                return 0;
+            //if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo))
+            //    return nanf("");
+
+            float angleFrom = (float)Math.Atan2(from.vertical, from.horizontal);
+            float angleTo = (float)Math.Atan2(to.vertical, to.horizontal);
+            return -(angleTo - angleFrom) * AngleFloat.Rad2Deg;
+        }
+
+        public static float UnsignedAngle(Vector2Float from, Vector2Float to) {
+            return MathF.Abs(SignedAngle(from, to));
+        }
+
+        /// <summary>
+        /// Rotates the vector with the given angle
+        /// </summary>
+        /// <param name="v1">The vector to rotate</param>
+        /// <param name="angle">The angle in degrees</param>
+        /// <returns></returns>
+        public static Vector2Float Rotate(Vector2Float v1, AngleFloat angle) {
+            float sin = (float)Math.Sin(angle.inRadians);
+            float cos = (float)Math.Cos(angle.inRadians);
+            // float sin = AngleFloat.Sin(angle);
+            // float cos = AngleFloat.Cos(angle);
+
+            float tx = v1.horizontal;
+            float ty = v1.vertical;
+            Vector2Float v = new Vector2Float() {
+                horizontal = (cos * tx) - (sin * ty),
+                vertical = (sin * tx) + (cos * ty)
+            };
+            return v;
+        }
+
+        /// <summary>
+        /// Lerp between two vectors
+        /// </summary>
+        /// <param name="v1">The from vector</param>
+        /// <param name="v2">The to vector</param>
+        /// <param name="f">The interpolation distance [0..1]</param>
+        /// <returns>The lerped vector</returns>
+        /// The factor f is unclamped. Value 0 matches the *v1* vector, Value 1
+        /// matches the *v2* vector Value -1 is *v1* vector minus the difference
+        /// between *v1* and *v2* etc.
+        public static Vector2Float Lerp(Vector2Float v1, Vector2Float v2, float f) {
+            Vector2Float v = v1 + (v2 - v1) * f;
+            return v;
+        }
+
+        /// <summary>
+        /// Map interval of angles between vectors [0..Pi] to interval [0..1]
+        /// </summary>
+        /// <param name="v1">The first vector</param>
+        /// <param name="v2">The second vector</param>
+        /// <returns>The resulting factor in interval [0..1]</returns>
+        /// Vectors a and b must be normalized
+        public static float ToFactor(Vector2Float v1, Vector2Float v2) {
+            return (1 - Vector2Float.Dot(v1, v2)) / 2;
+        }
+    }
+}

src/Vector2Int.cs

@@ -0,0 +1,176 @@
+using System;
+
+namespace LinearAlgebra {
+
+    public struct Vector2Int {
+        public int horizontal;
+        public int vertical;
+
+        public Vector2Int(int horizontal, int vertical) {
+            this.horizontal = horizontal;
+            this.vertical = vertical;
+        }
+
+        /// <summary>
+        /// A vector with zero for all axis
+        /// </summary>
+        public static readonly Vector2Int zero = new(0, 0);
+        /// <summary>
+        /// A vector with values (1, 1)
+        /// </summary>
+        public static readonly Vector2Int one = new(1, 1);
+        /// <summary>
+        /// A vector with values (0, 1)
+        /// </summary>
+        public static readonly Vector2Int up = new(0, 1);
+        /// <summary>
+        /// A vector with values (0, -1)
+        /// </summary>
+        public static readonly Vector2Int down = new(0, -1);
+        /// <summary>
+        /// A vector with values (0, 1)
+        /// </summary>
+        public static readonly Vector2Int forward = new(0, 1);
+        /// <summary>
+        /// A vector with values (0, -1)
+        /// </summary>
+        public static readonly Vector2Int back = new(0, -1);
+        /// <summary>
+        /// A vector3 with values (-1, 0)
+        /// </summary>
+        public static readonly Vector2Int left = new(-1, 0);
+        /// <summary>
+        /// A vector with values (1, 0)
+        /// </summary>
+        public static readonly Vector2Int right = new(1, 0);
+
+        /*
+        /// <summary>
+        /// Get an hash code for the vector
+        /// </summary>
+        /// <returns>The hash code</returns>
+        public override int GetHashCode() {
+            return (this.horizontal, this.vertical).GetHashCode();
+        }
+
+        /// <summary>
+        /// Tests if the vector has equal values as the given vector
+        /// </summary>
+        /// <param name="v1">The vector to compare to</param>
+        /// <returns><em>true</em> if the vector values are equal</returns>
+        public readonly bool Equals(Vector2Int v) => this.horizontal == v.horizontal && vertical == v.vertical;
+
+        /// <summary>
+        /// Tests if the vector is equal to the given object
+        /// </summary>
+        /// <param name="obj">The object to compare to</param>
+        /// <returns><em>false</em> when the object is not a Vector2 or does not have equal values</returns>
+        public override readonly bool Equals(object obj) {
+            if (obj is not Vector2Int v)
+                return false;
+
+            return (this.horizontal == v.horizontal && this.vertical == v.vertical);
+        }
+        */
+
+        /// <summary>
+        /// Tests if the two vectors have equal values
+        /// </summary>
+        /// <param name="v1">The first vector</param>
+        /// <param name="v2">The second vector</param>
+        /// <returns><em>true</em>when the vectors have equal values</returns>
+        /// Note that this uses a Float equality check which cannot be not exact in all cases.
+        /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon
+        /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon
+        public static bool operator ==(Vector2Int v1, Vector2Int v2) {
+            return (v1.horizontal == v2.horizontal && v1.vertical == v2.vertical);
+        }
+        /// <summary>
+        /// Tests if two vectors have different values
+        /// </summary>
+        /// <param name="v1">The first vector</param>
+        /// <param name="v2">The second vector</param>
+        /// <returns><em>true</em>when the vectors have different values</returns>
+        /// Note that this uses a Float equality check which cannot be not exact in all case.
+        /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon.
+        /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon
+        public static bool operator !=(Vector2Int v1, Vector2Int v2) {
+            return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical);
+        }
+
+        public readonly float sqrMagnitude => this.horizontal * this.horizontal + this.vertical * this.vertical;
+
+        public static float SqrMagnitudeOf(Vector2Int v) {
+            return v.sqrMagnitude;
+        }
+
+        public readonly float magnitude =>
+                MathF.Sqrt(this.horizontal * this.horizontal + this.vertical * this.vertical);
+
+        public static float MagnitudeOf(Vector2Int v) {
+            return v.magnitude;
+        }
+
+        /// @brief Convert the vector to a length of 1
+        /// @return The vector normalized to a length of 1
+        public readonly Vector2Float normalized {
+            get {
+                float l = magnitude;
+                Vector2Float v = Vector2Float.zero;
+                if (l > Float.epsilon) 
+                    v = new Vector2Float(this) / l;
+                return v;
+            }
+        }
+        /// @brief Convert the vector to a length of 1
+        /// @param v The vector to convert
+        /// @return The vector normalized to a length of 1
+        public static Vector2Float Normalize(Vector2Int v) {
+            float num = v.magnitude;
+            Vector2Float result = Vector2Float.zero;
+            if (num > Float.epsilon)
+                result = new Vector2Float(v) / num;
+
+            return result;
+        }
+
+        public static Vector2Int operator -(Vector2Int v) {
+            return new Vector2Int(-v.horizontal, -v.vertical);    
+        }
+        
+        public static Vector2Int operator -(Vector2Int v1, Vector2Int v2) {
+            return new Vector2Int(v1.horizontal - v2.horizontal, v1.vertical - v2.vertical);
+        }
+        public static Vector2Int operator +(Vector2Int v1, Vector2Int v2) {
+            return new Vector2Int(v1.horizontal + v2.horizontal, v1.vertical + v2.vertical);
+        }
+
+        public static Vector2Int operator /(Vector2Int v, int f) {
+            return new Vector2Int(v.horizontal / f, v.vertical / f);
+        }
+
+        public static Vector2Int operator *(Vector2Int v1, int d) {
+            return new Vector2Int(v1.horizontal * d, v1.vertical * d);
+        }
+
+        public static Vector2Int operator *(int d, Vector2Int v1) {
+            return new Vector2Int(d * v1.horizontal, d * v1.vertical);
+        }
+
+        public static Vector2Int Scale(Vector2Int v1, Vector2Int v2) {
+            return new Vector2Int(v1.horizontal * v2.horizontal, v1.vertical * v2.vertical);
+        }
+
+        /// @brief The dot product of two vectors
+        /// @param v1 The first vector
+        /// @param v2 The second vector
+        /// @return The dot product of the two vectors
+        public static int Dot(Vector2Int v1, Vector2Int v2) {
+            return v1.horizontal * v2.horizontal + v1.vertical * v2.vertical;
+        }
+
+        public static float Distance(Vector2Int v1, Vector2Int v2) {
+            return (v1 - v2).magnitude;
+        }
+    }
+}

src/Vector3Float.cs

@@ -0,0 +1,399 @@
+//#if !UNITY_5_3_OR_NEWER
+using System;
+
+namespace LinearAlgebra {
+    /*
+    public struct Vector3Float {
+        public float horizontal;
+        public float vertical;
+        public float depth;
+
+        public Vector3Float(float horizontal, float vertical, float depth) {
+            this.horizontal = horizontal;
+            this.vertical = vertical;
+            this.depth = depth;
+        }
+
+        /// <summary>
+        /// A vector with zero for all axis
+        /// </summary>
+        public static readonly Vector3Float zero = new(0, 0, 0);
+
+        public readonly float magnitude {
+            get => (float)Math.Sqrt(this.horizontal * this.horizontal + this.vertical * this.vertical + this.depth * this.depth);
+        }
+
+        /// <summary>
+        /// Convert the vector to a length of a 1
+        /// </summary>
+        /// <returns>The vector with length 1</returns>
+        public readonly Vector3Float normalized {
+            get {
+                float l = magnitude;
+                Vector3Float v = zero;
+                if (l > Float.epsilon)
+                    v = this / l;
+                return v;
+            }
+        }
+
+
+        public static Vector3Float operator *(Vector3Float v, float f) {
+            Vector3Float r = new(v.horizontal * f, v.vertical * f, v.depth * f);
+            return r;
+        }
+        public static Vector3Float operator /(Vector3Float v, float f) {
+            Vector3Float r = new(v.horizontal / f, v.vertical / f, v.depth / f);
+            return r;
+        }
+
+        public static float Dot(Vector3Float v1, Vector3Float v2) {
+            return v1.horizontal * v2.horizontal + v1.vertical * v2.vertical +
+                v1.depth * v2.depth;
+        }
+
+        const float epsilon = 1E-05f;
+        public static Vector3Float Project(Vector3Float v, Vector3Float n) {
+            float sqrMagnitude = Dot(n, n);
+            if (sqrMagnitude < epsilon)
+                return zero;
+            else {
+                float dot = Dot(v, n);
+                Vector3Float r = n * dot;
+                r /= sqrMagnitude;
+                return r;
+            }
+
+        }
+    }
+    */
+
+    /// <summary>
+    /// 3-dimensional vectors
+    /// </summary>
+    /// This uses the right-handed coordinate system.
+    public struct Vector3Float {
+
+        /// <summary>
+        /// The right axis of the vector
+        /// </summary>
+        public float horizontal; //> left/right
+        /// <summary>
+        /// The upward axis of the vector
+        /// </summary>
+        public float vertical; //> up/down
+        /// <summary>
+        /// The forward axis of the vector
+        /// </summary>
+        public float depth; //> forward/backward
+
+        /// <summary>
+        /// Create a new 3-dimensional vector
+        /// </summary>
+        /// <param name="horizontal">x axis value</param>
+        /// <param name="vertical">y axis value</param>
+        /// <param name="depth">z axis value</param>
+        public Vector3Float(float horizontal, float vertical, float depth) {
+            this.horizontal = horizontal;
+            this.vertical = vertical;
+            this.depth = depth;
+        }
+
+        public Vector3Float(Vector3Int v) {
+            this.horizontal = v.horizontal;
+            this.vertical = v.vertical;
+            this.depth = v.depth;
+        }
+
+        public static Vector3Float FromSpherical(Spherical s) {
+            float verticalRad = (AngleFloat.deg90 - s.direction.vertical).inRadians;
+            float horizontalRad = s.direction.horizontal.inRadians;
+            float cosVertical = MathF.Cos(verticalRad);
+            float sinVertical = MathF.Sin(verticalRad);
+            float cosHorizontal = MathF.Cos(horizontalRad);
+            float sinHorizontal = MathF.Sin(horizontalRad);
+
+            float horizontal = s.distance * sinVertical * sinHorizontal;
+            float vertical = s.distance * cosVertical;
+            float depth = s.distance * sinVertical * cosHorizontal;
+            return new Vector3Float(horizontal, vertical, depth);
+        }
+
+        /// <summary>
+        /// A vector with zero for all axis
+        /// </summary>
+        public static readonly Vector3Float zero = new Vector3Float(0, 0, 0);
+        /// <summary>
+        /// A vector with one for all axis
+        /// </summary>
+        public static readonly Vector3Float one = new Vector3Float(1, 1, 1);
+        /// <summary>
+        /// A Vector3Float with values (-1, 0, 0)
+        /// </summary>
+        public static readonly Vector3Float left = new Vector3Float(-1, 0, 0);
+        /// <summary>
+        /// A vector with values (1, 0, 0)
+        /// </summary>
+        public static readonly Vector3Float right = new Vector3Float(1, 0, 0);
+        /// <summary>
+        /// A vector with values (0, -1, 0)
+        /// </summary>
+        public static readonly Vector3Float down = new Vector3Float(0, -1, 0);
+        /// <summary>
+        /// A vector with values (0, 1, 0)
+        /// </summary>
+        public static readonly Vector3Float up = new Vector3Float(0, 1, 0);
+        /// <summary>
+        /// A vector with values (0, 0, -1)
+        /// </summary>
+        public static readonly Vector3Float back = new Vector3Float(0, -1, 0);
+        /// <summary>
+        /// A vector with values (0, 0, 1)
+        /// </summary>
+        public static readonly Vector3Float forward = new Vector3Float(0, 1, 0);
+
+        /// @brief The vector length
+        /// @return The vector length
+        public readonly float magnitude => MathF.Sqrt(horizontal * horizontal + vertical * vertical + depth * depth);
+        /// <summary>
+        /// The vector length
+        /// </summary>
+        /// <param name="v">The vector for which you need the length</param>
+        /// <returns>The vector length</returns>
+        public static float MagnitudeOf(Vector3Float v) {
+            return v.magnitude;
+        }
+
+        /// @brief The squared vector length
+        /// @return The squared vector length
+        /// @remark The squared length is computationally simpler than the real
+        /// length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the
+        /// calculation of the squared root of C.
+        public readonly float sqrMagnitude => (horizontal * horizontal + vertical * vertical + depth * depth);
+
+        /// <summary>
+        /// The squared vector length
+        /// </summary>
+        /// <param name="v">The vector for which you need the squared length</param>
+        /// <returns>The squared vector length</returns>
+        /// <remarks>The squared length is computationally simpler than the real
+        /// length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the
+        /// calculation of the squared root of C.</remarks>
+        public static float SqrMagnitudeOf(Vector3Float v) {
+            return v.sqrMagnitude;
+        }
+
+        /// @brief Convert the vector to a length of 1
+        /// @return The vector normalized to a length of 1
+        public readonly Vector3Float normalized {
+            get {
+                float l = magnitude;
+                Vector3Float v = zero;
+                if (l > Float.epsilon)
+                    v = this / l;
+                return v;
+            }
+        }
+        /// @brief Convert the vector to a length of 1
+        /// @param v The vector to convert
+        /// @return The vector normalized to a length of 1
+        public static Vector3Float Normalize(Vector3Float v) {
+            float num = v.magnitude;
+            Vector3Float result = zero;
+            if (num > Float.epsilon)
+                result = v / num;
+
+            return result;
+        }
+
+        /// <summary>
+        /// Negate te vector such that it points in the opposite direction
+        /// </summary>
+        /// <param name="v1"></param>
+        /// <returns>The negated vector</returns>
+        public static Vector3Float operator -(Vector3Float v1) {
+            Vector3Float v = new(-v1.horizontal, -v1.vertical, -v1.depth);
+            return v;
+        }
+
+        /// <summary>
+        /// Subtract two vectors
+        /// </summary>
+        /// <param name="v1"></param>
+        /// <param name="v2"></param>
+        /// <returns>The result of the subtraction</returns>
+        public static Vector3Float operator -(Vector3Float v1, Vector3Float v2) {
+            Vector3Float v = new(v1.horizontal - v2.horizontal, v1.vertical - v2.vertical, v1.depth - v2.depth);
+            return v;
+        }
+
+        /// <summary>
+        /// Add two vectors
+        /// </summary>
+        /// <param name="v1"></param>
+        /// <param name="v2"></param>
+        /// <returns>The result of the addition</returns>
+        public static Vector3Float operator +(Vector3Float v1, Vector3Float v2) {
+            Vector3Float v = new(v1.horizontal + v2.horizontal, v1.vertical + v2.vertical, v1.depth + v2.depth);
+            return v;
+        }
+
+        /// @brief Scale the vector using another vector
+        /// @param v1 The vector to scale
+        /// @param v2 A vector with the scaling factors
+        /// @return The scaled vector
+        /// @remark Each component of the vector v1 will be multiplied with the
+        /// matching component from the scaling vector v2.
+        public static Vector3Float Scale(Vector3Float v1, Vector3Float v2) {
+            return new Vector3Float(v1.horizontal * v2.horizontal, v1.vertical * v2.vertical, v1.depth * v2.depth);
+        }
+
+
+        public static Vector3Float operator *(Vector3Float v1, float d) {
+            Vector3Float v = new Vector3Float(v1.horizontal * d, v1.vertical * d, v1.depth * d);
+            return v;
+        }
+
+        public static Vector3Float operator *(float d, Vector3Float v1) {
+            Vector3Float v = new Vector3Float(d * v1.horizontal, d * v1.vertical, d * v1.depth);
+            return v;
+        }
+
+        public static Vector3Float operator /(Vector3Float v1, float d) {
+            Vector3Float v = new Vector3Float(v1.horizontal / d, v1.vertical / d, v1.depth / d);
+            return v;
+        }
+
+        /*
+        public bool Equals(Vector3Float v) => (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth);
+
+        public override bool Equals(object obj) {
+            if (!(obj is Vector3Float v))
+                return false;
+
+            return (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth);
+        }
+        */
+
+        public static bool operator ==(Vector3Float v1, Vector3Float v2) {
+            return (v1.horizontal == v2.horizontal && v1.vertical == v2.vertical && v1.depth == v2.depth);
+        }
+
+        public static bool operator !=(Vector3Float v1, Vector3Float v2) {
+            return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical || v1.depth != v2.depth);
+        }
+
+        // public override int GetHashCode() {
+        //     return (horizontal, vertical, depth).GetHashCode();
+        // }
+
+        /// @brief The distance between two vectors
+        /// @param v1 The first vector
+        /// @param v2 The second vector
+        /// @return The distance between the two vectors
+        public static float Distance(Vector3Float v1, Vector3Float v2) {
+            return (v2 - v1).magnitude;
+        }
+
+        /// @brief The dot product of two vectors
+        /// @param v1 The first vector
+        /// @param v2 The second vector
+        /// @return The dot product of the two vectors
+        public static float Dot(Vector3Float v1, Vector3Float v2) {
+            return v1.horizontal * v2.horizontal + v1.vertical * v2.vertical + v1.depth * v2.depth;
+        }
+
+        /// @brief The cross product of two vectors
+        /// @param v1 The first vector
+        /// @param v2 The second vector
+        /// @return The cross product of the two vectors
+        public static Vector3Float Cross(Vector3Float v1, Vector3Float v2) {
+            return new Vector3Float(v1.vertical * v2.depth - v1.depth * v2.vertical, v1.depth * v2.horizontal - v1.horizontal * v2.depth,
+                           v1.horizontal * v2.vertical - v1.vertical * v2.horizontal);
+
+        }
+
+        /// @brief Project the vector on another vector
+        /// @param v The vector to project
+        /// @param n The normal vecto to project on
+        /// @return The projected vector
+        public static Vector3Float Project(Vector3Float v, Vector3Float n) {
+            float sqrMagnitude = Dot(n, n);
+            if (sqrMagnitude < Float.epsilon)
+                return zero;
+            else {
+                float dot = Dot(v, n);
+                Vector3Float r = n * dot / sqrMagnitude;
+                return r;
+            }
+        }
+
+        /// @brief Project the vector on a plane defined by a normal orthogonal to the
+        /// plane.
+        /// @param v The vector to project
+        /// @param n The normal of the plane to project on
+        /// @return Teh projected vector
+        public static Vector3Float ProjectOnPlane(Vector3Float v, Vector3Float n) {
+            Vector3Float r = v - Project(v, n);
+            return r;
+        }
+
+        /// @brief The angle between two vectors
+        /// @param v1 The first vector
+        /// @param v2 The second vector
+        /// @return The angle between the two vectors
+        /// @remark This reterns an unsigned angle which is the shortest distance
+        /// between the two vectors. Use Vector3::SignedAngle if a signed angle is
+        /// needed.
+        public static AngleFloat UnsignedAngle(Vector3Float v1, Vector3Float v2) {
+            float denominator = MathF.Sqrt(v1.sqrMagnitude * v2.sqrMagnitude);
+            if (denominator < Float.epsilon)
+                return AngleFloat.zero;
+
+            float dot = Dot(v1, v2);
+            float fraction = dot / denominator;
+            if (float.IsNaN(fraction))
+                return AngleFloat.Degrees(
+                    fraction); // short cut to returning NaN universally
+
+            float cdot = Float.Clamp(fraction, -1.0f, 1.0f);
+            float r = MathF.Acos(cdot);
+            return AngleFloat.Radians(r);
+        }
+        /// @brief The signed angle between two vectors
+        /// @param v1 The starting vector
+        /// @param v2 The ending vector
+        /// @param axis The axis to rotate around
+        /// @return The signed angle between the two vectors
+        public static AngleFloat SignedAngle(Vector3Float v1, Vector3Float v2,
+                                          Vector3Float axis) {
+            // angle in [0,180]
+            AngleFloat angle = UnsignedAngle(v1, v2);
+
+            Vector3Float cross = Cross(v1, v2);
+            float b = Dot(axis, cross);
+            float signd = b < 0 ? -1.0F : (b > 0 ? 1.0F : 0.0F);
+
+            // angle in [-179,180]
+            AngleFloat signed_angle = angle * signd;
+
+            return signed_angle;
+        }
+
+
+        /// @brief Lerp (linear interpolation) between two vectors
+        /// @param v1 The starting vector
+        /// @param v2 The ending vector
+        /// @param f The interpolation distance
+        /// @return The lerped vector
+        /// @remark The factor f is unclamped. Value 0 matches the vector *v1*, Value
+        /// 1 matches vector *v2*. Value -1 is vector *v1* minus the difference
+        /// between *v1* and *v2* etc.
+        public static Vector3Float Lerp(Vector3Float v1, Vector3Float v2, float f) {
+            Vector3Float v = v1 + (v2 - v1) * f;
+            return v;
+        }
+
+    }
+}
+//#endif

src/Vector3Int.cs

@@ -0,0 +1,273 @@
+//#if !UNITY_5_3_OR_NEWER
+using System;
+
+namespace LinearAlgebra {
+
+    /// <summary>
+    /// 3-dimensional vectors
+    /// </summary>
+    /// This uses the right-handed coordinate system.
+    /// <remarks>
+    /// Create a new 3-dimensional vector
+    /// </remarks>
+    /// <param name="horizontal">x axis value</param>
+    /// <param name="vertical">y axis value</param>
+    /// <param name="depth">z axis value</param>
+    public struct Vector3Int {
+
+        /// <summary>
+        /// The right axis of the vector
+        /// </summary>
+        public int horizontal; //> left/right
+        /// <summary>
+        /// The upward axis of the vector
+        /// </summary>
+        public int vertical; //> up/down
+        /// <summary>
+        /// The forward axis of the vector
+        /// </summary>
+        public int depth; //> forward/backward
+
+        public Vector3Int(int horizontal, int vertical, int depth) {
+            this.horizontal = horizontal;
+            this.vertical = vertical;
+            this.depth = depth;
+        }
+
+        /// <summary>
+        /// A vector with zero for all axis
+        /// </summary>
+        public static readonly Vector3Int zero = new(0, 0, 0);
+        /// <summary>
+        /// A vector with one for all axis
+        /// </summary>
+        public static readonly Vector3Int one = new(1, 1, 1);
+        /// <summary>
+        /// A Vector3Int with values (-1, 0, 0)
+        /// </summary>
+        public static readonly Vector3Int left = new(-1, 0, 0);
+        /// <summary>
+        /// A vector with values (1, 0, 0)
+        /// </summary>
+        public static readonly Vector3Int right = new(1, 0, 0);
+        /// <summary>
+        /// A vector with values (0, -1, 0)
+        /// </summary>
+        public static readonly Vector3Int down = new(0, -1, 0);
+        /// <summary>
+        /// A vector with values (0, 1, 0)
+        /// </summary>
+        public static readonly Vector3Int up = new(0, 1, 0);
+        /// <summary>
+        /// A vector with values (0, 0, -1)
+        /// </summary>
+        public static readonly Vector3Int back = new(0, -1, 0);
+        /// <summary>
+        /// A vector with values (0, 0, 1)
+        /// </summary>
+        public static readonly Vector3Int forward = new(0, 1, 0);
+
+        /// @brief The vector length
+        /// @return The vector length
+        public readonly float magnitude => MathF.Sqrt(horizontal * horizontal + vertical * vertical + depth * depth);
+        /// <summary>
+        /// The vector length
+        /// </summary>
+        /// <param name="v">The vector for which you need the length</param>
+        /// <returns>The vector length</returns>
+        public static float MagnitudeOf(Vector3Int v) {
+            return v.magnitude;
+        }
+
+        /// @brief The squared vector length
+        /// @return The squared vector length
+        /// @remark The squared length is computationally simpler than the real
+        /// length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the
+        /// calculation of the squared root of C.
+        public readonly float sqrMagnitude => (horizontal * horizontal + vertical * vertical + depth * depth);
+
+        /// <summary>
+        /// The squared vector length
+        /// </summary>
+        /// <param name="v">The vector for which you need the squared length</param>
+        /// <returns>The squared vector length</returns>
+        /// <remarks>The squared length is computationally simpler than the real
+        /// length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the
+        /// calculation of the squared root of C.</remarks>
+        public static float SqrMagnitudeOf(Vector3Int v) {
+            return v.sqrMagnitude;
+        }
+
+        /// @brief Convert the vector to a length of 1
+        /// @return The vector normalized to a length of 1
+        public readonly Vector3Float normalized {
+            get {
+                float l = magnitude;
+                Vector3Float v = Vector3Float.zero;
+                if (l > Float.epsilon)
+                    v = new Vector3Float(this) / l;
+                return v;
+            }
+        }
+        /// @brief Convert the vector to a length of 1
+        /// @param v The vector to convert
+        /// @return The vector normalized to a length of 1
+        public static Vector3Float Normalize(Vector3Int v) {
+            float num = v.magnitude;
+            Vector3Float result = Vector3Float.zero;
+            if (num > Float.epsilon)
+                result = new Vector3Float(v) / num;
+
+            return result;
+        }
+
+        /// <summary>
+        /// Negate te vector such that it points in the opposite direction
+        /// </summary>
+        /// <param name="v1"></param>
+        /// <returns>The negated vector</returns>
+        public static Vector3Int operator -(Vector3Int v1) {
+            Vector3Int v = new(-v1.horizontal, -v1.vertical, -v1.depth);
+            return v;
+        }
+
+        /// <summary>
+        /// Subtract two vectors
+        /// </summary>
+        /// <param name="v1"></param>
+        /// <param name="v2"></param>
+        /// <returns>The result of the subtraction</returns>
+        public static Vector3Int operator -(Vector3Int v1, Vector3Int v2) {
+            Vector3Int v = new(v1.horizontal - v2.horizontal, v1.vertical - v2.vertical, v1.depth - v2.depth);
+            return v;
+        }
+
+        /// <summary>
+        /// Add two vectors
+        /// </summary>
+        /// <param name="v1"></param>
+        /// <param name="v2"></param>
+        /// <returns>The result of the addition</returns>
+        public static Vector3Int operator +(Vector3Int v1, Vector3Int v2) {
+            Vector3Int v = new(v1.horizontal + v2.horizontal, v1.vertical + v2.vertical, v1.depth + v2.depth);
+            return v;
+        }
+
+        /// @brief Scale the vector using another vector
+        /// @param v1 The vector to scale
+        /// @param v2 A vector with the scaling factors
+        /// @return The scaled vector
+        /// @remark Each component of the vector v1 will be multiplied with the
+        /// matching component from the scaling vector v2.
+        public static Vector3Int Scale(Vector3Int v1, Vector3Int v2) {
+            return new Vector3Int(v1.horizontal * v2.horizontal, v1.vertical * v2.vertical, v1.depth * v2.depth);
+        }
+
+
+        public static Vector3Int operator *(Vector3Int v1, int d) {
+            Vector3Int v = new(v1.horizontal * d, v1.vertical * d, v1.depth * d);
+            return v;
+        }
+
+        public static Vector3Int operator *(int d, Vector3Int v1) {
+            Vector3Int v = new(d * v1.horizontal, d * v1.vertical, d * v1.depth);
+            return v;
+        }
+
+        public static Vector3Int operator /(Vector3Int v1, int d) {
+            Vector3Int v = new(v1.horizontal / d, v1.vertical / d, v1.depth / d);
+            return v;
+        }
+
+        public bool Equals(Vector3Int v) => (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth);
+
+        public override bool Equals(object obj) {
+            if (!(obj is Vector3Int v))
+                return false;
+
+            return (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth);
+        }
+        
+        public static bool operator ==(Vector3Int v1, Vector3Int v2) {
+            return (v1.horizontal == v2.horizontal && v1.vertical == v2.vertical && v1.depth == v2.depth);
+        }
+
+        public static bool operator !=(Vector3Int v1, Vector3Int v2) {
+            return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical || v1.depth != v2.depth);
+        }
+
+        public override int GetHashCode() {
+            return (horizontal, vertical, depth).GetHashCode();
+        }
+
+        /// @brief The distance between two vectors
+        /// @param v1 The first vector
+        /// @param v2 The second vector
+        /// @return The distance between the two vectors
+        public static float Distance(Vector3Int v1, Vector3Int v2) {
+            return (v2 - v1).magnitude;
+        }
+
+        /// @brief The dot product of two vectors
+        /// @param v1 The first vector
+        /// @param v2 The second vector
+        /// @return The dot product of the two vectors
+        public static float Dot(Vector3Int v1, Vector3Int v2) {
+            return v1.horizontal * v2.horizontal + v1.vertical * v2.vertical + v1.depth * v2.depth;
+        }
+
+        /// @brief The cross product of two vectors
+        /// @param v1 The first vector
+        /// @param v2 The second vector
+        /// @return The cross product of the two vectors
+        public static Vector3Int Cross(Vector3Int v1, Vector3Int v2) {
+            return new Vector3Int(v1.vertical * v2.depth - v1.depth * v2.vertical, v1.depth * v2.horizontal - v1.horizontal * v2.depth,
+                           v1.horizontal * v2.vertical - v1.vertical * v2.horizontal);
+
+        }
+
+        /// @brief The angle between two vectors
+        /// @param v1 The first vector
+        /// @param v2 The second vector
+        /// @return The angle between the two vectors
+        /// @remark This reterns an unsigned angle which is the shortest distance
+        /// between the two vectors. Use Vector3::SignedAngle if a signed angle is
+        /// needed.
+        public static AngleFloat UnsignedAngle(Vector3Int v1, Vector3Int v2) {
+            float denominator = MathF.Sqrt(v1.sqrMagnitude * v2.sqrMagnitude);
+            if (denominator < Float.epsilon)
+                return AngleFloat.zero;
+
+            float dot = Dot(v1, v2);
+            float fraction = dot / denominator;
+            if (float.IsNaN(fraction))
+                return AngleFloat.Degrees(
+                    fraction); // short cut to returning NaN universally
+
+            float cdot = Float.Clamp(fraction, -1.0f, 1.0f);
+            float r = MathF.Acos(cdot);
+            return AngleFloat.Radians(r);
+        }
+        /// @brief The signed angle between two vectors
+        /// @param v1 The starting vector
+        /// @param v2 The ending vector
+        /// @param axis The axis to rotate around
+        /// @return The signed angle between the two vectors
+        public static AngleFloat SignedAngle(Vector3Int v1, Vector3Int v2,
+                                          Vector3Int axis) {
+            // angle in [0,180]
+            AngleFloat angle = UnsignedAngle(v1, v2);
+
+            Vector3Int cross = Cross(v1, v2);
+            float b = Dot(axis, cross);
+            float signd = b < 0 ? -1.0F : (b > 0 ? 1.0F : 0.0F);
+
+            // angle in [-179,180]
+            AngleFloat signed_angle = angle * signd;
+
+            return signed_angle;
+        }
+
+    }
+}
+//#endif

src/float16.cs

@@ -0,0 +1,322 @@
+using System;
+
+namespace LinearAlgebra {
+
+    public class float16 {
+        //
+        //    FILE: float16.cpp
+        //  AUTHOR: Rob Tillaart
+        // VERSION: 0.1.8
+        // PURPOSE: library for Float16s for Arduino
+        //     URL: http://en.wikipedia.org/wiki/Half-precision_floating-point_format
+
+        ushort _value;
+
+        public float16() { _value = 0; }
+
+        public float16(float f) {
+            //_value = f32tof16(f);
+            _value = F32ToF16__(f);
+        }
+
+        public float toFloat() {
+            return f16tof32(_value);
+        }
+
+        public ushort GetBinary() { return _value; }
+        public void SetBinary(ushort value) { _value = value; }
+
+        //////////////////////////////////////////////////////////
+        //
+        //  EQUALITIES
+        //
+        /*
+        bool float16::operator ==(const float16 &f) { return (_value == f._value); }
+
+    bool float16::operator !=(const float16 &f) { return (_value != f._value); }
+
+    bool float16::operator >(const float16 &f) {
+        if ((_value & 0x8000) && (f._value & 0x8000))
+            return _value < f._value;
+        if (_value & 0x8000)
+            return false;
+        if (f._value & 0x8000)
+            return true;
+        return _value > f._value;
+    }
+
+    bool float16::operator >=(const float16 &f) {
+        if ((_value & 0x8000) && (f._value & 0x8000))
+            return _value <= f._value;
+        if (_value & 0x8000)
+            return false;
+        if (f._value & 0x8000)
+            return true;
+        return _value >= f._value;
+    }
+
+    bool float16::operator <(const float16 &f) {
+        if ((_value & 0x8000) && (f._value & 0x8000))
+            return _value > f._value;
+        if (_value & 0x8000)
+            return true;
+        if (f._value & 0x8000)
+            return false;
+        return _value < f._value;
+    }
+
+    bool float16::operator <=(const float16 &f) {
+        if ((_value & 0x8000) && (f._value & 0x8000))
+            return _value >= f._value;
+        if (_value & 0x8000)
+            return true;
+        if (f._value & 0x8000)
+            return false;
+        return _value <= f._value;
+    }
+
+    //////////////////////////////////////////////////////////
+    //
+    //  NEGATION
+    //
+    float16 float16::operator -() {
+        float16 f16;
+        f16.setBinary(_value ^ 0x8000);
+        return f16;
+    }
+
+    //////////////////////////////////////////////////////////
+    //
+    //  MATH
+    //
+    float16 float16::operator +(const float16 &f) {
+        return float16(this->toDouble() + f.toDouble());
+    }
+
+    float16 float16::operator -(const float16 &f) {
+        return float16(this->toDouble() - f.toDouble());
+    }
+
+    float16 float16::operator *(const float16 &f) {
+        return float16(this->toDouble() * f.toDouble());
+    }
+
+    float16 float16::operator /(const float16 &f) {
+        return float16(this->toDouble() / f.toDouble());
+    }
+
+    float16 & float16::operator+=(const float16 &f) {
+        *this = this->toDouble() + f.toDouble();
+        return *this;
+    }
+
+    float16 & float16::operator-=(const float16 &f) {
+        *this = this->toDouble() - f.toDouble();
+        return *this;
+    }
+
+    float16 & float16::operator*=(const float16 &f) {
+        *this = this->toDouble() * f.toDouble();
+        return *this;
+    }
+
+    float16 & float16::operator/=(const float16 &f) {
+        *this = this->toDouble() / f.toDouble();
+        return *this;
+    }
+
+    //////////////////////////////////////////////////////////
+    //
+    //  MATH HELPER FUNCTIONS
+    //
+    int float16::sign() {
+        if (_value & 0x8000)
+            return -1;
+        if (_value & 0xFFFF)
+            return 1;
+        return 0;
+    }
+
+    bool float16::isZero() { return ((_value & 0x7FFF) == 0x0000); }
+
+    bool float16::isNaN() {
+        if ((_value & 0x7C00) != 0x7C00)
+            return false;
+        if ((_value & 0x03FF) == 0x0000)
+            return false;
+        return true;
+    }
+
+    bool float16::isInf() { return ((_value == 0x7C00) || (_value == 0xFC00)); }
+        */
+        //////////////////////////////////////////////////////////
+        //
+        //  CORE CONVERSION
+        //
+        float f16tof32(ushort _value) {
+            //ushort sgn;
+            ushort man;
+            int exp;
+            float f;
+
+            //Debug.Log($"{_value}");
+
+            bool sgn = (_value & 0x8000) > 0;
+            exp = (_value & 0x7C00) >> 10;
+            man = (ushort)(_value & 0x03FF);
+
+            //Debug.Log($"{sgn} {exp} {man}");
+
+            //  ZERO
+            if ((_value & 0x7FFF) == 0) {
+                return sgn ? -0 : 0;
+            }
+            //  NAN & INF
+            if (exp == 0x001F) {
+                if (man == 0)
+                    return sgn ? float.NegativeInfinity : float.PositiveInfinity; //-INFINITY : INFINITY;
+                else
+                    return float.NaN; // NAN;
+            }
+
+            //  SUBNORMAL/NORMAL
+            if (exp == 0)
+                f = 0;
+            else
+                f = 1;
+
+            //  PROCESS MANTISSE
+            for (int i = 9; i >= 0; i--) {
+                f *= 2;
+                if ((man & (1 << i)) != 0)
+                    f = f + 1;
+            }
+            //Debug.Log($"{f}");
+            f = f * (float)Math.Pow(2.0f, exp - 25);
+            if (exp == 0) {
+                f = f * (float)Math.Pow(2.0f, -13); // 5.96046447754e-8;
+            }
+            //Debug.Log($"{f}");
+            return sgn ? -f : f;
+        }
+
+        public static uint SingleToInt32Bits(float value) {
+            byte[] bytes = BitConverter.GetBytes(value);
+            if (BitConverter.IsLittleEndian)
+                Array.Reverse(bytes); // If the system is little-endian, reverse the byte order
+            return BitConverter.ToUInt32(bytes, 0);
+        }
+
+        public ushort F32ToF16__(float f) {
+            uint t = BitConverter.ToUInt32(BitConverter.GetBytes(f), 0);
+            ushort man = (ushort)((t & 0x007FFFFF) >> 12);
+            int exp = (int)((t & 0x7F800000) >> 23);
+            bool sgn = (t & 0x80000000) != 0;
+
+            // handle 0
+            if ((t & 0x7FFFFFFF) == 0) {
+                return sgn ? (ushort)0x8000 : (ushort)0x0000;
+            }
+            // denormalized float32 does not fit in float16
+            if (exp == 0x00) {
+                return sgn ? (ushort)0x8000 : (ushort)0x0000;
+            }
+            // handle infinity & NAN
+            if (exp == 0x00FF) {
+                if (man != 0)
+                    return 0xFE00;              // NAN
+                return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF
+            }
+
+            // normal numbers
+            exp = exp - 127 + 15;
+            // overflow does not fit => INF
+            if (exp > 30) {
+                return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF
+            }
+            // subnormal numbers
+            if (exp < -38) {
+                return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ?
+            }
+            if (exp <= 0) // subnormal
+            {
+                man >>= (exp + 14);
+                // rounding
+                man++;
+                man >>= 1;
+                if (sgn)
+                    return (ushort)(0x8000 | man);
+                return man;
+            }
+
+            // normal
+            // TODO rounding
+            exp <<= 10;
+            man++;
+            man >>= 1;
+            if (sgn)
+                return (ushort)(0x8000 | exp | man);
+            return (ushort)(exp | man);
+        }
+
+        //This function is faulty!!!!
+        ushort f32tof16(float f) {
+            //uint t = *(uint*)&f;
+            //uint t = (uint)BitConverter.SingleToInt32Bits(f);
+            uint t = SingleToInt32Bits(f);
+            //  man bits = 10; but we keep 11 for rounding
+            ushort man = (ushort)((t & 0x007FFFFF) >> 12);
+            short exp = (short)((t & 0x7F800000) >> 23);
+            bool sgn = (t & 0x80000000) != 0;
+
+            //  handle 0
+            if ((t & 0x7FFFFFFF) == 0) {
+                return sgn ? (ushort)0x8000 : (ushort)0x0000;
+            }
+            //  denormalized float32 does not fit in float16
+            if (exp == 0x00) {
+                return sgn ? (ushort)0x8000 : (ushort)0x0000;
+            }
+            //  handle infinity & NAN
+            if (exp == 0x00FF) {
+                if (man != 0)
+                    return 0xFE00;              //  NAN
+                return sgn ? (ushort)0xFC00 : (ushort)0x7C00; //  -INF : INF
+            }
+
+            //  normal numbers
+            exp = (short)(exp - 127 + 15);
+            //  overflow does not fit => INF
+            if (exp > 30) {
+                return sgn ? (ushort)0xFC00 : (ushort)0x7C00; //  -INF : INF
+            }
+            //  subnormal numbers
+            if (exp < -38) {
+                return sgn ? (ushort)0x8000 : (ushort)0x0000; //  -0 or 0  ?   just 0 ?
+            }
+            if (exp <= 0) //  subnormal
+            {
+                man >>= (exp + 14);
+                //  rounding
+                man++;
+                man >>= 1;
+                if (sgn)
+                    return (ushort)(0x8000 | man);
+                return man;
+            }
+
+            //  normal
+            //  TODO rounding
+            exp <<= 10;
+            man++;
+            man >>= 1;
+            ushort uexp = (ushort)exp;
+            if (sgn)
+                return (ushort)(0x8000 | uexp | man);
+            return (ushort)(uexp | man);
+        }
+
+        //  -- END OF FILE --
+    }
+
+}

test/AngleTest.cs

@@ -0,0 +1,497 @@
+#if !UNITY_5_6_OR_NEWER
+using System;
+using System.Formats.Asn1;
+using NUnit.Framework;
+
+namespace LinearAlgebra.Test {
+    public class AngleTests {
+        [SetUp]
+        public void Setup() {
+        }
+
+        [Test]
+        public void Construct() {
+            // Degrees
+            float angle = 0.0f;
+            AngleFloat a = AngleFloat.Degrees(angle);
+            Assert.AreEqual(angle, a.inDegrees);
+
+            angle = -180.0f;
+            a = AngleFloat.Degrees(angle);
+            Assert.AreEqual(angle, a.inDegrees);
+
+            angle = 270.0f;
+            a = AngleFloat.Degrees(angle);
+            Assert.AreEqual(-90, a.inDegrees);
+
+            // Radians
+            angle = 0.0f;
+            a = AngleFloat.Radians(angle);
+            Assert.AreEqual(angle, a.inRadians);
+
+            angle = (float)-Math.PI;
+            a = AngleFloat.Radians(angle);
+            Assert.AreEqual(angle, a.inRadians);
+
+            angle = (float)Math.PI * 1.5f;
+            a = AngleFloat.Radians(angle);
+            Assert.AreEqual(-Math.PI * 0.5f, a.inRadians, 1.0E-05F);
+
+            // Revolutions
+            angle = 0.0f;
+            a = AngleFloat.Revolutions(angle);
+            Assert.AreEqual(angle, a.inRevolutions);
+
+            angle = -0.5f;
+            a = AngleFloat.Revolutions(angle);
+            Assert.AreEqual(angle, a.inRevolutions);
+
+            angle = 0.75f;
+            a = AngleFloat.Revolutions(angle);
+            Assert.AreEqual(-0.25f, a.inRevolutions);
+
+        }
+
+        [Test]
+        public void Revolutions() {
+            AngleFloat a;
+
+            // Test zero
+            a = AngleFloat.Revolutions(0.0f);
+            Assert.AreEqual(0.0f, a.inRevolutions);
+
+            // Test positive values within range
+            a = AngleFloat.Revolutions(0.25f);
+            Assert.AreEqual(0.25f, a.inRevolutions);
+
+            a = AngleFloat.Revolutions(0.5f);
+            Assert.AreEqual(-0.5f, a.inRevolutions);
+
+            // Test negative values within range
+            a = AngleFloat.Revolutions(-0.25f);
+            Assert.AreEqual(-0.25f, a.inRevolutions);
+
+            a = AngleFloat.Revolutions(-0.5f);
+            Assert.AreEqual(-0.5f, a.inRevolutions);
+
+            // Test values outside range (positive)
+            a = AngleFloat.Revolutions(1.0f);
+            Assert.AreEqual(0.0f, a.inRevolutions);
+
+            a = AngleFloat.Revolutions(1.25f);
+            Assert.AreEqual(0.25f, a.inRevolutions);
+
+            a = AngleFloat.Revolutions(1.75f);
+            Assert.AreEqual(-0.25f, a.inRevolutions);
+
+            // Test values outside range (negative)
+            a = AngleFloat.Revolutions(-1.0f);
+            Assert.AreEqual(0.0f, a.inRevolutions);
+
+            a = AngleFloat.Revolutions(-1.25f);
+            Assert.AreEqual(-0.25f, a.inRevolutions);
+
+            a = AngleFloat.Revolutions(-1.75f);
+            Assert.AreEqual(0.25f, a.inRevolutions);
+
+            // Test infinity
+            a = AngleFloat.Revolutions(float.PositiveInfinity);
+            Assert.AreEqual(float.PositiveInfinity, a.inRevolutions);
+
+            a = AngleFloat.Revolutions(float.NegativeInfinity);
+            Assert.AreEqual(float.NegativeInfinity, a.inRevolutions);
+        }
+
+        [Test]
+        public void Equality() {
+            // Test equality operator
+            Assert.IsTrue(AngleFloat.Degrees(90) == AngleFloat.Degrees(90), "90 == 90");
+            Assert.IsFalse(AngleFloat.Degrees(90) == AngleFloat.Degrees(45), "90 == 45");
+            Assert.IsTrue(AngleFloat.Degrees(0) == AngleFloat.Degrees(0), "0 == 0");
+            Assert.IsTrue(AngleFloat.Degrees(-180) == AngleFloat.Degrees(-180), "-180 == -180");
+
+            // Test inequality operator
+            Assert.IsTrue(AngleFloat.Degrees(90) != AngleFloat.Degrees(45), "90 != 45");
+            Assert.IsFalse(AngleFloat.Degrees(90) != AngleFloat.Degrees(90), "90 != 90");
+            Assert.IsTrue(AngleFloat.Degrees(0) != AngleFloat.Degrees(1), "0 != 1");
+
+            // Test greater than operator
+            Assert.IsTrue(AngleFloat.Degrees(90) > AngleFloat.Degrees(45), "90 > 45");
+            Assert.IsFalse(AngleFloat.Degrees(45) > AngleFloat.Degrees(90), "45 > 90");
+            Assert.IsFalse(AngleFloat.Degrees(90) > AngleFloat.Degrees(90), "90 > 90");
+
+            // Test greater than or equal operator
+            Assert.IsTrue(AngleFloat.Degrees(90) >= AngleFloat.Degrees(45), "90 >= 45");
+            Assert.IsTrue(AngleFloat.Degrees(90) >= AngleFloat.Degrees(90), "90 >= 90");
+            Assert.IsFalse(AngleFloat.Degrees(45) >= AngleFloat.Degrees(90), "45 >= 90");
+
+            // Test less than operator
+            Assert.IsTrue(AngleFloat.Degrees(45) < AngleFloat.Degrees(90), "45 < 90");
+            Assert.IsFalse(AngleFloat.Degrees(90) < AngleFloat.Degrees(45), "90 < 45");
+            Assert.IsFalse(AngleFloat.Degrees(90) < AngleFloat.Degrees(90), "90 < 90");
+
+            // Test less than or equal operator
+            Assert.IsTrue(AngleFloat.Degrees(45) <= AngleFloat.Degrees(90), "45 <= 90");
+            Assert.IsTrue(AngleFloat.Degrees(90) <= AngleFloat.Degrees(90), "90 <= 90");
+            Assert.IsFalse(AngleFloat.Degrees(90) <= AngleFloat.Degrees(45), "90 <= 45");
+        }
+
+        // [Test]
+        // public void Normalize() {
+        //     float r = 0;
+
+        //     r = Angle.Normalize(90);
+        //     Assert.AreEqual(r, 90, "Normalize 90");
+
+        //     r = Angle.Normalize(-90);
+        //     Assert.AreEqual(r, -90, "Normalize -90");
+
+        //     r = Angle.Normalize(270);
+        //     Assert.AreEqual(r, -90, "Normalize 270");
+
+        //     r = Angle.Normalize(270 + 360);
+        //     Assert.AreEqual(r, -90, "Normalize 270+360");
+
+        //     r = Angle.Normalize(-270);
+        //     Assert.AreEqual(r, 90, "Normalize -270");
+
+        //     r = Angle.Normalize(-270 - 360);
+        //     Assert.AreEqual(r, 90, "Normalize -270-360");
+
+        //     r = Angle.Normalize(0);
+        //     Assert.AreEqual(r, 0, "Normalize 0");
+
+        //     r = Angle.Normalize(float.PositiveInfinity);
+        //     Assert.AreEqual(r, float.PositiveInfinity, "Normalize INFINITY");
+
+        //     r = Angle.Normalize(float.NegativeInfinity);
+        //     Assert.AreEqual(r, float.NegativeInfinity, "Normalize INFINITY");
+        // }
+
+        [Test]
+        public void Clamp() {
+            float r = 0;
+
+            r = AngleFloat.Clamp(AngleFloat.Degrees(1), AngleFloat.Degrees(0), AngleFloat.Degrees(2));
+            Assert.AreEqual(1, r, "Clamp 1 0 2");
+
+            r = AngleFloat.Clamp(AngleFloat.Degrees(-1), AngleFloat.Degrees(0), AngleFloat.Degrees(2));
+            Assert.AreEqual(0, r, "Clamp -1 0 2");
+
+            r = AngleFloat.Clamp(AngleFloat.Degrees(3), AngleFloat.Degrees(0), AngleFloat.Degrees(2));
+            Assert.AreEqual(2, r, "Clamp 3 0 2");
+
+            r = AngleFloat.Clamp(AngleFloat.Degrees(1), AngleFloat.Degrees(0), AngleFloat.Degrees(0));
+            Assert.AreEqual(0, r, "Clamp 1 0 0");
+
+            r = AngleFloat.Clamp(AngleFloat.Degrees(0), AngleFloat.Degrees(0), AngleFloat.Degrees(0));
+            Assert.AreEqual(0, r, "Clamp 0 0 0");
+
+            r = AngleFloat.Clamp(AngleFloat.Degrees(0), AngleFloat.Degrees(1), AngleFloat.Degrees(-1));
+            Assert.AreEqual(1, r, "Clamp 0 1 -1");
+
+            r = AngleFloat.Clamp(AngleFloat.Degrees(1), AngleFloat.Degrees(0), AngleFloat.Degrees(float.PositiveInfinity));
+            Assert.AreEqual(1, r, "Clamp 1 0 INFINITY");
+
+            r = AngleFloat.Clamp(AngleFloat.Degrees(1), AngleFloat.Degrees(float.NegativeInfinity), AngleFloat.Degrees(1));
+            Assert.AreEqual(1, r, "Clamp 1 -INFINITY 1");
+        }
+
+        [Test]
+        public void Cos() {
+            // Test zero
+            Assert.AreEqual(1.0f, AngleFloat.Cos(AngleFloat.Degrees(0)), 1.0E-05F, "Cos(0°)");
+
+            // Test 90 degrees
+            Assert.AreEqual(0.0f, AngleFloat.Cos(AngleFloat.Degrees(90)), 1.0E-05F, "Cos(90°)");
+
+            // Test 180 degrees
+            Assert.AreEqual(-1.0f, AngleFloat.Cos(AngleFloat.Degrees(180)), 1.0E-05F, "Cos(180°)");
+
+            // Test 270 degrees
+            Assert.AreEqual(0.0f, AngleFloat.Cos(AngleFloat.Degrees(270)), 1.0E-05F, "Cos(270°)");
+
+            // Test 45 degrees
+            Assert.AreEqual(MathF.Sqrt(2) / 2, AngleFloat.Cos(AngleFloat.Degrees(45)), 1.0E-05F, "Cos(45°)");
+
+            // Test negative angle
+            Assert.AreEqual(1.0f, AngleFloat.Cos(AngleFloat.Degrees(-360)), 1.0E-05F, "Cos(-360°)");
+
+            // Test using radians
+            Assert.AreEqual(1.0f, AngleFloat.Cos(AngleFloat.Radians(0)), 1.0E-05F, "Cos(0 rad)");
+            Assert.AreEqual(0.0f, AngleFloat.Cos(AngleFloat.Radians((float)Math.PI / 2)), 1.0E-05F, "Cos(π/2)");
+            Assert.AreEqual(-1.0f, AngleFloat.Cos(AngleFloat.Radians((float)Math.PI)), 1.0E-05F, "Cos(π)");
+        }
+
+        [Test]
+        public void Sin() {
+            // Test zero
+            Assert.AreEqual(0.0f, AngleFloat.Sin(AngleFloat.Degrees(0)), 1.0E-05F, "Sin(0°)");
+
+            // Test 90 degrees
+            Assert.AreEqual(1.0f, AngleFloat.Sin(AngleFloat.Degrees(90)), 1.0E-05F, "Sin(90°)");
+
+            // Test 180 degrees
+            Assert.AreEqual(0.0f, AngleFloat.Sin(AngleFloat.Degrees(180)), 1.0E-05F, "Sin(180°)");
+
+            // Test 270 degrees
+            Assert.AreEqual(-1.0f, AngleFloat.Sin(AngleFloat.Degrees(270)), 1.0E-05F, "Sin(270°)");
+
+            // Test 45 degrees
+            Assert.AreEqual(MathF.Sqrt(2) / 2, AngleFloat.Sin(AngleFloat.Degrees(45)), 1.0E-05F, "Sin(45°)");
+
+            // Test negative angle
+            Assert.AreEqual(0.0f, AngleFloat.Sin(AngleFloat.Degrees(-360)), 1.0E-05F, "Sin(-360°)");
+
+            // Test using radians
+            Assert.AreEqual(0.0f, AngleFloat.Sin(AngleFloat.Radians(0)), 1.0E-05F, "Sin(0 rad)");
+            Assert.AreEqual(1.0f, AngleFloat.Sin(AngleFloat.Radians((float)Math.PI / 2)), 1.0E-05F, "Sin(π/2)");
+            Assert.AreEqual(0.0f, AngleFloat.Sin(AngleFloat.Radians((float)Math.PI)), 1.0E-05F, "Sin(π)");
+        }
+
+        [Test]
+        public void Tan() {
+            // Test zero
+            Assert.AreEqual(0.0f, AngleFloat.Tan(AngleFloat.Degrees(0)), 1.0E-05F, "Tan(0°)");
+
+            // Test 45 degrees
+            Assert.AreEqual(1.0f, AngleFloat.Tan(AngleFloat.Degrees(45)), 1.0E-05F, "Tan(45°)");
+
+            // Test -45 degrees
+            Assert.AreEqual(-1.0f, AngleFloat.Tan(AngleFloat.Degrees(-45)), 1.0E-05F, "Tan(-45°)");
+
+            // Test using radians
+            Assert.AreEqual(0.0f, AngleFloat.Tan(AngleFloat.Radians(0)), 1.0E-05F, "Tan(0 rad)");
+            Assert.AreEqual(1.0f, AngleFloat.Tan(AngleFloat.Radians((float)Math.PI / 4)), 1.0E-05F, "Tan(π/4)");
+        }
+
+        [Test]
+        public void Acos() {
+            // Test 1 (0 degrees)
+            Assert.AreEqual(0.0f, AngleFloat.Acos(1.0f).inRadians, 1.0E-05F, "Acos(1)");
+
+            // Test 0 (90 degrees or π/2 radians)
+            Assert.AreEqual((float)Math.PI / 2, AngleFloat.Acos(0.0f).inRadians, 1.0E-05F, "Acos(0)");
+
+            // Test -1 (-180 degrees or π radians)
+            Assert.AreEqual((float)-Math.PI, AngleFloat.Acos(-1.0f).inRadians, 1.0E-05F, "Acos(-1)");
+
+            // Test 0.5 (60 degrees or π/3 radians)
+            Assert.AreEqual((float)Math.PI / 3, AngleFloat.Acos(0.5f).inRadians, 1.0E-05F, "Acos(0.5)");
+
+            // Test sqrt(2)/2 (45 degrees or π/4 radians)
+            Assert.AreEqual((float)Math.PI / 4, AngleFloat.Acos(MathF.Sqrt(2) / 2).inRadians, 1.0E-05F, "Acos(√2/2)");
+        }
+
+        [Test]
+        public void Asin() {
+            // Test 0 (0 degrees)
+            Assert.AreEqual(0.0f, AngleFloat.Asin(0.0f).inRadians, 1.0E-05F, "Asin(0)");
+
+            // Test 1 (90 degrees or π/2 radians)
+            Assert.AreEqual((float)Math.PI / 2, AngleFloat.Asin(1.0f).inRadians, 1.0E-05F, "Asin(1)");
+
+            // Test -1 (-90 degrees or -π/2 radians)
+            Assert.AreEqual(-(float)Math.PI / 2, AngleFloat.Asin(-1.0f).inRadians, 1.0E-05F, "Asin(-1)");
+
+            // Test 0.5 (30 degrees or π/6 radians)
+            Assert.AreEqual((float)Math.PI / 6, AngleFloat.Asin(0.5f).inRadians, 1.0E-05F, "Asin(0.5)");
+
+            // Test sqrt(2)/2 (45 degrees or π/4 radians)
+            Assert.AreEqual((float)Math.PI / 4, AngleFloat.Asin(MathF.Sqrt(2) / 2).inRadians, 1.0E-05F, "Asin(√2/2)");
+        }
+
+
+        [Test]
+        public void Atan() {
+            // Test zero
+            Assert.AreEqual(0.0f, AngleFloat.Atan(0.0f).inRadians, 1.0E-05F, "Atan(0)");
+
+            // Test 1 (45 degrees or π/4 radians)
+            Assert.AreEqual((float)Math.PI / 4, AngleFloat.Atan(1.0f).inRadians, 1.0E-05F, "Atan(1)");
+
+            // Test -1 (-45 degrees or -π/4 radians)
+            Assert.AreEqual(-(float)Math.PI / 4, AngleFloat.Atan(-1.0f).inRadians, 1.0E-05F, "Atan(-1)");
+
+            // Test sqrt(3) (60 degrees or π/3 radians)
+            Assert.AreEqual((float)Math.PI / 3, AngleFloat.Atan(MathF.Sqrt(3)).inRadians, 1.0E-05F, "Atan(√3)");
+
+            // Test 1/sqrt(3) (30 degrees or π/6 radians)
+            Assert.AreEqual((float)Math.PI / 6, AngleFloat.Atan(1.0f / MathF.Sqrt(3)).inRadians, 1.0E-05F, "Atan(1/√3)");
+
+            // Test positive infinity
+            Assert.AreEqual((float)Math.PI / 2, AngleFloat.Atan(float.PositiveInfinity).inRadians, 1.0E-05F, "Atan(+∞)");
+
+            // Test negative infinity
+            Assert.AreEqual(-(float)Math.PI / 2, AngleFloat.Atan(float.NegativeInfinity).inRadians, 1.0E-05F, "Atan(-∞)");
+        }
+
+        [Test]
+        public void Atan2() {
+            // Test basic quadrant I
+            Assert.AreEqual((float)Math.PI / 4, AngleFloat.Atan2(1.0f, 1.0f).inRadians, 1.0E-05F, "Atan2(1, 1)");
+
+            // Test quadrant II
+            Assert.AreEqual(3 * (float)Math.PI / 4, AngleFloat.Atan2(1.0f, -1.0f).inRadians, 1.0E-05F, "Atan2(1, -1)");
+
+            // Test quadrant III
+            Assert.AreEqual(-(float)Math.PI * 0.75f, AngleFloat.Atan2(-1.0f, -1.0f).inRadians, 1.0E-05F, "Atan2(-1, -1)");
+
+            // Test quadrant IV
+            Assert.AreEqual(-(float)Math.PI / 4, AngleFloat.Atan2(-1.0f, 1.0f).inRadians, 1.0E-05F, "Atan2(-1, 1)");
+
+            // Test positive x-axis
+            Assert.AreEqual(0.0f, AngleFloat.Atan2(0.0f, 1.0f).inRadians, 1.0E-05F, "Atan2(0, 1)");
+
+            // Test positive y-axis
+            Assert.AreEqual((float)Math.PI / 2, AngleFloat.Atan2(1.0f, 0.0f).inRadians, 1.0E-05F, "Atan2(1, 0)");
+
+            // Test negative y-axis
+            Assert.AreEqual(-(float)Math.PI / 2, AngleFloat.Atan2(-1.0f, 0.0f).inRadians, 1.0E-05F, "Atan2(-1, 0)");
+
+            // Test origin
+            Assert.AreEqual(0.0f, AngleFloat.Atan2(0.0f, 0.0f).inRadians, 1.0E-05F, "Atan2(0, 0)");
+
+            // Test with different magnitudes
+            Assert.AreEqual((float)Math.PI / 3, AngleFloat.Atan2(MathF.Sqrt(3), 1.0f).inRadians, 1.0E-05F, "Atan2(√3, 1)");
+
+            // Test negative x-axis
+            Assert.AreEqual((float)-Math.PI, AngleFloat.Atan2(0.0f, -1.0f).inRadians, 1.0E-05F, "Atan2(0, -1)");
+        }
+
+        [Test]
+        public void Multiplication() {
+            AngleFloat r = AngleFloat.zero;
+
+            // Angle * float
+            r = AngleFloat.Degrees(90) * 2;
+            Assert.AreEqual(-180, r.inDegrees, "Multiply 90 * 2");
+
+            r = AngleFloat.Degrees(45) * 0.5f;
+            Assert.AreEqual(22.5f, r.inDegrees, "Multiply 45 * 0.5");
+
+            r = AngleFloat.Degrees(90) * 0;
+            Assert.AreEqual(0, r.inDegrees, "Multiply 90 * 0");
+
+            r = AngleFloat.Degrees(-90) * 2;
+            Assert.AreEqual(-180, r.inDegrees, "Multiply -90 * 2");
+
+            r = AngleFloat.Degrees(270) * 2;
+            Assert.AreEqual(-180, r.inDegrees, "Multiply 270 * 2 (normalized)");
+
+            // float * Angle
+            r = 2 * AngleFloat.Degrees(90);
+            Assert.AreEqual(-180, r.inDegrees, "Multiply 2 * 90");
+
+            r = 0.5f * AngleFloat.Degrees(45);
+            Assert.AreEqual(22.5, r.inDegrees, "Multiply 0.5 * 45");
+
+            r = 0 * AngleFloat.Degrees(90);
+            Assert.AreEqual(0, r.inDegrees, "Multiply 0 * 90");
+
+            r = 2 * AngleFloat.Degrees(-90);
+            Assert.AreEqual(-180, r.inDegrees, "Multiply 2 * -90");
+
+            // Negative factor
+            r = AngleFloat.Degrees(90) * -1;
+            Assert.AreEqual(-90, r.inDegrees, "Multiply 90 * -1");
+
+            r = -1 * AngleFloat.Degrees(90);
+            Assert.AreEqual(-90, r.inDegrees, "Multiply -1 * 90");
+        }
+
+        [Test]
+        public void MoveTowards() {
+            AngleFloat r = AngleFloat.zero;
+
+            r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(90), 30);
+            Assert.AreEqual(30, r.inDegrees, "MoveTowards 0 90 30");
+
+            r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(90), 90);
+            Assert.AreEqual(90, r.inDegrees, "MoveTowards 0 90 90");
+
+            r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(90), 180);
+            Assert.AreEqual(90, r.inDegrees, "MoveTowards 0 90 180");
+
+            r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(90), 270);
+            Assert.AreEqual(90, r.inDegrees, "MoveTowrads 0 90 270");
+
+            r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(90), -30);
+            Assert.AreEqual(0, r.inDegrees, "MoveTowards 0 90 -30");
+
+            r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(-90), -30);
+            Assert.AreEqual(0, r.inDegrees, "MoveTowards 0 -90 -30");
+
+            r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(-90), -90);
+            Assert.AreEqual(0, r.inDegrees, "MoveTowards 0 -90 -90");
+
+            r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(-90), -180);
+            Assert.AreEqual(0, r.inDegrees, "MoveTowards 0 -90 -180");
+
+            r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(-90), -270);
+            Assert.AreEqual(0, r.inDegrees, "MoveTowrads 0 -90 -270");
+
+            r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(90), 0);
+            Assert.AreEqual(0, r.inDegrees, "MoveTowards 0 90 0");
+
+            r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(0), 0);
+            Assert.AreEqual(0, r.inDegrees, "MoveTowards 0 0 0");
+
+            r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(0), 30);
+            Assert.AreEqual(0, r.inDegrees, "MoveTowrads 0 0 30");
+
+            r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(90), float.PositiveInfinity);
+            Assert.AreEqual(90, r.inDegrees, "MoveTowards 0 90 INFINITY");
+
+            r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(float.PositiveInfinity), 30);
+            Assert.AreEqual(30, r.inDegrees, "MoveTowrads 0 INFINITY 30");
+
+            r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(-90), float.NegativeInfinity);
+            Assert.AreEqual(0, r.inDegrees, "MoveTowards 0 -90 -INFINITY");
+
+            r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(float.NegativeInfinity), -30);
+            Assert.AreEqual(0, r.inDegrees, "MoveTowrads 0 -INFINITY -30");
+
+        }
+
+        [Test]
+        public void Difference() {
+            float r = 0;
+
+            r = Angles.Difference(0, 90);
+            Assert.AreEqual(90, r, "Difference 0 90");
+
+            r = Angles.Difference(0, -90);
+            Assert.AreEqual(-90, r, "Difference 0 -90");
+
+            r = Angles.Difference(0, 270);
+            Assert.AreEqual(-90, r, "Difference 0 270");
+
+            r = Angles.Difference(0, -270);
+            Assert.AreEqual(90, r, "Difference 0 -270");
+
+            r = Angles.Difference(90, 0);
+            Assert.AreEqual(-90, r, "Difference 90 0");
+
+            r = Angles.Difference(-90, 0);
+            Assert.AreEqual(90, r, "Difference -90 0");
+
+            r = Angles.Difference(0, 0);
+            Assert.AreEqual(0, r, "Difference 0 0");
+
+            r = Angles.Difference(90, 90);
+            Assert.AreEqual(0, r, "Difference 90 90");
+
+            r = Angles.Difference(0, float.PositiveInfinity);
+            Assert.AreEqual(float.PositiveInfinity, r, "Difference 0 INFINITY");
+
+            r = Angles.Difference(0, float.NegativeInfinity);
+            Assert.AreEqual(float.NegativeInfinity, r, "Difference 0 -INFINITY");
+
+            r = Angles.Difference(float.NegativeInfinity, float.PositiveInfinity);
+            Assert.AreEqual(float.PositiveInfinity, r, "Difference -INFINITY INFINITY");
+        }
+    }
+
+}
+#endif

test/DirectionTest.cs

@@ -0,0 +1,201 @@
+#if !UNITY_5_6_OR_NEWER
+using System;
+using NUnit.Framework;
+
+namespace LinearAlgebra.Test {
+    public class DirectionTest {
+
+        [Test]
+        public void RadiansForward() {
+            Direction d = Direction.Radians(0, 0);
+            Assert.AreEqual(0, d.horizontal.inDegrees, 0.0001f);
+            Assert.AreEqual(0, d.vertical.inDegrees, 0.0001f);
+        }
+
+        [Test]
+        public void RadiansUp() {
+            Direction d = Direction.Radians(0, (float)Math.PI / 2);
+            Assert.AreEqual(0, d.horizontal.inDegrees, 0.0001f);
+            Assert.AreEqual(90, d.vertical.inDegrees, 0.0001f);
+        }
+
+        [Test]
+        public void RadiansDown() {
+            Direction d = Direction.Radians(0, -(float)Math.PI / 2);
+            Assert.AreEqual(0, d.horizontal.inDegrees, 0.0001f);
+            Assert.AreEqual(-90, d.vertical.inDegrees, 0.0001f);
+        }
+
+        [Test]
+        public void RadiansArbitrary() {
+            Direction d = Direction.Radians((float)Math.PI / 4, (float)Math.PI / 6);
+            Assert.AreEqual(45, d.horizontal.inDegrees, 0.0001f);
+            Assert.AreEqual(30, d.vertical.inDegrees, 0.0001f);
+        }
+
+        [Test]
+        public void RadiansEquivalentToDegreesConversion() {
+            Direction d1 = Direction.Radians((float)Math.PI / 3, (float)Math.PI / 4);
+            Direction d2 = Direction.Degrees(60, 45);
+            Assert.AreEqual(d1.horizontal.inDegrees, d2.horizontal.inDegrees, 0.0001f);
+            Assert.AreEqual(d1.vertical.inDegrees, d2.vertical.inDegrees, 0.0001f);
+        }
+
+        [Test]
+        public void ToVector3Forward() {
+            Direction d = Direction.forward;
+            Vector3Float v = d.ToVector3();
+            Assert.AreEqual(0, v.horizontal, 0.0001f);
+            Assert.AreEqual(0, v.vertical, 0.0001f);
+            Assert.AreEqual(1, v.depth, 0.0001f);
+        }
+
+        [Test]
+        public void ToVector3Up() {
+            Direction d = Direction.up;
+            Vector3Float v = d.ToVector3();
+            Assert.AreEqual(0, v.horizontal, 0.0001f);
+            Assert.AreEqual(1, v.vertical, 0.0001f);
+            Assert.AreEqual(0, v.depth, 0.0001f);
+        }
+
+        [Test]
+        public void ToVector3Down() {
+            Direction d = Direction.down;
+            Vector3Float v = d.ToVector3();
+            Assert.AreEqual(0, v.horizontal, 0.0001f);
+            Assert.AreEqual(-1, v.vertical, 0.0001f);
+            Assert.AreEqual(0, v.depth, 0.0001f);
+        }
+
+        [Test]
+        public void FromVector3Forward() {
+            Vector3Float v = new(0, 0, 1);
+            Direction d = Direction.FromVector3(v);
+            Assert.AreEqual(0, d.horizontal.inDegrees, 0.0001f);
+            Assert.AreEqual(0, d.vertical.inDegrees, 0.0001f);
+        }
+
+        [Test]
+        public void ToVector3AndBack() {
+            Direction d1 = Direction.Degrees(45, 30);
+            Vector3Float v = d1.ToVector3();
+            Direction d2 = Direction.FromVector3(v);
+            Assert.AreEqual(d1.horizontal.inDegrees, d2.horizontal.inDegrees, 0.0001f);
+            Assert.AreEqual(d1.vertical.inDegrees, d2.vertical.inDegrees, 0.0001f);
+        }
+
+        [Test]
+        public void Compare() {
+            Direction d1 = Direction.Degrees(45, 135);
+            Direction d2 = new(AngleFloat.Degrees(45), AngleFloat.Degrees(135));
+            bool r;
+            r = d1 == d2;
+            Assert.True(r);
+            Assert.AreEqual(d1, d2);
+        }
+
+        [Test]
+        public void NotEqualWithDifferentHorizontal() {
+            Direction d1 = Direction.Degrees(45, 30);
+            Direction d2 = Direction.Degrees(90, 30);
+            Assert.True(d1 != d2);
+        }
+
+        [Test]
+        public void NotEqualWithDifferentVertical() {
+            Direction d1 = Direction.Degrees(45, 30);
+            Direction d2 = Direction.Degrees(45, 60);
+            Assert.True(d1 != d2);
+        }
+
+        [Test]
+        public void NotEqualWithDifferentBoth() {
+            Direction d1 = Direction.Degrees(45, 30);
+            Direction d2 = Direction.Degrees(90, 60);
+            Assert.True(d1 != d2);
+        }
+
+        [Test]
+        public void NotEqualWithSameValues() {
+            Direction d1 = Direction.Degrees(45, 30);
+            Direction d2 = Direction.Degrees(45, 30);
+            Assert.False(d1 != d2);
+        }
+
+
+        [Test]
+        public void EqualsWithSameValues() {
+            Direction d1 = Direction.Degrees(45, 30);
+            Direction d2 = Direction.Degrees(45, 30);
+            Assert.True(d1.Equals(d2));
+        }
+
+        [Test]
+        public void EqualsWithDifferentHorizontal() {
+            Direction d1 = Direction.Degrees(45, 30);
+            Direction d2 = Direction.Degrees(90, 30);
+            Assert.False(d1.Equals(d2));
+        }
+
+        [Test]
+        public void EqualsWithDifferentVertical() {
+            Direction d1 = Direction.Degrees(45, 30);
+            Direction d2 = Direction.Degrees(45, 60);
+            Assert.False(d1.Equals(d2));
+        }
+
+        [Test]
+        public void EqualsWithDifferentBoth() {
+            Direction d1 = Direction.Degrees(45, 30);
+            Direction d2 = Direction.Degrees(90, 60);
+            Assert.False(d1.Equals(d2));
+        }
+
+        [Test]
+        public void EqualsWithNonDirectionObject() {
+            Direction d = Direction.Degrees(45, 30);
+            Assert.False(d.Equals("not a direction"));
+        }
+
+        [Test]
+        public void EqualsWithNull() {
+            Direction d = Direction.Degrees(45, 30);
+            Assert.False(d.Equals(null));
+        }
+
+        [Test]
+        public void EqualsWithZeros() {
+            Direction d1 = Direction.forward;
+            Direction d2 = Direction.Degrees(0, 0);
+            Assert.True(d1.Equals(d2));
+        }
+
+        [Test]
+        public void HashCode() {
+            Direction d1 = Direction.Degrees(45, 30);
+            Direction d2 = Direction.Degrees(45, 30);
+            Assert.AreEqual(d1.GetHashCode(), d2.GetHashCode());
+
+            d1 = Direction.Degrees(45, 30);
+            d2 = Direction.Degrees(90, 30);
+            Assert.AreNotEqual(d1.GetHashCode(), d2.GetHashCode());
+
+            d1 = Direction.Degrees(45, 30);
+            d2 = Direction.Degrees(45, 60);
+            Assert.AreNotEqual(d1.GetHashCode(), d2.GetHashCode());
+
+            Direction d = Direction.Degrees(45, 30);
+            int hash1 = d.GetHashCode();
+            int hash2 = d.GetHashCode();
+            Assert.AreEqual(hash1, hash2);
+
+            d1 = Direction.forward;
+            d2 = Direction.Degrees(0, 0);
+            Assert.AreEqual(d1.GetHashCode(), d2.GetHashCode());
+        }
+
+    };
+}
+#endif
+

test/LinearAlgebra_Test.csproj

@@ -0,0 +1,19 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <TargetFramework>net8.0</TargetFramework>
+    <IsPackable>false</IsPackable>
+    <IsTestProject>true</IsTestProject>
+  </PropertyGroup>
+
+  <ItemGroup>
+    <PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.13.0" />
+    <PackageReference Include="NUnit" Version="3.13.2" />
+    <PackageReference Include="NUnit3TestAdapter" Version="3.17.0" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <ProjectReference Include="..\src\LinearAlgebra.csproj" />
+  </ItemGroup>
+
+</Project>

test/QuaternionTest.cs

@@ -0,0 +1,185 @@
+#if !UNITY_5_6_OR_NEWER
+using NUnit.Framework;
+
+namespace LinearAlgebra.Test {
+
+    public class QuaternionTest {
+
+        [SetUp]
+        public void Setup() {
+        }
+
+        [Test]
+        public void Normalize() {
+            Quaternion q1 = new(0, 0, 0, 1);
+            Quaternion r = Quaternion.identity;
+
+            r = q1.normalized;
+            Assert.AreEqual(r, q1, "q.normalized 0 0 0 1");
+
+            r = Quaternion.Normalize(q1);
+            Assert.AreEqual(r, q1, "q.normalized 0 0 0 1");
+        }
+
+        [Test]
+        public void ToAngles() {
+            Quaternion q1 = new(0, 0, 0, 1);
+            Vector3Float v = Vector3Float.zero;
+
+            v = Quaternion.ToAngles(q1);
+            Assert.AreEqual(v, new Vector3Float(0, 0, 0), "ToAngles 0 0 0 1");
+
+            q1 = new(1, 0, 0, 0);
+            v = Quaternion.ToAngles(q1);
+            Assert.AreEqual(0, v.horizontal, "1 0 0 0  H");
+            Assert.AreEqual(180, v.vertical, "1 0 0 0  V");
+            Assert.AreEqual(180, v.depth, "1 0 0 0  D");
+
+        }
+
+        [Test]
+        public void Multiplication() {
+            Quaternion q1 = new(0, 0, 0, 1);
+            Quaternion q2 = new(1, 0, 0, 0);
+            Quaternion r;
+
+            r = q1 * q2;
+            Assert.AreEqual(r, new Quaternion(1, 0, 0, 0), "0 0 0 1 * 1 0 0 0 ");
+        }
+
+        [Test]
+        public void MultiplicationVector() {
+            Quaternion q1 = new(0, 0, 0, 1);
+            Vector3Float v1 = new(0, 1, 0);
+            Vector3Float r;
+
+            r = q1 * v1;
+            Assert.AreEqual(r, new Vector3Float(0, 1, 0), "0 0 0 1 * Vector 0 1 0");
+
+            q1 = new(1, 0, 0, 0);
+            r = q1 * v1;
+            Assert.AreEqual(r, new Vector3Float(0, -1, 0), "1 0 0 0 * Vector 0 1 0");
+        }
+
+        [Test]
+        public void Equality() {
+            Quaternion q1 = new(0, 0, 0, 1);
+            Quaternion q2 = new(1, 0, 0, 0);
+            Assert.AreNotEqual(q1, q2, "0 0 0 1 == 1 0 0 0");
+
+            q2 = new(0, 0, 0, 1);
+            Assert.AreEqual(q1, q2, "0 0 0 1 == 1 0 0 0");
+        }
+
+        [Test, Ignore("ToDo")]
+        public void Inverse() { }
+
+        [Test, Ignore("ToDo")]
+        public void LookRotation() { }
+
+        [Test, Ignore("ToDo")]
+        public void FromToRotation() { }
+
+        [Test, Ignore("ToDo")]
+        public void RotateTowards() { }
+
+        [Test, Ignore("ToDo")]
+        public void AngleAxis() { }
+
+        [Test, Ignore("ToDo")]
+        public void Angle() { }
+
+        [Test, Ignore("ToDo")]
+        public void Slerp() { }
+
+        [Test, Ignore("ToDo")]
+        public void SlerpUnclamped() { }
+
+        [Test]
+        public void Euler() {
+            Vector3Float v1 = new(0, 0, 0);
+            Quaternion q;
+
+            q = Quaternion.Euler(v1);
+            Assert.AreEqual(q, Quaternion.identity, "Euler Vector 0 0 0");
+
+            q = Quaternion.Euler(0, 0, 0);
+            Assert.AreEqual(q, Quaternion.identity, "Euler 0 0 0");
+
+            v1 = new(90, 90, -90);
+            q = Quaternion.Euler(v1);
+            Assert.AreEqual(q, new Quaternion(0, 0.707106709F, -0.707106709F, 0), "Euler Vector 90 90 -90");
+
+            q = Quaternion.Euler(90, 90, -90);
+            Assert.AreEqual(q, new Quaternion(0, 0.707106709F, -0.707106709F, 0), "Euler 90 90 -90");
+        }
+
+        [Test]
+        public void EulerToAngles() {
+            Vector3Float v;
+            Quaternion q;
+            Quaternion r;
+
+            //v = new(0, 0, 0);
+            q = Quaternion.Euler(0, 0 , 0);
+            v = Quaternion.ToAngles(q);
+            r = Quaternion.Euler(v);
+            Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f, "0 0 0");
+
+            q = Quaternion.Euler(-45, -30, -15);
+            v = Quaternion.ToAngles(q);
+            r = Quaternion.Euler(v);
+            Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f, "-45, -30, -15");
+
+            // Gimball lock
+            // q = Quaternion.Euler(90, 90, -90);
+            // v = Quaternion.ToAngles(q);
+            // r = Quaternion.Euler(v);
+            // Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f, "0 0 0");
+
+        }
+
+        [Test]
+        public void GetAngleAround() {
+            Vector3Float v1 = new(0, 1, 0);
+            Quaternion q1 = new(0, 0, 0, 1);
+
+            float f = Quaternion.GetAngleAround(v1, q1);
+            Assert.AreEqual(f, 0, "GetAngleAround 0 1 0 , 0 0 0 1");
+
+            q1 = new(0, 0.707106709F, -0.707106709F, 0);
+            f = Quaternion.GetAngleAround(v1, q1);
+            Assert.AreEqual(f, 180, "GetAngleAround 0 1 0 , 0 0.7 -0.7 0");
+
+            v1 = new(0, 0, 0);
+            f = Quaternion.GetAngleAround(v1, q1);
+            Assert.IsTrue(float.IsNaN(f), "GetAngleAround 0 0 0 , 0 0.7 -0.7 0");
+        }
+
+        [Test]
+        public void GetRotationAround() {
+            Vector3Float v1 = new(0, 1, 0);
+            Quaternion q1 = new(0, 0, 0, 1);
+
+            Quaternion q = Quaternion.GetRotationAround(v1, q1);
+            Assert.AreEqual(q, new Quaternion(0, 0, 0, 1), "GetRotationAround 0 1 0 , 0 0 0 1");
+
+            q1 = new(0, 0.707106709F, -0.707106709F, 0);
+            q = Quaternion.GetRotationAround(v1, q1);
+            Assert.AreEqual(q, new Quaternion(0, 1, 0, 0), "GetRotationAround 0 1 0 , 0 0.7 -0.7 0");
+
+            v1 = new(0, 0, 0);
+            q = Quaternion.GetRotationAround(v1, q1);
+            bool r = float.IsNaN(q.x) && float.IsNaN(q.y) && float.IsNaN(q.z) && float.IsNaN(q.w);
+            Assert.IsTrue(r, "GetRotationAround 0 0 0 , 0 0.7 -0.7 0");
+        }
+
+        [Test, Ignore("ToDo")]
+        public void GetSwingTwist() { }
+
+        [Test, Ignore("ToDo")]
+        public void Dot() { }
+
+    }
+}
+#endif

test/SphericalTest.cs

@@ -0,0 +1,53 @@
+#if !UNITY_5_6_OR_NEWER
+using System;
+using NUnit.Framework;
+
+namespace LinearAlgebra.Test {
+    public class SphericalTest {
+        [SetUp]
+        public void Setup() {
+        }
+
+        [Test]
+        public void FromVector3() {
+            Vector3Float v = new(0, 0, 1);
+            Spherical s = Spherical.FromVector3(v);
+            Assert.AreEqual(1.0f, s.distance, "s.distance 0 0 1");
+            Assert.AreEqual(0.0f, s.direction.horizontal.inDegrees, "s.hor 0 0 1");
+            Assert.AreEqual(0.0f, s.direction.vertical.inDegrees, 1.0E-05F, "s.vert 0 0 1");
+
+            v = new(0, 1, 0);
+            s = Spherical.FromVector3(v);
+            Assert.AreEqual(1.0f, s.distance, "s.distance 0 1 0");
+            Assert.AreEqual(0.0f, s.direction.horizontal.inDegrees, "s.hor 0 1 0");
+            Assert.AreEqual(90.0f, s.direction.vertical.inDegrees, "s.vert 0 1 0");
+
+            v = new(1, 0, 0);
+            s = Spherical.FromVector3(v);
+            Assert.AreEqual(1.0f, s.distance, "s.distance 1 0 0");
+            Assert.AreEqual(90.0f, s.direction.horizontal.inDegrees, "s.hor 1 0 0");
+            Assert.AreEqual(0.0f, s.direction.vertical.inDegrees, 1.0E-05F, "s.vert 1 0 0");
+        }
+
+        [Test]
+        public void Addition() {
+            Spherical v1 = Spherical.Degrees(1, 45, 0);
+            Spherical v2 = Spherical.zero;
+            Spherical r = Spherical.zero;
+
+            r = v1 + v2;
+            Assert.AreEqual(v1.distance, r.distance, 1.0E-05F, "Addition(0,0,0)");
+
+            r = v1;
+            r += v2;
+            Assert.AreEqual(v1.distance, r.distance, 1.0E-05F, "Addition(0,0,0)");
+
+            v2 = Spherical.Degrees(1, 0, 90);
+            r = v1 + v2;
+            Assert.AreEqual(Math.Sqrt(2), r.distance, 1.0E-05F, "Addition(1 0 90)");
+            Assert.AreEqual(45.0f, r.direction.horizontal.inDegrees, "Addition(1 0 90)");
+            Assert.AreEqual(45.0f, r.direction.vertical.inDegrees, 1.0E-05F, "Addition(1 0 90)");
+        }
+    }
+}
+#endif

test/SwingTwistTest.cs

@@ -0,0 +1,131 @@
+#if !UNITY_5_6_OR_NEWER
+using NUnit.Framework;
+
+namespace LinearAlgebra.Test {
+
+    [TestFixture]
+    public class SwingTwistTest {
+
+        [Test]
+        public void Degrees_CreatesSwingTwistWithDegreeAngles() {
+            SwingTwist st = SwingTwist.Degrees(45, 30, 15);
+            Assert.IsNotNull(st);
+            Assert.AreEqual(45, st.swing.horizontal.inDegrees, 0.01f);
+            Assert.AreEqual(30, st.swing.vertical.inDegrees, 0.01f);
+            Assert.AreEqual(15, st.twist.inDegrees, 0.01f);
+        }
+
+        [Test]
+        public void Radians_CreatesSwingTwistWithRadianAngles() {
+            float pi = (float)System.Math.PI;
+            SwingTwist st = SwingTwist.Radians(pi / 4, pi / 6, pi / 12);
+            Assert.IsNotNull(st);
+            Assert.AreEqual(45, st.swing.horizontal.inDegrees, 0.01f);
+            Assert.AreEqual(30, st.swing.vertical.inDegrees, 0.01f);
+            Assert.AreEqual(15, st.twist.inDegrees, 0.01f);
+        }
+
+        [Test]
+        public void Zero_CreatesZeroRotation() {
+            SwingTwist st = SwingTwist.zero;
+            Assert.AreEqual(0, st.swing.horizontal.inDegrees, 0.01f);
+            Assert.AreEqual(0, st.swing.vertical.inDegrees, 0.01f);
+            Assert.AreEqual(0, st.twist.inDegrees, 0.01f);
+        }
+
+        [Test]
+        public void QuaternionTest() {
+            Quaternion q;
+            SwingTwist s;
+            Quaternion r;
+
+            q = Quaternion.identity;
+            s = SwingTwist.FromQuaternion(q);
+            r = s.ToQuaternion();
+            Assert.AreEqual(q, r);
+
+            q = Quaternion.Euler(90, 0, 0);
+            s = SwingTwist.FromQuaternion(q);
+            Assert.AreEqual(0, s.swing.horizontal.inDegrees, 10e-2f);
+            Assert.AreEqual(90, s.swing.vertical.inDegrees, 10e-2f);
+            Assert.AreEqual(0, s.twist.inDegrees, 0.01f);
+            r = s.ToQuaternion();
+            Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f);
+
+            q = Quaternion.Euler(0, 90, 0);
+            s = SwingTwist.FromQuaternion(q);
+            Assert.AreEqual(90, s.swing.horizontal.inDegrees,10e-2f);
+            Assert.AreEqual(0, s.swing.vertical.inDegrees, 0.01f);
+            Assert.AreEqual(0, s.twist.inDegrees, 0.01f);
+            r = s.ToQuaternion();
+            Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f);
+
+            q = Quaternion.Euler(0, 0, 90);
+            s = SwingTwist.FromQuaternion(q);
+            Assert.AreEqual(0, s.swing.horizontal.inDegrees, 0.01f);
+            Assert.AreEqual(0, s.swing.vertical.inDegrees, 0.01f);
+            Assert.AreEqual(90, s.twist.inDegrees, 0.01f);
+            r = s.ToQuaternion();
+            Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f);
+
+            q = Quaternion.Euler(0, 180, 0);
+            s = SwingTwist.FromQuaternion(q);
+            Assert.AreEqual(-180, s.swing.horizontal.inDegrees, 0.01f);
+            Assert.AreEqual(0, s.swing.vertical.inDegrees, 0.01f);
+            Assert.AreEqual(0, s.twist.inDegrees, 0.01f);
+            r = s.ToQuaternion();
+            Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f);
+
+            q = Quaternion.Euler(0, 135, 0);
+            s = SwingTwist.FromQuaternion(q);
+            Assert.AreEqual(135, s.swing.horizontal.inDegrees, 0.01f);
+            Assert.AreEqual(0, s.swing.vertical.inDegrees, 0.01f);
+            Assert.AreEqual(0, s.twist.inDegrees, 0.01f);
+            r = s.ToQuaternion();
+            Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f);
+
+            q = Quaternion.Euler(60, 45, 30);
+            s = SwingTwist.FromQuaternion(q);
+            Assert.AreEqual(45, s.swing.horizontal.inDegrees, 0.01f);
+            Assert.AreEqual(60, s.swing.vertical.inDegrees, 0.01f);
+            Assert.AreEqual(30, s.twist.inDegrees, 0.01f);
+            // r = s.ToQuaternion();
+            // Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f);
+
+            // q = Quaternion.Euler(-45, -30, -15);
+            // s = SwingTwist.FromQuaternion(q);
+            // Assert.AreEqual(-30, s.swing.horizontal.inDegrees, 0.01f);
+            // Assert.AreEqual(-45, s.swing.vertical.inDegrees, 0.01f);
+            // Assert.AreEqual(-15, s.twist.inDegrees, 0.01f);
+            // r = s.ToQuaternion();
+            // Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f);
+
+            // q = Quaternion.Euler(180, 180, 180);
+            // s = SwingTwist.FromQuaternion(q);
+            // Assert.AreEqual(-180, s.swing.horizontal.inDegrees, 0.01f);
+            // Assert.AreEqual(-180, s.swing.vertical.inDegrees, 0.01f);
+            // Assert.AreEqual(-180, s.twist.inDegrees, 0.01f);
+            // r = s.ToQuaternion();
+            // Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f);
+
+        }
+
+        [Test]
+        public void ToAngleAxis_ConvertsToSpherical() {
+            SwingTwist st = SwingTwist.Degrees(45, 30, 15);
+            Spherical s = st.ToAngleAxis();
+            Assert.IsNotNull(s);
+        }
+
+        [Test]
+        public void FromAngleAxis_ConvertsFromSpherical() {
+            Spherical s = new(90, Direction.Degrees(45, 0));
+            SwingTwist st = SwingTwist.FromAngleAxis(s);
+            Assert.IsNotNull(st);
+        }
+
+    }
+
+}
+
+#endif

test/Vector2FloatTest.cs

@@ -0,0 +1,364 @@
+#if !UNITY_5_6_OR_NEWER
+using NUnit.Framework;
+
+namespace LinearAlgebra.Test {
+    using Vector2 = Vector2Float;
+
+    public class Vector2FloatTest {
+
+        [SetUp]
+        public void Setup() {
+        }
+
+        [Test]
+        public void FromPolar() {
+        }
+
+        [Test]
+        public void Equality() {
+            Vector2 v1 = new(4, 5);
+            Vector2 v2 = new(1, 2);
+
+            Assert.IsFalse(v1 == v2, "4 5 == 1 2");
+            Assert.IsTrue(v1 != v2, "4 5 != 1 2");
+
+            v2 = new(4, 5);
+            Assert.IsTrue(v1 == v2, "4 5 == 4 5");
+            Assert.IsFalse(v1 != v2, "4 5 != 4 5");
+        }
+
+        [Test]
+        public void Magnitude() {
+            Vector2 v = new(1, 2);
+            float m = 0;
+            m = v.magnitude;
+            Assert.AreEqual(m, 2.236068F, "v.magnitude 1 2");
+
+            m = Vector2.MagnitudeOf(v);
+            Assert.AreEqual(m, 2.236068F, "MagnitudeOf 1 2");
+
+            v = new(-1, -2);
+            m = v.magnitude;
+            Assert.AreEqual(m, 2.236068F, "v.magnitude -1 -2");
+
+            v = new(0, 0);
+            m = v.magnitude;
+            Assert.AreEqual(m, 0, "v.magnitude 0 0");
+        }
+
+        [Test]
+        public void SqrMagnitude() {
+            Vector2 v = new(1, 2);
+            float m = 0;
+
+            m = v.sqrMagnitude;
+            Assert.AreEqual(m, 5, "v.sqrMagnitude 1 2");
+
+            m = Vector2.SqrMagnitudeOf(v);
+            Assert.AreEqual(m, 5, "SqrMagnitudeOf 1 2");
+
+            v = new(-1, -2);
+            m = v.sqrMagnitude;
+            Assert.AreEqual(m, 5, "v.sqrMagnitude -1 -2");
+
+            v = new(0, 0);
+            m = v.sqrMagnitude;
+            Assert.AreEqual(m, 0, "v.sqrMagnitude 0 0");
+        }
+
+        [Test]
+        public void Distance() {
+            Vector2 v1 = new(4, 5);
+            Vector2 v2 = new(1, 2);
+            float f = 0;
+
+            f = Vector2.Distance(v1, v2);
+            Assert.AreEqual(f, 4.24264002f, 1.0E-05F, "Distance(4 5, 1 2)");
+
+            v2 = new(-1, -2);
+            f = Vector2.Distance(v1, v2);
+            Assert.AreEqual(f, 8.602325F, "Distance(4 5, 1 2)");
+
+            v2 = new(0, 0);
+            f = Vector2.Distance(v1, v2);
+            Assert.AreEqual(f, 6.403124F, 1.0E-05F, "Distance(4 5, 1 2)");
+        }
+
+        [Test]
+        public void Normalize() {
+            Vector2 v = new(0, 3);
+            Vector2Float r;
+
+            r = v.normalized;
+            Assert.AreEqual(0, r.horizontal, "normalized 0 3  H");
+            Assert.AreEqual(1, r.vertical, "normalized 0 3  V");
+
+            r = Vector2.Normalize(v);
+            Assert.AreEqual(0, r.horizontal, "Normalize 0 3  H");
+            Assert.AreEqual(1, r.vertical, "Normalize 0 3  V");
+
+            v = new(0, -3);
+            r = v.normalized;
+            Assert.AreEqual(0, r.horizontal, "normalized 0 -3  H");
+            Assert.AreEqual(-1, r.vertical, "normalized 0 -3  V");
+
+            v = new(0, 0);
+            r = v.normalized;
+            Assert.AreEqual(0, r.horizontal, "normalized 0 0  H");
+            Assert.AreEqual(0, r.vertical, "normalized 0 0  V");
+        }
+
+        [Test]
+        public void Negate() {
+            Vector2 v = new(4, 5);
+            Vector2 r;
+
+            r = -v;
+            Assert.AreEqual(-4, r.horizontal, "- 4 5 H");
+            Assert.AreEqual(-5, r.vertical, "- 4 5 V");
+
+            v = new(-4, -5);
+            r = -v;
+            Assert.AreEqual(4, r.horizontal, "- -4 -5 H");
+            Assert.AreEqual(5, r.vertical, "- -4 -5 V");
+
+            v = new(0, 0);
+            r = -v;
+            Assert.AreEqual(0, r.horizontal, "- 0 0 H");
+            Assert.AreEqual(0, r.vertical, "- 0 0 V");
+        }
+
+        [Test]
+        public void Subtract() {
+            Vector2 v1 = new(4, 5);
+            Vector2 v2 = new(1, 2);
+            Vector2 r = Vector2.zero;
+
+            r = v1 - v2;
+            Assert.IsTrue(r == new Vector2(3, 3), "4 5 - 1 2");
+
+            v2 = new(-1, -2);
+            r = v1 - v2;
+            Assert.IsTrue(r == new Vector2(5, 7), "4 5 - -1 -2");
+
+            v2 = new(4, 5);
+            r = v1 - v2;
+            Assert.IsTrue(r == new Vector2(0, 0), "4 5 - 4 5");
+            r = v1;
+            r -= v2;
+            Assert.AreEqual(r, new Vector2(0, 0), "4 5 - 4 5");
+
+            v2 = new(0, 0);
+            r = v1 - v2;
+            Assert.AreEqual(r, new Vector2(4, 5), "4 5 - 0 0");
+            r -= v2;
+            Assert.AreEqual(r, new Vector2(4, 5), "4 5 - 0 0");
+        }
+
+        [Test]
+        public void Addition() {
+            Vector2 v1 = new(4, 5);
+            Vector2 v2 = new(1, 2);
+            Vector2 r = Vector2.zero;
+
+            r = v1 + v2;
+            Assert.IsTrue(r == new Vector2(5, 7), "4 5 + 1 2");
+
+            v2 = new(-1, -2);
+            r = v1 + v2;
+            Assert.IsTrue(r == new Vector2(3, 3), "4  5 + -1 -2");
+            r = v1;
+            r += v2;
+            Assert.AreEqual(r, new Vector2(3, 3), "4 5 + -1 -2");
+
+            v2 = new(0, 0);
+            r = v1 + v2;
+            Assert.AreEqual(r, new Vector2(4, 5), "4 5 + 0 0");
+            r += v2;
+            Assert.AreEqual(r, new Vector2(4, 5), "4 5 + 0 0");
+        }
+
+        [Test]
+        public void Scale() {
+            Vector2 v1 = new(4, 5);
+            Vector2 v2 = new(1, 2);
+            Vector2 r;
+
+            r = Vector2.Scale(v1, v2);
+            Assert.AreEqual(4, r.horizontal, "Scale 4 5 , 1 2  H");
+            Assert.AreEqual(10, r.vertical, "Scale 4 5 , 1 2  V");
+
+            v2 = new(-1, -2);
+            r = Vector2.Scale(v1, v2);
+            Assert.AreEqual(-4, r.horizontal, "Scale 4 5 , -1 -2  H");
+            Assert.AreEqual(-10, r.vertical, "Scale 4 5 , -1 -2  V");
+
+            v2 = new(0, 0);
+            r = Vector2.Scale(v1, v2);
+            Assert.AreEqual(0, r.horizontal, "Scale 4 5 , 0 0  H");
+            Assert.AreEqual(0, r.vertical, "Scale 4 5 , 0 0  V");
+        }
+
+        [Test]
+        public void Multiply() {
+            Vector2 v1 = new(4, 5);
+            int f = 3;
+            Vector2 r;
+
+            r = v1 * f;
+            Assert.AreEqual(12, r.horizontal, "4 5 * 3  H");
+            Assert.AreEqual(15, r.vertical, "4 5 * 3  V");
+
+            r = f * v1;
+            Assert.AreEqual(12, r.horizontal, "3 * 4 5  H");
+            Assert.AreEqual(15, r.vertical, "3 * 4 5  V");
+
+            f = -3;
+            r = v1 * f;
+            Assert.AreEqual(-12, r.horizontal, "4 5 * -3  H");
+            Assert.AreEqual(-15, r.vertical, "4 5 * -3  V");
+
+            f = 0;
+            r = v1 * f;
+            Assert.AreEqual(0, r.horizontal, "4 5 * 0  H");
+            Assert.AreEqual(0, r.vertical, "4 5 * 0  V");
+        }
+
+        [Test]
+        public void Divide() {
+            Vector2 v1 = new(4, 5);
+            float f = 2;
+            Vector2 r;
+
+            r = v1 / f;
+            Assert.AreEqual(2, r.horizontal, "4 5 / 2  H");
+            Assert.AreEqual(2.5, r.vertical, "4 5 / 2  V");
+
+            f = -2;
+            r = v1 / f;
+            Assert.AreEqual(-2, r.horizontal, "4 5 / -2 H");
+            Assert.AreEqual(-2.5, r.vertical, "4 5 / -2  V");
+
+            f = 0;
+            r = v1 / f;
+            Assert.AreEqual(float.PositiveInfinity, r.horizontal, "4 5 / 0  H");
+            Assert.AreEqual(float.PositiveInfinity, r.vertical, "4 5 / 0  V");
+        }
+
+        [Test]
+        public void Dot() {
+            Vector2 v1 = new(4, 5);
+            Vector2 v2 = new(1, 2);
+            float f;
+
+            f = Vector2.Dot(v1, v2);
+            Assert.AreEqual(14, f, "Dot(4 5, 1 2)");
+
+            v2 = new(-1, -2);
+            f = Vector2.Dot(v1, v2);
+            Assert.AreEqual(-14, f, "Dot(4 5, -1 -2)");
+
+            v2 = new(0, 0);
+            f = Vector2.Dot(v1, v2);
+            Assert.AreEqual(0, f, "Dot(4 5, 0 0)");
+        }
+
+        [Test]
+        public void SignedAngle() {
+            Vector2 v1 = new(4, 5);
+            Vector2 v2 = new(1, 2);
+            float f;
+
+            f = Vector2.SignedAngle(v1, v2);
+            Assert.AreEqual(-12.094758f, f);
+
+            v2 = new(-1, -2);
+            f = Vector2.SignedAngle(v1, v2);
+            Assert.AreEqual(167.905228f, f);
+
+            v2 = new(0, 0);
+            f = Vector2.SignedAngle(v1, v2);
+            Assert.AreEqual(0, f);
+
+            v1 = new(0, 1);
+            v2 = new(1, 0);
+            f = Vector2.SignedAngle(v1, v2);
+            Assert.AreEqual(90, f);
+
+            v1 = new(0, 1);
+            v2 = new(0, -1);
+            f = Vector2.SignedAngle(v1, v2);
+            Assert.AreEqual(180, f);
+        }
+
+        [Test]
+        public void UnsignedAngle() {
+            Vector2 v1 = new(4, 5);
+            Vector2 v2 = new(1, 2);
+            float f;
+
+            f = Vector2.UnsignedAngle(v1, v2);
+            Assert.AreEqual(12.094758f, f);
+
+            v2 = new(-1, -2);
+            f = Vector2.UnsignedAngle(v1, v2);
+            Assert.AreEqual(167.905228f, f);
+
+            v2 = new(0, 0);
+            f = Vector2.UnsignedAngle(v1, v2);
+            Assert.AreEqual(0, f);
+
+            v1 = new(0, 1);
+            v2 = new(1, 0);
+            f = Vector2.UnsignedAngle(v1, v2);
+            Assert.AreEqual(90, f);
+
+            v1 = new(0, 1);
+            v2 = new(0, -1);
+            f = Vector2.UnsignedAngle(v1, v2);
+            Assert.AreEqual(180, f);            
+        }
+
+        [Test]
+        public void Rotate() {
+            Vector2 v1 = new(1, 2);
+            Vector2 r;
+
+            r = Vector2.Rotate(v1, AngleFloat.Degrees(0));
+            Assert.AreEqual(0, Vector2.Distance(r, v1));
+
+            r = Vector2.Rotate(v1, AngleFloat.Degrees(180));
+            Assert.AreEqual(0, Vector2.Distance(r, new Vector2(-1, -2)), 1.0e-06);
+
+            r = Vector2.Rotate(v1, AngleFloat.Degrees(-90));
+            Assert.AreEqual(0, Vector2.Distance(r, new Vector2(2, -1)), 1.0e-06);
+
+            r = Vector2.Rotate(v1, AngleFloat.Degrees(270));
+            Assert.AreEqual(0, Vector2.Distance(r, new Vector2(2, -1)), 1.0e-06);
+        }
+
+        [Test]
+        public void Lerp() {
+            Vector2 v1 = new(4, 5);
+            Vector2 v2 = new(1, 2);
+            Vector2 r;
+
+            r = Vector2.Lerp(v1, v2, 0);
+            Assert.AreEqual(0, Vector2.Distance(r, v1), 0);
+
+            r = Vector2.Lerp(v1, v2, 1);
+            Assert.AreEqual(0, Vector2.Distance(r, v2), 0);
+
+            r = Vector2.Lerp(v1, v2, 0.5f);
+            Assert.AreEqual(0, Vector2.Distance(r, new Vector2(2.5f, 3.5f)), 0);
+
+            r = Vector2.Lerp(v1, v2, -1);
+            Assert.AreEqual(0, Vector2.Distance(r, new Vector2(7, 8)), 0);
+
+            r = Vector2.Lerp(v1, v2, 2);
+            Assert.AreEqual(0, Vector2.Distance(r, new Vector2(-2, -1)), 0);
+        }
+
+    }
+}
+#endif

test/Vector2IntTest.cs

@@ -0,0 +1,270 @@
+#if !UNITY_5_6_OR_NEWER
+using NUnit.Framework;
+
+namespace LinearAlgebra.Test {
+    using Vector2 = Vector2Int;
+
+    public class Vector2IntTest {
+
+        [SetUp]
+        public void Setup() {
+        }
+
+        [Test]
+        public void FromPolar() {
+        }
+
+        [Test]
+        public void Equality() {
+            Vector2 v1 = new(4, 5);
+            Vector2 v2 = new(1, 2);
+
+            Assert.IsFalse(v1 == v2, "4 5 == 1 2");
+            Assert.IsTrue(v1 != v2, "4 5 != 1 2");
+
+            v2 = new(4, 5);
+            Assert.IsTrue(v1 == v2, "4 5 == 4 5");
+            Assert.IsFalse(v1 != v2, "4 5 != 4 5");
+        }
+
+        [Test]
+        public void Magnitude() {
+            Vector2 v = new(1, 2);
+            float m = 0;
+            m = v.magnitude;
+            Assert.AreEqual(m, 2.236068F, "v.magnitude 1 2");
+
+            m = Vector2.MagnitudeOf(v);
+            Assert.AreEqual(m, 2.236068F, "MagnitudeOf 1 2");
+
+            v = new(-1, -2);
+            m = v.magnitude;
+            Assert.AreEqual(m, 2.236068F, "v.magnitude -1 -2");
+
+            v = new(0, 0);
+            m = v.magnitude;
+            Assert.AreEqual(m, 0, "v.magnitude 0 0");
+        }
+
+        [Test]
+        public void SqrMagnitude() {
+            Vector2 v = new(1, 2);
+            float m = 0;
+
+            m = v.sqrMagnitude;
+            Assert.AreEqual(m, 5, "v.sqrMagnitude 1 2");
+
+            m = Vector2.SqrMagnitudeOf(v);
+            Assert.AreEqual(m, 5, "SqrMagnitudeOf 1 2");
+
+            v = new(-1, -2);
+            m = v.sqrMagnitude;
+            Assert.AreEqual(m, 5, "v.sqrMagnitude -1 -2");
+
+            v = new(0, 0);
+            m = v.sqrMagnitude;
+            Assert.AreEqual(m, 0, "v.sqrMagnitude 0 0");
+        }
+
+        [Test]
+        public void Distance() {
+            Vector2 v1 = new(4, 5);
+            Vector2 v2 = new(1, 2);
+            float f = 0;
+
+            f = Vector2.Distance(v1, v2);
+            Assert.AreEqual(f, 4.24264002f, 1.0E-05F, "Distance(4 5, 1 2)");
+
+            v2 = new(-1, -2);
+            f = Vector2.Distance(v1, v2);
+            Assert.AreEqual(f, 8.602325F, "Distance(4 5, 1 2)");
+
+            v2 = new(0, 0);
+            f = Vector2.Distance(v1, v2);
+            Assert.AreEqual(f, 6.403124F, 1.0E-05F, "Distance(4 5, 1 2)");
+        }
+
+        [Test]
+        public void Normalize() {
+            Vector2 v = new(0, 3);
+            Vector2Float r;
+
+            r = v.normalized;
+            Assert.AreEqual(0, r.horizontal, "normalized 0 3  H");
+            Assert.AreEqual(1, r.vertical, "normalized 0 3  V");
+
+            r = Vector2.Normalize(v);
+            Assert.AreEqual(0, r.horizontal, "Normalize 0 3  H");
+            Assert.AreEqual(1, r.vertical, "Normalize 0 3  V");
+
+            v = new(0, -3);
+            r = v.normalized;
+            Assert.AreEqual(0, r.horizontal, "normalized 0 -3  H");
+            Assert.AreEqual(-1, r.vertical, "normalized 0 -3  V");
+
+            v = new(0, 0);
+            r = v.normalized;
+            Assert.AreEqual(0, r.horizontal, "normalized 0 0  H");
+            Assert.AreEqual(0, r.vertical, "normalized 0 0  V");
+        }
+
+        [Test]
+        public void Negate() {
+            Vector2 v = new(4, 5);
+            Vector2 r;
+
+            r = -v;
+            Assert.AreEqual(-4, r.horizontal, "- 4 5 H");
+            Assert.AreEqual(-5, r.vertical, "- 4 5 V");
+
+            v = new(-4, -5);
+            r = -v;
+            Assert.AreEqual(4, r.horizontal, "- -4 -5 H");
+            Assert.AreEqual(5, r.vertical, "- -4 -5 V");
+
+            v = new(0, 0);
+            r = -v;
+            Assert.AreEqual(0, r.horizontal, "- 0 0 H");
+            Assert.AreEqual(0, r.vertical, "- 0 0 V");
+        }
+
+        [Test]
+        public void Subtract() {
+            Vector2 v1 = new(4, 5);
+            Vector2 v2 = new(1, 2);
+            Vector2 r = Vector2.zero;
+
+            r = v1 - v2;
+            Assert.IsTrue(r == new Vector2(3, 3), "4 5 - 1 2");
+
+            v2 = new(-1, -2);
+            r = v1 - v2;
+            Assert.IsTrue(r == new Vector2(5, 7), "4 5 - -1 -2");
+
+            v2 = new(4, 5);
+            r = v1 - v2;
+            Assert.IsTrue(r == new Vector2(0, 0), "4 5 - 4 5");
+            r = v1;
+            r -= v2;
+            Assert.AreEqual(r, new Vector2(0, 0), "4 5 - 4 5");
+
+            v2 = new(0, 0);
+            r = v1 - v2;
+            Assert.AreEqual(r, new Vector2(4, 5), "4 5 - 0 0");
+            r -= v2;
+            Assert.AreEqual(r, new Vector2(4, 5), "4 5 - 0 0");
+        }
+
+        [Test]
+        public void Addition() {
+            Vector2 v1 = new(4, 5);
+            Vector2 v2 = new(1, 2);
+            Vector2 r = Vector2.zero;
+
+            r = v1 + v2;
+            Assert.IsTrue(r == new Vector2(5, 7), "4 5 + 1 2");
+
+            v2 = new(-1, -2);
+            r = v1 + v2;
+            Assert.IsTrue(r == new Vector2(3, 3), "4  5 + -1 -2");
+            r = v1;
+            r += v2;
+            Assert.AreEqual(r, new Vector2(3, 3), "4 5 + -1 -2");
+
+            v2 = new(0, 0);
+            r = v1 + v2;
+            Assert.AreEqual(r, new Vector2(4, 5), "4 5 + 0 0");
+            r += v2;
+            Assert.AreEqual(r, new Vector2(4, 5), "4 5 + 0 0");
+        }
+
+        [Test]
+        public void Scale() {
+            Vector2 v1 = new(4, 5);
+            Vector2 v2 = new(1, 2);
+            Vector2 r;
+
+            r = Vector2.Scale(v1, v2);
+            Assert.AreEqual(4, r.horizontal, "Scale 4 5 , 1 2  H");
+            Assert.AreEqual(10, r.vertical, "Scale 4 5 , 1 2  V");
+
+            v2 = new(-1, -2);
+            r = Vector2.Scale(v1, v2);
+            Assert.AreEqual(-4, r.horizontal, "Scale 4 5 , -1 -2  H");
+            Assert.AreEqual(-10, r.vertical, "Scale 4 5 , -1 -2  V");
+
+            v2 = new(0, 0);
+            r = Vector2.Scale(v1, v2);
+            Assert.AreEqual(0, r.horizontal, "Scale 4 5 , 0 0  H");
+            Assert.AreEqual(0, r.vertical, "Scale 4 5 , 0 0  V");
+        }
+
+        [Test]
+        public void Multiply() {
+            Vector2 v1 = new(4, 5);
+            int f = 3;
+            Vector2 r;
+
+            r = v1 * f;
+            Assert.AreEqual(12, r.horizontal, "4 5 * 3  H");
+            Assert.AreEqual(15, r.vertical, "4 5 * 3  V");
+
+            r = f * v1;
+            Assert.AreEqual(12, r.horizontal, "3 * 4 5  H");
+            Assert.AreEqual(15, r.vertical, "3 * 4 5  V");
+
+            f = -3;
+            r = v1 * f;
+            Assert.AreEqual(-12, r.horizontal, "4 5 * -3  H");
+            Assert.AreEqual(-15, r.vertical, "4 5 * -3  V");
+
+            f = 0;
+            r = v1 * f;
+            Assert.AreEqual(0, r.horizontal, "4 5 * 0  H");
+            Assert.AreEqual(0, r.vertical, "4 5 * 0  V");
+        }
+
+        [Test]
+        public void Divide() {
+            Vector2 v1 = new(4, 5);
+            int f = 2;
+            Vector2 r;
+
+            r = v1 / f;
+            Assert.AreEqual(2, r.horizontal, "4 5 / 2  H");
+            Assert.AreEqual(2, r.vertical, "4 5 / 2  V");
+
+            f = -2;
+            r = v1 / f;
+            Assert.AreEqual(-2, r.horizontal, "4 5 / -2 H");
+            Assert.AreEqual(-2, r.vertical, "4 5 / -2  V");
+
+            Assert.Throws<System.DivideByZeroException>(() => {
+                f = 0;
+                r = v1 / f;
+                Assert.AreEqual(float.PositiveInfinity, r.horizontal, "4 5 / 0  H");
+                Assert.AreEqual(float.PositiveInfinity, r.vertical, "4 5 / 0  V");
+            });
+        }
+
+        [Test]
+        public void Dot() {
+            Vector2 v1 = new(4, 5);
+            Vector2 v2 = new(1, 2);
+            int f;
+
+            f = Vector2.Dot(v1, v2);
+            Assert.AreEqual(14, f, "Dot(4 5, 1 2)");
+
+            v2 = new(-1, -2);
+            f = Vector2.Dot(v1, v2);
+            Assert.AreEqual(-14, f, "Dot(4 5, -1 -2)");
+
+            v2 = new(0, 0);
+            f = Vector2.Dot(v1, v2);
+            Assert.AreEqual(0, f, "Dot(4 5, 0 0)");
+        }
+
+    }
+}
+#endif

test/Vector3FloatTest.cs

@@ -0,0 +1,581 @@
+#if !UNITY_5_6_OR_NEWER
+using NUnit.Framework;
+
+namespace LinearAlgebra.Test {
+    using Vector3 = Vector3Float;
+
+    public class Vector3FloatTest {
+
+        [Test]
+        public void FromSpherical() {
+            Vector3 v = new(0, 0, 1);
+            Spherical s = Spherical.FromVector3(v);
+            Vector3 r = Vector3.FromSpherical(s);
+
+            Assert.AreEqual(0, r.horizontal, "0 0 1");
+            Assert.AreEqual(0, r.vertical, 1.0e-06, "0 0 1");
+            Assert.AreEqual(1, r.depth, "0 0 1");
+
+            v = new(0, 1, 0);
+            s = Spherical.FromVector3(v);
+            r = Vector3.FromSpherical(s);
+            Assert.AreEqual(0, r.horizontal, "0 0 1");
+            Assert.AreEqual(1, r.vertical, "0 0 1");
+            Assert.AreEqual(0, r.depth, 1.0e-06, "0 0 1");
+
+            v = new(1, 0, 0);
+            s = Spherical.FromVector3(v);
+            r = Vector3.FromSpherical(s);
+            Assert.AreEqual(1, r.horizontal, "0 0 1");
+            Assert.AreEqual(0, r.vertical, 1.0e-06, "0 0 1");
+            Assert.AreEqual(0, r.depth, 1.0e-06, "0 0 1");
+        }
+
+        [Test]
+        public void Magnitude() {
+            Vector3 v = new(1, 2, 3);
+            float m = 0;
+
+            m = v.magnitude;
+            Assert.AreEqual(3.7416575f, m, "magnitude 1 2 3");
+
+            m = Vector3.MagnitudeOf(v);
+            Assert.AreEqual(3.7416575f, m, "MagnitudeOf 1 2 3");
+
+            v = new(-1, -2, -3);
+            m = v.magnitude;
+            Assert.AreEqual(3.7416575f, m, "magnitude -1 -2 -3");
+
+            v = new(0, 0, 0);
+            m = v.magnitude;
+            Assert.AreEqual(0, m, "magnitude 0 0 0");
+
+            // Infinity tests are still missing
+        }
+
+        [Test]
+        public void SqrMagnitude() {
+            Vector3 v = new(1, 2, 3);
+            float m = 0;
+
+            m = v.sqrMagnitude;
+            Assert.AreEqual(14, m, "sqrMagnitude 1 2 3");
+
+            m = Vector3.SqrMagnitudeOf(v);
+            Assert.AreEqual(14, m, "SqrMagnitudeOf 1 2 3");
+
+            v = new(-1, -2, -3);
+            m = v.sqrMagnitude;
+            Assert.AreEqual(14, m, "sqrMagnitude -1 -2 -3");
+
+            v = new(0, 0, 0);
+            m = v.sqrMagnitude;
+            Assert.AreEqual(0, m, "sqrMagnitude 0 0 0");
+
+            // Infinity tests are still missing
+        }
+
+        [Test]
+        public void Normalize() {
+            Vector3 v = new(0, 2, 0);
+            Vector3 r;
+
+            r = v.normalized;
+            Assert.AreEqual(new Vector3(0, 1, 0), r, "normalized 0 2 0");
+
+            r = Vector3.Normalize(v);
+            Assert.AreEqual(new Vector3(0, 1, 0), r, "Normalize 0 2 0");
+
+            v = new(0, -2, 0);
+            r = v.normalized;
+            Assert.AreEqual(new Vector3(0, -1, 0), r, "normalized 0 -2 0");
+            v = new(0, 0, 0);
+            r = v.normalized;
+            Assert.AreEqual(new Vector3(0, 0, 0), r, "normalized 0 0 0");
+
+            v = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
+            r = v.normalized;
+            Assert.IsTrue(float.IsNaN(r.horizontal), "normalized infinity infinity infinity");
+            Assert.IsTrue(float.IsNaN(r.vertical), "normalized infinity infinity infinity");
+            Assert.IsTrue(float.IsNaN(r.depth), "normalized infinity infinity infinity");
+
+            v = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity);
+            r = v.normalized;
+            Assert.IsTrue(float.IsNaN(r.horizontal), "normalized -infinity -infinity -infinity");
+            Assert.IsTrue(float.IsNaN(r.vertical), "normalized -infinity -infinity -infinity");
+            Assert.IsTrue(float.IsNaN(r.depth), "normalized -infinity -infinity -infinity");
+        }
+
+        [Test]
+        public void Negate() {
+            Vector3 v = new(4, 5, 6);
+            Vector3 r;
+
+            r = -v;
+            Assert.AreEqual(-4, r.horizontal, "- 4 5 6 H");
+            Assert.AreEqual(-5, r.vertical, "- 4 5 6 V");
+            Assert.AreEqual(-6, r.depth, "- 4 5 6 D");
+
+            v = new(-4, -5, -6);
+            r = -v;
+            Assert.AreEqual(4, r.horizontal, "- -4 -5 -6 H");
+            Assert.AreEqual(5, r.vertical, "- -4 -5 -6 V");
+            Assert.AreEqual(6, r.depth, "- -4 -5 -6 D");
+
+            v = new(0, 0, 0);
+            r = -v;
+            Assert.AreEqual(new Vector3(0, 0, 0), r, "- 0 0 0");
+            Assert.AreEqual(0, r.horizontal, "- 0 0 0 H");
+            Assert.AreEqual(0, r.vertical, "- 0 0 0 V");
+            Assert.AreEqual(0, r.depth, "- 0 0 0 D");
+
+
+            v = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
+            r = -v;
+            Assert.AreEqual(new Vector3(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity), r, "- inifinty infinity infinity");
+
+            v = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity);
+            r = -v;
+            Assert.AreEqual(new Vector3(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity), r, "- -inifinty -infinity -infinity");
+        }
+
+        [Test]
+        public void Subtract() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            Vector3 r = Vector3.zero;
+
+            r = v1 - v2;
+            Assert.IsTrue(r == new Vector3(3, 3, 3), "4 5 6 - 1 2 3");
+
+            v2 = new(-1, -2, -3);
+            r = v1 - v2;
+            Assert.IsTrue(r == new Vector3(5, 7, 9), "4 5 6 - -1 -2 -3");
+
+            v2 = new(4, 5, 6);
+            r = v1 - v2;
+            Assert.IsTrue(r == new Vector3(0, 0, 0), "4 5 6 - 4 5 6");
+            r = v1;
+            r -= v2;
+            Assert.AreEqual(r, new Vector3(0, 0, 0), "4 5 6 - 4 5 6");
+
+            v2 = new(0, 0, 0);
+            r = v1 - v2;
+            Assert.AreEqual(r, new Vector3(4, 5, 6), "4 5 6 - 0 0 0");
+            r -= v2;
+            Assert.AreEqual(r, new Vector3(4, 5, 6), "4 5 6 - 0 0 0");
+
+            // Infinity tests are still missing
+        }
+
+        [Test]
+        public void Addition() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            Vector3 r = Vector3.zero;
+
+            r = v1 + v2;
+            Assert.IsTrue(r == new Vector3(5, 7, 9), "4 5 6 + 1 2 3");
+
+            v2 = new(-1, -2, -3);
+            r = v1 + v2;
+            Assert.IsTrue(r == new Vector3(3, 3, 3), "4 5 6 + -1 -2 -3");
+            r = v1;
+            r += v2;
+            Assert.AreEqual(r, new Vector3(3, 3, 3), "4 5 6 + -1 -2 -3");
+
+            v2 = new(0, 0, 0);
+            r = v1 + v2;
+            Assert.AreEqual(r, new Vector3(4, 5, 6), "4 5 6 + 0 0 0");
+            r += v2;
+            Assert.AreEqual(r, new Vector3(4, 5, 6), "4 5 6 + 0 0 0");
+
+            // Infinity tests are still missing
+        }
+
+        [Test]
+        public void Scale() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            Vector3 r;
+
+            r = Vector3.Scale(v1, v2);
+            Assert.AreEqual(4, r.horizontal, "Scale 4 5 6 , 1 2 3  H");
+            Assert.AreEqual(10, r.vertical, "Scale 4 5 6 , 1 2 3  V");
+            Assert.AreEqual(18, r.depth, "Scale 4 5 6 , 1 2 3  D");
+
+            v2 = new(-1, -2, -3);
+            r = Vector3.Scale(v1, v2);
+            Assert.AreEqual(-4, r.horizontal, "Scale 4 5 6 , -1 -2 -3  H");
+            Assert.AreEqual(-10, r.vertical, "Scale 4 5 6 , -1 -2 -3  V");
+            Assert.AreEqual(-18, r.depth, "Scale 4 5 6 , -1 -2 -3  D");
+
+            v2 = new(0, 0, 0);
+            r = Vector3.Scale(v1, v2);
+            Assert.AreEqual(0, r.horizontal, "Scale 4 5 6 , 0 0 0  H");
+            Assert.AreEqual(0, r.vertical, "Scale 4 5 6 , 0 0 0  V");
+            Assert.AreEqual(0, r.depth, "Scale 4 5 6 , 0 0 0  D");
+
+            v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
+            r = Vector3.Scale(v1, v2);
+            Assert.AreEqual(float.PositiveInfinity, r.horizontal, "Scale 4 5 6 , inf inf inf H");
+            Assert.AreEqual(float.PositiveInfinity, r.vertical, "Scale 4 5 6 , inf inf inf  V");
+            Assert.AreEqual(float.PositiveInfinity, r.depth, "Scale 4 5 6 , inf inf inf  D");
+
+            v2 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity);
+            r = Vector3.Scale(v1, v2);
+            Assert.AreEqual(float.NegativeInfinity, r.horizontal, "Scale 4 5 6 , -inf -inf -inf H");
+            Assert.AreEqual(float.NegativeInfinity, r.vertical, "Scale 4 5 6 , -inf -inf -inf  V");
+            Assert.AreEqual(float.NegativeInfinity, r.depth, "Scale 4 5 6 , -inf -inf -inf  D");
+        }
+
+        [Test]
+        public void Multiply() {
+            Vector3 v1 = new(4, 5, 6);
+            float f = 3;
+            Vector3 r;
+
+            r = v1 * f;
+            Assert.AreEqual(12, r.horizontal, "4 5 6 * 3  H");
+            Assert.AreEqual(15, r.vertical, "4 5 6 * 3  V");
+            Assert.AreEqual(18, r.depth, "4 5 6 * 3  D");
+
+            f = -3;
+            r = v1 * f;
+            Assert.AreEqual(-12, r.horizontal, "4 5 6 * -3  H");
+            Assert.AreEqual(-15, r.vertical, "4 5 6 * -3  V");
+            Assert.AreEqual(-18, r.depth, "4 5 6 * -3  D");
+
+            f = 0;
+            r = v1 * f;
+            Assert.AreEqual(0, r.horizontal, "4 5 6 * 0  H");
+            Assert.AreEqual(0, r.vertical, "4 5 6 * 0  V");
+            Assert.AreEqual(0, r.depth, "4 5 6 * 0  D");
+
+            f = float.PositiveInfinity;
+            r = v1 * f;
+            Assert.AreEqual(float.PositiveInfinity, r.horizontal, "4 5 6 * inf  H");
+            Assert.AreEqual(float.PositiveInfinity, r.vertical, "4 5 6 * inf  V");
+            Assert.AreEqual(float.PositiveInfinity, r.depth, "4 5 6 * inf  D");
+
+            f = float.NegativeInfinity;
+            r = v1 * f;
+            Assert.AreEqual(float.NegativeInfinity, r.horizontal, "4 5 6 * -inf  H");
+            Assert.AreEqual(float.NegativeInfinity, r.vertical, "4 5 6 * -inf  V");
+            Assert.AreEqual(float.NegativeInfinity, r.depth, "4 5 6 * -inf  D");
+        }
+
+        [Test]
+        public void Divide() {
+            Vector3 v1 = new(4, 5, 6);
+            float f = 2;
+            Vector3 r;
+
+            r = v1 / f;
+            Assert.AreEqual(2, r.horizontal, "4 5 6 / 2  H");
+            Assert.AreEqual(2.5, r.vertical, "4 5 6 / 2  V");
+            Assert.AreEqual(3, r.depth, "4 5 6 / 2  D");
+
+            f = -2;
+            r = v1 / f;
+            Assert.AreEqual(-2, r.horizontal, "4 5 6 / -2 H");
+            Assert.AreEqual(-2.5, r.vertical, "4 5 6 / -2  V");
+            Assert.AreEqual(-3, r.depth, "4 5 6 / -2  D");
+
+            f = 0;
+            r = v1 / f;
+            Assert.AreEqual(float.PositiveInfinity, r.horizontal, "4 5 6 / 0  H");
+            Assert.AreEqual(float.PositiveInfinity, r.vertical, "4 5 6 / 0  V");
+            Assert.AreEqual(float.PositiveInfinity, r.depth, "4 5 6 / 0  D");
+
+            f = float.PositiveInfinity;
+            r = v1 / f;
+            Assert.AreEqual(0, r.horizontal, "4 5 6 / inf  H");
+            Assert.AreEqual(0, r.vertical, "4 5 6 / inf  V");
+            Assert.AreEqual(0, r.depth, "4 5 6 / inf  D");
+
+            f = float.NegativeInfinity;
+            r = v1 / f;
+            Assert.AreEqual(0, r.horizontal, "4 5 6 / -inf  H");
+            Assert.AreEqual(0, r.vertical, "4 5 6 / -inf  V");
+            Assert.AreEqual(0, r.depth, "4 5 6 / -inf  D");
+        }
+
+        [Test]
+        public void Dot() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            float f;
+
+            f = Vector3.Dot(v1, v2);
+            Assert.AreEqual(32, f, "Dot(4 5 6, 1 2 3)");
+
+            v2 = new(-1, -2, -3);
+            f = Vector3.Dot(v1, v2);
+            Assert.AreEqual(-32, f, "Dot(4 5 6, -1 -2 -3)");
+
+            v2 = new(0, 0, 0);
+            f = Vector3.Dot(v1, v2);
+            Assert.AreEqual(0, f, "Dot(4 5 6, 0 0 0)");
+
+            v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
+            f = Vector3.Dot(v1, v2);
+            Assert.AreEqual(float.PositiveInfinity, f, "Dot(4 5 6, inf inf inf)");
+
+            v2 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity);
+            f = Vector3.Dot(v1, v2);
+            Assert.AreEqual(float.NegativeInfinity, f, "Dot(4 5 6, -inf -inf -inf)");
+        }
+
+        [Test]
+        public void Equality() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            bool r;
+
+            r = v1 == v2;
+            Assert.IsFalse(r, "4 5 6 == 1 2 3");
+
+            v2 = new(4, 5, 6);
+            r = v1 == v2;
+            Assert.IsTrue(r, "4 5 6 == 4 5 6");
+
+            v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
+            r = v1 == v2;
+            Assert.IsFalse(r, "4 5 6 == inf inf inf");
+
+            v1 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity);
+            r = v1 == v2;
+            Assert.IsFalse(r, "-inf -inf -inf == inf inf inf");
+        }
+
+        [Test]
+        public void Distance() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            float f;
+
+            f = Vector3.Distance(v1, v2);
+            Assert.AreEqual(5.19615221F, f, "Distance(4 5 6, 1 2 3)");
+
+            v2 = new(-1, -2, -3);
+            f = Vector3.Distance(v1, v2);
+            Assert.AreEqual(12.4498997F, f, "Distance(4 5 6, -1 -2 -3)");
+
+            v2 = new(0, 0, 0);
+            f = Vector3.Distance(v1, v2);
+            Assert.AreEqual(v1.magnitude, f, "Distance(4 5 6, 0 0 0)");
+
+            v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
+            f = Vector3.Distance(v1, v2);
+            Assert.AreEqual(float.PositiveInfinity, f, "Distance(4 5 6, inf inf inf)");
+
+            v2 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity);
+            f = Vector3.Distance(v1, v2);
+            Assert.AreEqual(float.PositiveInfinity, f, "Distance(4 5 6, -inf -inf -inf)");
+        }
+
+        [Test]
+        public void Cross() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            Vector3 r;
+
+            r = Vector3.Cross(v1, v2);
+            Assert.AreEqual(3, r.horizontal, "Cross(4 5 6, 1 2 3) H");
+            Assert.AreEqual(-6, r.vertical, "Cross(4 5 6, 1 2 3)  V");
+            Assert.AreEqual(3, r.depth, "Cross(4 5 6, 1 2 3)  D");
+
+            v2 = new(-1, -2, -3);
+            r = Vector3.Cross(v1, v2);
+            Assert.AreEqual(-3, r.horizontal, "Cross(4 5 6, -1 -2 -3) H");
+            Assert.AreEqual(6, r.vertical, "Cross(4 5 6, -1 -2 -3)  V");
+            Assert.AreEqual(-3, r.depth, "Cross(4 5 6, -1 -2 -3)  D");
+
+            v2 = new(0, 0, 0);
+            r = Vector3.Cross(v1, v2);
+            Assert.AreEqual(0, r.horizontal, "Cross(4 5 6, 0 0 0) H");
+            Assert.AreEqual(0, r.vertical, "Cross(4 5 6, 0 0 0)  V");
+            Assert.AreEqual(0, r.depth, "Cross(4 5 6, 0 0 0)  D");
+
+            v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
+            r = Vector3.Cross(v1, v2);
+            Assert.IsTrue(float.IsNaN(r.horizontal), "Cross(4 5 6, inf inf inf) H");
+            Assert.IsTrue(float.IsNaN(r.vertical), "Cross(4 5 6, inf inf inf)  V");
+            Assert.IsTrue(float.IsNaN(r.depth), "Cross(4 5 6, inf inf inf)  D");
+
+            v2 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity);
+            r = Vector3.Cross(v1, v2);
+            Assert.IsTrue(float.IsNaN(r.horizontal), "Cross(4 5 6, -inf -inf -inf) H");
+            Assert.IsTrue(float.IsNaN(r.vertical), "Cross(4 5 6, -inf -inf -inf)  V");
+            Assert.IsTrue(float.IsNaN(r.depth), "Cross(4 5 6, -inf -inf -inf)  D");
+        }
+
+        [Test]
+        public void Project() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            Vector3 r;
+
+            r = Vector3.Project(v1, v2);
+            Assert.AreEqual(2.28571439F, r.horizontal, "Project(4 5 6, 1 2 3) H");
+            Assert.AreEqual(4.57142878F, r.vertical, "Project(4 5 6, 1 2 3)  V");
+            Assert.AreEqual(6.85714293F, r.depth, "Project(4 5 6, 1 2 3)  D");
+
+            v2 = new(-1, -2, -3);
+            r = Vector3.Project(v1, v2);
+            Assert.AreEqual(2.28571439F, r.horizontal, "Project(4 5 6, -1 -2 -3) H");
+            Assert.AreEqual(4.57142878F, r.vertical, "Project(4 5 6, -1 -2 -3)  V");
+            Assert.AreEqual(6.85714293F, r.depth, "Project(4 5 6, -1 -2 -3)  D");
+
+            v2 = new(0, 0, 0);
+            r = Vector3.Project(v1, v2);
+            Assert.AreEqual(0, r.horizontal, "Project(4 5 6, 0 0 0) H");
+            Assert.AreEqual(0, r.vertical, "Project(4 5 6, 0 0 0)  V");
+            Assert.AreEqual(0, r.depth, "Project(4 5 6, 0 0 0)  D");
+
+            r = Vector3.Project(v2, v1);
+            Assert.AreEqual(0, r.horizontal, "Project(0 0 0, 4 5 6) H");
+            Assert.AreEqual(0, r.vertical, "Project(0 0 0, 4 5 6)  V");
+            Assert.AreEqual(0, r.depth, "Project(0 0 0, 4 5 6)  D");
+
+            v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
+            r = Vector3.Project(v1, v2);
+            Assert.IsTrue(float.IsNaN(r.horizontal), "Project(4 5 6, inf inf inf) H");
+            Assert.IsTrue(float.IsNaN(r.vertical), "Project(4 5 6, inf inf inf)  V");
+            Assert.IsTrue(float.IsNaN(r.depth), "Project(4 5 6, inf inf inf)  D");
+
+            v2 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity);
+            r = Vector3.Project(v1, v2);
+            Assert.IsTrue(float.IsNaN(r.horizontal), "Project(4 5 6, -inf -inf -inf) H");
+            Assert.IsTrue(float.IsNaN(r.vertical), "Project(4 5 6, -inf -inf -inf)  V");
+            Assert.IsTrue(float.IsNaN(r.depth), "Project(4 5 6, -inf -inf -inf)  D");
+        }
+
+        [Test]
+        public void ProjectOnPlane() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            Vector3 r;
+
+            r = Vector3.ProjectOnPlane(v1, v2);
+            Assert.AreEqual(1.71428561F, r.horizontal, "ProjectOnPlane(4 5 6, 1 2 3) H");
+            Assert.AreEqual(0.428571224F, r.vertical, "ProjectOnPlane(4 5 6, 1 2 3)  V");
+            Assert.AreEqual(-0.857142925F, r.depth, "ProjectOnPlane(4 5 6, 1 2 3)  D");
+
+            v2 = new(-1, -2, -3);
+            r = Vector3.ProjectOnPlane(v1, v2);
+            Assert.AreEqual(1.71428561F, r.horizontal, "ProjectOnPlane(4 5 6, -1 -2 -3) H");
+            Assert.AreEqual(0.428571224F, r.vertical, "ProjectOnPlane(4 5 6, -1 -2 -3)  V");
+            Assert.AreEqual(-0.857142925F, r.depth, "ProjectOnPlane(4 5 6, -1 -2 -3)  D");
+
+            v2 = new(0, 0, 0);
+            r = Vector3.ProjectOnPlane(v1, v2);
+            Assert.AreEqual(4, r.horizontal, "ProjectOnPlane(4 5 6, 0 0 0) H");
+            Assert.AreEqual(5, r.vertical, "ProjectOnPlane(4 5 6, 0 0 0)  V");
+            Assert.AreEqual(6, r.depth, "ProjectOnPlane(4 5 6, 0 0 0)  D");
+
+            r = Vector3.ProjectOnPlane(v2, v1);
+            Assert.AreEqual(0, r.horizontal, "ProjectOnPlane(0 0 0, 4 5 6) H");
+            Assert.AreEqual(0, r.vertical, "ProjectOnPlane(0 0 0, 4 5 6)  V");
+            Assert.AreEqual(0, r.depth, "ProjectOnPlane(0 0 0, 4 5 6)  D");
+
+            v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
+            r = Vector3.ProjectOnPlane(v1, v2);
+            Assert.IsTrue(float.IsNaN(r.horizontal), "ProjectOnPlane(4 5 6, inf inf inf) H");
+            Assert.IsTrue(float.IsNaN(r.vertical), "ProjectOnPlane(4 5 6, inf inf inf)  V");
+            Assert.IsTrue(float.IsNaN(r.depth), "ProjectOnPlane(4 5 6, inf inf inf)  D");
+
+            v2 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity);
+            r = Vector3.ProjectOnPlane(v1, v2);
+            Assert.IsTrue(float.IsNaN(r.horizontal), "ProjectOnPlane(4 5 6, -inf -inf -inf) H");
+            Assert.IsTrue(float.IsNaN(r.vertical), "ProjectOnPlane(4 5 6, -inf -inf -inf)  V");
+            Assert.IsTrue(float.IsNaN(r.depth), "ProjectOnPlane(4 5 6, -inf -inf -inf)  D");
+        }
+
+        [Test]
+        public void UnsignedAngle() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            AngleFloat a;
+
+            a = Vector3.UnsignedAngle(v1, v2);
+            Assert.AreEqual(12.9331379F, a.inDegrees, "Angle(4 5 6, 1 2 3)");
+
+            v2 = new(-1, -2, -3);
+            a = Vector3.UnsignedAngle(v1, v2);
+            Assert.AreEqual(167.066849F, a.inDegrees, "Angle(4 5 6, -1 -2 -3)");
+
+            v2 = new(0, 0, 0);
+            a = Vector3.UnsignedAngle(v1, v2);
+            Assert.AreEqual(0, a.inDegrees, "Angle(4 5 6, 0 0 0)");
+
+            v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
+            a = Vector3.UnsignedAngle(v1, v2);
+            Assert.IsTrue(float.IsNaN(a.inDegrees), "Angle(4 5 6, inf inf inf)");
+
+            v2 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity);
+            a = Vector3.UnsignedAngle(v1, v2);
+            Assert.IsTrue(float.IsNaN(a.inDegrees), "Angle(4 5 6, inf inf inf)");
+        }
+
+        [Test]
+        public void SignedAngle() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            Vector3 v3 = new(7, 8, -9);
+            AngleFloat a;
+
+            a = Vector3.SignedAngle(v1, v2, v3);
+            Assert.AreEqual(-12.9331379F, a.inDegrees, "SignedAngle(4 5 6, 1 2 3, 7 8 -9)");
+
+            v2 = new(-1, -2, -3);
+            a = Vector3.SignedAngle(v1, v2, v3);
+            Assert.AreEqual(167.066849F, a.inDegrees, "SignedAngle(4 5 6, -1 -2 -3, 7 8 -9)");
+
+            v2 = new(0, 0, 0);
+            a = Vector3.SignedAngle(v1, v2, v3);
+            Assert.AreEqual(0, a.inDegrees, "SignedAngle(4 5 6, 0 0 0, 7 8 -9)");
+
+            v2 = new(1, 2, 3);
+            v3 = new(-7, -8, 9);
+            a = Vector3.SignedAngle(v1, v2, v3);
+            Assert.AreEqual(12.9331379F, a.inDegrees, "SignedAngle(4 5 6, 1 2 3, -7 -8 9)");
+
+            v3 = new(0, 0, 0);
+            a = Vector3.SignedAngle(v1, v2, v3);
+            Assert.AreEqual(0, a.inDegrees, "SignedAngle(4 5 6, 1 2 3, 0 0 0)");
+
+            v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
+            a = Vector3.SignedAngle(v1, v2, v3);
+            Assert.IsTrue(float.IsNaN(a.inDegrees), "SignedAngle(4 5 6, inf inf inf, 0 0 0)");
+
+            v2 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity);
+            a = Vector3.SignedAngle(v1, v2, v3);
+            Assert.IsTrue(float.IsNaN(a.inDegrees), "SignedAngle(4 5 6, -inf -inf -inf, 0 0 0)");
+        }
+
+        [Test]
+        public void Lerp() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            Vector3 r;
+
+            r = Vector3.Lerp(v1, v2, 0);
+            Assert.AreEqual(0, Vector3.Distance(r, v1), 0);
+
+            r = Vector3.Lerp(v1, v2, 1);
+            Assert.AreEqual(0, Vector3.Distance(r, v2), 0);
+
+            r = Vector3.Lerp(v1, v2, 0.5f);
+            Assert.AreEqual(0, Vector3.Distance(r, new Vector3(2.5f, 3.5f, 4.5f)), 0);
+
+            r = Vector3.Lerp(v1, v2, -1);
+            Assert.AreEqual(0, Vector3.Distance(r, new Vector3(7, 8, 9)), 0);
+
+            r = Vector3.Lerp(v1, v2, 2);
+            Assert.AreEqual(0, Vector3.Distance(r, new Vector3(-2, -1, 0)), 0);
+        }
+    }
+}
+#endif

test/Vector3IntTest.cs

@@ -0,0 +1,349 @@
+#if !UNITY_5_6_OR_NEWER
+using NUnit.Framework;
+
+namespace LinearAlgebra.Test {
+    using Vector3 = Vector3Int;
+
+    public class Vector3IntTest {
+
+        [Test]
+        public void Equality() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+
+            Assert.IsFalse(v1 == v2, "4 5 6 == 1 2 3");
+            Assert.IsTrue(v1 != v2, "4 5 6 != 1 2 3");
+
+            v2 = new(4, 5, 6);
+            Assert.IsTrue(v1 == v2, "4 5 6 == 4 5 6");
+            Assert.IsFalse(v1 != v2, "4 5 6 != 4 5 6");
+        }
+
+        [Test]
+        public void Magnitude() {
+            Vector3 v = new(1, 2, 3);
+            float m = 0;
+
+            m = v.magnitude;
+            Assert.AreEqual(3.7416575f, m, "magnitude 1 2 3");
+
+            m = Vector3.MagnitudeOf(v);
+            Assert.AreEqual(3.7416575f, m, "MagnitudeOf 1 2 3");
+
+            v = new(-1, -2, -3);
+            m = v.magnitude;
+            Assert.AreEqual(3.7416575f, m, "magnitude -1 -2 -3");
+
+            v = new(0, 0, 0);
+            m = v.magnitude;
+            Assert.AreEqual(0, m, "magnitude 0 0 0");
+
+            // Infinity tests are still missing
+        }
+
+        [Test]
+        public void SqrMagnitude() {
+            Vector3 v = new(1, 2, 3);
+            float m = 0;
+
+            m = v.sqrMagnitude;
+            Assert.AreEqual(14, m, "sqrMagnitude 1 2 3");
+
+            m = Vector3.SqrMagnitudeOf(v);
+            Assert.AreEqual(14, m, "SqrMagnitudeOf 1 2 3");
+
+            v = new(-1, -2, -3);
+            m = v.sqrMagnitude;
+            Assert.AreEqual(14, m, "sqrMagnitude -1 -2 -3");
+
+            v = new(0, 0, 0);
+            m = v.sqrMagnitude;
+            Assert.AreEqual(0, m, "sqrMagnitude 0 0 0");
+
+            // Infinity tests are still missing
+        }
+
+        [Test]
+        public void Distance() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            float f;
+
+            f = Vector3.Distance(v1, v2);
+            Assert.AreEqual(5.19615221F, f, "Distance(4 5 6, 1 2 3)");
+
+            v2 = new(-1, -2, -3);
+            f = Vector3.Distance(v1, v2);
+            Assert.AreEqual(12.4498997F, f, "Distance(4 5 6, -1 -2 -3)");
+
+            v2 = new(0, 0, 0);
+            f = Vector3.Distance(v1, v2);
+            Assert.AreEqual(v1.magnitude, f, "Distance(4 5 6, 0 0 0)");
+        }
+
+        [Test]
+        public void Normalize() {
+            Vector3 v = new(0, 2, 0);
+            Vector3Float r;
+
+            r = v.normalized;
+            //Assert.AreEqual(new Vector3(0, 1, 0), r, "normalized 0 2 0");
+            Assert.AreEqual(0, r.horizontal, "normalized 0 2 0");
+            Assert.AreEqual(1, r.vertical, "normalized 0 2 0");
+            Assert.AreEqual(0, r.depth, "normalized 0 2 0");
+
+            r = Vector3.Normalize(v);
+            Assert.AreEqual(new Vector3Float(0, 1, 0), r, "Normalize 0 2 0");
+
+            v = new(0, -2, 0);
+            r = v.normalized;
+            Assert.AreEqual(new Vector3Float(0, -1, 0), r, "normalized 0 -2 0");
+            v = new(0, 0, 0);
+            r = v.normalized;
+            Assert.AreEqual(new Vector3Float(0, 0, 0), r, "normalized 0 0 0");
+        }
+
+        [Test]
+        public void Negate() {
+            Vector3 v = new(4, 5, 6);
+            Vector3 r;
+
+            r = -v;
+            Assert.AreEqual(-4, r.horizontal, "- 4 5 6 H");
+            Assert.AreEqual(-5, r.vertical, "- 4 5 6 V");
+            Assert.AreEqual(-6, r.depth, "- 4 5 6 D");
+
+            v = new(-4, -5, -6);
+            r = -v;
+            Assert.AreEqual(4, r.horizontal, "- -4 -5 -6 H");
+            Assert.AreEqual(5, r.vertical, "- -4 -5 -6 V");
+            Assert.AreEqual(6, r.depth, "- -4 -5 -6 D");
+
+            v = new(0, 0, 0);
+            r = -v;
+            Assert.AreEqual(new Vector3(0, 0, 0), r, "- 0 0 0");
+            Assert.AreEqual(0, r.horizontal, "- 0 0 0 H");
+            Assert.AreEqual(0, r.vertical, "- 0 0 0 V");
+            Assert.AreEqual(0, r.depth, "- 0 0 0 D");
+        }
+
+        [Test]
+        public void Subtract() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            Vector3 r = Vector3.zero;
+
+            r = v1 - v2;
+            Assert.IsTrue(r == new Vector3(3, 3, 3), "4 5 6 - 1 2 3");
+
+            v2 = new(-1, -2, -3);
+            r = v1 - v2;
+            Assert.IsTrue(r == new Vector3(5, 7, 9), "4 5 6 - -1 -2 -3");
+
+            v2 = new(4, 5, 6);
+            r = v1 - v2;
+            Assert.IsTrue(r == new Vector3(0, 0, 0), "4 5 6 - 4 5 6");
+            r = v1;
+            r -= v2;
+            Assert.AreEqual(r, new Vector3(0, 0, 0), "4 5 6 - 4 5 6");
+
+            v2 = new(0, 0, 0);
+            r = v1 - v2;
+            Assert.AreEqual(r, new Vector3(4, 5, 6), "4 5 6 - 0 0 0");
+            r -= v2;
+            Assert.AreEqual(r, new Vector3(4, 5, 6), "4 5 6 - 0 0 0");
+
+            // Infinity tests are still missing
+        }
+
+        [Test]
+        public void Addition() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            Vector3 r = Vector3.zero;
+
+            r = v1 + v2;
+            Assert.IsTrue(r == new Vector3(5, 7, 9), "4 5 6 + 1 2 3");
+
+            v2 = new(-1, -2, -3);
+            r = v1 + v2;
+            Assert.IsTrue(r == new Vector3(3, 3, 3), "4 5 6 + -1 -2 -3");
+            r = v1;
+            r += v2;
+            Assert.AreEqual(r, new Vector3(3, 3, 3), "4 5 6 + -1 -2 -3");
+
+            v2 = new(0, 0, 0);
+            r = v1 + v2;
+            Assert.AreEqual(r, new Vector3(4, 5, 6), "4 5 6 + 0 0 0");
+            r += v2;
+            Assert.AreEqual(r, new Vector3(4, 5, 6), "4 5 6 + 0 0 0");
+
+            // Infinity tests are still missing
+        }
+
+        [Test]
+        public void Scale() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            Vector3 r;
+
+            r = Vector3.Scale(v1, v2);
+            Assert.AreEqual(4, r.horizontal, "Scale 4 5 6 , 1 2 3  H");
+            Assert.AreEqual(10, r.vertical, "Scale 4 5 6 , 1 2 3  V");
+            Assert.AreEqual(18, r.depth, "Scale 4 5 6 , 1 2 3  D");
+
+            v2 = new(-1, -2, -3);
+            r = Vector3.Scale(v1, v2);
+            Assert.AreEqual(-4, r.horizontal, "Scale 4 5 6 , -1 -2 -3  H");
+            Assert.AreEqual(-10, r.vertical, "Scale 4 5 6 , -1 -2 -3  V");
+            Assert.AreEqual(-18, r.depth, "Scale 4 5 6 , -1 -2 -3  D");
+
+            v2 = new(0, 0, 0);
+            r = Vector3.Scale(v1, v2);
+            Assert.AreEqual(0, r.horizontal, "Scale 4 5 6 , 0 0 0  H");
+            Assert.AreEqual(0, r.vertical, "Scale 4 5 6 , 0 0 0  V");
+            Assert.AreEqual(0, r.depth, "Scale 4 5 6 , 0 0 0  D");
+        }
+
+        [Test]
+        public void Multiply() {
+            Vector3 v1 = new(4, 5, 6);
+            int f = 3;
+            Vector3 r;
+
+            r = v1 * f;
+            Assert.AreEqual(12, r.horizontal, "4 5 6 * 3  H");
+            Assert.AreEqual(15, r.vertical, "4 5 6 * 3  V");
+            Assert.AreEqual(18, r.depth, "4 5 6 * 3  D");
+
+            r = f * v1;
+            Assert.AreEqual(12, r.horizontal, "3 * 4 5 6  H");
+            Assert.AreEqual(15, r.vertical, "3 * 4 5 6  V");
+            Assert.AreEqual(18, r.depth, "3 * 4 5 6  D");
+
+            f = -3;
+            r = v1 * f;
+            Assert.AreEqual(-12, r.horizontal, "4 5 6 * -3  H");
+            Assert.AreEqual(-15, r.vertical, "4 5 6 * -3  V");
+            Assert.AreEqual(-18, r.depth, "4 5 6 * -3  D");
+
+            f = 0;
+            r = v1 * f;
+            Assert.AreEqual(0, r.horizontal, "4 5 6 * 0  H");
+            Assert.AreEqual(0, r.vertical, "4 5 6 * 0  V");
+            Assert.AreEqual(0, r.depth, "4 5 6 * 0  D");
+        }
+
+        [Test]
+        public void Divide() {
+            Vector3 v1 = new(4, 5, 6);
+            int f = 2;
+            Vector3 r;
+
+            r = v1 / f;
+            Assert.AreEqual(2, r.horizontal, "4 5 6 / 2  H");
+            Assert.AreEqual(2, r.vertical, "4 5 6 / 2  V");
+            Assert.AreEqual(3, r.depth, "4 5 6 / 2  D");
+
+            f = -2;
+            r = v1 / f;
+            Assert.AreEqual(-2, r.horizontal, "4 5 6 / -2 H");
+            Assert.AreEqual(-2, r.vertical, "4 5 6 / -2  V");
+            Assert.AreEqual(-3, r.depth, "4 5 6 / -2  D");
+
+            Assert.Throws<System.DivideByZeroException>(() => {
+                f = 0;
+                r = v1 / f;
+            });
+        }
+
+        [Test]
+        public void Dot() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            float f;
+
+            f = Vector3.Dot(v1, v2);
+            Assert.AreEqual(32, f, "Dot(4 5 6, 1 2 3)");
+
+            v2 = new(-1, -2, -3);
+            f = Vector3.Dot(v1, v2);
+            Assert.AreEqual(-32, f, "Dot(4 5 6, -1 -2 -3)");
+
+            v2 = new(0, 0, 0);
+            f = Vector3.Dot(v1, v2);
+            Assert.AreEqual(0, f, "Dot(4 5 6, 0 0 0)");
+        }
+
+        [Test]
+        public void Cross() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            Vector3 r;
+
+            r = Vector3.Cross(v1, v2);
+            Assert.AreEqual(3, r.horizontal, "Cross(4 5 6, 1 2 3) H");
+            Assert.AreEqual(-6, r.vertical, "Cross(4 5 6, 1 2 3)  V");
+            Assert.AreEqual(3, r.depth, "Cross(4 5 6, 1 2 3)  D");
+
+            v2 = new(-1, -2, -3);
+            r = Vector3.Cross(v1, v2);
+            Assert.AreEqual(-3, r.horizontal, "Cross(4 5 6, -1 -2 -3) H");
+            Assert.AreEqual(6, r.vertical, "Cross(4 5 6, -1 -2 -3)  V");
+            Assert.AreEqual(-3, r.depth, "Cross(4 5 6, -1 -2 -3)  D");
+
+            v2 = new(0, 0, 0);
+            r = Vector3.Cross(v1, v2);
+            Assert.AreEqual(0, r.horizontal, "Cross(4 5 6, 0 0 0) H");
+            Assert.AreEqual(0, r.vertical, "Cross(4 5 6, 0 0 0)  V");
+            Assert.AreEqual(0, r.depth, "Cross(4 5 6, 0 0 0)  D");
+        }
+
+        [Test]
+        public void UnsignedAngle() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            AngleFloat a;
+
+            a = Vector3.UnsignedAngle(v1, v2);
+            Assert.AreEqual(12.9331379F, a.inDegrees, "Angle(4 5 6, 1 2 3)");
+
+            v2 = new(-1, -2, -3);
+            a = Vector3.UnsignedAngle(v1, v2);
+            Assert.AreEqual(167.066849F, a.inDegrees, "Angle(4 5 6, -1 -2 -3)");
+
+            v2 = new(0, 0, 0);
+            a = Vector3.UnsignedAngle(v1, v2);
+            Assert.AreEqual(0, a.inDegrees, "Angle(4 5 6, 0 0 0)");
+        }
+
+        [Test]
+        public void SignedAngle() {
+            Vector3 v1 = new(4, 5, 6);
+            Vector3 v2 = new(1, 2, 3);
+            Vector3 v3 = new(7, 8, -9);
+            AngleFloat a;
+
+            a = Vector3.SignedAngle(v1, v2, v3);
+            Assert.AreEqual(-12.9331379F, a.inDegrees, "SignedAngle(4 5 6, 1 2 3, 7 8 -9)");
+
+            v2 = new(-1, -2, -3);
+            a = Vector3.SignedAngle(v1, v2, v3);
+            Assert.AreEqual(167.066849F, a.inDegrees, "SignedAngle(4 5 6, -1 -2 -3, 7 8 -9)");
+
+            v2 = new(0, 0, 0);
+            a = Vector3.SignedAngle(v1, v2, v3);
+            Assert.AreEqual(0, a.inDegrees, "SignedAngle(4 5 6, 0 0 0, 7 8 -9)");
+
+            v2 = new(1, 2, 3);
+            v3 = new(-7, -8, 9);
+            a = Vector3.SignedAngle(v1, v2, v3);
+            Assert.AreEqual(12.9331379F, a.inDegrees, "SignedAngle(4 5 6, 1 2 3, -7 -8 9)");
+
+            v3 = new(0, 0, 0);
+            a = Vector3.SignedAngle(v1, v2, v3);
+            Assert.AreEqual(0, a.inDegrees, "SignedAngle(4 5 6, 1 2 3, 0 0 0)");
+        }
+    }
+}
+#endif