Implement additional Matrix unit tests

.gitignore

@@ -0,0 +1,2 @@
+/cmake-build-debug
+/.idea

Doxyfile

@@ -61,7 +61,7 @@ PROJECT_BRIEF          = "Linear Algebra, Geometry, and Algorithms in C++"
 # pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
 # the logo to the output directory.
 
-PROJECT_LOGO           =
+PROJECT_LOGO           = logo_light_small.png
 
 # The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
 # into which the generated documentation will be written. If a relative path is

README.md

@@ -1,5 +1,6 @@
 # Josh's 3D Math Library - J3ML
 
+![J3ML Logo](logo_light_small.png)
 
 Yet Another C++ Math Standard
 

include/J3ML/Algorithm/RNG.h

@@ -1,11 +1,10 @@
 #pragma once
 
-
 #include "J3ML/J3ML.h"
 
 namespace J3ML::Algorithm
 {
-/** @brief A linear congruential random number generator.
+    /** @brief A linear congruential random number generator.
 
 	Uses D.H. Lehmer's Linear Congruential Method (1949) for generating random numbers.
 	Supports both Multiplicative Congruential Method (increment==0) and
@@ -45,14 +44,18 @@ namespace J3ML::Algorithm
     class RNG {
     public:
         /// Initializes the generator from the current system clock.
-        RNG();
+        RNG()
+        {
+            // TODO: Implement Clock class
+            Seed(0);
+        }
         RNG(u32 seed, u32 multiplier = 69621,
             u32 increment = 0, u32 modulus = 0x7FFFFFFF) // 2^31 - 1
         {
             Seed(seed, multiplier, increment, modulus);
         }
         /// Reinitializes the generator to the new settings.
-        void Seed(u32 seed, u32 multiplier, u32 increment, u32 modulus = 0x7FFFFFFF);
+        void Seed(u32 seed, u32 multiplier = 69621, u32 increment = 0, u32 modulus = 0x7FFFFFFF);
         /// Returns a random integer picked uniformly in the range [0, MaxInt()]
         u32 Int();
         /// Returns the biggest number the generator can yield. [modulus - 1]

include/J3ML/Geometry/AABB.h

@@ -68,6 +68,11 @@ namespace J3ML::Geometry
         float MaxY() const; ///< [similarOverload: MaxX]
         float MaxZ() const; ///< [similarOverload: MaxX]
 
+
+        Vector3 MinPoint() const;
+
+        Vector3 MaxPoint() const;
+
         /// Returns the smallest sphere that contains this AABB.
         /// This function computes the minimal volume sphere that contains all the points inside this AABB
         Sphere MinimalEnclosingSphere() const;
@@ -304,10 +309,10 @@ namespace J3ML::Geometry
         /** This function computes the smallest possible AABB (in terms of volume) that contains the given sphere, and stores the result in this structure. */
         void SetFrom(const Sphere &s);
 
-        Vector3 GetRandomPointInside(RNG& rng) const;
-        Vector3 GetRandomPointOnSurface(RNG& rng) const;
-        Vector3 GetRandomPointOnEdge(RNG& rng) const;
-        Vector3 GetRandomCornerPoint(RNG& rng) const;
+        Vector3 RandomPointInside(RNG& rng) const;
+        Vector3 RandomPointOnSurface(RNG& rng) const;
+        Vector3 RandomPointOnEdge(RNG& rng) const;
+        Vector3 RandomCornerPoint(RNG& rng) const;
 
         /// Sets this structure to a degenerate AABB that does not have any volume.
         /** This function is useful for initializing the AABB to "null" before a loop of calls to Enclose(),

include/J3ML/Geometry/Capsule.h

@@ -7,14 +7,7 @@
 
 namespace J3ML::Geometry
 {
-    // TODO: Move to separate math lib, find duplicates!
-    template <typename T>
-    void Swap(T &a, T &b)
-    {
-        T temp = std::move(a);
-        a = std::move(b);
-        b = std::move(temp);
-    }
+
 
     /// A 3D cylinder with spherical ends.
     class Capsule : public Shape

include/J3ML/Geometry/OBB.h

@@ -30,14 +30,20 @@ namespace J3ML::Geometry {
         inline static int NumEdges() { return 12; }
         inline static int NumVertices() { return 8; }
 
+
+        float MinX() const;
+        float MinY() const;
+        float MinZ() const;
+        float MaxX() const;
+        float MaxY() const;
+        float MaxZ() const;
+
+        Vector3 MinPoint() const;
+        Vector3 MaxPoint() const;
+
         Polyhedron ToPolyhedron() const;
         //PBVolume<6> ToPBVolume() const;
-        AABB MinimalEnclosingAABB() const
-        {
-            AABB aabb;
-            aabb.SetFrom(*this);
-            return aabb;
-        }
+        AABB MinimalEnclosingAABB() const;
 
         bool Intersects(const LineSegment &lineSegment) const;
 

include/J3ML/Geometry/Shape.h

@@ -13,6 +13,7 @@ namespace J3ML::Geometry
     class Shape
     {
     public:
+        virtual ~Shape() = default; //Polymorphic for dynamic_cast.
     protected:
     private:
     };
@@ -20,6 +21,7 @@ namespace J3ML::Geometry
     class Shape2D
     {
     public:
+        virtual ~Shape2D() = default;
     protected:
     private:
     };

include/J3ML/Geometry/Sphere.h

@@ -24,6 +24,28 @@ namespace J3ML::Geometry
         Sphere() {}
         Sphere(const Vector3& pos, float radius) : Position(pos), Radius(radius) {}
 
+        /// Generates a random point on the surface of this sphere
+        /** The points are distributed uniformly.
+            This function uses the rejection method to generate a uniform distribution of points on the surface.
+            Therefore, it is assumed that this sphere is not degenerate, i.e. it has a positive radius.
+            A fixed number of 1000 tries is performed, after which a fixed point on the surface is returned as a fallback.
+            @param rng A pre-seeded random number generator object that is to be used by this function to generate random vlaues.
+            @see class RNG, RandomPointInside(), IsDegenerate()
+            @todo Add Sphere::PointOnSurface(polarYaw, polarPitch). */
+        Vector3 RandomPointOnSurface(RNG& rng) const;
+        static Vector3 RandomPointOnSurface(RNG& rng, const Vector3& center, float radius);
+
+        /// Generates a random point inside this sphere.
+        /** The points are distributed uniformly.
+            This function uses the rejection method to generate a uniform distribution of points inside a sphere.
+            Therefore, it is assumed that this sphere is not degenerate, i.e. it has a positive radius.
+            A fixed number of 1000 tries is performed, after which the sphere center position is returned as a fallback.
+            @param rng A pre-seeded random number generator object that is to be used by this function to generate random values.
+            @see class RNG, RandomPointOnSurface(), IsDegenerate().
+            @todo Add Sphere::Point(polarYaw, polarPitch, radius). */
+        Vector3 RandomPointInside(RNG& rng);
+        static Vector3 RandomPointInside(RNG& rng, const Vector3& center, float radius);
+    public:
         /// Quickly returns an arbitrary point inside this Sphere. Used in GJK intersection test.
         Vector3 AnyPointFast() const { return Position; }
         /// Computes the extreme point of this Sphere in the given direction.

include/J3ML/J3ML.h

@@ -1,7 +1,5 @@
 #pragma once
 
-
-
 //
 // Created by josh on 12/25/2023.
 //
@@ -10,6 +8,15 @@
 #include <string>
 #include <cassert>
 
+// TODO: Move to separate math lib, find duplicates!
+template <typename T>
+void Swap(T &a, T &b)
+{
+    T temp = std::move(a);
+    a = std::move(b);
+    b = std::move(temp);
+}
+
 namespace J3ML::SizedIntegralTypes
 {
     using u8 = uint8_t;

include/J3ML/LinearAlgebra/Matrices.inl

@@ -0,0 +1,105 @@
+#pragma once
+
+/// Template Parameterized (Generic) Matrix Functions.
+
+#include <J3ML/LinearAlgebra/Quaternion.h>
+
+
+
+namespace J3ML::LinearAlgebra {
+
+    /** Sets the top-left 3x3 area of the matrix to the rotation matrix about the X-axis. Elements
+        outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
+        in the order M*v, and clockwise if rotated in the order v*M.
+        @param m The matrix to store the result.
+        @param angle the rotation angle in radians. */
+    template<typename Matrix>
+    void Set3x3PartRotateX(Matrix &m, float angle) {
+        float sinz, cosz;
+        sinz = std::sin(angle);
+        cosz = std::cos(angle);
+
+        m[0][0] = 1.f;
+        m[0][1] = 0.f;
+        m[0][2] = 0.f;
+        m[1][0] = 0.f;
+        m[1][1] = cosz;
+        m[1][2] = -sinz;
+        m[2][0] = 0.f;
+        m[2][1] = sinz;
+        m[2][2] = cosz;
+    }
+
+    /** Sets the top-left 3x3 area of the matrix to the rotation matrix about the Y-axis. Elements
+        outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
+        in the order M*v, and clockwise if rotated in the order v*M.
+        @param m The matrix to store the result
+        @param angle The rotation angle in radians. */
+    template<typename Matrix>
+    void Set3x3PartRotateY(Matrix &m, float angle) {
+        float sinz, cosz;
+        sinz = std::sin(angle);
+        cosz = std::cos(angle);
+
+        m[0][0] = cosz;
+        m[0][1] = 0.f;
+        m[0][2] = sinz;
+        m[1][0] = 0.f;
+        m[1][1] = 1.f;
+        m[1][2] = 0.f;
+        m[2][0] = -sinz;
+        m[2][1] = 0.f;
+        m[2][2] = cosz;
+    }
+
+    /** Sets the top-left 3x3 area of the matrix to the rotation matrix about the Z-axis. Elements
+        outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
+        in the order of M*v, and clockwise if rotated in the order v*M.
+        @param m The matrix to store the result.
+        @param angle The rotation angle in radians. */
+    template<typename Matrix>
+    void Set3x3PartRotateZ(Matrix &m, float angle) {
+        float sinz, cosz;
+        sinz = std::sin(angle);
+        cosz = std::cos(angle);
+
+        m[0][0] = cosz;
+        m[0][1] = -sinz;
+        m[0][2] = 0.f;
+        m[1][0] = sinz;
+        m[1][1] = cosz;
+        m[1][2] = 0.f;
+        m[2][0] = 0.f;
+        m[2][1] = 0.f;
+        m[2][2] = 1.f;
+    }
+
+
+    /** Computes the matrix M = R_x * R_y * R_z, where R_d is the cardinal rotation matrix
+        about the axis +d, rotating counterclockwise.
+        This function was adapted from https://www.geometrictools.com/Documentation/EulerAngles.pdf .
+        Parameters x y and z are the angles of rotation, in radians. */
+    template<typename Matrix>
+    void Set3x3PartRotateEulerXYZ(Matrix &m, float x, float y, float z) {
+        // TODO: vectorize to compute 4 sines + cosines at one time;
+        float cx = std::cos(x);
+        float sx = std::sin(x);
+        float cy = std::cos(y);
+        float sy = std::sin(y);
+        float cz = std::cos(z);
+        float sz = std::sin(z);
+
+        m[0][0] = cy * cz;
+        m[0][1] = -cy * sz;
+        m[0][2] = sy;
+        m[1][0] = cz * sx * sy + cx * sz;
+        m[1][1] = cx * cz - sx * sy * sz;
+        m[1][2] = -cy * sx;
+        m[2][0] = -cx * cz * sy + sx * sz;
+        m[2][1] = cz * sx + cx * sy * sz;
+        m[2][2] = cx * cy;
+    }
+
+
+
+}

include/J3ML/LinearAlgebra/Matrix2x2.h

@@ -2,6 +2,8 @@
 
 
 #include <J3ML/LinearAlgebra/Vector2.h>
+#include <J3ML/LinearAlgebra/Common.h>
+#include <J3ML/LinearAlgebra/Matrices.inl>
 
 namespace J3ML::LinearAlgebra {
     class Matrix2x2 {

include/J3ML/LinearAlgebra/Matrix3x3.h

@@ -1,101 +1,38 @@
 #pragma once
 
+
 #include <J3ML/LinearAlgebra/Common.h>
 #include <J3ML/LinearAlgebra/Vector2.h>
 #include <J3ML/LinearAlgebra/Vector3.h>
 #include <J3ML/LinearAlgebra/Quaternion.h>
+#include <J3ML/Algorithm/RNG.h>
+
+using namespace J3ML::Algorithm;
+
+#include <cstring>
 
 namespace J3ML::LinearAlgebra {
 
-
-
-    /** Sets the top-left 3x3 area of the matrix to the rotation matrix about the X-axis. Elements
-        outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
-        in the order M*v, and clockwise if rotated in the order v*M.
-        @param m The matrix to store the result.
-        @param angle the rotation angle in radians. */
-    template <typename Matrix>
-    void Set3x3PartRotateX(Matrix &m, float angle)
-    {
-        float sinz, cosz;
-        sinz = std::sin(angle);
-        cosz = std::cos(angle);
-
-        m[0][0] = 1.f; m[0][1] =  0.f; m[0][2] =   0.f;
-        m[1][0] = 0.f; m[1][1] = cosz; m[1][2] = -sinz;
-        m[2][0] = 0.f; m[2][1] = sinz; m[2][2] =  cosz;
-    }
-
-    /** Sets the top-left 3x3 area of the matrix to the rotation matrix about the Y-axis. Elements
-        outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
-        in the order M*v, and clockwise if rotated in the order v*M.
-        @param m The matrix to store the result
-        @param angle The rotation angle in radians. */
-    template <typename Matrix>
-    void Set3x3PartRotateY(Matrix &m, float angle)
-    {
-        float sinz, cosz;
-        sinz = std::sin(angle);
-        cosz = std::cos(angle);
-
-        m[0][0] = cosz;  m[0][1] = 0.f; m[0][2] = sinz;
-        m[1][0] = 0.f;   m[1][1] = 1.f; m[1][2] = 0.f;
-        m[2][0] = -sinz; m[2][1] = 0.f; m[2][2] = cosz;
-    }
-
-    /** Sets the top-left 3x3 area of the matrix to the rotation matrix about the Z-axis. Elements
-        outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
-        in the order of M*v, and clockwise if rotated in the order v*M.
-        @param m The matrix to store the result.
-        @param angle The rotation angle in radians. */
-    template <typename Matrix>
-    void Set3x3RotatePartZ(Matrix &m, float angle)
-    {
-        float sinz, cosz;
-        sinz = std::sin(angle);
-        cosz = std::cos(angle);
-
-        m[0][0] = cosz; m[0][1] = -sinz; m[0][2] = 0.f;
-        m[1][0] = sinz; m[1][1] =  cosz; m[1][2] = 0.f;
-        m[2][0] =  0.f; m[2][1] =   0.f; m[2][2] = 1.f;
-    }
-
-
-    /** Computes the matrix M = R_x * R_y * R_z, where R_d is the cardinal rotation matrix
-        about the axis +d, rotating counterclockwise.
-        This function was adapted from https://www.geometrictools.com/Documentation/EulerAngles.pdf .
-        Parameters x y and z are the angles of rotation, in radians. */
-    template <typename Matrix>
-    void Set3x3PartRotateEulerXYZ(Matrix &m, float x, float y, float z)
-    {
-        // TODO: vectorize to compute 4 sines + cosines at one time;
-        float cx = std::cos(x);
-        float sx = std::sin(x);
-        float cy = std::cos(y);
-        float sy = std::sin(y);
-        float cz = std::cos(z);
-        float sz = std::sin(z);
-
-        m[0][0] = cy * cz; m[0][1] = -cy * sz; m[0][2] = sy;
-        m[1][0] = cz*sx*sy + cx*sz; m[1][1] = cx*cz - sx*sy*sz; m[1][2] = -cy*sx;
-        m[2][0] = -cx*cz*sy + sx*sz; m[2][1] = cz*sx + cx*sy*sz; m[2][2] = cx*cy;
-    }
-
-
-    template <typename Matrix>
-    void SetMatrixRotatePart(Matrix &m, const Quaternion &q)
-    {
+    template<typename Matrix>
+    void SetMatrixRotatePart(Matrix &m, const Quaternion &q) {
         // See https://www.geometrictools.com/Documentation/LinearAlgebraicQuaternions.pdf .
 
         assert(q.IsNormalized(1e-3f));
-        const float x = q.x; const float y = q.y; const float z = q.z; const float w = q.w;
-        m[0][0] = 1 - 2*(y*y + z*z); m[0][1] =     2*(x*y - z*w); m[0][2] =     2*(x*y + y*w);
-        m[1][0] =     2*(x*y + z*w); m[1][1] = 1 - 2*(x*x + z*z); m[1][2] =     2*(y*z - x*w);
-        m[2][0] =     2*(x*z - y*w); m[2][1] =     2*(y*z + x*w); m[2][2] = 1 - 2*(x*x + y*y);
+        const float x = q.x;
+        const float y = q.y;
+        const float z = q.z;
+        const float w = q.w;
+        m[0][0] = 1 - 2 * (y * y + z * z);
+        m[0][1] = 2 * (x * y - z * w);
+        m[0][2] = 2 * (x * y + y * w);
+        m[1][0] = 2 * (x * y + z * w);
+        m[1][1] = 1 - 2 * (x * x + z * z);
+        m[1][2] = 2 * (y * z - x * w);
+        m[2][0] = 2 * (x * z - y * w);
+        m[2][1] = 2 * (y * z + x * w);
+        m[2][2] = 1 - 2 * (x * x + y * y);
     }
 
-    class Quaternion;
-
     /// A 3-by-3 matrix for linear transformations of 3D geometry.
     /* This can represent any kind of linear transformations of 3D geometry, which include
      * rotation, scale, shear, mirroring, and orthographic projection.
@@ -110,9 +47,7 @@ namespace J3ML::LinearAlgebra {
      *
      *  @note The member functions in this class use the convention that transforms are applied to
      *  vectors in the form M * v. This means that "Matrix3x3 M, M1, M2; M = M1 * M2;" gives a transformation M
-     *  that applies M2 first, followed by M1 second
-     */
-
+     *  that applies M2 first, followed by M1 second. */
     class Matrix3x3 {
     public: /// Constant Values
         enum { Rows = 3 };
@@ -123,10 +58,11 @@ namespace J3ML::LinearAlgebra {
         static const Matrix3x3 NaN;
     public: /// Constructors
         /// Creates a new Matrix3x3 with uninitalized member values.
-        Matrix3x3() {}
+        Matrix3x3() = default;
 
         Matrix3x3(const Matrix3x3& rhs) { Set(rhs); }
-        Matrix3x3(float val);
+        /// Creates a new Matrix3x3 by setting all matrix elements equal to val.
+        explicit Matrix3x3(float val);
         /// Creates a new Matrix3x3 by explicitly specifying all the matrix elements.
         /// The elements are specified in row-major format, i.e. the first row first followed by the second and third row.
         /// E.g. The element m10 denotes the scalar at second (idx 1) row, first (idx 0) column.
@@ -193,6 +129,19 @@ namespace J3ML::LinearAlgebra {
                 view->world. In GLM, the LookAt function is tied to cameras only, and it returns the inverse mapping world->view.
          */
         static Matrix3x3 LookAt(const Vector3& forward, const Vector3& target, const Vector3& localUp, const Vector3& worldUp);
+
+        // Returns a uniformly random 3x3 matrix that performs only rotation.
+        /** This matrix produces a random orthonormal bassi for an orientation of an object. There is no mirroring
+            or scaling present in the generated matrix. Also, naturally since Matrix3x3 cannot represent translation or projection,
+            these properties are not present either. */
+        static Matrix3x3 RandomRotation(RNG& rng);
+
+        /// Returns a random 3x3 matrix with each entry randomized between the range [minElem, maxElem]
+        /** Warning: The matrices returned by this function do not represent well-formed 3D transformations.
+            This function is mostly used for testing and debugging purposes only. */
+        static Matrix3x3 RandomGeneral(RNG& rng, float minElem, float maxElem);
+
+
         /// Creates a new Matrix3x3 that performs the rotation expressed by the given quaternion.
         static Matrix3x3 FromQuat(const Quaternion& orientation);
         /// Creates a new Matrix3x3 as a combination of rotation and scale.
@@ -220,10 +169,7 @@ namespace J3ML::LinearAlgebra {
         void SetRotatePart(const Vector3& a, float angle);
         void SetRotatePart(const AxisAngle& axisAngle);
         /// Sets this matrix to perform the rotation expressed by the given quaternion.
-        void SetRotatePart(const Quaternion& quat)
-        {
-            SetMatrixRotatePart(*this, quat);
-        }
+        void SetRotatePart(const Quaternion& quat);
 
         /// Returns the given row.
         /** @param row The zero-based index [0, 2] of the row to get. */
@@ -240,16 +186,16 @@ namespace J3ML::LinearAlgebra {
 
         /// Returns the given column.
         /** @param col The zero-based index [0, 2] of the column to get. */
-        Vector3 GetColumn(int index) const;
-        Vector3 Column(int index) const;
-        Vector3 Col(int index) const;
+        [[nodiscard]] Vector3 GetColumn(int index) const;
+        [[nodiscard]] Vector3 Column(int index) const;
+        [[nodiscard]] Vector3 Col(int index) const;
         /// This method also allows assignment to the retrieved column.
         //Vector3& Col(int index);
 
         /// Returns only the first three elements of the given column.
-        Vector3 GetColumn3(int index) const;
-        Vector3 Column3(int index) const;
-        Vector3 Col3(int index) const;
+        [[nodiscard]] Vector3 GetColumn3(int index) const;
+        [[nodiscard]] Vector3 Column3(int index) const;
+        [[nodiscard]] Vector3 Col3(int index) const;
         /// This method also allows assignment to the retrieved column.
         //Vector3& Col3(int index);
 
@@ -278,13 +224,14 @@ namespace J3ML::LinearAlgebra {
         /// Sets this matrix to equal the identity.
         void SetIdentity();
 
-
+        /// Swaps two columns.
         void SwapColumns(int col1, int col2);
 
+        /// Swaps two rows.
         void SwapRows(int row1, int row2);
 
         float &At(int row, int col);
-        float At(int x, int y) const;
+        [[nodiscard]] float At(int x, int y) const;
 
         /// Sets this to be a copy of the matrix rhs.
         void Set(const Matrix3x3 &rhs);
@@ -295,15 +242,21 @@ namespace J3ML::LinearAlgebra {
         /// Sets all values of this matrix.
         /// @param valuesThe values in this array will be copied over to this matrix. The source must contain 9 floats in row-major order
         /// (the same order as the Set() function aove has its input parameters in).
-        void Set(const float *values);
+        void Set(const float *p);
 
         /// Orthonormalizes the basis formed by the column vectors of this matrix.
         void Orthonormalize(int c0, int c1, int c2);
 
+        /// Accesses this structure as a float array.
+        /// @return A pointer to the upper-left element. The data is contiguous in memory.
+        /// ptr[0] gives the element [0][0], ptr[1] is [0][1]. ptr[2] is [0][2]
+        inline float *ptr() { return &elems[0][0];}
+        [[nodiscard]] inline const float *ptr() const {return &elems[0][0];}
+
 
         /// Convers this rotation matrix to a quaternion.
         /// This function assumes that the matrix is orthonormal (no shear or scaling) and does not perform any mirroring (determinant > 0)
-        Quaternion ToQuat() const;
+        [[nodiscard]] Quaternion ToQuat() const;
         /// Attempts to convert this matrix to a quaternion. Returns false if the conversion cannot succeed (this matrix was not a rotation
         /// matrix, and there is scaling ,shearing, or mirroring in this matrix)
         bool TryConvertToQuat(Quaternion& q) const;
@@ -311,16 +264,16 @@ namespace J3ML::LinearAlgebra {
 
         /// Returns the main diagonal.
         /// The main diagonal consists of the elements at m[0][0], m[1][1], m[2][2]
-        Vector3 Diagonal() const;
+        [[nodiscard]] Vector3 Diagonal() const;
         /// Returns the local +X/+Y/+Z axis in world space.
         /// This is the same as transforming the vector{1,0,0} by this matrix.
-        Vector3 WorldX() const;
+        [[nodiscard]] Vector3 WorldX() const;
         /// Returns the local +Y axis in world space.
         /// This is the same as transforming the vector{0,1,0} by this matrix.
-        Vector3 WorldY() const;
+        [[nodiscard]] Vector3 WorldY() const;
         /// Returns the local +Z axis in world space.
         /// This is the same as transforming the vector{0,0,1} by this matrix.
-        Vector3 WorldZ() const;
+        [[nodiscard]] Vector3 WorldZ() const;
 
         /// Computes the determinant of this matrix.
         // If the determinant is nonzero, this matrix is invertible.
@@ -329,23 +282,28 @@ namespace J3ML::LinearAlgebra {
         // "If the determinant is positive, the basis is said to be "positively" oriented (or right-handed).
         // If the determinant is negative, the basis is said to be "negatively" oriented (or left-handed)."
         // @note This function computes 9 LOADs, 9 MULs and 5 ADDs. */
-        float Determinant() const;
+        [[nodiscard]] float Determinant() const;
 
         /// Computes the determinant of a symmetric matrix.
         /** This function can be used to compute the determinant of a matrix in the case the matrix is known beforehand
             to be symmetric. This function is slightly faster than Determinant().
          * @return
          */
-        float DeterminantSymmetric() const;
+        [[nodiscard]] float DeterminantSymmetric() const;
 
         // Returns an inverted copy of this matrix.
-        Matrix3x3 Inverted() const;
+        [[nodiscard]] Matrix3x3 Inverted() const;
 
         // Returns a transposed copy of this matrix.
-        Matrix3x3 Transposed() const;
+        [[nodiscard]] Matrix3x3 Transposed() const;
 
         /// Returns the inverse transpose of this matrix.
-        Matrix3x3 InverseTransposed() const;
+        /// Use the matrix to transform covariant vectors (normal vectors).
+        [[nodiscard]] Matrix3x3 InverseTransposed() const;
+
+        /// Computes the inverse transpose of this matrix.
+        /// Use the matrix to transform covariant vectors (normal vectors).
+        bool InverseTranspose();
 
         /// Inverts this matrix using numerically stable Gaussian elimination.
         /// @return Returns true on success, false otherwise;
@@ -364,7 +322,7 @@ namespace J3ML::LinearAlgebra {
         /** If a matrix is of form M=R*S, where
             R is a rotation matrix and S is a diagonal matrix with non-zero but potentially non-uniform scaling
             factors (possibly mirroring), then the matrix M is column-orthogonal and this function can be used to compute the inverse.
-            Calling this function is faster than calling the generic matrix Inverse() function.\
+            Calling this function is faster than calling the generic matrix Inverted() function.\
             Returns true on success. On failure, the matrix is not modified. This function fails if any of the
             elements of this vector are not finite, or if the matrix contains a zero scaling factor on X, Y, or Z.
             @note The returned matrix will be row-orthogonal, but not column-orthogonal in general.
@@ -376,38 +334,63 @@ namespace J3ML::LinearAlgebra {
         /** If a matrix is of form M=R*S, where R is a rotation matrix and S is either identity or a mirroring matrix, then
             the matrix M is orthonormal and this function can be used to compute the inverse.
             This function is faster than calling InverseOrthogonalUniformScale(), InverseColOrthogonal(), or the generic
-            Inverse().
+            Inverted().
             This function may not be called if this matrix contains any scaling or shearing, but it may contain mirroring.*/
         bool InverseOrthogonalUniformScale();
 
+        /// Inverts a rotation matrix.
+        /// If a matrix is of form M = R*S, where R is a rotation matrix and S is either identity or a mirroring matrix, then
+        /// the matrix M is orthonormal and this function can be used to compute the inverse.
+        /// This function is faster than calling InverseOrthogonalUniformScale(), InverseColOrthogonal(), or the generic Inverted()
+        /// This function may not be called if this matrix contains any scaling or shearing, but it may contain mirroring.
         void InverseOrthonormal();
 
+        /// Inverts a symmetric matrix.
+        /** This function is faster than directly calling Inverted()
+            This function computes 6 LOADs, 9 STOREs, 21 MULs, 1 DIV, 1 CMP, and 8 ADDs.
+            @return True if computing the inverse succeeded, false otherwise (determinant was zero).
+            @note If this function fails, the original matrix is not modified.
+            @note This function operates in-place. */
         bool InverseSymmetric();
 
+        /// Transposes this matrix.
+        /// This operation swaps all elements with respect to the diagonal.
         void Transpose();
 
-        bool InverseTranspose();
 
+        /// Removes the scaling performed by this matrix. That is, decomposes this matrix M into a form M = M' * S, where
+        /// M' has unitary column vectors and S is a diagonal matrix. Then replaces this matrix with M'
+        /// @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0 0 0 1]).
+        /// @note This function does not remove reflection (-1 scale along some axis).
         void RemoveScale();
 
 
-
         // Transforms the given vectors by this matrix M, i.e. returns M * (x,y,z)
-        Vector2 Transform(const Vector2& rhs) const;
-        Vector3 Transform(const Vector3& rhs) const;
+        [[nodiscard]] Vector2 Transform(const Vector2& rhs) const;
+        [[nodiscard]] Vector3 Transform(const Vector3& rhs) const;
 
         /// Performs a batch transformation of the given array.
         void BatchTransform(Vector3 *pointArray, int numPoints) const;
+        /// Performs a batch transformation of the given array.
         void BatchTransform(Vector3 *pointArray, int numPoints, int stride) const;
+        /// Performs a batch transformation of the given array.
+        /// This function ignores the w component of the input vectors. These components are assumed to be either 0 or 1.
         void BatchTransform(Vector4 *vectorArray, int numVectors) const;
+        /// Performs a batch transformation of the given array.
+        /// This function ignores the w component of the input vectors. These components are assumed to be either 0 or 1.
         void BatchTransform(Vector4 *vectorArray, int numVectors, int stride) const;
 
         /// Returns the sum of the diagonal elements of this matrix.
-        float Trace() const;
+        [[nodiscard]] float Trace() const;
 
 
         Matrix3x3 ScaleBy(const Vector3& rhs);
-        Vector3 GetScale() const;
+        [[nodiscard]] Vector3 GetScale() const;
+
+        /// Scales the given row by a scalar value.
+        void ScaleRow(int row, float scalar);
+        /// Scales the given column by a scalar value.
+        void ScaleCol(int col, float scalar);
 
         Vector3 operator[](int row) const;
 
@@ -418,79 +401,88 @@ namespace J3ML::LinearAlgebra {
         /// This function ignores the W component of the given input vector. This component is assumed to be either 0 or 1.
         Vector4 operator * (const Vector4& rhs) const;
 
+        /// Unary operator + allows this structure to be used in an expression '+x'.
+        Matrix3x3 operator + () const { return *this;}
+
+
         /// Multiplies the two matrices.
         Matrix3x3 operator * (const Matrix3x3& rhs) const;
-        Matrix3x3 Mul(const Matrix3x3& rhs) const;
+        [[nodiscard]] Matrix3x3 Mul(const Matrix3x3& rhs) const;
         /// Multiplies the two matrices.
         Matrix4x4 operator * (const Matrix4x4& rhs) const;
-        Matrix4x4 Mul(const Matrix4x4& rhs) const;
+        [[nodiscard]] Matrix4x4 Mul(const Matrix4x4& rhs) const;
 
-        Vector2 Mul(const Vector2& rhs) const;
-        Vector3 Mul(const Vector3& rhs) const;
-        Vector4 Mul(const Vector4& rhs) const;
+        [[nodiscard]] Vector2 Mul(const Vector2& rhs) const;
+        [[nodiscard]] Vector3 Mul(const Vector3& rhs) const;
+        [[nodiscard]] Vector4 Mul(const Vector4& rhs) const;
 
         /// Converts the quaternion to a M3x3 and multiplies the two matrices together.
         Matrix3x3 operator *(const Quaternion& rhs) const;
 
-        Quaternion Mul(const Quaternion& rhs) const;
+        [[nodiscard]] Matrix3x3 Mul(const Quaternion& rhs) const;
 
         // Returns true if the column vectors of this matrix are all perpendicular to each other.
-        bool IsColOrthogonal(float epsilon = 1e-3f) const;
-        bool IsColOrthogonal3(float epsilon = 1e-3f) const { return IsColOrthogonal(epsilon);}
+        [[nodiscard]] bool IsColOrthogonal(float epsilon = 1e-3f) const;
+        [[nodiscard]] bool IsColOrthogonal3(float epsilon = 1e-3f) const { return IsColOrthogonal(epsilon);}
         // Returns true if the row vectors of this matrix are all perpendicular to each other.
-        bool IsRowOrthogonal(float epsilon = 1e-3f) const;
+        [[nodiscard]] bool IsRowOrthogonal(float epsilon = 1e-3f) const;
 
 
-        bool HasUniformScale(float epsilon  = 1e-3f) const;
+        [[nodiscard]] bool HasUniformScale(float epsilon  = 1e-3f) const;
 
-        Vector3 ExtractScale() const;
+        [[nodiscard]] Vector3 ExtractScale() const;
 
 
         /// Tests if this matrix does not contain any NaNs or infs
         /// @return Returns true if the entries of this M3x3 are all finite.
-        bool IsFinite() const;
+        [[nodiscard]] bool IsFinite() const;
 
         /// Tests if this is the identity matrix.
         /// @return Returns true if this matrix is the identity matrix, up to the given epsilon.
-        bool IsIdentity(float epsilon = 1e-3f) const;
+        [[nodiscard]] bool IsIdentity(float epsilon = 1e-3f) const;
         /// Tests if this matrix is in lower triangular form.
         /// @return Returns true if this matrix is in lower triangular form, up to the given epsilon.
-        bool IsLowerTriangular(float epsilon = 1e-3f) const;
+        [[nodiscard]] bool IsLowerTriangular(float epsilon = 1e-3f) const;
         /// Tests if this matrix is in upper triangular form.
         /// @return Returns true if this matrix is in upper triangular form, up to the given epsilon.
-        bool IsUpperTriangular(float epsilon = 1e-3f) const;
+        [[nodiscard]] bool IsUpperTriangular(float epsilon = 1e-3f) const;
         /// Tests if this matrix has an inverse.
         /// @return Returns true if this matrix can be inverted, up to the given epsilon.
-        bool IsInvertible(float epsilon = 1e-3f) const;
+        [[nodiscard]] bool IsInvertible(float epsilon = 1e-3f) const;
         /// Tests if this matrix is symmetric (M == M^T).
         /// The test compares the elements for equality. Up to the given epsilon. A matrix is symmetric if it is its own transpose.
-        bool IsSymmetric(float epsilon = 1e-3f) const;
+        [[nodiscard]] bool IsSymmetric(float epsilon = 1e-3f) const;
         /// Tests if this matrix is skew-symmetric (M == -M^T).
         /// The test compares the elements of this matrix up to the given epsilon. A matrix M is skew-symmetric if the identity M=-M^T holds.
-        bool IsSkewSymmetric(float epsilon = 1e-3f) const;
+        [[nodiscard]] bool IsSkewSymmetric(float epsilon = 1e-3f) const;
         /// Returns true if this matrix does not perform any scaling,
         /** A matrix does not do any scaling if the column vectors of this matrix are normalized in length,
             compared to the given epsilon. Note that this matrix may still perform reflection,
             i.e. it has a -1 scale along some axis.
             @note This function only examines the upper 3-by-3 part of this matrix.
             @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0,0,0,1] */
-        bool HasUnitaryScale(float epsilon = 1e-3f) const;
+        [[nodiscard]] bool HasUnitaryScale(float epsilon = 1e-3f) const;
         /// Returns true if this matrix performs a reflection along some plane.
         /** In 3D space, an even number of reflections corresponds to a rotation about some axis, so a matrix consisting of
             an odd number of consecutive mirror operations can only reflect about one axis. A matrix that contains reflection reverses
             the handedness of the coordinate system. This function tests if this matrix does perform mirroring.
             This occurs if this matrix has a negative determinant.*/
-        bool HasNegativeScale() const;
+        [[nodiscard]] bool HasNegativeScale() const;
+
+
 
         /// Returns true if the column and row vectors of this matrix form an orthonormal set.
         /// @note In math terms, there does not exist such a thing as an 'orthonormal matrix'. In math terms, a matrix
         /// is orthogonal if the column and row vectors are orthogonal *unit* vectors.
         /// In terms of this library however, a matrix is orthogonal if its column and row vectors are orthogonal. (no need to be unitary),
         /// and a matrix is orthonormal if the column and row vectors are orthonormal.
-        bool IsOrthonormal(float epsilon = 1e-3f) const;
+        [[nodiscard]] bool IsOrthonormal(float epsilon = 1e-3f) const;
+
+        /// Returns true if this Matrix3x3 is equal to the given Matrix3x3, up to given per-element epsilon.
+        bool Equals(const Matrix3x3& other, float epsilon = 1e-3f) const;
 
 
     protected: /// Member values
-        float elems[3][3];
+        float elems[3][3]{};
     };
 }

include/J3ML/LinearAlgebra/Matrix4x4.h

@@ -4,11 +4,14 @@
 
 #include <J3ML/LinearAlgebra/Matrix3x3.h>
 #include <J3ML/LinearAlgebra/Quaternion.h>
-
-
 #include <J3ML/LinearAlgebra/Vector4.h>
+#include <J3ML/LinearAlgebra/Matrices.inl>
+#include <J3ML/Algorithm/RNG.h>
+
 #include <algorithm>
 
+using namespace J3ML::Algorithm;
+
 namespace J3ML::LinearAlgebra {
 
     template <typename Matrix>
@@ -158,7 +161,7 @@ namespace J3ML::LinearAlgebra {
         Matrix4x4(float val);
         /// Constructs this Matrix4x4 to represent the same transformation as the given float3x3.
         /** This function expands the last row and column of this matrix with the elements from the identity matrix. */
-        Matrix4x4(const Matrix3x3&);
+        Matrix4x4(const Matrix3x3& m);
         explicit Matrix4x4(const float* data);
 
         /// Constructs a new float4x4 by explicitly specifying all the matrix elements.
@@ -217,12 +220,16 @@ namespace J3ML::LinearAlgebra {
             @see RotateFromTo(). */
         static Matrix4x4 LookAt(const Vector3& localFwd, const Vector3& targetDir, const Vector3& localUp, const Vector3& worldUp);
 
+        /// Returns a random 4x4 matrix with each entry randomized between the range [minElem, maxElem]
+        /** Warning: The matrices returned by this function do not represent well-formed 3D transformations.
+            This function is mostly used for testing and debugging purposes only. */
+        static Matrix4x4 RandomGeneral(RNG& rng, float minElem, float maxElem);
+
         /// Creates a new Matrix4x4 that rotates about one of the principal axes.
         /** Calling RotateX, RotateY, or RotateZ is slightly faster than calling the more generic RotateAxisAngle function.
             @param radians The angle to rotate by, in radians. For example, Pi/4.f equals 45 degrees.
             @param pointOnAxis If specified, the rotation is performed about an axis that passes through this point,
-            and not through the origin. The returned matrix will not be a pure rotation matrix, but will also contain translation.
-         */
+            and not through the origin. The returned matrix will not be a pure rotation matrix, but will also contain translation. */
         static Matrix4x4 RotateX(float radians, const Vector3 &pointOnAxis);
         /// [similarOverload: RotateX] [hideIndex]
         static Matrix4x4 RotateX(float radians);
@@ -293,27 +300,49 @@ namespace J3ML::LinearAlgebra {
         /// Identical to D3DXMatrixPerspectiveLH, except transposed to account for Matrix * vector convention used in J3ML.
         /// See http://msdn.microsoft.com/en-us/library/windows/desktop/bb205352(v=vs.85).aspx
         /// @note Use the M*v multiplication order to project points with this matrix.
-        static Matrix4x4 D3DPerspProjLH(float n, float f, float h, float v);
+        static Matrix4x4 D3DPerspProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
         /// Identical to D3DXMatrixPerspectiveRH, except transposed to account for Matrix * vector convention used in J3ML.
         /// See http://msdn.microsoft.com/en-us/library/windows/desktop/bb205355(v=vs.85).aspx
         /// @note Use the M*v multiplication order to project points with this matrix.
-        static Matrix4x4 D3DPerspProjRH(float n, float f, float h, float v);
+        static Matrix4x4 D3DPerspProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
 
         /// Computes a left-handled orthographic projection matrix for OpenGL.
         /// @note Use the M*v multiplication order to project points with this matrix.
-        static Matrix4x4 OpenGLOrthoProjLH(float n, float f, float h, float v);
+        static Matrix4x4 OpenGLOrthoProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
         /// Computes a right-handled orthographic projection matrix for OpenGL.
         /// @note Use the M*v multiplication order to project points with this matrix.
-        static Matrix4x4 OpenGLOrthoProjRH(float n, float f, float h, float v);
+        static Matrix4x4 OpenGLOrthoProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
 
         /// Computes a left-handed perspective projection matrix for OpenGL.
         /// @note Use the M*v multiplication order to project points with this matrix.
+        /// @param n Near-plane
+        /// @param f Far-plane
+        /// @param h Horizontal FOV
+        /// @param v Vertical FOV
         static Matrix4x4 OpenGLPerspProjLH(float n, float f, float h, float v);
         /// Identical to http://www.opengl.org/sdk/docs/man/xhtml/gluPerspective.xml , except uses viewport sizes instead of FOV to set up the
         /// projection matrix.
         /// @note Use the M*v multiplication order to project points with this matrix.
+        // @param n Near-plane
+        /// @param f Far-plane
+        /// @param h Horizontal FOV
+        /// @param v Vertical FOV
         static Matrix4x4 OpenGLPerspProjRH(float n, float f, float h, float v);
 
+        /// Creates a new transformation matrix that translates by the given offset.
+        static Matrix4x4 Translate(const Vector3& translation);
+        /// Creates a new transformation matrix that scales by the given factors.
+        static Matrix4x4 Scale(const Vector3& scale);
+
+        /// Creates a new Matrix4x4 as a combination of translation, rotation, and scale.
+        /** This function creates a new Matrix4x4 M of the form M = T * R * S, where T is a translation matrix, R is a
+            rotation matrix, and S a scale matrix. Transforming a vector v using this matrix computes the vector
+            v' = M * v = T*R*S*v = (T * (R * (S * v))), which means that the scale operation is applied to the
+            vector first, followed by rotation, and finally translation. */
+        static Matrix4x4 FromTRS(const Vector3& translate, const Quaternion& rotate, const Vector3& scale);
+        static Matrix4x4 FromTRS(const Vector3& translate, const Matrix3x3& rotate, const Vector3& scale);
+        static Matrix4x4 FromTRS(const Vector3& translate, const Matrix4x4& rotate, const Vector3& scale);
+
     public:
         /// Returns the translation part.
         /** The translation part is stored in the fourth column of this matrix.
@@ -365,6 +394,8 @@ namespace J3ML::LinearAlgebra {
         void SetCol(int col, const Vector4& colVector);
         void SetCol(int col, float m_c0, float m_c1, float m_c2, float m_c3);
 
+        void SetCol3(int col, float x, float y, float z);
+
         /// Returns the given row.
         /** @param The zero-based index [0, 3] of the row to get */
         Vector4 GetRow(int row) const;
@@ -394,19 +425,9 @@ namespace J3ML::LinearAlgebra {
         Vector3 GetScale() const;
 
 
-        Matrix4x4 Scale(const Vector3&);
-
         float &At(int row, int col);
         float At(int x, int y) const;
 
-        template <typename T>
-        void Swap(T &a, T &b)
-        {
-            T temp = std::move(a);
-            a = std::move(b);
-            b = std::move(temp);
-        }
-
 
         void SwapColumns(int col1, int col2);
 
@@ -419,7 +440,7 @@ namespace J3ML::LinearAlgebra {
         void ScaleRow3(int row, float scalar);
         void ScaleColumn(int col, float scalar);
         void ScaleColumn3(int col, float scalar);
-        /// Algorithm from Eric Lengyel's Mathematics for 3D Game Programming & Computer Graphics, 2nd Ed.
+        /// Reduces this matrix to its row-echelon form.
         void Pivot();
 
         /// Tests if this matrix does not contain any NaNs or infs.
@@ -472,12 +493,9 @@ namespace J3ML::LinearAlgebra {
         Vector4 Transform(float tx, float ty, float tz, float tw) const;
         Vector4 Transform(const Vector4& rhs) const;
 
-
-        Matrix4x4 Translate(const Vector3& rhs) const;
         static Matrix4x4 FromTranslation(const Vector3& rhs);
 
 
-
         Vector4 operator[](int row);
 
         Matrix4x4 operator-() const;
@@ -507,9 +525,40 @@ namespace J3ML::LinearAlgebra {
         Matrix4x4 &operator = (const Quaternion& rhs);
         Matrix4x4 &operator = (const Matrix4x4& rhs) = default;
 
-
+        /// Returns the scale component of this matrix.
+        /** This function decomposes this matrix M into a form M = M' * S, where M' has the unitary column vectors and S is a diagonal matrix.
+            @return ExtractScale returns the diagonal entries of S, i.e. the scale of the columns of this matrix. If this matrix
+            represents a local->world space transformation for an object, then this scale represents a 'local scale', i.e.
+            scaling that is performed before translating and rotating the object from its local coordinate system to its world
+            position.
+            @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0 0 0 1]). */
         Vector3 ExtractScale() const;
 
+        /// Removes the scaling performed by this matrix. That is, decomposes this matrix M into a form M = M' * S, where
+        /// M' has unitary column vectors and S is a diagonal matrix. Then replaces this matrix with M'.
+        /// @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0 0 0 1]).
+        /// @note This function assumes that this matrix has orthogonal basis vectors (row and column vector sets are orthogonal).
+        /// @note This function does not remove reflection (-1 scale along some axis).
+        void RemoveScale()
+        {
+            float tx = Row3(0).Normalize();
+            float ty = Row3(1).Normalize();
+        }
+
+
+        /// Decomposes this matrix to translate, rotate, and scale parts.
+        /** This function decomposes this matrix M to a form M = T * R * S, where T is a translation matrix, R is a rotation matrix, and S is a scale matrix
+            @note Remember that in the convention of this class, transforms are applied in the order M * v, so scale is
+            applied first, then rotation, and finally the translation last.
+            @note This function assumes that this matrix does not contain projection (The fourth row of this matrix is [0 0 0 1]).
+            @param translate [out] This vector receives the translation component this matrix performs. The translation is applied last
+            after rotation and scaling.
+            @param rotate [out] This object receives the rotation part of this transform.
+            @param scale [out] This vector receives the scaling along the local (before transformation by R) X, Y, and Z axes performed by this matrix. */
+        void Decompose(Vector3& translate, Quaternion& rotate, Vector3& scale) const;
+        void Decompose(Vector3& translate, Matrix3x3& rotate, Vector3& scale) const;
+        void Decompose(Vector3& translate, Matrix4x4& rotate, Vector3& scale) const;
+
         /// Returns true if this matrix only contains uniform scaling, compared to the given epsilon.
         /// @note If the matrix does not really do any scaling, this function returns true (scaling uniformly by a factor of 1).
         /// @note This function only examines the upper 3-by-3 part of this matrix.
@@ -532,7 +581,7 @@ namespace J3ML::LinearAlgebra {
         void Set(float _00, float _01, float _02, float _03,
                  float _10, float _11, float _12, float _13,
                  float _20, float _21, float _22, float _23,
-                 float _30, float _31, float _32, float _34);
+                 float _30, float _31, float _32, float _33);
 
         /// Sets this to be a copy of the matrix rhs.
         void Set(const Matrix4x4 &rhs);
@@ -540,7 +589,7 @@ namespace J3ML::LinearAlgebra {
         /// Sets all values of this matrix.
         /** @param values The values in this array will be copied over to this matrix. The source must contain 16 floats in row-major order
             (the same order as the Set() ufnction above has its input parameters in. */
-        void Set(const float *values);
+        void Set(const float *p);
 
         /// Sets this matrix to equal the identity.
         void SetIdentity();
@@ -570,7 +619,7 @@ namespace J3ML::LinearAlgebra {
         /// If a matrix is of form M=T*R*S, where T is an affine translation matrix
         /// R is a rotation matrix and S is a diagonal matrix with non-zero but pote ntially non-uniform scaling
         /// factors (possibly mirroring), then the matrix M is column-orthogonal and this function can be used to compute the inverse.
-        /// Calling this function is faster than the calling the generic matrix Inverse() function.
+        /// Calling this function is faster than the calling the generic matrix Inverted() function.
         /// Returns true on success. On failure, the matrix is not modified. This function fails if any of the
         /// elements of this vector are not finite, or if the matrix contains a zero scaling factor on X, Y, or Z.
         /// This function may not be called if this matrix contains any projection (last row differs from (0 0 0 1)).
@@ -584,7 +633,7 @@ namespace J3ML::LinearAlgebra {
         /// If a matrix is of form M = T*R*S, where T is an affine translation matrix,
         /// R is a rotation matrix and S is a diagonal matrix with non-zero and uniform scaling factors (possibly mirroring),
         /// then the matrix M is both column- and row-orthogonal and this function can be used to compute this inverse.
-        /// This function is faster than calling InverseColOrthogonal() or the generic Inverse().
+        /// This function is faster than calling InverseColOrthogonal() or the generic Inverted().
         /// Returns true on success. On failure, the matrix is not modified. This function fails if any of the
         /// elements of this vector are not finite, or if the matrix contains a zero scaling factor on X, Y, or Z.
         /// This function may not be called if this matrix contains any shearing or nonuniform scaling.
@@ -594,7 +643,7 @@ namespace J3ML::LinearAlgebra {
         /// Inverts a matrix that is a concatenation of only translate and rotate operations.
         /// If a matrix is of form M = T*R*S, where T is an affine translation matrix, R is a rotation
         /// matrix and S is either identity or a mirroring matrix, then the matrix M is orthonormal and this function can be used to compute the inverse.
-        /// This function is faster than calling InverseOrthogonalUniformScale(), InverseColOrthogonal(), or the generic Inverse().
+        /// This function is faster than calling InverseOrthogonalUniformScale(), InverseColOrthogonal(), or the generic Inverted().
         /// This function may not be called if this matrix contains any scaling or shearing, but it may contain mirroring.
         /// This function may not be called if this matrix contains any projection (last row differs from (0 0 0 1)).
         void InverseOrthonormal();
@@ -613,6 +662,10 @@ namespace J3ML::LinearAlgebra {
         /// Returns the sum of the diagonal elements of this matrix.
         float Trace() const;
 
+
+        /// Returns true if this Matrix4x4 is equal to the given Matrix4x4, up to given per-element epsilon.
+        bool Equals(const Matrix4x4& other, float epsilon = 1e-3f) const;
+
     protected:
         float elems[4][4];
 

include/J3ML/LinearAlgebra/Quaternion.h

@@ -1,92 +1,215 @@
 #pragma once
 
-
-
-
+#include <J3ML/LinearAlgebra/Common.h>
 #include <J3ML/LinearAlgebra/Matrix3x3.h>
 #include <J3ML/LinearAlgebra/Matrix4x4.h>
 #include <J3ML/LinearAlgebra/Vector4.h>
 #include <J3ML/LinearAlgebra/AxisAngle.h>
-//#include <J3ML/LinearAlgebra/AxisAngle.h>
+#include <J3ML/Algorithm/RNG.h>
 #include <cmath>
 
-
-
 namespace J3ML::LinearAlgebra
 {
-
-    class Matrix3x3;
-
-    class Quaternion : public Vector4 {
+    class Quaternion {
     public:
+        /// The identity quaternion performs no rotation when applied to a vector.
+        static const Quaternion Identity;
+        /// A compile-time constant Quaternion with the value (NAN, NAN, NAN, NAN).
+        /// For this constant, each element has the value of quiet NAN, or Not-A-Number.
+        /// @note Never compare a Quaternion to this value! Due to how IEEE floats work, "nan == nan" returns false!
+        /// That is, nothing is equal to NaN, not even NaN itself!
+        static const Quaternion NaN;
+    public:
+        /// The default constructor does not initialize any member values.
         Quaternion();
-
+        /// Copy constructor
         Quaternion(const Quaternion &rhs) = default;
+        /// Constructs a quaternion from the given data buffer.
+        /// @param data An array of four floats to use for the quaternion, in the order 'x,y,z,w.' (== 'i,j,k,w')
+        explicit Quaternion(const float *data);
 
         explicit Quaternion(const Matrix3x3 &rotationMtrx);
-
         explicit Quaternion(const Matrix4x4 &rotationMtrx);
 
-        // @note The input data is not normalized after construction, this has to be done manually.
+        /// @param x The factor of i.
+        /// @param y The factor of j.
+        /// @param z The factor of k.
+        /// @param w The scalar factor (or 'w').
+        /// @note The input data is not normalized after construction, this has to be done manually.
         Quaternion(float X, float Y, float Z, float W);
 
-        // Constructs this quaternion by specifying a rotation axis and the amount of rotation to be performed about that axis
-        // @param rotationAxis The normalized rotation axis to rotate about. If using Vector4 version of the constructor, the w component of this vector must be 0.
-        Quaternion(const Vector3 &rotationAxis, float rotationAngleBetween);
-
-        Quaternion(const Vector4 &rotationAxis, float rotationAngleBetween);
-        //void Inverted();
+        /// Constructs this quaternion by specifying a rotation axis and the amount of rotation to be performed about that axis
+        /// @param rotationAxis The normalized rotation axis to rotate about. If using Vector4 version of the constructor, the w component of this vector must be 0.
+        /// @param rotationAngleRadians The angle to rotate by, in radians. For example, Pi/4.f equals to 45 degrees, Pi/2.f is 90 degrees, etc.
+        /// @see DegToRad()
+        Quaternion(const Vector3 &rotationAxis, float rotationAngleRadians);
+        Quaternion(const Vector4 &rotationAxis, float rotationAngleRadians);
 
         explicit Quaternion(Vector4 vector4);
         explicit Quaternion(const EulerAngle& angle);
         explicit Quaternion(const AxisAngle& angle);
 
+        /// Creates a LookAt quaternion.
+        /** A LookAt quaternion is a quaternion that orients an object to face towards a specified target direction.
+            @param localForward Specifies the forward direction in the local space of the object. This is the direction
+                the model is facing at in its own local/object space, often +X(1,0,0), +Y(0,1,0), or +Z(0,0,1).
+                The vector to pass in here depends on the conventions you or your modeling software is using, and it is best
+                to pick one convention for all your objects, and be consistent.
+                This input parameter must be a normalized vector.
+            @param targetDirection Specifies the desired world space direction the object should look at. This function
+                will compute a quaternion which will rotate the localForward vector to orient towards this targetDirection
+                vector. This input parameter must be a normalized vector.
+            @param localUp Specifies the up direction in the local space of the object. This is the up direction the model
+                was authored in, often +Y (0,1,0) or +Z (0,0,1). The vector to pass in here depends on the conventions you
+                or your modeling software is using, and it is best to pick one convention for all your objects, and be
+                consistent. This input parameter must be a normalized vector. This vector must be perpendicular to the
+                vector localForward, i.e. localForward.Dot(localUp) == 0.
+            @param worldUp Specifies the global up direction of the scene in world space. Simply rotating one vector to
+                coincide with another (localForward->targetDirection) would cause the up direction of the resulting
+                orientation to drift (e.g. the model could be looking at its target its head slanted sideways). To keep
+                the up direction straight, this function orients the localUp direction of the model to point towards
+                the specified worldUp direction (as closely as possible). The worldUp and targetDirection vectors cannot be
+                collinear, but they do not need to be perpendicular either.
+            @return A quaternion that maps the given local space forward direction vector to point towards the given target
+                direction, and the given local up direction towards the given target world up direction. For the returned
+                quaternion Q it holds that M * localForward = targetDirection, and M * localUp lies in the plane spanned
+                by the vectors targetDirection and worldUp.
+            @see RotateFromTo() */
+        static Quaternion LookAt(const Vector3& localForward, const Vector3& targetDirection, const Vector3& localUp, const Vector3& worldUp);
+
+        /// Creates a new Quaternion that rotates about the given axis by the given angle.
+        static Quaternion RotateAxisAngle(const AxisAngle& axisAngle);
+
+        /// Creates a new quaternion that rotates about the positive X axis by the given rotation.
+        static Quaternion RotateX(float angleRadians);
+        /// Creates a new quaternion that rotates about the positive Y axis by the given rotation.
+        static Quaternion RotateY(float angleRadians);
+        /// Creates a new quaternion that rotates about the positive Z axis by the given rotation.
+        static Quaternion RotateZ(float angleRadians);
+
+        /// Creates a new quaternion that rotates sourceDirection vector (in world space) to coincide with the
+        /// targetDirection vector (in world space).
+        /// Rotation is performed about the origin.
+        /// The vectors sourceDirection and targetDirection are assumed to be normalized.
+        /// @note There are multiple such rotations - this function returns the rotation that has the shortest angle
+        /// (when decomposed to axis-angle notation).
+        static Quaternion RotateFromTo(const Vector3& sourceDirection, const Vector3& targetDirection);
+        static Quaternion RotateFromTo(const Vector4& sourceDirection, const Vector4& targetDirection);
+
+        /// Creates a new quaternion that
+        /// 1. rotates sourceDirection vector to coincide with the targetDirection vector, and then
+        /// 2. rotates sourceDirection2 (which was transformed by 1.) to targetDirection2, but keeping the constraint that
+        /// sourceDirection must look at targetDirection
+        static Quaternion RotateFromTo(const Vector3& sourceDirection, const Vector3& targetDirection, const Vector3& sourceDirection2, const Vector3& targetDirection2);
+
+
+        /// Returns a uniformly random unitary quaternion.
+        static Quaternion RandomRotation(J3ML::Algorithm::RNG &rng);
+    public:
         void SetFromAxisAngle(const Vector3 &vector3, float between);
 
         void SetFromAxisAngle(const Vector4 &vector4, float between);
         void SetFrom(const AxisAngle& angle);
 
-        Quaternion Inverse() const;
+        /// Inverses this quaternion in-place.
+        /// @note For optimization purposes, this function assumes that the quaternion is unitary, in which
+        /// case the inverse of the quaternion is simply just the same as its conjugate.
+        /// This function does not detect whether the operation succeeded or failed.
+        void Inverse();
 
-        Quaternion Conjugate() const;
+        /// Returns an inverted copy of this quaternion.
+        [[nodiscard]] Quaternion Inverted() const;
+        /// Computes the conjugate of this quaternion in-place.
+        void Conjugate();
+        /// Returns a conjugated copy of this quaternion.
+        [[nodiscard]] Quaternion Conjugated() const;
 
-        //void Normalize();
-        Vector3 GetWorldX() const;
+        /// Inverses this quaternion in-place.
+        /// Call this function when the quaternion is not known beforehand to be normalized.
+        /// This function computes the inverse proper, and normalizes the result.
+        /// @note Because of the normalization, it does not necessarily hold that q * q.InverseAndNormalize() == id.
+        /// @return Returns the old length of this quaternion (not the old length of the inverse quaternion).
+        float InverseAndNormalize();
 
-        Vector3 GetWorldY() const;
+        /// Returns the local +X axis in the post-transformed coordinate space. This is the same as transforming the vector (1,0,0) by this quaternion.
+        [[nodiscard]] Vector3 WorldX() const;
+        /// Returns the local +Y axis in the post-transformed coordinate space. This is the same as transforming the vector (0,1,0) by this quaternion.
+        [[nodiscard]] Vector3 WorldY() const;
+        /// Returns the local +Z axis in the post-transformed coordinate space. This is the same as transforming the vector (0,0,1) by this quaternion.
+        [[nodiscard]] Vector3 WorldZ() const;
 
-        Vector3 GetWorldZ() const;
+        /// Returns the axis of rotation for this quaternion.
+        [[nodiscard]] Vector3 Axis() const;
 
-        Vector3 GetAxis() const;
+        /// Returns the angle of rotation for this quaternion, in radians.
+        [[nodiscard]] float Angle() const;
 
-        float GetAngle() const;
+        [[nodiscard]] float LengthSquared() const;
+        [[nodiscard]] float Length() const;
 
-        EulerAngle ToEulerAngle() const;
+        [[nodiscard]] EulerAngle ToEulerAngle() const;
 
 
-        Matrix3x3 ToMatrix3x3() const;
+        [[nodiscard]] Matrix3x3 ToMatrix3x3() const;
+        [[nodiscard]] Matrix4x4 ToMatrix4x4() const;
 
-        Matrix4x4 ToMatrix4x4() const;
+        [[nodiscard]] Matrix4x4 ToMatrix4x4(const Vector3 &translation) const;
 
-        Matrix4x4 ToMatrix4x4(const Vector3 &translation) const;
-
-        Vector3 Transform(const Vector3& vec) const;
-        Vector3 Transform(float X, float Y, float Z) const;
+        [[nodiscard]] Vector3 Transform(const Vector3& vec) const;
+        [[nodiscard]] Vector3 Transform(float X, float Y, float Z) const;
         // Note: We only transform the x,y,z components of 4D vectors, w is left untouched
-        Vector4 Transform(const Vector4& vec) const;
-        Vector4 Transform(float X, float Y, float Z, float W) const;
+        [[nodiscard]] Vector4 Transform(const Vector4& vec) const;
+        [[nodiscard]] Vector4 Transform(float X, float Y, float Z, float W) const;
 
-        Quaternion GetInverse() const;
-        Quaternion Lerp(const Quaternion& b, float t) const;
-        Quaternion Slerp(const Quaternion& q2, float t) const;
+        [[nodiscard]] Quaternion Lerp(const Quaternion& b, float t) const;
+        static Quaternion Lerp(const Quaternion &source, const Quaternion& target, float t);
+        [[nodiscard]] Quaternion Slerp(const Quaternion& q2, float t) const;
+        static Quaternion Slerp(const Quaternion &source, const Quaternion& target, float t);
 
-        Quaternion Normalize() const;
+        /// Returns the 'from' vector rotated towards the 'to' vector by the given normalized time parameter.
+        /** This function slerps the given 'form' vector toward the 'to' vector.
+            @param from A normalized direction vector specifying the direction of rotation at t=0
+            @param to A normalized direction vector specifying the direction of rotation at t=1
+            @param t The interpolation time parameter, in the range [0, 1]. Input values outside this range are
+            silently clamped to the [0, 1] interval.
+            @return A spherical linear interpolation of the vector 'from' towards the vector 'to'. */
+        static Vector3 SlerpVector(const Vector3& from, const Vector3& to, float t);
 
+        /// Returns the 'from' vector rotated towards the 'to' vector by the given absolute angle, in radians.
+        /** This function slerps the given 'from' vector towards the 'to' vector.
+            @param from A normalized direction vector specifying the direction of rotation at angleRadians=0.
+            @param to A normalized direction vector specifying the target direction to rotate towards.
+            @param angleRadians The maximum angle to rotate the 'from' vector by, in the range [0, pi]. If the
+                angle between 'from' and 'to' is smaller than this angle, then the vector 'to' is returned.
+                Input values outside this range are silently clamped to the [0, pi] interval.
+            @return A spherical linear interpolation of the vector 'from' towards the vector 'to'. */
+        static Vector3 SlerpVectorAbs(const Vector3 &from, const Vector3& to, float angleRadians);
 
-        static Quaternion LookAt(const Vector3& position, const Vector3& direction, const Vector3& axisUp);
+        /// Normalizes this quaternion in-place.
+        /// Returns the old length of this quaternion, or 0 if normalization failed.
+        float Normalize();
+        /// Returns a normalized copy of this quaternion.
+        [[nodiscard]] Quaternion Normalized() const;
+
+        /// Returns true if the length of this quaternion is one.
+        [[nodiscard]] bool IsNormalized(float epsilon = 1e-5f) const;
+        [[nodiscard]] bool IsInvertible(float epsilon = 1e-3f) const;
+
+        /// Returns true if the entries of this quaternion are all finite.
+        [[nodiscard]] bool IsFinite() const;
+
+        /// Returns true if this quaternion equals rhs, up to the given epsilon.
+        [[nodiscard]] bool Equals(const Quaternion& rhs, float epsilon = 1e-3f) const;
+
+        /// Compares whether this Quaternion and the given Quaternion are identical bit-by-bit in the underlying representation.
+        /// @note Prefer using this over e.g. memcmp, since there can be SSE-related padding in the structures.
+        bool BitEquals(const Quaternion& rhs) const;
+
+        /// @return A pointer to the first element (x). The data is contiguous in memory.
+        /// ptr[0] gives x, ptr[1] gives y, ptr[2] gives z, ptr[3] gives w.
+        inline float *ptr() { return &x; }
+        [[nodiscard]] inline const float *ptr() const { return &x; }
 
-        // TODO: Document (But do not override!) certain math functions that are the same for Vec4
-        // TODO: Double Check which operations need to be overriden for correct behavior!
 
         // Multiplies two quaternions together.
         // The product q1 * q2 returns a quaternion that concatenates the two orientation rotations.
@@ -102,14 +225,37 @@ namespace J3ML::LinearAlgebra
         // Divides a quaternion by another. Divison "a / b" results in a quaternion that rotates the orientation b to coincide with orientation of
         Quaternion operator / (const Quaternion& rhs) const;
         Quaternion operator + (const Quaternion& rhs) const;
+        Quaternion operator / (float scalar) const
+        {
+            assert(!Math::EqualAbs(scalar, 0.f));
+        }
         Quaternion operator + () const;
         Quaternion operator - () const;
-        float Dot(const Quaternion &quaternion) const;
 
-        float Angle() const { return acos(w) * 2.f;}
+        /// Computes the dot product of this and the given quaternion.
+        /// Dot product is commutative.
+        [[nodiscard]] float Dot(const Quaternion &quaternion) const;
 
-        float AngleBetween(const Quaternion& target) const;
+        /// Returns the angle between this and the target orientation (the shortest route) in radians.
+        [[nodiscard]] float AngleBetween(const Quaternion& target) const;
+        /// Returns the axis of rotation to get from this orientation to target orientation (the shortest route).
+        [[nodiscard]] Vector3 AxisFromTo(const Quaternion& target) const;
 
-        AxisAngle ToAxisAngle() const;
+
+        [[nodiscard]] AxisAngle ToAxisAngle() const;
+        void SetFromAxisAngle(const AxisAngle& axisAngle);
+
+        /// Sets this quaternion to represent the same rotation as the given matrix.
+        void Set(const Matrix3x3& matrix);
+        void Set(const Matrix4x4& matrix);
+        void Set(float x, float y, float z, float w);
+        void Set(const Quaternion& q);
+        void Set(const Vector4& v);
+
+    public:
+        float x;
+        float y;
+        float z;
+        float w;
     };
 }

include/J3ML/LinearAlgebra/Vector2.h

@@ -8,12 +8,45 @@ namespace J3ML::LinearAlgebra {
     /// A 2D (x, y) ordered pair.
     class Vector2 {
     public:
+        /// The x component.
+        /// A Vector2 is 8 bytes in size. This element lies in the memory offsets 0-3 of this class
         float x = 0;
+        /// The y component.
+        /// This element is packed to the memory offsets 4-7 of this class.
         float y = 0;
     public:
+        /// Specifies the number of elements in this vector.
         enum {Dimensions = 2};
     public:
-        /// Default Constructor - Initializes values to zero
+
+        /// @note Due to static data initialization order being undefined in C++,
+        /// do NOT use these members to initialize other static data in other compilation units!
+
+        /// Specifies a compile-time constant Vector2 with value (0,0).
+        static const Vector2 Zero;
+        /// Specifies a compile-time constant Vector2 with value (1,1)
+        static const Vector2 One;
+        /// Specifies a compile-time constant Vector2 with value (1,0).
+        static const Vector2 UnitX;
+        /// Specifies a compile-time constant Vector2 with value (0,1).
+        static const Vector2 UnitY;
+        /// Specifies a compile-time constant Vector2 with value (0,-1).
+        static const Vector2 Up;
+        /// Specifies a compile-time constant Vector2 with value (-1,0).
+        static const Vector2 Left;
+        /// Specifies a compile-time constant Vector2 with value (0,1).
+        static const Vector2 Down;
+        /// Specifies a compile-time constant Vector2 with value (1,0).
+        static const Vector2 Right;
+        /// Specifies a compile-time constant Vector2 with value (NaN, NaN).
+        /// For this constant, each element has the value of quiet NaN, or Not-A-Number.
+        /// @note Never compare a Vector2 to this value! Due to how IEEE floats work, "nan == nan" returns false!
+        /// That is, nothing is equal to NaN, not even NaN itself!
+        static const Vector2 NaN;
+        /// Specifies a compile-time constant Vector2 with value (+infinity, +infinity).
+        static const Vector2 Infinity;
+    public:
+        /// Default Constructor does not initialize any members of this class.
         Vector2();
         /// Constructs a new Vector2 with the value (X, Y)
         Vector2(float X, float Y);
@@ -24,19 +57,15 @@ namespace J3ML::LinearAlgebra {
         Vector2(const Vector2& rhs);   // Copy Constructor
         //Vector2(Vector2&&) = default;   // Move Constructor
 
-        static const Vector2 Zero;
-        static const Vector2 Up;
-        static const Vector2 Left;
-        static const Vector2 Down;
-        static const Vector2 Right;
-        static const Vector2 NaN;
-        static const Vector2 Infinity;
 
-        float GetX() const;
-        float GetY() const;
+        [[nodiscard]] float GetX() const;
+        [[nodiscard]] float GetY() const;
         void SetX(float newX);
         void SetY(float newY);
 
+        /// Sets all elements of this vector.
+        void Set(float newX, float newY);
+
         /// Casts this float2 to a C array.
         /** This function does not allocate new memory or make a copy of this float2. This function simply
 		returns a C pointer view to this data structure. Use ptr()[0] to access the x component of this float2
@@ -49,144 +78,297 @@ namespace J3ML::LinearAlgebra {
 		@return A pointer to the first float element of this class. The data is contiguous in memory.
 		@see operator [](), At(). */
         float* ptr();
-        const float *ptr() const;
+        [[nodiscard]] const float *ptr() const;
 
+        /// Accesses an element of this vector using array notation.
+        /** @param index The element to get. Pass in 0 for x and 1 for y.
+            @note If you have a non-const instance of this class, you can use this notation to set the elements of
+            this vector as well, e.g. vec[1] = 10.f; would set the y-component of this vector.
+            @see ptr(), At(). */
         float operator[](std::size_t index) const;
         float &operator[](std::size_t index);
 
-        const float At(std::size_t index) const;
+        /// Accesses an element of this vector using array notation.
+        /** @param index The element to get. Pass in 0 for x and 1 for y.
+            @note If you have a non-const instance of this class, you can use this notation to set the elements of
+            this vector as well, e.g. vec[1] = 10.f; would set the y-component of this vector.
+            @see ptr(), operator [](). */
+        [[nodiscard]] const float At(std::size_t index) const;
 
         float &At(std::size_t index);
 
-        Vector2 Abs() const;
+        [[nodiscard]] Vector2 Abs() const;
 
-        bool IsWithinMarginOfError(const Vector2& rhs, float margin=0.001f) const;
+        [[nodiscard]] bool IsWithinMarginOfError(const Vector2& rhs, float margin=0.001f) const;
 
-        bool IsNormalized(float epsilonSq = 1e-5f) const;
-        bool IsZero(float epsilonSq = 1e-6f) const;
-        bool IsPerpendicular(const Vector2& other, float epsilonSq=1e-5f) const;
+        [[nodiscard]] bool IsNormalized(float epsilonSq = 1e-5f) const;
+        [[nodiscard]] bool IsZero(float epsilonSq = 1e-6f) const;
+        [[nodiscard]] bool IsPerpendicular(const Vector2& other, float epsilonSq=1e-5f) const;
 
 
         bool operator == (const Vector2& rhs) const;
         bool operator != (const Vector2& rhs) const;
 
-        Vector2 Min(const Vector2& min) const;
+        /// Returns an element-wise minimum between two vectors.
+        [[nodiscard]] Vector2 Min(const Vector2& min) const;
         static Vector2 Min(const Vector2& value, const Vector2& minimum);
 
-        Vector2 Max(const Vector2& max) const;
+        /// Returns an element-wise maximum between two vectors.
+        [[nodiscard]] Vector2 Max(const Vector2& max) const;
         static Vector2 Max(const Vector2& value, const Vector2& maximum);
 
-        Vector2 Clamp(const Vector2& min, const Vector2& max) const;
+        /// Returns a Vector2 that has floor <= this[i] <= ceil for each element.
+        [[nodiscard]] Vector2 Clamp(float floor, float ceil) const;
+        static Vector2 Clamp(const Vector2& value, float floor, float ceil);
+
+        /// Limits each element of this vector between the corresponding elements in floor and ceil.
+        [[nodiscard]] Vector2 Clamp(const Vector2& min, const Vector2& max) const;
         static Vector2 Clamp(const Vector2& min, const Vector2& middle, const Vector2& max);
 
+        /// Limits each element of this vector in the range [0, 1].
+        [[nodiscard]] Vector2 Clamp01() const;
+        static Vector2 Clamp01(const Vector2& value);
+
         /// Returns the magnitude between the two vectors.
-        float Distance(const Vector2& to) const;
+        /// @see DistanceSq(), Length(), LengthSquared()
+        [[nodiscard]] float Distance(const Vector2& to) const;
         static float Distance(const Vector2& from, const Vector2& to);
 
-        float DistanceSq(const Vector2& to) const;
+        [[nodiscard]] float DistanceSq(const Vector2& to) const;
         static float DistanceSq(const Vector2& from, const Vector2& to);
 
-        float MinElement() const;
+        /// Returns the smaller element of the vector.
+        [[nodiscard]] float MinElement() const;
 
-        float MaxElement() const;
+        /// Returns the larger element of the vector.
+        [[nodiscard]] float MaxElement() const;
 
-        float Length() const;
+        /// Computes the length of this vector
+        /// @return sqrt(x*x + y*y)
+        /// @see LengthSquared(), Distance(), DistanceSq()
+        [[nodiscard]] float Length() const;
         static float Length(const Vector2& of);
 
-        float LengthSquared() const;
+        /// Computes the squared length of this vector.
+        /** Calling this function is faster than using Length(), since this function avoids computing a square root.
+            If you only need to compare lengths to each other, but are not interested in the actual length values,
+            you can compare by using LengthSquared(), instead of Length(), since Sqrt() is an order-preserving
+            (monotonous and non-decreasing) function.
+            @return x*x + y*y
+            @see LengthSquared(), Distance(), DistanceSq() */
+        [[nodiscard]] float LengthSquared() const;
         static float LengthSquared(const Vector2& of);
 
         /// Returns the length of the vector, which is sqrt(x^2 + y^2)
-        float Magnitude() const;
+        [[nodiscard]] float Magnitude() const;
         static float Magnitude(const Vector2& of);
 
 
-        bool IsFinite() const;
+        [[nodiscard]] bool IsFinite() const;
         static bool IsFinite(const Vector2& v);
 
+        /// @return x+y;
+        [[nodiscard]] float SumOfElements() const;
+        /// @return x*y;
+        [[nodiscard]] float ProductOfElements() const;
+        /// @return (x+y)/2
+        [[nodiscard]] float AverageOfElements() const;
+
+        /// Returns a copy of this vector with each element negated.
+        /** This function returns a new vector where each element x of the original vector is replaced by the value -x.
+            @return Vector2(-x, -y)
+            @see Abs(); */
+        [[nodiscard]] Vector2 Neg() const;
+
+        /// Computes the element-wise reciprocal of this vector.
+        /** This function returns a new vector where each element x of the original vector is replaced by the value 1/x.
+            @return Vector2(1/x, 1/y). */
+        [[nodiscard]] Vector2 Recip() const;
+
+        /// Returns the aimed angle direction of this vector, in radians.
+        /** The aimed angle of a 2D vector corresponds to the theta part (or azimuth) of the polar coordinate representation of this vector. Essentially,
+            describes the direction this vector is pointing at. A vector pointing towards +X returns 0, vector pointing towards +Y returns pi/2, vector
+            pointing towards -X returns pi, and a vector pointing towards -Y returns -pi/2 (equal to 3pi/2).
+            @note This vector does not need to be normalized for this function to work, but it DOES need to be non-zero (unlike the function ToPolarCoordinates).
+            @return The aimed angle in the range ]-pi/2, pi/2].
+            @see ToPolarCoordinates, FromPolarCoordinates, SetFromPolarCoordinates */
+        [[nodiscard]] float AimedAngle() const;
+
+        /// Converts this euclidian (x,y) Vector2 to polar coordinates representation in the form (theta, lenght).
+        /** @note It is valid for the magnitude of this vector to be (very close to) zero, in which case the return value is the zero vector.
+            @return A vector2 that has the first component (x) representing the aimed angle (azimuth) of this direction vector, in radians,
+            and is equal to atan2(y, x). The x component has a range of ]-pi/2, pi/2]. The second component (y) of the returned vector
+            stores the length (radius) of this vector.
+            @see SetFromPolarCoordinates, FromPolarCoordinates, AimedAngle */
+        [[nodiscard]] Vector2 ToPolarCoordinates() const;
+
         /// Returns a float value equal to the magnitudes of the two vectors multiplied together and then multiplied by the cosine of the angle between them.
         /// For normalized vectors, dot returns 1 if they point in exactly the same direction,
         /// -1 if they point in completely opposite directions, and 0 if the vectors are perpendicular.
-        float Dot(const Vector2& rhs) const;
+        [[nodiscard]] float Dot(const Vector2& rhs) const;
         static float Dot(const Vector2& lhs, const Vector2& rhs);
 
         /// Projects one vector onto another and returns the result. (IDK)
-        Vector2 Project(const Vector2& rhs) const;
+        [[nodiscard]] Vector2 Project(const Vector2& rhs) const;
         /// @see Project
         static Vector2 Project(const Vector2& lhs, const Vector2& rhs);
 
-        /// Returns a copy of this vector, resized to have a magnitude of 1, while preserving "direction"
-        Vector2 Normalize() const;
-        static Vector2 Normalize(const Vector2& of);
+        /// Normalizes this Vector2
+        /** In the case of failure, this vector is set to (1, 0), so calling this function will never result in an un-normalized vector.
+            @note If this function fials to normalize the vector, no error message is printed, the vector is set to (1, 0) and
+            an error code of 0 is returned. This is different than the behavior of the Normalized() function, which prints an
+            error if normalization fails.
+            @note This function operates in-place.
+            @return The old length of this vector, or 0 if normalization failed.
+            @see Normalized() */
+        float Normalize();
+
+
+        /// Returns a normalized copy of this vector.
+        /** @note If the vector is zero and cannot be normalized, the vector (1, 0) is returned, and an error message is printed.
+            If you do not want to generate an error message on failure, but want to handle the failure yourself, use the
+            Normalize() function instead.
+            @see Normalize() */
+        [[nodiscard]] Vector2 Normalized() const;
 
         /// Linearly interpolates between two points.
         /// Interpolates between the points and b by the interpolant t.
         /// The parameter is (TODO: SHOULD BE!) clamped to the range[0, 1].
         /// This is most commonly used to find a point some fraction of the wy along a line between two endpoints (eg. to move an object gradually between those points).
-        Vector2 Lerp(const Vector2& rhs, float alpha) const;
+        [[nodiscard]] Vector2 Lerp(const Vector2& rhs, float alpha) const;
         /// @see Lerp
         static Vector2 Lerp(const Vector2& lhs, const Vector2& rhs, float alpha);
         /// Note: Input vectors MUST be normalized first!
-        float AngleBetween(const Vector2& rhs) const;
+        [[nodiscard]] float AngleBetween(const Vector2& rhs) const;
         static float AngleBetween(const Vector2& lhs, const Vector2& rhs);
 
         /// Adds two vectors.
         Vector2 operator +(const Vector2& rhs) const;
-        Vector2 Add(const Vector2& rhs) const;
+        [[nodiscard]] Vector2 Add(const Vector2& rhs) const;
         static Vector2 Add(const Vector2& lhs, const Vector2& rhs);
 
         /// Subtracts two vectors.
         Vector2 operator -(const Vector2& rhs) const;
-        Vector2 Sub(const Vector2& rhs) const;
+        [[nodiscard]] Vector2 Sub(const Vector2& rhs) const;
         static Vector2 Sub(const Vector2& lhs, const Vector2& rhs);
 
         /// Multiplies this vector by a scalar value.
         Vector2 operator *(float rhs) const;
-        Vector2 Mul(float scalar) const;
+        [[nodiscard]] Vector2 Mul(float scalar) const;
         static Vector2 Mul(const Vector2& lhs, float rhs);
 
         /// Multiplies this vector by a vector, element-wise
         /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
         ///	 but this function is provided here for syntactical convenience.
-        Vector2 operator *(const Vector2& rhs) const
-        {
-
-        }
-        Vector2 Mul(const Vector2& v) const;
+        Vector2 operator *(const Vector2& rhs) const;
+        [[nodiscard]] Vector2 Mul(const Vector2& v) const;
 
         /// Divides this vector by a scalar.
         Vector2 operator /(float rhs) const;
-        Vector2 Div(float scalar) const;
+        [[nodiscard]] Vector2 Div(float scalar) const;
         static Vector2 Div(const Vector2& lhs, float rhs);
 
         /// Divides this vector by a vector, element-wise
         /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
         /// but this function is provided here for syntactical convenience
-        Vector2 Div(const Vector2& v) const;
+        Vector2 operator / (const Vector2& rhs) const;
+        [[nodiscard]] Vector2 Div(const Vector2& v) const;
 
         /// Unary operator +
-        Vector2 operator +() const; // TODO: Implement
+        Vector2 operator +() const { return *this;}
         Vector2 operator -() const;
-        /// Assigns a vector to another
+        /// Assigns a vector to another.
         Vector2 &operator =(const Vector2 &rhs);
+
+        /// Adds a vector to this vector, in-place.
         Vector2& operator+=(const Vector2& rhs);
+        /// Subtracts a vector from this vector, in-place.
         Vector2& operator-=(const Vector2& rhs);
+        /// Multiplies this vector by a scalar, in-place.
         Vector2& operator*=(float scalar);
+        /// Divides this vector by a scalar, in-place
         Vector2& operator/=(float scalar);
 
+        /// Returns this vector with the "perp" operator applied to it.
+        /** The perp operator rotates a vector 90 degrees ccw (around the "z axis"), i.e.
+            for a 2D vector (x,y), this function returns the vector (-y, x).
+            @note This function is identical to Rotate90CCW().
+            @return (-y, x). The returned vector is perpendicular to this vector.
+            @see PerpDot(), Rotated90CCW() */
+        [[nodiscard]] Vector2 Perp() const;
+
+        /// Computes the perp-dot product of this and the given Vector2 in the order this^perp (dot) rhs.
+        /// @see Dot(), Perp()
+        [[nodiscard]] float PerpDot(const Vector2& rhs) const;
+
+        /// Rotates this vector 90 degrees clock-wise
+        /// This rotation is interpreted in a coordinate system on a plane where +x extends to the right, and +y extends upwards.
+        /// @see Perp(), Rotated90CW(), Rotate90CCW(), Rotated90CCW();
+        void Rotate90CW();
+
+        /// Returns a vector that is perpendicular to this vector (rotated 90 degrees clock-wise).
+        /// @note This function is identical to Perp().
+        /// @see Perp(), Rotate90CW(), Rotate90CCW(), Rotated90CCW()
+        [[nodiscard]] Vector2 Rotated90CW() const;
+
+        /// Rotates this vector 90 degrees counterclock-wise.
+        /// This is a coordinate system on a plane +x extends to the right, and +y extends upwards.
+        /// @see Perp(), Rotate90CW(), Rotated90CW(), Rotated90CCW();
+        void Rotate90CCW();
+
+        /// Returns a vector that is perpendicular to this vector (rotated 90 degrees counter-clock-wise)
+        /// @see Perp(), Rotate90CW(), Rotated90CW(), Rotate90CCW()
+        [[nodiscard]] Vector2 Rotated90CCW() const;
+
+        /// Returns this vector reflected about a plane with the given normal.
+        /// By convention, both this and the reflected vector
+        [[nodiscard]] Vector2 Reflect(const Vector2& normal) const;
+
+        /// Refracts this vector about a plane with the given normal.
+        /** By convention, the this vector points towards the plane, and the returned vector points away from the plane.
+            When the ray is going from a denser material to a lighter one, total internal reflection can occur.
+            In this case, this function will just return a reflected vector from a call to Reflect().
+            @param normal Specifies the plane normal direction
+            @param negativeSideRefractionIndex The refraction index of the material we are exiting.
+            @param positiveSideRefractionIndex The refraction index of the material we are entering.
+            @see Reflect(). */
+        [[nodiscard]] Vector2 Refract(const Vector2& normal, float negativeSideRefractionIndex, float positiveSideRefractionIndex) const;
+
+        /// Projects this vector onto the given un-normalized direction vector.
+        /// @param direction The direction vector to project this vector onto.
+        /// This function will normalize the vector, so you can pass in an un-normalized vector.
+        /// @see ProjectToNorm
+        [[nodiscard]] Vector2 ProjectTo(const Vector2& direction) const;
+        /// Projects this vector onto the given normalized direction vector.
+        /// @param direction The vector to project onto.
+        [[nodiscard]] Vector2 ProjectToNorm(const Vector2& direction) const;
 
         /// Tests if the triangle a->b->c is oriented counter-clockwise.
         /** Returns true if the triangle a->b->c is oriented counter-clockwise, when viewed in the XY-plane
             where x spans to the right and y spans up.
             Another way to think of this is that this function returns true, if the point C lies to the left
             of the directed line AB. */
-        static bool OrientedCCW(const Vector2 &, const Vector2 &, const Vector2 &);
+        static bool OrientedCCW(const Vector2&, const Vector2&, const Vector2&);
 
+        /// Makes the given vectors linearly independent.
+        /** This function directly follows the Gram-Schmidt procedure on the input vectors.
+            The vector a is kept unmodified, and vector B is modified to be perpendicular to a.
+            @note If any of the input vectors is zero, then the resulting set of vectors cannot be made orthogonal.
+            @see AreOrthogonal(), Orthonormalize(), AreOrthonormal() */
+        static void Orthogonalize(const Vector2& a, Vector2& b);
+
+        /// Returns true if the given vectors are orthogonal to each other.
+        /// @see Orthogonalize(), Orthonormalize(), AreOrthonormal()
+        static bool AreOrthogonal(const Vector2& a, const Vector2& b, float epsilon = 1e-3f);
+
+        /// Makes the given vectors linearly independent and normalized in length.
+        /** This function directly follows the Gram-Schmidt procedure on the input vectors.
+            The vector a is first normalized, and vector b is modified to be perpendicular to a, and also normalized.
+            @note If either of the input vectors is zero, then the resulting set of vectors cannot be made orthonormal.
+            @see Orthogonalize(), AreOrthogonal(), AreOrthonormal() */
+        static void Orthonormalize(Vector2& a, Vector2& b);
 
     };
 
-    static Vector2 operator*(float lhs, const Vector2 &rhs)
-    {
-        return rhs * lhs;
-    }
+    Vector2 operator*(float lhs, const Vector2 &rhs);
 }

include/J3ML/LinearAlgebra/Vector3.h

@@ -5,6 +5,10 @@
 #include <cstddef>
 #include <cstdlib>
 #include <J3ML/LinearAlgebra/Angle2D.h>
+#include <J3ML/Algorithm/RNG.h>
+
+
+using namespace J3ML::Algorithm;
 
 namespace J3ML::LinearAlgebra {
 
@@ -50,11 +54,11 @@ public:
         member to initialize other static data in other compilation units! */
     static const Vector3 Backward;
     /// Specifies a compile-time constant Vector3 with value (NAN, NAN, NAN).
-    /** For this constant, aeach element has the value of quet NaN, or Not-A-Number.
+    /** For this constant, each element has the value of quiet NaN, or Not-A-Number.
         @note Never compare a Vector3 to this value! Due to how IEEE floats work, "nan == nan" returns false!
         That is, nothing is equal to NaN, not even NaN itself!
         @note Due to static data initialization order being undefined in C++, do NOT use this
-        member data to intialize other static data in other compilation units! */
+        member data to initalize other static data in other compilation units! */
     static const Vector3 NaN;
     /// Specifies a compile-time constant Vector3 with value (+infinity, +infinity, +infinity).
     /** @note Due to static data initialization order being undefined in C++, do NOT use this
@@ -87,7 +91,7 @@ public:
     // Constructs a new Vector3 with the value (X, Y, Z)
     Vector3(float X, float Y, float Z);
     Vector3(const Vector2& XY, float Z);
-    Vector3(const Vector3& rhs); // Copy Constructor
+    Vector3(const Vector3& rhs) = default; // Copy Constructor
     Vector3(Vector3&&) = default; // Move Constructor
     /// Constructs this float3 from a C array, to the value (data[0], data[1], data[2]).
     /** @param data An array containing three elements for x, y and z. This pointer may not be null. */
@@ -95,7 +99,23 @@ public:
     /// Constructs a new Vector3 with the value (scalar, scalar, scalar).
     explicit Vector3(float scalar);
 
+    /// Generates a new Vector3 by fillings its entries by the given scalar.
+    /// @see Vector3::Vector3(float scalar), SetFromScalar()
+    static Vector3 FromScalar(float scalar);
 
+    /// Generates a direction vector of the given length.
+    /** The returned vector points at a uniformly random direction.
+        @see RandomSphere(), RandomBox() */
+    static Vector3 RandomDir(RNG& rng, float length = 1.f);
+    static Vector3 RandomSphere(RNG& rng, const Vector3& center, float radius);
+    static Vector3 RandomBox(RNG& rng, float xmin, float xmax, float ymin, float ymax, float zmin, float zmax);
+    static Vector3 RandomBox(RNG& rng, const Vector3& min, const Vector3 &max);
+
+    static Vector3 RandomBox(RNG& rng, float min, float max);
+
+
+    static inline Vector3 RandomGeneral(RNG& rng, float minElem, float maxElem) { return RandomBox(rng, minElem, maxElem); }
+public:
     /// Casts this float3 to a C array.
     /** This function does not allocate new memory or make a copy of this Vector3. This function simply
             returns a C pointer view to this data structure. Use ptr()[0] to access the x component of this float3,
@@ -126,7 +146,19 @@ public:
     float At(int index) const;
     float &At(int index);
 
+    /// Makes the given vectors linearly independent and normalized in length.
+    /** This function directly follows the Gram-Schmidt procedure on the input vectors.
+        The vector a is first normalized, and vector b is modified to be perpendicular to a, and also normalized.
+        Finally, if specified, the vector c is adjusted to be perpendicular to a and b, and normalized.
+        @note If any of the input vectors is zero, then the resulting set of vectors cannot be made orthonormal.
+        @see Orthogonalize(), AreOrthogonal(), AreOrthonormal(). */
     static void Orthonormalize(Vector3& a, Vector3& b);
+    static void Orthonormalize(Vector3& a, Vector3& b, Vector3& c);
+
+    /// Returns true if the given vectors are orthogonal to each other and all of length 1.
+    /** @see Orthogonalize(), AreOrthogonal(), Orthonormalize(), AreCollinear(). */
+    static bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon = 1e-3f);
+    static bool AreOrthonormal(const Vector3& a, const Vector3& b, const Vector3& c, float epsilon = 1e-3f);
 
     Vector3 Abs() const;
     static Vector3 Abs(const Vector3& rhs);
@@ -134,9 +166,9 @@ public:
     /// Returns the DirectionVector for a given angle.
     static Vector3 Direction(const Vector3 &rhs) ;
 
-    static void Orthonormalize(Vector3& a, Vector3& b, Vector3& c);
 
-    bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon);
+
+
 
     Vector3 ProjectToNorm(const Vector3& direction) const;
 
@@ -160,7 +192,7 @@ public:
     float MinElement() const;
     static float MinElement(const Vector3& of);
 
-    // Normalizes this Vector3.
+    /// Normalizes this Vector3.
     /** In the case of failure, this vector is set to (1, 0, 0), so calling this function will never result in an
 		unnormalized vector.
 		@note If this function fails to normalize the vector, no error message is printed, the vector is set to (1,0,0) and
@@ -226,8 +258,23 @@ public:
     static Vector3 Cross(const Vector3& lhs, const Vector3& rhs);
 
     /// Returns a copy of this vector, resized to have a magnitude of 1, while preserving "direction"
-    Vector3 Normalize() const;
-    static Vector3 Normalize(const Vector3& targ);
+    /// @note If the vector is zero and cannot be normalized, the vector (1, 0, 0) is returned, and an error message is printed.
+    /// If you do not want to generate an error message on failure, but want to handle the failure yourself, use the Normalize() function instead.
+    /// @see Normalize()
+    Vector3 Normalized() const;
+    static Vector3 Normalized(const Vector3& targ);
+
+    /// Normalizes this Vector3.
+    /** In the case of failure, this vector is set to (1, 0, 0), so calling this function will never result in an
+        un-normalized vector.
+        @note If this function fails to normalize the vector, no error message is printed, the vector is set to (1,0,0) and
+        an error code 0 is returned. This is different than the behavior of the Normalized() function, which prints an
+        error if normalization fails.
+        @note This function operates in-place.
+        @return The old length of this vector, or 0 if normalization fails.
+        @see Normalized(). */
+    float Normalize();
+
 
     /// Linearly interpolates between two points.
     /// Interpolates between the points and b by the interpolant t.
@@ -290,6 +337,7 @@ public:
     Vector3& operator*=(float scalar);
     Vector3& operator/=(float scalar);
 
+
     void Set(float d, float d1, float d2);
 };
 

src/J3ML/Algorithm/RNG.cpp

@@ -39,17 +39,17 @@ namespace J3ML::Algorithm {
         assert(modulus != 0);
         /// TODO: Convert to using Shrage's method for approximate factorization (Numerical Recipes in C)
 
-        // Currently we cast everything to 65-bit to avoid overflow, which is quite dumb.
+        // Currently we cast everything to 64-bit to avoid overflow, which is quite dumb.
 
         // Creates the new random number
-        u64 newNum  = ((u64)lastNumber * (u64)multiplier + (u64)increment % (u64)modulus);
+        //u64 newNum  = ((u64)lastNumber * (u64)multiplier + (u64)increment % (u64)modulus);
 
         // TODO: use this on console platforms to rely on smaller sequences.
-        // u32 m = lastNumber * multiplier;
-        // u32 i = m + increment;
-        // u32 f = i & 0x7FFFFFFF;
-        // u32 m = (lastNumber * 214013 + 2531011) & 0x7FFFFFFF;
-        // unsigned __int64 newNum = (lastNumber * multiplier + increment) & 0x7FFFFFFF;
+        u32 m = lastNumber * multiplier;
+        u32 i = m + increment;
+        u32 f = i & 0x7FFFFFFF;
+        //u32 m = (lastNumber * 214013 + 2531011) & 0x7FFFFFFF;
+        u64 newNum = (lastNumber * multiplier + increment) & 0x7FFFFFFF;
         assert( ((u32)newNum!=0 || increment != 0) && "RNG degenerated to producing a stream of zeroes!");
         lastNumber = (u32)newNum;
         return lastNumber;
@@ -89,8 +89,9 @@ namespace J3ML::Algorithm {
 
     float RNG::Float() {
         u32 i = ((u32)Int() & 0x007FFFFF /* random mantissa */) | 0x3F800000 /* fixed exponent */;
-        auto f = ReinterpretAs<float, u32>(i); // f is now in range [1, 2[
-        f -= 1.f; // Map to range [0, 1[
+        // TODO: PROPERLY fix this.
+        auto f = (float)i / 1e31; //ReinterpretAs<float, u32>(i); // f is now in range [1, 2[
+        //f -= 1.f; // Map to range [0, 1[
         assert(f >= 0.f);
         assert(f < 1.f);
         return f;

src/J3ML/Geometry/AABB.cpp

@@ -237,7 +237,7 @@ namespace J3ML::Geometry {
             Enclose(pointArray[i]);
     }
 
-    Vector3 AABB::GetRandomPointInside(J3ML::Algorithm::RNG &rng) const {
+    Vector3 AABB::RandomPointInside(RNG &rng) const {
         float f1 = rng.Float();
         float f2 = rng.Float();
         float f3 = rng.Float();
@@ -573,20 +573,20 @@ namespace J3ML::Geometry {
 
     Vector3 AABB::AnyPointFast() const { return minPoint;}
 
-    Vector3 AABB::GetRandomPointOnSurface(RNG &rng) const {
+    Vector3 AABB::RandomPointOnSurface(RNG &rng) const {
         int i = rng.Int(0, 5);
         float f1 = rng.Float();
         float f2 = rng.Float();
         return FacePoint(i, f1, f2);
     }
 
-    Vector3 AABB::GetRandomPointOnEdge(RNG &rng) const {
+    Vector3 AABB::RandomPointOnEdge(RNG &rng) const {
         int i = rng.Int(0, 11);
         float f = rng.Float();
         return PointOnEdge(i, f);
     }
 
-    Vector3 AABB::GetRandomCornerPoint(RNG &rng) const {
+    Vector3 AABB::RandomCornerPoint(RNG &rng) const {
         return CornerPoint(rng.Int(0, 7));
     }
 
@@ -786,4 +786,12 @@ namespace J3ML::Geometry {
         dMax = s + r;
     }
 
+    Vector3 AABB::MinPoint() const {
+        return {MinX(), MinY(), MinZ()};
+    }
+
+    Vector3 AABB::MaxPoint() const {
+        return {MaxX(), MaxY(), MaxZ()};
+    }
+
 }

src/J3ML/Geometry/LineSegment.cpp

@@ -197,7 +197,7 @@ namespace J3ML::Geometry {
     }
 
     Vector3 LineSegment::Dir() const {
-        return (B - A).Normalize();
+        return (B - A).Normalized();
     }
 
     float LineSegment::Length() const {

src/J3ML/Geometry/OBB.cpp

@@ -343,9 +343,9 @@ namespace J3ML::Geometry
         o.axis[0] = transform.Mul(o.r.x * o.axis[0]);
         o.axis[1] = transform.Mul(o.r.y * o.axis[1]);
         o.axis[2] = transform.Mul(o.r.z * o.axis[2]);
-        o.r.x = o.axis[0].Normalize().x;
-        o.r.y = o.axis[1].Normalize().y;
-        o.r.z = o.axis[2].Normalize().z;
+        o.r.x = o.axis[0].Normalized().x;
+        o.r.y = o.axis[1].Normalized().y;
+        o.r.z = o.axis[2].Normalized().z;
     }
 
     void OBB::SetFrom(const AABB& aabb, const Matrix3x3 &transform) {
@@ -446,6 +446,98 @@ namespace J3ML::Geometry
         this->axis[2] = axis2;
     }
 
+    AABB OBB::MinimalEnclosingAABB() const {
+        AABB aabb;
+        aabb.SetFrom(*this);
+        return aabb;
+    }
+
+    Vector3 OBB::MinPoint() const {
+        return {MinX(), MinY(), MinZ()};
+    }
+
+    float OBB::MinX() const {
+        auto c0 = CornerPoint(0);
+        auto c1 = CornerPoint(1);
+        auto c2 = CornerPoint(2);
+        auto c3 = CornerPoint(3);
+        auto c4 = CornerPoint(4);
+        auto c5 = CornerPoint(5);
+        auto c6 = CornerPoint(6);
+        auto c7 = CornerPoint(7);
+
+        return std::min({c0.x, c1.x, c2.x, c3.x, c4.x, c5.x, c6.x, c7.x});
+    }
+
+    float OBB::MinY() const {
+        auto c0 = CornerPoint(0);
+        auto c1 = CornerPoint(1);
+        auto c2 = CornerPoint(2);
+        auto c3 = CornerPoint(3);
+        auto c4 = CornerPoint(4);
+        auto c5 = CornerPoint(5);
+        auto c6 = CornerPoint(6);
+        auto c7 = CornerPoint(7);
+
+        return std::min({c0.y, c1.y, c2.y, c3.y, c4.y, c5.y, c6.y, c7.y});
+    }
+
+    float OBB::MinZ() const {
+        auto c0 = CornerPoint(0);
+        auto c1 = CornerPoint(1);
+        auto c2 = CornerPoint(2);
+        auto c3 = CornerPoint(3);
+        auto c4 = CornerPoint(4);
+        auto c5 = CornerPoint(5);
+        auto c6 = CornerPoint(6);
+        auto c7 = CornerPoint(7);
+
+        return std::min({c0.z, c1.z, c2.z, c3.z, c4.z, c5.z, c6.z, c7.z});
+    }
+
+    float OBB::MaxX() const {
+        auto c0 = CornerPoint(0);
+        auto c1 = CornerPoint(1);
+        auto c2 = CornerPoint(2);
+        auto c3 = CornerPoint(3);
+        auto c4 = CornerPoint(4);
+        auto c5 = CornerPoint(5);
+        auto c6 = CornerPoint(6);
+        auto c7 = CornerPoint(7);
+
+        return std::max({c0.x, c1.x, c2.x, c3.x, c4.x, c5.x, c6.x, c7.x});
+    }
+
+    float OBB::MaxY() const {
+        auto c0 = CornerPoint(0);
+        auto c1 = CornerPoint(1);
+        auto c2 = CornerPoint(2);
+        auto c3 = CornerPoint(3);
+        auto c4 = CornerPoint(4);
+        auto c5 = CornerPoint(5);
+        auto c6 = CornerPoint(6);
+        auto c7 = CornerPoint(7);
+
+        return std::max({c0.y, c1.y, c2.y, c3.y, c4.y, c5.y, c6.y, c7.y});
+    }
+
+    float OBB::MaxZ() const {
+        auto c0 = CornerPoint(0);
+        auto c1 = CornerPoint(1);
+        auto c2 = CornerPoint(2);
+        auto c3 = CornerPoint(3);
+        auto c4 = CornerPoint(4);
+        auto c5 = CornerPoint(5);
+        auto c6 = CornerPoint(6);
+        auto c7 = CornerPoint(7);
+
+        return std::max({c0.z, c1.z, c2.z, c3.z, c4.z, c5.z, c6.z, c7.z});
+    }
+
+    Vector3 OBB::MaxPoint() const {
+        return {MaxX(), MaxY(), MaxZ()};
+    }
+
 
 }
 

src/J3ML/Geometry/Plane.cpp

@@ -107,7 +107,7 @@ namespace J3ML::Geometry
 
     Plane::Plane(const Line &line, const Vector3 &normal) {
         Vector3 perpNormal = normal - normal.ProjectToNorm(line.Direction);
-        Set(line.Position, perpNormal.Normalize());
+        Set(line.Position, perpNormal.Normalized());
     }
 
     void Plane::Set(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3) {

src/J3ML/Geometry/Polygon.cpp

@@ -78,7 +78,7 @@ namespace J3ML::Geometry {
         if (vertices.size() < 2)
             return Vector3::Right;
         Vector3 u = (Vector3)vertices[1] - (Vector3)vertices[0];
-        u = u.Normalize(); // Always succeeds, even if u was zero (generates (1,0,0)).
+        u = u.Normalized(); // Always succeeds, even if u was zero (generates (1,0,0)).
         return u;
     }
 
@@ -86,7 +86,7 @@ namespace J3ML::Geometry {
     {
         if (vertices.size() < 2)
             return Vector3::Down;
-        return Vector3::Cross(PlaneCCW().Normal, BasisU()).Normalize();
+        return Vector3::Cross(PlaneCCW().Normal, BasisU()).Normalized();
     }
 
     Plane Polygon::PlaneCCW() const
@@ -116,14 +116,14 @@ namespace J3ML::Geometry {
 
             // Polygon contains multiple points, but they are all collinear.
             // Pick an arbitrary plane along the line as the polygon plane (as if the polygon had only two points)
-            Vector3 dir = (Vector3(vertices[1])-Vector3(vertices[0])).Normalize();
+            Vector3 dir = (Vector3(vertices[1])-Vector3(vertices[0])).Normalized();
             return Plane(Line(vertices[0], dir), dir.Perpendicular());
         }
         if (vertices.size() == 3)
             return Plane(vertices[0], vertices[1], vertices[2]);
         if (vertices.size() == 2)
         {
-            Vector3 dir = (Vector3(vertices[1])-Vector3(vertices[0])).Normalize();
+            Vector3 dir = (Vector3(vertices[1])-Vector3(vertices[0])).Normalized();
             return Plane(Line(vertices[0], dir), dir.Perpendicular());
         }
         if (vertices.size() == 1)

src/J3ML/Geometry/Polyhedron.cpp

@@ -213,7 +213,7 @@ namespace J3ML::Geometry
 
         Vector3 basisU = (Vector3)v[vertices[1]] - (Vector3)v[vertices[0]];
         basisU.Normalize();
-        Vector3 basisV = Vector3::Cross(p.Normal, basisU).Normalize();
+        Vector3 basisV = Vector3::Cross(p.Normal, basisU).Normalized();
         assert(basisU.IsNormalized());
         assert(basisV.IsNormalized());
         assert(basisU.IsPerpendicular(basisV));

src/J3ML/Geometry/Ray.cpp

@@ -20,7 +20,7 @@ namespace J3ML::Geometry
     RaycastResult Ray::Cast(const Sphere &target, float maxDistance)
     {
         Vector3 p0 = this->Origin;
-        Vector3 d = this->Direction.Normalize();
+        Vector3 d = this->Direction.Normalized();
 
         Vector3 c = target.Position;
         float r = target.Radius;
@@ -48,7 +48,7 @@ namespace J3ML::Geometry
         Vector3 intersection = p0.Project(d*t);
         Vector3 intersection_from_sphere_origin = (intersection - target.Position);
 
-        Vector3 normal = -intersection_from_sphere_origin.Normalize();
+        Vector3 normal = -intersection_from_sphere_origin.Normalized();
 
         return RaycastResult{
             intersection,
@@ -85,7 +85,7 @@ namespace J3ML::Geometry
         t = tmin;
 
         Vector3 p0 = this->Origin;
-        Vector3 d = this->Direction.Normalize();
+        Vector3 d = this->Direction.Normalized();
 
         // TODO: Verify this
         Vector3 intersection = p0.Project(d*t);
@@ -94,7 +94,7 @@ namespace J3ML::Geometry
         // TODO: Calculate surfacenormal against rectangle
         Vector3 intersection_from_sphere_origin = (intersection - target.Centroid());
 
-        Vector3 normal = -intersection_from_sphere_origin.Normalize();
+        Vector3 normal = -intersection_from_sphere_origin.Normalized();
 
         return RaycastResult
         {
@@ -114,7 +114,7 @@ namespace J3ML::Geometry
 
         float t = (target.distance - pn) / nd;
 
-        Vector3 d = this->Direction.Normalize();
+        Vector3 d = this->Direction.Normalized();
 
         // TODO: verify this
         Vector3 intersection = this->Origin.Project(d*t);

src/J3ML/Geometry/Sphere.cpp

@@ -25,4 +25,47 @@ namespace J3ML::Geometry
         projectionDistance = extremePoint.Dot(direction);
         return extremePoint;
     }
+
+    Vector3 Sphere::RandomPointOnSurface(RNG &rng) const {
+        Vector3 v = Vector3::Zero;
+        // Rejection sampling analysis: The unit sphere fills ~52.4% of the volume of its enclosing box,
+        // so this loop is expected to take only very few iterations before succeeding.
+        for (int i = 0; i < 1000; ++i)
+        {
+            v.x = rng.FloatNeg1_1();
+            v.y = rng.FloatNeg1_1();
+            v.z = rng.FloatNeg1_1();
+            float lenSq = v.LengthSquared();
+            if (lenSq >= 1e-6f && lenSq <= 1.f)
+                return Position + (Radius / std::sqrt(lenSq)) * v;
+        }
+        // Astronomically small probability to reach here, and if we do so, the provided RNG must have been in a bad state.
+        assert(false && "Sphere::RandomPointOnSurface failed!");
+
+        // Failed to generate a point inside this sphere. Return an arbitrary point on the surface as fallback.
+        return Position + Vector3(Radius, 0, 0);
+    }
+
+    Vector3 Sphere::RandomPointInside(RNG &rng) {
+        assert(Radius > 1e-3f);
+        Vector3 v = Vector3::Zero;
+
+        // Rejection sampling analysis: The unit sphere fills ~52.4% of the volume of its enclosing box
+        // so this loop is expected to take only very few iterations before succeeding.
+        for (int i = 0; i < 1000; ++i)
+        {
+            v.x = rng.Float(-Radius, Radius);
+            v.y = rng.Float(-Radius, Radius);
+            v.z = rng.Float(-Radius, Radius);
+
+            if (v.LengthSquared() <= Radius*Radius)
+                return Position + v;
+        }
+        assert(false && "Sphere::RandomPointInside failed!");
+
+        // Failed to generate a point inside this sphere. Return the sphere center as fallback.
+        return Position;
+    }
+
+
 }

src/J3ML/LinearAlgebra/Matrix3x3.cpp

@@ -1,4 +1,5 @@
 #include <J3ML/LinearAlgebra/Matrix3x3.h>
+#include <J3ML/LinearAlgebra/Matrices.inl>
 #include <cmath>
 
 namespace J3ML::LinearAlgebra {
@@ -249,14 +250,14 @@ namespace J3ML::LinearAlgebra {
         // In the local space, the forward and up directions must be perpendicular to be well-formed.
         // In the world space, the target direction and world up cannot be degenerate (co-linear)
         // Generate the third basis vector in the local space;
-        Vector3 localRight = localUp.Cross(forward).Normalize();
+        Vector3 localRight = localUp.Cross(forward).Normalized();
         // A. Now we have an orthonormal linear basis {localRight, localUp, forward} for the object local space.
         // Generate the third basis vector for the world space
-        Vector3 worldRight = worldUp.Cross(target).Normalize();
+        Vector3 worldRight = worldUp.Cross(target).Normalized();
         // Since the input worldUp vector is not necessarily perpendicular to the target direction vector
         // We need to compute the real world space up vector that the "head" of the object will point
         // towards when the model is looking towards the desired target direction
-        Vector3 perpWorldUp = target.Cross(worldRight).Normalize();
+        Vector3 perpWorldUp = target.Cross(worldRight).Normalized();
         // B. Now we have an orthonormal linear basis {worldRight, perpWorldUp, targetDirection } for the desired target orientation.
         // We want to build a matrix M that performs the following mapping:
         // 1. localRight must be mapped to worldRight.        (M * localRight = worldRight)
@@ -487,7 +488,7 @@ namespace J3ML::LinearAlgebra {
     }
 
     void Matrix3x3::SetRotatePartZ(float angle) {
-        Set3x3RotatePartZ(*this, angle);
+        Set3x3PartRotateZ(*this, angle);
     }
 
     Vector3 Matrix3x3::ExtractScale() const {
@@ -527,8 +528,8 @@ namespace J3ML::LinearAlgebra {
 
     Vector3 Matrix3x3::Row3(int index) const { return GetRow3(index);}
 
-    void Matrix3x3::Set(const Matrix3x3 &x3) {
-
+    void Matrix3x3::Set(const Matrix3x3 &rhs) {
+        Set(rhs.ptr());
     }
 
     bool Matrix3x3::IsFinite() const {
@@ -574,7 +575,7 @@ namespace J3ML::LinearAlgebra {
     {
         float d = Determinant();
         bool isSingular = EqualAbs(d, 0.f, epsilon);
-        //assert(Inverse(epsilon) == isSingular);
+        //assert(Inverted(epsilon) == isSingular);
         return !isSingular;
     }
 
@@ -615,6 +616,470 @@ namespace J3ML::LinearAlgebra {
 
     Vector3 Matrix3x3::Row(int index) const { return GetRow(index);}
 
+    float Matrix3x3::DeterminantSymmetric() const {
+        assert(IsSymmetric());
+
+        const float a = this->elems[0][0];
+        const float b = this->elems[0][1];
+        const float c = this->elems[0][2];
+        const float d = this->elems[1][0];
+        const float e = this->elems[1][1];
+        const float f = this->elems[1][2];
+
+        // If the matrix is symmetric, it is of the following form:
+        // a b c
+        // b d e
+        // c e f
+
+        // A direct cofactor expansion gives
+        // det = a * (df - ee) -b * (bf - ce) + c * (be-dc)
+        //     = adf - aee - bbf + bce + bce - ccd
+        //     = adf - aee - bbf - ccd + 2*bce
+        //     = a(df-ee) + b(2*ce - bf) - ccd
+
+        return a * (d*f - e*e) + b * (2.f * c * e - b * f) - c*c*d;
+    }
+
+    Matrix3x3 Matrix3x3::InverseTransposed() const {
+        Matrix3x3 copy = *this;
+        copy.Transpose();
+        copy.Inverse();
+        return copy;
+    }
+
+    bool Matrix3x3::InverseTranspose() {
+        bool success = Inverse();
+        Transpose();
+        return success;
+    }
+
+    bool Matrix3x3::Inverse(float epsilon) {
+        /// There exists a generic matrix inverse calculator that uses Gaussian elimination
+
+        Matrix3x3 i = *this;
+        bool success = InverseMatrix(i, epsilon);
+        if (!success)
+            return false;
+
+        *this = i;
+        return true;
+    }
+
+    bool Matrix3x3::InverseFast(float epsilon) {
+        // Compute the inverse directly using Cramer's rule.
+        // Warning: This method is numerically very unstable!
+        float d = Determinant();
+        if (Math::EqualAbs(d, 0.f, epsilon))
+            return true;
+
+        d = 1.f / d;
+        Matrix3x3 i;
+        i[0][0] = d * (At(1, 1) * At(2, 2) - At(1, 2) * At(2, 1));
+        i[0][1] = d * (At(0, 2) * At(2, 1) - At(0, 1) * At(2, 2));
+        i[0][2] = d * (At(0, 1) * At(1, 2) - At(0, 2) * At(1, 1));
+
+        i[1][0] = d * (At(1, 2) * At(2, 0) - At(1, 0) * At(2, 2));
+        i[1][1] = d * (At(0, 0) * At(2, 2) - At(0, 2) * At(2, 0));
+        i[1][2] = d * (At(0, 2) * At(1, 0) - At(0, 0) * At(1, 2));
+
+        i[2][0] = d * (At(1, 0) * At(2, 1) - At(1, 1) * At(2, 0));
+        i[2][1] = d * (At(2, 0) * At(0, 1) - At(0, 0) * At(2, 1));
+        i[2][2] = d * (At(0, 0) * At(1, 1) - At(0, 1) * At(1, 0));
+
+
+        *this = i;
+        return true;
+    }
+
+    bool Matrix3x3::SolveAxb(Vector3 b, Vector3 &x) const {
+        // Solve by pivotization.
+        float v00 = At(0, 0);
+        float v10 = At(1, 0);
+        float v20 = At(2, 0);
+
+        float v01 = At(0, 1);
+        float v11 = At(1, 1);
+        float v21 = At(2, 1);
+
+        float v02 = At(0, 2);
+        float v12 = At(1, 2);
+        float v22 = At(2, 2);
+
+        float av00 = std::abs(v00);
+        float av10 = std::abs(v10);
+        float av20 = std::abs(v20);
+
+        // Find which item in first column has largest absolute value.
+        if (av10 >= av00 && av10 >= av20) {
+            Swap(v00, v10);
+            Swap(v01, v11);
+            Swap(v02, v12);
+            Swap(b[0], b[1]);
+        } else if (av20 >= av00) {
+            Swap(v00, v20);
+            Swap(v01, v21);
+            Swap(v02, v22);
+            Swap(b[0], b[2]);
+        }
+
+        /* a b c | x
+           d e f | y
+           g h i | z, where |a| >= |d| && |a| >= |g| */
+
+        if (Math::EqualAbs(v00, 0.f))
+            return false;
+
+
+        // Scale row so that leading element is one.
+        float denom = 1.f / v00;
+        v01 *= denom;
+        v02 *= denom;
+        b[0] *= denom;
+
+        /* 1 b c | x
+           d e f | y
+           g h i | z */
+
+
+        // Pivotize again.
+        if (std::abs(v21) >= std::abs(v11))
+        {
+            Swap(v11, v21);
+            Swap(v12, v22);
+            Swap(b[1], b[2]);
+        }
+
+        if (Math::EqualAbs(v11, 0.f))
+            return false;
+
+        /* 1 b c | x
+           0 e f | y
+           0 h i | z, where |e| >= |h| */
+
+        denom = 1.f / v11;
+        // v11 = 1.f;
+        v12 *= denom;
+        b[1] *= denom;
+
+        /* 1 b c | x
+           0 1 f | y
+           0 h i | z */
+
+        if (Math::EqualAbs(v22, 0.f))
+            return false;
+
+        x[2] = b[2] / v22;
+        x[1] = b[1] - x[2] * v12;
+        x[0] = b[0] - x[2] * v02 - x[1] * v01;
+    }
+
+    void Matrix3x3::ScaleCol(int col, float scalar) {
+        assert(col >= 0);
+        assert(col < Cols);
+        At(0, col) *= scalar;
+        At(1, col) *= scalar;
+        At(2, col) *= scalar;
+    }
+
+    void Matrix3x3::ScaleRow(int row, float scalar) {
+        assert(row >= 0);
+        assert(row < Rows);
+        assert(std::isfinite(scalar));
+        At(row, 0) *= scalar;
+        At(row, 1) *= scalar;
+        At(row, 2) *= scalar;
+    }
+
+    Vector3 Matrix3x3::Column3(int index) const {
+        return GetColumn3(index);
+    }
+
+    Vector3 Matrix3x3::Col3(int index) const {
+        return GetColumn3(index);
+    }
+
+    void Matrix3x3::Transpose() {
+        Swap(elems[0][1], elems[1][0]);
+        Swap(elems[0][2], elems[2][0]);
+        Swap(elems[1][2], elems[2][1]);
+    }
+
+    void Matrix3x3::SwapRows(int row1, int row2) {
+        assert(row1 >= 0);
+        assert(row1 < Rows);
+        assert(row2 >= 0);
+        assert(row2 < Rows);
+        Swap(At(row1, 0), At(row2, 0));
+        Swap(At(row1, 1), At(row2, 1));
+        Swap(At(row1, 2), At(row2, 2));
+    }
+
+    void Matrix3x3::SetIdentity() {
+        Set(1,0,0,
+            0,1,0,
+            0,0,1);
+    }
+
+    void Matrix3x3::SwapColumns(int col1, int col2) {
+        assert(col1 >= 0);
+        assert(col1 < Cols);
+        assert(col2 >= 0);
+        assert(col2 < Cols);
+        Swap(At(0, col1), At(0, col2));
+        Swap(At(1, col1), At(1, col2));
+        Swap(At(2, col1), At(2, col2));
+    }
+
+    void Matrix3x3::Set(const float *p) {
+        assert(p);
+        Set(p[0], p[1], p[2],
+            p[3], p[4], p[5],
+            p[6], p[7], p[8]);
+    }
+
+    void
+    Matrix3x3::Set(float _00, float _01, float _02, float _10, float _11, float _12, float _20, float _21, float _22) {
+        At(0, 0) = _00; At(0, 1) = _01; At(0, 2) = _02;
+        At(1, 0) = _10; At(1, 1) = _11; At(1, 2) = _12;
+        At(2, 0) = _20; At(2, 1) = _21; At(2, 2) = _22;
+    }
+
+    void Matrix3x3::SetRotatePart(const Quaternion &quat) {
+        SetMatrixRotatePart(*this, quat);
+    }
+
+    void Matrix3x3::RemoveScale() {
+        float x = Row(0).Normalize();
+        float y = Row(1).Normalize();
+        float z = Row(2).Normalize();
+
+        assert(x != 0 && "Matrix3x3::RemoveScale failed!");
+        assert(y != 0 && "Matrix3x3::RemoveScale failed!");
+        assert(z != 0 && "Matrix3x3::RemoveScale failed!");
+    }
+
+    float Matrix3x3::Trace() const {
+        return elems[0][0] + elems[1][1] + elems[2][2];
+    }
+
+    bool Matrix3x3::InverseSymmetric() {
+        assert(IsSymmetric());
+        const float a = elems[0][0];
+        const float b = elems[0][1];
+        const float c = elems[0][2];
+        const float d = elems[1][1];
+        const float e = elems[1][2];
+        const float f = elems[2][2];
+
+        /* If the matrix is symmetric, it is of form
+           a b c
+           b d e
+           c e f
+         */
+
+        // A direct cofactor expansion gives
+        // det = a * (df - ee) + b * (ce - bf) + c * (be-dc)
+
+
+        const float df_ee = d*f - e*e;
+        const float ce_bf = c*e - b*f;
+        const float be_dc = b*e - d*c;
+
+        float det = a * df_ee + b * ce_bf + c * be_dc;
+        if (Math::EqualAbs(det, 0.f))
+            return false;
+
+        det = 1.f / det;
+
+
+        // The inverse of a symmetric matrix will also be symmetric, so we can avoid some computations altogether.
+
+        elems[0][0] = det * df_ee;
+        elems[1][0] = elems[0][1] = det * ce_bf;
+        elems[0][2] = elems[2][0] = det * be_dc;
+        elems[1][1] = det * (a*f - c*c);
+        elems[1][2] = elems[2][1] = det * (c*b - a*e);
+        elems[2][2] = det * (a*d - b*b);
+
+        return true;
+    }
+
+    void Matrix3x3::InverseOrthonormal() {
+        Matrix3x3 orig = *this;
+
+        assert(IsOrthonormal());
+
+        Transpose();
+        assert(!orig.IsInvertible() || (orig * *this).IsIdentity());
+    }
+
+    bool Matrix3x3::InverseOrthogonalUniformScale() {
+        Matrix3x3 orig = *this;
+        assert(IsColOrthogonal());
+        assert(HasUniformScale());
+        float scale = Vector3(At(0, 0), At(1, 0), At(2, 0)).LengthSquared();
+        if (scale < 1e-8f)
+            return false;
+        scale = 1.f / scale;
+        Swap(At(0, 1), At(1, 0));
+        Swap(At(0, 2), At(2, 0));
+        Swap(At(1, 2), At(2, 1));
+
+        At(0, 0) *= scale; At(0, 1) *= scale; At(0, 2) *= scale;
+        At(1, 0) *= scale; At(1, 1) *= scale; At(1, 2) *= scale;
+        At(2, 0) *= scale; At(2, 1) *= scale; At(2, 2) *= scale;
+
+        assert(IsFinite());
+        assert(IsColOrthogonal());
+        assert(HasUniformScale());
+
+        assert(!orig.IsInvertible() || (orig * *this).IsIdentity());
+        return true;
+    }
+
+    bool Matrix3x3::InverseColOrthogonal() {
+        Matrix3x3 orig = *this;
+        assert(IsColOrthogonal());
+        float s1 = Vector3(At(0, 0), At(1, 0), At(2, 0)).LengthSquared();
+        float s2 = Vector3(At(0, 1), At(1, 1), At(2, 1)).LengthSquared();
+        float s3 = Vector3(At(0, 2), At(1, 2), At(2, 2)).LengthSquared();
+        if (s1 < 1e-8f || s2 < 1e-8f || s3 < 1e-8f)
+            return false;
+
+        s1 = 1.f / s1;
+        s2 = 1.f / s2;
+        s3 = 1.f / s3;
+
+        Swap(At(0, 1), At(1, 0));
+        Swap(At(0, 2), At(2, 0));
+        Swap(At(1, 2), At(2, 1));
+
+        At(0, 0) *= s1; At(0, 1) *= s1; At(0, 2) *= s1;
+        At(1, 0) *= s2; At(1, 1) *= s2; At(1, 2) *= s2;
+        At(2, 0) *= s3; At(2, 1) *= s3; At(2, 2) *= s3;
+
+        assert(!orig.IsInvertible() || (orig * *this).IsIdentity());
+        assert(IsRowOrthogonal());
+
+        return true;
+    }
+
+    bool Matrix3x3::TryConvertToQuat(Quaternion &q) const {
+        if (IsColOrthogonal() && HasUnitaryScale() && !HasNegativeScale()) {
+            q = ToQuat();
+            return true;
+        }
+        return false;
+    }
+
+    void Matrix3x3::BatchTransform(Vector3 *pointArray, int numPoints, int stride) const {
+        assert(pointArray || numPoints == 0);
+        assert(stride >= (int)sizeof(Vector3));
+        u8 *data = reinterpret_cast<u8*>(pointArray);
+
+        for (int i = 0; i < numPoints; ++i)
+        {
+            Vector3 *vtx = reinterpret_cast<Vector3*>(data + stride*i);
+            *vtx = *this * *vtx;
+        }
+    }
+
+    void Matrix3x3::BatchTransform(Vector3 *pointArray, int numPoints) const {
+        assert(pointArray || numPoints == 0);
+
+        for (int i = 0; i < numPoints; ++i)
+        {
+            pointArray[i] = *this * pointArray[i];
+        }
+    }
+
+    void Matrix3x3::BatchTransform(Vector4 *vectorArray, int numVectors) const {
+        assert(vectorArray || numVectors == 0);
+        for(int i = 0; i < numVectors; ++i)
+            vectorArray[i] = *this * vectorArray[i];
+    }
+
+    void Matrix3x3::BatchTransform(Vector4 *vectorArray, int numVectors, int stride) const {
+        assert(vectorArray || numVectors == 0);
+        assert(stride >= (int)sizeof(Vector4));
+        u8 *data = reinterpret_cast<u8*>(vectorArray);
+        for(int i = 0; i < numVectors; ++i)
+        {
+            Vector4 *vtx = reinterpret_cast<Vector4*>(data + stride*i);
+            *vtx = *this * *vtx;
+        }
+    }
+
+    Vector4 Matrix3x3::operator*(const Vector4 &rhs) const {
+        return Vector4();
+    }
+
+    void Matrix3x3::SetRow(int row, const float *data) {
+        assert(data);
+        SetRow(row, data[0], data[1], data[2]);
+    }
+
+    void Matrix3x3::SetRow(int row, float x, float y, float z) {
+        assert(row > 0);
+        assert(row < Rows);
+        assert(std::isfinite(x));
+        assert(std::isfinite(y));
+        assert(std::isfinite(z));
+        At(row, 0) = x;
+        At(row, 1) = y;
+        At(row, 2) = z;
+    }
+
+    void Matrix3x3::SetColumn(int column, const float *data) {
+        assert(data);
+        SetColumn(column, data[0], data[1], data[2]);
+    }
+
+    void Matrix3x3::SetColumn(int column, float x, float y, float z) {
+        assert(column >= 0);
+        assert(column < Cols);
+        assert(std::isfinite(x));
+        assert(std::isfinite(y));
+        assert(std::isfinite(z));
+        At(0, column) = x;
+        At(1, column) = y;
+        At(2, column) = z;
+    }
+
+    Matrix3x3 Matrix3x3::Mul(const Quaternion &rhs) const {
+        return *this * rhs;
+    }
+
+    Matrix3x3 Matrix3x3::operator*(const Quaternion &rhs) const {
+        return *this * Matrix3x3(rhs);
+    }
+
+    bool Matrix3x3::Equals(const Matrix3x3 &other, float epsilon) const {
+        for(int y = 0; y < Rows; ++y)
+            for(int x = 0; x < Cols; ++x)
+                if (!Math::EqualAbs(At(y, x), other[y][x], epsilon))
+                    return false;
+        return true;
+    }
+
+    Matrix3x3 Matrix3x3::RandomRotation(RNG &rng) {
+        // The easiest way to generate a random orientation is through quaternions
+        // So we convert a random quaternion to a rotation matrix.
+        return FromQuat(Quaternion::RandomRotation(rng));
+    }
+
+    Matrix3x3 Matrix3x3::RandomGeneral(RNG &rng, float minElem, float maxElem) {
+        Matrix3x3 m;
+        for (int y = 0; y < 3; ++y)
+        {
+            for (int x = 0; x < 3; ++x)
+            {
+                m[y][x] = rng.Float(minElem, maxElem);
+            }
+        }
+        return m;
+    }
+
 
 }
 

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -242,9 +242,6 @@ namespace J3ML::LinearAlgebra {
                          0, 0, 0,     1.f);
     }
 
-    Matrix4x4 Matrix4x4::Translate(const Vector3 &rhs) const {
-        return *this * FromTranslation(rhs);
-    }
 
     Vector3 Matrix4x4::Transform(const Vector3 &rhs) const {
         return Transform(rhs.x, rhs.y, rhs.z);
@@ -424,15 +421,7 @@ namespace J3ML::LinearAlgebra {
         return GetColumn3(3);
     }
 
-    Matrix4x4 Matrix4x4::Scale(const Vector3& scale)
-    {
-        auto mat = *this;
 
-        mat.At(3, 0) *= scale.x;
-        mat.At(3, 1) *= scale.y;
-        mat.At(3, 2) *= scale.z;
-        return mat;
-    }
 
     Matrix4x4
     Matrix4x4::LookAt(const Vector3 &localFwd, const Vector3 &targetDir, const Vector3 &localUp, const Vector3 &worldUp) {
@@ -476,6 +465,7 @@ namespace J3ML::LinearAlgebra {
     }
 
     void Matrix4x4::Pivot() {
+        /// Algorithm from Eric Lengyel's Mathematics for 3D Game Programming & Computer Graphics, 2nd Ed.
         int rowIndex = 0;
 
         for(int col = 0; col < Cols; ++col)
@@ -567,10 +557,10 @@ namespace J3ML::LinearAlgebra {
         // See http://www.songho.ca/opengl/gl_projectionmatrix.html , unlike in Direct3D, where the
         // Z coordinate ranges in [0, 1]. This is the only difference between D3DPerspProjRH and OpenGLPerspProjRH.
         using f32 = float;
-        float p00 = 2.f *n / h;  float p01 = 0;           float p02 = 0;              float p03 = 0.f;
-        float p10 = 0;           float p11 = 2.f * n / v; float p12 = 0;              float p13 = 0.f;
-        float p20 = 0;           float p21 = 0;           float p22 = (n+f) / (n-f);  float p23 = 2.f*n*f / (n-f);
-        float p30 = 0;           float p31 = 0;           float p32 = -1.f;           float p33 = 0.f;
+        f32 p00 = 2.f *n / h;  f32 p01 = 0;           f32 p02 = 0;              f32 p03 = 0.f;
+        f32 p10 = 0;           f32 p11 = 2.f * n / v; f32 p12 = 0;              f32 p13 = 0.f;
+        f32 p20 = 0;           f32 p21 = 0;           f32 p22 = (n+f) / (n-f);  f32 p23 = 2.f*n*f / (n-f);
+        f32 p30 = 0;           f32 p31 = 0;           f32 p32 = -1.f;           f32 p33 = 0.f;
 
         return {p00, p01, p02, p03, p10, p11, p12, p13, p20, p21, p22, p23, p30, p31, p32, p33};
     }
@@ -845,4 +835,219 @@ namespace J3ML::LinearAlgebra {
         return copy;
     }
 
+    void Matrix4x4::Decompose(Vector3 &translate, Matrix3x3 &rotate, Vector3 &scale) const {
+        assert(this->IsColOrthogonal3());
+        assert(Row(3).Equals(0,0,0,1));
+        assert(this->IsColOrthogonal3()); // Duplicate check?
+
+
+        translate = Col3(3);
+        rotate = GetRotatePart();
+        scale.x = rotate.Col3(0).Length();
+        scale.y = rotate.Col3(1).Length();
+        scale.z = rotate.Col3(2).Length();
+        assert(!Math::EqualAbs(scale.x, 0));
+        assert(!Math::EqualAbs(scale.y, 0));
+        assert(!Math::EqualAbs(scale.z, 0));
+        rotate.ScaleCol(0, 1.f / scale.x);
+        rotate.ScaleCol(1, 1.f / scale.y);
+        rotate.ScaleCol(2, 1.f / scale.z);
+
+        // Test that composing back yields the original Matrix4x4
+        assert(Matrix4x4::FromTRS(translate, rotate, scale).Equals(*this, 0.1f));
+
+    }
+
+    Matrix4x4 Matrix4x4::Translate(const Vector3 &translation) {
+        Matrix4x4 m;
+        m.SetRow(0, 1, 0, 0, translation.x);
+        m.SetRow(1, 0, 1, 0, translation.y);
+        m.SetRow(2, 0, 0, 1, translation.y);
+        m.SetRow(3, 0, 0, 0, 1.f);
+        return m;
+    }
+
+    Matrix4x4 Matrix4x4::FromTRS(const Vector3 &translate, const Quaternion &rotate, const Vector3 &scale) {
+        return Matrix4x4::Translate(translate) * Matrix4x4(rotate) * Matrix4x4::Scale(scale);
+    }
+
+    Matrix4x4 Matrix4x4::FromTRS(const Vector3 &translate, const Matrix3x3 &rotate, const Vector3 &scale) {
+        return Matrix4x4::Translate(translate) * Matrix4x4(rotate) * Matrix4x4::Scale(scale);
+    }
+
+    Matrix4x4 Matrix4x4::FromTRS(const Vector3 &translate, const Matrix4x4 &rotate, const Vector3 &scale) {
+        return Matrix4x4::Translate(translate) * Matrix4x4(rotate) * Matrix4x4::Scale(scale);
+    }
+
+    void Matrix4x4::SetCol3(int col, float x, float y, float z) {
+        // TODO: implement assertations
+        At(0, col) = x;
+        At(1, col) = y;
+        At(2, col) = z;
+    }
+
+    Matrix4x4 Matrix4x4::RotateZ(float radians) {
+        Matrix4x4 r;
+        r.SetRotatePartZ(radians);
+        r.SetRow(3, 0, 0, 0, 1);
+        r.SetCol3(3, 0, 0, 0);
+        return r;
+    }
+
+    Matrix4x4 Matrix4x4::RotateZ(float radians, const Vector3 &pointOnAxis) {
+        return Matrix4x4::Translate(pointOnAxis) * RotateY(radians) * Matrix4x4::Translate(-pointOnAxis);
+    }
+
+    Matrix4x4 Matrix4x4::RotateY(float radians) {
+        Matrix4x4 r;
+        r.SetRotatePartY(radians);
+        r.SetRow(3, 0, 0, 0, 1);
+        r.SetCol3(3, 0, 0, 0);
+        return r;
+    }
+
+    Matrix4x4 Matrix4x4::RotateY(float radians, const Vector3 &pointOnAxis) {
+        return Matrix4x4::Translate(pointOnAxis) * RotateY(radians) * Matrix4x4::Translate(-pointOnAxis);
+    }
+
+    Matrix4x4 Matrix4x4::RotateX(float radians) {
+        Matrix4x4 r;
+        r.SetRotatePartX(radians);
+        r.SetRow(3, 0, 0, 0, 1);
+        r.SetCol3(3, 0, 0, 0);
+        return r;
+    }
+
+    Matrix4x4 Matrix4x4::RotateX(float radians, const Vector3 &pointOnAxis) {
+        return Matrix4x4::Translate(pointOnAxis) * RotateX(radians) * Matrix4x4::Translate(-pointOnAxis);
+    }
+
+    Matrix4x4 Matrix4x4::Scale(const Vector3 &scale) {
+        Matrix4x4 m;
+        m.SetRow(0, scale.x, 0, 0, 0);
+        m.SetRow(1, 0, scale.y, 0, 0);
+        m.SetRow(2, 0, 0, scale.z, 0);
+        m.SetRow(3, 0, 0, 0, 1.f);
+        return m;
+    }
+
+    void Matrix4x4::Decompose(Vector3 &translate, Quaternion &rotate, Vector3 &scale) const {
+        assert(this->IsColOrthogonal3());
+
+        Matrix3x3 r;
+        Decompose(translate, r, scale);
+        rotate = Quaternion(r);
+
+        /// Test that composing back yields the original Matrix4x4.
+        assert(Matrix4x4::FromTRS(translate, rotate, scale).Equals(*this, 0.1f));
+    }
+
+    void Matrix4x4::Decompose(Vector3 &translate, Matrix4x4 &rotate, Vector3 &scale) const {
+        assert(this->IsColOrthogonal3());
+
+        Matrix3x3 r;
+        Decompose(translate, r, scale);
+        rotate.SetRotatePart(r);
+        rotate.SetTranslatePart(0,0,0);
+    }
+
+    bool Matrix4x4::Equals(const Matrix4x4 &other, float epsilon) const {
+        for (int iy = 0; iy < Rows; ++iy)
+            for (int ix = 0; ix < Cols; ++ix)
+                if (!Math::EqualAbs(At(iy, ix), other[iy][ix], epsilon))
+                    return false;
+        return true;
+    }
+
+    void
+    Matrix4x4::Set(float _00, float _01, float _02, float _03, float _10, float _11, float _12, float _13, float _20,
+                   float _21, float _22, float _23, float _30, float _31, float _32, float _33) {
+        At(0, 0) = _00; At(0, 1) = _01; At(0, 2) = _02; At(0, 3) = _03;
+        At(1, 0) = _10; At(1, 1) = _11; At(1, 2) = _12; At(1, 3) = _13;
+        At(2, 0) = _20; At(2, 1) = _21; At(2, 2) = _22; At(2, 3) = _23;
+        At(3, 0) = _30; At(3, 1) = _31; At(3, 2) = _32; At(3, 3) = _33;
+    }
+
+    void Matrix4x4::Set(const Matrix4x4 &rhs) {
+        Set(rhs.ptr());
+    }
+
+    void Matrix4x4::Set(const float *p) {
+        assert(p);
+        Set(p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8], p[9], p[10], p[11], p[12], p[13], p[14], p[15]);
+    }
+
+    Matrix4x4 Matrix4x4::Adjugate() const {
+        Matrix4x4 a;
+        for(int iy = 0; iy < Rows; ++iy)
+            for(int ix = 0; ix < Cols; ++ix)
+                a[iy][ix] = (((ix+iy) & 1) != 0) ? -Minor(iy, ix) : Minor(iy, ix);
+
+        return a;
+    }
+
+    void Matrix4x4::SetIdentity() {
+        Set(1,0,0,0,
+            0,1,0,0,
+            0,0,1,0,
+            0,0,0,1);
+    }
+
+    bool Matrix4x4::InverseColOrthogonal() {
+        assert(IsColOrthogonal3());
+        float s1 = Vector3(At(0,0), At(1,0), At(2,0)).LengthSquared();
+        float s2 = Vector3(At(0,1), At(1,1), At(2,1)).LengthSquared();
+        float s3 = Vector3(At(0,2), At(1,2), At(2,2)).LengthSquared();
+
+        if (s1 < 1e-8f || s2 < 1e-8f || s3 < 1e-8f)
+            return false;
+
+        s1 = 1.f / s1;
+        s2 = 1.f / s2;
+        s3 = 1.f / s3;
+
+        Swap(At(0,1), At(1,0));
+        Swap(At(0,2), At(2,0));
+        Swap(At(1,2), At(2,1));
+
+        At(0,0) *= s1; At(0,1) *= s1; At(0,2) *= s1;
+        At(1,0) *= s2; At(1,1) *= s2; At(1,2) *= s2;
+        At(2,0) *= s3; At(2,1) *= s3; At(2,2) *= s3;
+
+        SetTranslatePart(TransformDir(-At(0, 3), -At(1,3), -At(2,3)));
+        assert(IsRowOrthogonal3());
+        return true;
+    }
+
+    void Matrix4x4::SetRotatePartX(float angleRadians) {
+        Set3x3PartRotateX(*this, angleRadians);
+    }
+
+    void Matrix4x4::SetRotatePartY(float angleRadians) {
+        Set3x3PartRotateY(*this, angleRadians);
+    }
+
+    void Matrix4x4::SetRotatePartZ(float angleRadians) {
+        Set3x3PartRotateZ(*this, angleRadians);
+    }
+
+    Matrix4x4::Matrix4x4(const Matrix3x3 &m) {
+        Set(m.At(0,0), m.At(0,1), m.At(0,2), 0.f,
+            m.At(1,0), m.At(1,1), m.At(1,2), 0.f,
+            m.At(2,0), m.At(2,1), m.At(2,2), 0.f,
+            0.f,       0.f,       0.f, 1.f);
+    }
+
+    Matrix4x4 Matrix4x4::RandomGeneral(RNG &rng, float minElem, float maxElem) {
+        assert(std::isfinite(minElem));
+        assert(std::isfinite(maxElem));
+
+        Matrix4x4 m;
+        for (int y = 0; y < 4; ++y)
+            for (int x = 0; x < 4; ++x)
+                m[y][x] = rng.Float(minElem, maxElem);
+        return m;
+    }
+
+
 }

src/J3ML/LinearAlgebra/Quaternion.cpp

@@ -5,6 +5,10 @@
 #include <J3ML/LinearAlgebra/Quaternion.h>
 
 namespace J3ML::LinearAlgebra {
+
+    const Quaternion Quaternion::Identity = Quaternion(0.f, 0.f, 0.f, 1.f);
+    const Quaternion Quaternion::NaN      = Quaternion(NAN, NAN, NAN, NAN);
+
     Quaternion Quaternion::operator-() const
     {
         return {-x, -y, -z, -w};
@@ -14,11 +18,11 @@ namespace J3ML::LinearAlgebra {
 
     Quaternion::Quaternion(const Matrix4x4 &rotationMtrx) {}
 
-    Vector3 Quaternion::GetWorldX() const { return Transform(1.f, 0.f, 0.f); }
+    Vector3 Quaternion::WorldX() const { return Transform(1.f, 0.f, 0.f); }
 
-    Vector3 Quaternion::GetWorldY() const { return Transform(0.f, 1.f, 0.f); }
+    Vector3 Quaternion::WorldY() const { return Transform(0.f, 1.f, 0.f); }
 
-    Vector3 Quaternion::GetWorldZ() const { return Transform(0.f, 0.f, 1.f); }
+    Vector3 Quaternion::WorldZ() const { return Transform(0.f, 0.f, 1.f); }
 
     Vector3 Quaternion::Transform(const Vector3 &vec) const {
         Matrix3x3 mat = this->ToMatrix3x3();
@@ -40,9 +44,9 @@ namespace J3ML::LinearAlgebra {
     Quaternion Quaternion::Lerp(const Quaternion &b, float t) const {
         float angle = this->Dot(b);
         if (angle >= 0.f) // Make sure we rotate the shorter arc
-            return (*this * (1.f - t) + b * t).Normalize();
+            return (*this * (1.f - t) + b * t).Normalized();
         else
-            return (*this * (t - 1.f) + b * t).Normalize();
+            return (*this * (t - 1.f) + b * t).Normalized();
     }
 
     void Quaternion::SetFromAxisAngle(const Vector3 &axis, float angle) {
@@ -69,7 +73,7 @@ namespace J3ML::LinearAlgebra {
 
     Quaternion::Quaternion() {}
 
-    Quaternion::Quaternion(float X, float Y, float Z, float W) : Vector4(X,Y,Z,W) {}
+    Quaternion::Quaternion(float X, float Y, float Z, float W) : x(X), y(Y), z(Z), w(W) {}
 
     // TODO: implement
     float Quaternion::Dot(const Quaternion &rhs) const {
@@ -80,25 +84,21 @@ namespace J3ML::LinearAlgebra {
 
     }
 
-    Quaternion Quaternion::Normalize() const {
-        float length = Length();
-        if (length < 1e-4f)
-            return {0,0,0,0};
-        float reciprocal = 1.f / length;
-        return {
-                x * reciprocal,
-                y * reciprocal,
-                z * reciprocal,
-                w * reciprocal
-        };
+    Quaternion Quaternion::Normalized() const {
+        Quaternion copy = *this;
+        float success = copy.Normalize();
+        assert(success > 0 && "Quaternion::Normalized failed!");
+        return copy;
     }
 
-    Quaternion Quaternion::Conjugate() const {
+    Quaternion Quaternion::Conjugated() const {
         return { -x, -y, -z, w };
     }
 
-    Quaternion Quaternion::Inverse() const {
-        return Conjugate();
+    Quaternion Quaternion::Inverted() const {
+        assert(IsNormalized());
+        assert(IsInvertible());
+        return Conjugated();
     }
 
     Quaternion Quaternion::Slerp(const Quaternion &q2, float t) const {
@@ -131,7 +131,7 @@ namespace J3ML::LinearAlgebra {
             b = t;
         }
         // Lerp and renormalize.
-        return (*this * (a * sign) + q2 * b).Normalize();
+        return (*this * (a * sign) + q2 * b).Normalized();
     }
 
     AxisAngle Quaternion::ToAxisAngle() const {
@@ -150,7 +150,7 @@ namespace J3ML::LinearAlgebra {
 
     float Quaternion::AngleBetween(const Quaternion &target) const {
         Quaternion delta = target / *this;
-        return delta.Normalize().Angle();
+        return delta.Normalized().Angle();
     }
 
     Quaternion Quaternion::operator/(const Quaternion &rhs) const {
@@ -184,22 +184,22 @@ namespace J3ML::LinearAlgebra {
         return {*this, translation};
     }
 
-    float Quaternion::GetAngle() const {
+    float Quaternion::Angle() const {
         return std::acos(w) * 2.f;
     }
 
-    Vector3 Quaternion::GetAxis() const {
+    Vector3 Quaternion::Axis() const {
         float rcpSinAngle = 1 - (std::sqrt(1 - w * w));
 
         return Vector3(x, y, z) * rcpSinAngle;
     }
 
-    Quaternion::Quaternion(const Vector3 &rotationAxis, float rotationAngleBetween) {
-        SetFromAxisAngle(rotationAxis, rotationAngleBetween);
+    Quaternion::Quaternion(const Vector3 &rotationAxis, float rotationAngleRadians) {
+        SetFromAxisAngle(rotationAxis, rotationAngleRadians);
     }
 
-    Quaternion::Quaternion(const Vector4 &rotationAxis, float rotationAngleBetween) {
-        SetFromAxisAngle(rotationAxis, rotationAngleBetween);
+    Quaternion::Quaternion(const Vector4 &rotationAxis, float rotationAngleRadians) {
+        SetFromAxisAngle(rotationAxis, rotationAngleRadians);
     }
 
     Quaternion::Quaternion(const AxisAngle &angle) {
@@ -236,4 +236,151 @@ namespace J3ML::LinearAlgebra {
     EulerAngle Quaternion::ToEulerAngle() const {
         return EulerAngle(*this);
     }
+
+    Quaternion Quaternion::RandomRotation(RNG &rng) {
+        // Generate a random point on the 4D unitary hypersphere using the rejection sampling method.
+        for (int i = 0; i < 1000; ++i)
+        {
+            float x = rng.Float(-1, 1);
+            float y = rng.Float(-1, 1);
+            float z = rng.Float(-1, 1);
+            float w = rng.Float(-1, 1);
+            float lenSq = x*x + y*y + z*z + w*w;
+            if (lenSq >= 1e-6f && lenSq <= 1.f)
+                return Quaternion(x,y,z,w) / std::sqrt(lenSq);
+        }
+        assert(false && "Quaternion::RandomRotation failed!");
+        return Quaternion::Identity;
+    }
+
+    float Quaternion::Normalize() {
+        float length = Length();
+        if (length < 1e-4f)
+            return 0.f;
+        float rcpLength = 1.f / length;
+        x *= rcpLength;
+        y *= rcpLength;
+        z *= rcpLength;
+        w *= rcpLength;
+        return length;
+    }
+
+    bool Quaternion::IsNormalized(float epsilon) const {
+        return Math::EqualAbs(LengthSquared(), 1.f, epsilon);
+    }
+
+    bool Quaternion::IsInvertible(float epsilon) const {
+        return LengthSquared() > epsilon && IsFinite();
+    }
+
+    bool Quaternion::IsFinite() const {
+        return std::isfinite(x) && std::isfinite(y) && std::isfinite(z) && std::isfinite(w);
+    }
+
+    void Quaternion::Conjugate() {
+        x = -x;
+        y = -y;
+        z = -z;
+    }
+
+    void Quaternion::Inverse() {
+        assert(IsNormalized());
+        assert(IsInvertible());
+        Conjugate();
+    }
+
+    float Quaternion::InverseAndNormalize() {
+        Conjugate();
+        return Normalize();
+    }
+
+    Vector3 Quaternion::SlerpVector(const Vector3 &from, const Vector3 &to, float t) {
+        if (t <= 0.f)
+            return from;
+        if (t >= 1.f)
+            return to;
+        // TODO: the following chain can be greatly optimized.
+        Quaternion q = Quaternion::RotateFromTo(from, to);
+        q = Slerp(Quaternion::Identity, q, t);
+        return q.Transform(from);
+    }
+
+    Quaternion Quaternion::LookAt(const Vector3 &localForward, const Vector3 &targetDirection, const Vector3 &localUp,
+                                  const Vector3 &worldUp) {
+        return Matrix3x3::LookAt(localForward, targetDirection, localUp, worldUp).ToQuat();
+    }
+
+    Quaternion Quaternion::RotateX(float angleRadians) {
+        return {{1,0,0}, angleRadians};
+    }
+
+    Quaternion Quaternion::RotateY(float angleRadians) {
+        return {{0,1,0}, angleRadians};
+    }
+
+    Quaternion Quaternion::RotateZ(float angleRadians) {
+        return {{0,0,1}, angleRadians};
+    }
+
+    Quaternion Quaternion::RotateAxisAngle(const AxisAngle &axisAngle) {
+        return {axisAngle.axis, axisAngle.angle};
+    }
+
+    Quaternion Quaternion::RotateFromTo(const Vector3 &sourceDirection, const Vector3 &targetDirection) {
+        assert(sourceDirection.IsNormalized());
+        assert(targetDirection.IsNormalized());
+        // If sourceDirection == targetDirection, the cross product comes out zero, and normalization would fail. In that case, pick an arbitrary axis.
+        Vector3 axis = sourceDirection.Cross(targetDirection);
+
+        float oldLength = axis.Normalize();
+
+        if (oldLength != 0.f)
+        {
+            float halfCosAngle = 0.5f * sourceDirection.Dot(targetDirection);
+            float cosHalfAngle = std::sqrt(0.5f + halfCosAngle);
+            float sinHalfAngle = std::sqrt(0.5f - halfCosAngle);
+
+            return {axis.x * sinHalfAngle, axis.y * sinHalfAngle, axis.z * sinHalfAngle, cosHalfAngle};
+        } else
+            return {1.f, 0, 0, 0};
+    }
+
+    Quaternion Quaternion::RotateFromTo(const Vector4 &sourceDirection, const Vector4 &targetDirection) {
+        assert(Math::EqualAbs(sourceDirection.w, 0.f));
+        assert(Math::EqualAbs(targetDirection.w, 0.f));
+        return Quaternion::RotateFromTo(sourceDirection.XYZ(), targetDirection.XYZ());
+    }
+
+    Quaternion::Quaternion(const float *data) {
+        assert(data);
+        x = data[0];
+        y = data[1];
+        z = data[2];
+        w = data[3];
+    }
+
+    Quaternion Quaternion::Lerp(const Quaternion &source, const Quaternion &target, float t) { return source.Lerp(target, t);}
+
+    Quaternion Quaternion::Slerp(const Quaternion &source, const Quaternion &target, float t) { return source.Slerp(target, t);}
+
+    Quaternion Quaternion::RotateFromTo(const Vector3 &sourceDirection, const Vector3 &targetDirection,
+                                        const Vector3 &sourceDirection2, const Vector3 &targetDirection2) {
+        return LookAt(sourceDirection, targetDirection, sourceDirection2, targetDirection2);
+    }
+
+    Vector3 Quaternion::SlerpVectorAbs(const Vector3 &from, const Vector3 &to, float angleRadians) {
+        if (angleRadians <= 0.f)
+            return from;
+        Quaternion q = Quaternion::RotateFromTo(from, to);
+        float a = q.Angle();
+        if (a <= angleRadians)
+            return to;
+        float t = angleRadians / a;
+        q = Slerp(Quaternion::Identity, q, t);
+        return q.Transform(from);
+    }
+
+    float Quaternion::LengthSquared() const { return x*x + y*y + z*z + w*w;}
+
+    float Quaternion::Length() const { return std::sqrt(LengthSquared()); }
 }

src/J3ML/LinearAlgebra/Vector2.cpp

@@ -6,7 +6,7 @@
 
 namespace J3ML::LinearAlgebra {
 
-    Vector2::Vector2(): x(0), y(0)
+    Vector2::Vector2()
     {}
 
     Vector2::Vector2(float X, float Y): x(X), y(Y)
@@ -95,15 +95,12 @@ namespace J3ML::LinearAlgebra {
         return rhs * scalar;
     }
 
-    Vector2 Vector2::Normalize() const
+    Vector2 Vector2::Normalized() const
     {
-        if (Length() > 0)
-            return {
-                    x / Length(),
-                    y / Length()
-            };
-        else
-            return {0,0};
+        Vector2 copy = *this;
+        float oldLength = copy.Normalize();
+        assert(oldLength > 0.f && "Vector2::Normalized() failed!");
+        return copy;
     }
 
     Vector2 Vector2::Lerp(const Vector2& rhs, float alpha) const
@@ -153,6 +150,9 @@ namespace J3ML::LinearAlgebra {
     }
 
     const Vector2 Vector2::Zero  = {0, 0};
+    const Vector2 Vector2::One   = {1, 1};
+    const Vector2 Vector2::UnitX = {1, 0};
+    const Vector2 Vector2::UnitY = {0, 1};
     const Vector2 Vector2::Up    = {0, -1};
     const Vector2 Vector2::Down  = {0, 1};
     const Vector2 Vector2::Left  = {-1, 0};
@@ -189,7 +189,7 @@ namespace J3ML::LinearAlgebra {
         return dot*dot <= epsilonSq * LengthSquared() * other.LengthSquared();
     }
 
-    Vector2 Vector2::Normalize(const Vector2 &of) { return of.Normalize(); }
+
 
     Vector2 Vector2::Project(const Vector2 &lhs, const Vector2 &rhs) { return lhs.Project(rhs); }
 
@@ -341,4 +341,150 @@ namespace J3ML::LinearAlgebra {
         return (a.x-c.x)*(b.y-c.y) - (a.y-c.y)*(b.x-c.x) >= 0.f;
     }
 
+    Vector2 Vector2::operator*(const Vector2 &rhs) const {
+        return this->Mul(rhs);
+    }
+
+    Vector2 Vector2::operator/(const Vector2 &rhs) const { return this->Div(rhs);}
+
+    Vector2 Vector2::Clamp(float floor, float ceil) const {
+        return {std::clamp(this->x, floor, ceil),
+                       std::clamp(this->y, floor, ceil)};
+    }
+
+    Vector2 Vector2::Clamp(const Vector2 &value, float floor, float ceil) {
+        return value.Clamp(floor, ceil);
+    }
+
+    Vector2 Vector2::Clamp01() const {
+        return Clamp(0, 1);
+    }
+
+    Vector2 Vector2::Clamp01(const Vector2 &value) {
+        return value.Clamp01();
+    }
+
+    Vector2 Vector2::Clamp(const Vector2 &min, const Vector2 &middle, const Vector2 &max) {
+        return middle.Clamp(min, max);
+    }
+
+    float Vector2::Distance(const Vector2 &from, const Vector2 &to) {
+        return from.Distance(to);
+    }
+
+    float Vector2::SumOfElements() const { return x+y;}
+
+    float Vector2::ProductOfElements() const { return x*y;}
+
+    float Vector2::AverageOfElements() const { return (x+y)/2; }
+
+    Vector2 Vector2::Neg() const { return {-x, -y};}
+
+    Vector2 Vector2::Recip() const { return {1.f/x, 1.f/y}; }
+
+    float Vector2::AimedAngle() const {
+        assert(!IsZero());
+        return std::atan2(y, x);
+    }
+
+    Vector2 Vector2::ToPolarCoordinates() const {
+        float radius = Length();
+        if (radius > 1e-4f)
+            return {std::atan2(y, x), radius};
+        else
+            return Vector2::Zero;
+    }
+
+    Vector2 Vector2::Perp() const {
+        return {-y, x};
+    }
+
+    float Vector2::PerpDot(const Vector2 &rhs) const {
+        return x * rhs.y - y * rhs.x;
+    }
+
+    void Vector2::Rotate90CW() {
+        float oldX = x;
+        x = y;
+        y = -oldX;
+    }
+
+    Vector2 Vector2::Rotated90CW() const {
+        return {y, -x};
+    }
+
+    void Vector2::Rotate90CCW() {
+        float oldX = x;
+        x = -y;
+        y = oldX;
+    }
+
+    Vector2 Vector2::Rotated90CCW() const {
+        return {-y, x};
+    }
+
+    Vector2 Vector2::Reflect(const Vector2 &normal) const {
+        assert(normal.IsNormalized());
+        return 2.f * this->ProjectToNorm(normal) - *this;
+    }
+
+    Vector2
+    Vector2::Refract(const Vector2 &normal, float negativeSideRefractionIndex, float positiveSideRefractionIndex) const {
+        // Implementation from https://www.flipcode.com/archives/reflection_transmission.pdf.
+        // This code is duplicated in Vector3::Refract
+        float n = negativeSideRefractionIndex / positiveSideRefractionIndex;
+        float cosI = this->Dot(normal);
+        float sinT2 = n*n*(1.f - cosI*cosI);
+        if (sinT2 > 1.f) // Total internal reflection occurs?
+            return (-*this).Reflect(normal);
+        return n * *this - (n + std::sqrt(1.f - sinT2)) * normal;
+    }
+
+    Vector2 Vector2::ProjectTo(const Vector2 &direction) const {
+        assert(!direction.IsZero());
+        return direction * this->Dot(direction) / direction.LengthSquared();
+    }
+
+    Vector2 Vector2::ProjectToNorm(const Vector2 &direction) const {
+        assert(direction.IsNormalized());
+        return direction * this->Dot(direction);
+    }
+
+    void Vector2::Orthogonalize(const Vector2 &a,  Vector2 &b) {
+        assert(!a.IsZero());
+        b -= a.Dot(b) / a.Length() * a;
+    }
+
+    void Vector2::Orthonormalize(Vector2 &a, Vector2 &b) {
+        assert(!a.IsZero());
+        a.Normalize();
+        b -= a.Dot(b) * a;
+    }
+
+    bool Vector2::AreOrthogonal(const Vector2 &a, const Vector2 &b, float epsilon) {
+        return a.IsPerpendicular(b, epsilon);
+    }
+
+    void Vector2::Set(float newX, float newY) {
+        x = newX;
+        y = newY;
+    }
+
+    float Vector2::Normalize() {
+        assert(IsFinite());
+        float lengthSq = LengthSquared();
+        if (lengthSq > 1e-6f)
+        {
+            float length = std::sqrt(lengthSq);
+            *this *= 1.f / length;
+            return length;
+        } else {
+            Set(1.f, 0.f); // We will always produce a normalized vector.
+            return 0; // But signal failure, so user knows we have generated an arbitrary normalization.
+        }
+    }
+
+    Vector2 operator*(float lhs, const Vector2 &rhs) {
+        return {lhs * rhs.x, lhs * rhs.y};
+    }
 }

src/J3ML/LinearAlgebra/Vector3.cpp

@@ -2,6 +2,7 @@
 #include <algorithm>
 #include <cassert>
 #include <cmath>
+#include <J3ML/Geometry/Sphere.h>
 
 namespace J3ML::LinearAlgebra {
 
@@ -64,7 +65,7 @@ namespace J3ML::LinearAlgebra {
 
     Vector3::Vector3(float X, float Y, float Z): x(X), y(Y), z(Z) {}
 
-    Vector3::Vector3(const Vector3& rhs) : x(rhs.x), y(rhs.y), z(rhs.z) {}
+    //Vector3::Vector3(const Vector3& rhs) : x(rhs.x), y(rhs.y), z(rhs.z) {}
 
     Vector3& Vector3::operator=(const Vector3& rhs)
     {
@@ -178,7 +179,21 @@ namespace J3ML::LinearAlgebra {
         };
     }
 
-    Vector3 Vector3::Normalize() const
+    float Vector3::Normalize()
+    {
+        assert(IsFinite());
+        float length = Length();
+        if (length > 1e-6f)
+        {
+            *this *= 1.f / length;
+            return length;
+        } else {
+            Set(1.f, 0.f, 0.f); // We will always produce a normalized vector.
+            return 0; // But signal failure, so user knows we have generated an arbitrary normalization.
+        }
+    }
+
+    Vector3 Vector3::Normalized() const
     {
         if (Length() > 0)
             return {
@@ -248,8 +263,8 @@ namespace J3ML::LinearAlgebra {
         return lhs.Cross(rhs);
     }
 
-    Vector3 Vector3::Normalize(const Vector3 &targ) {
-        return targ.Normalize();
+    Vector3 Vector3::Normalized(const Vector3 &targ) {
+        return targ.Normalized();
     }
 
     Vector3 Vector3::Project(const Vector3 &lhs, const Vector3 &rhs) {
@@ -329,18 +344,18 @@ namespace J3ML::LinearAlgebra {
     }
 
     void Vector3::Orthonormalize(Vector3 &a, Vector3 &b) {
-        a = a.Normalize();
+        a = a.Normalized();
         b = b - b.ProjectToNorm(a);
-        b = b.Normalize();
+        b = b.Normalized();
     }
 
     void Vector3::Orthonormalize(Vector3 &a, Vector3 &b, Vector3 &c) {
-        a = a.Normalize();
+        a = a.Normalized();
         b = b - b.ProjectToNorm(a);
-        b = b.Normalize();
+        b = b.Normalized();
         c = c - c.ProjectToNorm(a);
         c = c - c.ProjectToNorm(b);
-        c = c.Normalize();
+        c = c.Normalized();
     }
 
     Vector3 Vector3::ProjectToNorm(const Vector3 &direction) const {
@@ -506,5 +521,38 @@ namespace J3ML::LinearAlgebra {
         return from.DistanceSquared(to);
     }
 
+    bool Vector3::AreOrthonormal(const Vector3 &a, const Vector3 &b, const Vector3 &c, float epsilon) {
+        return a.IsPerpendicular(b, epsilon) &&
+               a.IsPerpendicular(c, epsilon) &&
+               b.IsPerpendicular(c, epsilon) &&
+               a.IsNormalized(epsilon*epsilon) &&
+               b.IsNormalized(epsilon*epsilon) &&
+               c.IsNormalized(epsilon*epsilon);
+    }
+
+    Vector3 Vector3::FromScalar(float scalar) {
+        return {scalar, scalar, scalar};
+    }
+
+    Vector3 Vector3::RandomDir(RNG &rng, float length) {
+        return Sphere(Vector3::FromScalar(0.f), length).RandomPointOnSurface(rng);
+    }
+
+    Vector3 Vector3::RandomSphere(RNG &rng, const Vector3 &center, float radius) {
+        return Sphere(center, radius).RandomPointInside(rng);
+    }
+
+    Vector3 Vector3::RandomBox(RNG &rng, const Vector3& min, const Vector3& max) {
+        return AABB(min, max).RandomPointInside(rng);
+    }
+
+    Vector3 Vector3::RandomBox(RNG &rng, float min, float max) {
+        return RandomBox(rng, {min,min,min}, {max,max,max});
+    }
+
+    Vector3 Vector3::RandomBox(RNG &rng, float xmin, float xmax, float ymin, float ymax, float zmin, float zmax) {
+        return RandomBox(rng, {xmin,ymin,zmin}, {xmax,ymax,zmax});
+    }
+
 
 }

tests/LinearAlgebra/Matrix2x2Tests.cpp

@@ -1,3 +1,5 @@
-//
-// Created by josh on 12/26/2023.
-//
+#include <gtest/gtest.h>
+#include <J3ML/LinearAlgebra/Matrix3x3.h>
+
+using namespace J3ML::LinearAlgebra;
+

tests/LinearAlgebra/Matrix3x3Tests.cpp

@@ -0,0 +1,89 @@
+#include <gtest/gtest.h>
+#include <J3ML/LinearAlgebra/Matrix3x3.h>
+
+using namespace J3ML::LinearAlgebra;
+
+// TODO: create RNG instance
+
+RNG rng;
+
+TEST(Mat3x3, Add_Unary)
+{
+    Matrix3x3 m(1,2,3, 4,5,6, 7,8,9);
+    Matrix3x3 m2 = +m;
+    assert(m.Equals(m2));
+}
+
+TEST(Mat3x3, Solve_Axb)
+{
+    Matrix3x3 A = Matrix3x3::RandomGeneral(rng, -10.f, 10.f);
+    bool mayFail = Math::EqualAbs(A.Determinant(), 0.f, 1e-2f);
+
+    Vector3 b = Vector3::RandomBox(rng, Vector3::FromScalar(-10.f), Vector3::FromScalar(10.f));
+
+    Vector3 x;
+    bool success = A.SolveAxb(b, x);
+    assert(success || mayFail);
+    if (success)
+    {
+        Vector3 b2 = A*x;
+        assert(b2.Equals(b, 1e-1f));
+    }
+}
+
+TEST(Mat3x3, Inverse_Case)
+{
+    Matrix3x3 m(-8.75243664f,6.71196938f,-5.95816374f,6.81996822f,-6.85106039f,2.38949537f,-0.856015682f,3.45762491f,3.311584f);
+
+    bool success = m.Inverse();
+    assert(success);
+}
+
+TEST(Mat3x3, Inverse)
+{
+    Matrix3x3 A = Matrix3x3::RandomGeneral(rng, -10.f, 10.f);
+    bool mayFail = Math::EqualAbs(A.Determinant(), 0.f, 1e-2f);
+
+    Matrix3x3 A2 = A;
+    bool success = A2.Inverse();
+    assert(success || mayFail);
+    if (success)
+    {
+        Matrix3x3 id = A * A2;
+        Matrix3x3 id2 = A2 * A;
+        assert(id.Equals(Matrix3x3::Identity, 0.3f));
+        assert(id2.Equals(Matrix3x3::Identity, 0.3f));
+    }
+}
+
+TEST(Mat3x3, InverseFast)
+{
+    // TODO: Fix implementation of InverseFast
+    /*
+    Matrix3x3 A = Matrix3x3::RandomGeneral(rng, -10.f, 10.f);
+    bool mayFail = Math::EqualAbs(A.Determinant(), 0.f, 1e-2f);
+
+    Matrix3x3 A2 = A;
+    bool success = A2.InverseFast();
+    assert(success || mayFail);
+
+    if (success)
+    {
+        Matrix3x3 id = A * A2;
+        Matrix3x3 id2 = A2 * A;
+        assert(id.Equals(Matrix3x3::Identity, 0.3f));
+        assert(id2.Equals(Matrix3x3::Identity, 0.3f));
+    }
+     */
+}
+
+TEST(Mat3x3, MulMat4x4)
+{
+    Matrix3x3 m = Matrix3x3::RandomGeneral(rng, -10.f, 10.f);
+    Matrix4x4 m_ = m;
+    Matrix4x4 m2 = Matrix4x4::RandomGeneral(rng, -10.f, 10.f);
+
+    Matrix4x4 test = m * m2;
+    Matrix4x4 correct = m_ * m2;
+    assert(test.Equals(correct));
+}

tests/LinearAlgebra/Matrix4x4Tests.cpp

@@ -0,0 +1,11 @@
+#include <gtest/gtest.h>
+#include <J3ML/LinearAlgebra/Matrix4x4.h>
+
+using namespace J3ML::LinearAlgebra;
+
+TEST(Mat4x4, Add_Unary)
+{
+    Matrix4x4 m(1,2,3,4, 5,6,7,8, 9,10,11,12, 13,14,15,16);
+    Matrix4x4 m2 = +m;
+    assert(m.Equals(m2));
+}

tests/LinearAlgebra/Vector2Tests.cpp

@@ -156,7 +156,7 @@ TEST(Vector2Test, V2_Normalize)
 {
     Vector2 A{2, 0};
     Vector2 B{1, 0};
-    EXPECT_EQ(A.Normalize(), B);
+    EXPECT_EQ(A.Normalized(), B);
 }
 
 TEST(Vector2Test, V2_Lerp)
@@ -175,8 +175,8 @@ TEST(Vector2Test, V2_AngleBetween)
     Vector2 A {0.5f, 0.5};
     Vector2 B {0.5f, 0.1f};
 
-    A = A.Normalize();
-    B = B.Normalize();
+    A = A.Normalized();
+    B = B.Normalized();
 
     // TODO: AngleBetween returns not a number
     EXPECT_FLOAT_EQ(A.AngleBetween(B), 0.58800244);

tests/LinearAlgebra/Vector3Tests.cpp

@@ -174,7 +174,7 @@ TEST(Vector3Test, V3_Normalize) {
     Vector3 Input {2, 0, 0};
     Vector3 ExpectedResult {1, 0, 0};
 
-    EXPECT_V3_EQ(Input.Normalize(), ExpectedResult);
+    EXPECT_V3_EQ(Input.Normalized(), ExpectedResult);
 }
 TEST(Vector3Test, V3_Lerp)
 {
@@ -188,8 +188,8 @@ TEST(Vector3Test, V3_AngleBetween) {
     using J3ML::LinearAlgebra::Angle2D;
     Vector3 A{ .5f, .5f, .5f};
     Vector3 B {.25f, .75f, .25f};
-    A = A.Normalize();
-    B = B.Normalize();
+    A = A.Normalized();
+    B = B.Normalized();
     Angle2D ExpectedResult {-0.69791365, -2.3561945};
     std::cout << A.AngleBetween(B).x << ", " << A.AngleBetween(B).y << "";
     auto angle = A.AngleBetween(B);