Update OBB.cpp

Constructor

CMakeLists.txt

@@ -10,9 +10,6 @@ endif()
 
 set(CMAKE_CXX_STANDARD 20)
 #set(CMAKE_POSITION_INDEPENDENT_CODE ON)
-if (WIN32)
-    set(CMAKE_CXX_FLAGS "-municode")
-endif(WIN32)
 
 
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
@@ -28,10 +25,17 @@ file(GLOB_RECURSE J3ML_SRC     "src/J3ML/*.c" "src/J3ML/*.cpp")
 
 include_directories("include")
 
-add_library(J3ML SHARED ${J3ML_SRC}
-        include/J3ML/Geometry/Common.h
-        src/J3ML/Geometry/Triangle.cpp)
+if (UNIX)
+    add_library(J3ML SHARED ${J3ML_SRC})
+endif()
+
+if (WIN32)
+        add_library(J3ML STATIC ${J3ML_SRC})
+endif()
 set_target_properties(J3ML PROPERTIES LINKER_LANGUAGE CXX)
+if(WIN32)
+    #target_compile_options(J3ML PRIVATE -Wno-multichar)
+endif()
 
 install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME})
 
@@ -40,4 +44,8 @@ install(FILES ${J3ML_HEADERS} DESTINATION include/${PROJECT_NAME})
 add_subdirectory(tests)
 
 add_executable(MathDemo main.cpp)
-target_link_libraries(MathDemo ${PROJECT_NAME})
+target_link_libraries(MathDemo ${PROJECT_NAME})
+
+if(WIN32)
+    #target_compile_options(MathDemo PRIVATE -mwindows)
+endif()

include/J3ML/Algorithm/GJK.h

@@ -55,6 +55,6 @@ namespace J3ML::Algorithms
 
         Vector3 offset = (Vector3::Min(ab.minPoint, bb.minPoint) + Vector3::Max(ab.maxPoint, bb.maxPoint)) * 0.5f;
         const Vector3 floatingPtPrecisionOffset = -offset;
-        return GJLIntersect(a.Translated(floatingPtPrecisionOffset), b.Translated(floatingPtPrecisionOffset));
+        return GJKIntersect(a.Translated(floatingPtPrecisionOffset), b.Translated(floatingPtPrecisionOffset));
     }
 }

include/J3ML/Geometry/AABB.h

@@ -183,9 +183,9 @@ namespace J3ML::Geometry
         AABB Translated(const Vector3& offset) const;
         void Scale(const Vector3& scale);
         AABB Scaled(const Vector3& scale) const;
-        AABB TransformAABB(const Matrix3x3& transform);
-        AABB TransformAABB(const Matrix4x4& transform);
-        AABB TransformAABB(const Quaternion& transform);
+        void TransformAABB(const Matrix3x3& transform);
+        void TransformAABB(const Matrix4x4& transform);
+        void TransformAABB(const Quaternion& transform);
         /// Applies a transformation to this AABB and returns the resulting OBB.
         /** Transforming an AABB produces an oriented bounding box. This set of functions does not apply the transformation
             to this object itself, but instead returns the OBB that results in the transformation.

include/J3ML/Geometry/Capsule.h

@@ -66,6 +66,7 @@ namespace J3ML::Geometry
             @return The extreme point of this Capsule in the given direction. */
         Vector3 ExtremePoint(const Vector3 &direction) const;
         Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
+
         /// Tests if this Capsule is degenerate.
         /** @return True if this Capsule does not span a strictly positive volume. */
         bool IsDegenerate() const;
@@ -196,5 +197,7 @@ namespace J3ML::Geometry
         bool Intersects(const Polygon &polygon) const;
         bool Intersects(const Frustum &frustum) const;
         bool Intersects(const Polyhedron &polyhedron) const;
+
+        Capsule Translated(const Vector3&) const;
     };
 }

include/J3ML/Geometry/Sphere.h

@@ -1,5 +1,7 @@
 #pragma once
 
+
+
 #include <J3ML/Geometry.h>
 #include <J3ML/LinearAlgebra/Matrix3x3.h>
 #include <J3ML/LinearAlgebra/Matrix4x4.h>

include/J3ML/J3ML.h

@@ -7,7 +7,6 @@
 //
 #include <cstdint>
 #include <cmath>
-#include <stdfloat>
 #include <string>
 #include <cassert>
 
@@ -17,13 +16,11 @@ namespace J3ML::SizedIntegralTypes
     using u16 = uint16_t;
     using u32 = uint32_t;
     using u64 = uint64_t;
-    using u128 = __uint128_t;
 
     using s8 = int8_t;
     using s16 = int16_t;
     using s32 = int32_t;
     using s64 = int64_t;
-    using s128 = __int128_t;
 }
 
 
@@ -39,6 +36,11 @@ namespace J3ML::SizedFloatTypes
 using namespace J3ML::SizedIntegralTypes;
 using namespace J3ML::SizedFloatTypes;
 
+//On windows there is no shorthand for pi???? - Redacted.
+#ifdef _WIN32
+#define M_PI 3.14159265358979323846
+#endif
+
 namespace J3ML::Math
 {
 

include/J3ML/LinearAlgebra/Matrix3x3.h

@@ -8,6 +8,79 @@
 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;
+    }
+
     class Quaternion;
 
     /// A 3-by-3 matrix for linear transformations of 3D geometry.
@@ -28,56 +101,87 @@ namespace J3ML::LinearAlgebra {
      */
 
     class Matrix3x3 {
-    public:
+    public: /// Constant Values
         enum { Rows = 3 };
         enum { Cols = 3 };
-
+    public: /// Constant Members
         static const Matrix3x3 Zero;
         static const Matrix3x3 Identity;
         static const Matrix3x3 NaN;
-
+    public: /// Constructors
+        /// Creates a new Matrix3x3 with uninitalized member values.
         Matrix3x3() {}
+
+        Matrix3x3(const Matrix3x3& rhs) { Set(rhs); }
         Matrix3x3(float val);
-        Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22);
-        Matrix3x3(const Vector3& r1, const Vector3& r2, const Vector3& r3);
+        /// 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.
+        Matrix3x3(float m00, float m01, float m02,
+                  float m10, float m11, float m12,
+                  float m20, float m21, float m22);
+        /// Constructs the matrix by explicitly specifying the three column vectors.
+        /** @param col0 The first column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
+            direction of the local X axis.
+            @param col1 The second column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
+            direction of the local Y axis.
+            @param col2 The third column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
+            direction of the local Z axis. */
+        Matrix3x3(const Vector3& col0, const Vector3& col1, const Vector3& col2);
+        /// Constructs this matrix3x3 from the given quaternion.
         explicit Matrix3x3(const Quaternion& orientation);
+        /// Constructs this Matrix3x3 from a pointer to an array of floats.
         explicit Matrix3x3(const float *data);
-
-
+        /// Creates a new Matrix3x3 that rotates about one of the principal axes by the given angle.
+        /// Calling RotateX, RotateY, or RotateZ is slightly faster than calling the more generic RotateAxisAngle function.
         static Matrix3x3 RotateX(float radians);
+        /// [similarOverload: RotateX] [hideIndex]
         static Matrix3x3 RotateY(float radians);
+        /// [similarOverload: RotateX] [hideIndex]
         static Matrix3x3 RotateZ(float radians);
-
-        Vector3 GetRow(int index) const;
-        Vector3 GetColumn(int index) const;
-
-        Vector3 GetRow3(int index) const;
-
-        Vector3 GetColumn3(int index) const;
-
-        float &At(int row, int col);
-        float At(int x, int y) const;
-
-        void SetRotatePart(const Vector3& a, float angle);
-
         /// Creates a new M3x3 that rotates about the given axis by the given angle
         static Matrix3x3 RotateAxisAngle(const Vector3& axis, float angleRadians);
-
-        // TODO: Implement
+        /// Creates a matrix that rotates the sourceDirection vector to coincide with the targetDirection vector.]
+        /** Both input direction vectors must be normalized.
+            @note There are infinite such rotations - this function returns the rotation that has the shortest angle
+            (when decomposed to axis-angle notation)
+            @return An orthonormal matrix M with a determinant of +1. For the matrix M it holds that
+            M * sourceDirection = targetDirection */
         static Matrix3x3 RotateFromTo(const Vector3& source, const Vector3& direction);
-
-        void SetRow(int i, const Vector3 &vector3);
-        void SetColumn(int i, const Vector3& vector);
-        void SetAt(int x, int y, float value);
-
-        void Orthonormalize(int c0, int c1, int c2);
-
+        /// Creates a LookAt matrix.
+        /** A LookAt matrix is a rotation matrix that orients an object to face towards a specified target direction.
+         * @param forward 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
+                pick one convention for all your objects, and be consistent.
+         * @param target Specifies the desired world space direction the object should look at. This function
+                will compute a rotation matrix 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 matrix 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. This matrix can be
+                used as the 'world transform' of an object. THe returned matrix M is orthogonal with a determinant of +1.
+                For the matrix M it holds that M * localForward = targetDirection, and M * localUp lies in the plane spanned by
+                the vectors targetDirection and worldUp.
+         * @see RotateFromTo()
+         * @note Be aware that the convention of a 'LookAt' matrix in J3ML differs from e.g. GLM. In J3ML, the returned
+                matrix is a mapping from local space to world space, meaning that the returned matrix can be used as the 'world transform'
+                for any 3D object (camera or not). The view space is the local space of the camera, so this function returns the mapping
+                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);
-
+        /// Creates a new Matrix3x3 that performs the rotation expressed by the given quaternion.
         static Matrix3x3 FromQuat(const Quaternion& orientation);
-
-        Quaternion ToQuat() const;
-
         /// Creates a new Matrix3x3 as a combination of rotation and scale.
         // This function creates a new matrix M in the form M = R * S
         // where R is a rotation matrix and S is a scale matrix.
@@ -86,14 +190,111 @@ namespace J3ML::LinearAlgebra {
         // is applied to the vector first, followed by rotation, and finally translation
         static Matrix3x3 FromRS(const Quaternion& rotate, const Vector3& scale);
         static Matrix3x3 FromRS(const Matrix3x3 &rotate, const Vector3& scale);
-
-
         /// Creates a new transformation matrix that scales by the given factors.
         // This matrix scales with respect to origin.
         static Matrix3x3 FromScale(float sx, float sy, float sz);
         static Matrix3x3 FromScale(const Vector3& scale);
+    public: /// Member Methods
+        /// Sets this matrix to perform rotation about the positive X axis which passes through the origin
+        /// [similarOverload: SetRotatePart] [hideIndex]
+        void SetRotatePartX(float angle);
+        /// Sets this matrix to perform rotation about the positive Y axis.
+        void SetRotatePartY(float angle);
+        /// Sets this matrix to perform rotation about the positive Z axis.
+        void SetRotatePartZ(float angle);
+
+        /// Sets this matrix to perform a rotation about the given axis and angle.
+        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);
+
+        /// Returns the given row.
+        /** @param row The zero-based index [0, 2] of the row to get. */
+        Vector3 GetRow(int index) const;
+        Vector3 Row(int index) const { return GetRow(index);}
+        /// This method also allows assignment to the retrieved row.
+        Vector3& Row(int row);
+
+        /// Returns only the first-three elements of the given row.
+        Vector3 GetRow3(int index) const;
+        Vector3 Row3(int index) const;
+        /// This method also allows assignment to the retrieved row.
+        Vector3& Row3(int index);
+
+        /// 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;
+        /// 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;
+        /// This method also allows assignment to the retrieved column.
+        //Vector3& Col3(int index);
+
+        /// Sets the value of a given row.
+        /** @param row The index of the row to a set, in the range [0-2].
+            @param data A pointer to an array of 3 floats that contain the new x, y, and z values for the row.*/
+        void SetRow(int row, const float* data);
+        void SetRow(int row, const Vector3 & data);
+        void SetRow(int row, float x, float y, float z);
+
+        /// Sets the value of a given column.
+        /** @param column The index of the column to set, in the range [0-2]
+            @param data A pointer ot an array of 3 floats that contain the new x, y, and z values for the column.*/
+        void SetColumn(int column, const float* data);
+        void SetColumn(int column, const Vector3 & data);
+        void SetColumn(int column, float x, float y, float z);
+
+
+        /// Sets a single element of this matrix
+        /** @param row The row index (y-coordinate) of the element to set, in the range [0-2].
+            @param col The col index (x-coordinate) of the element to set, in the range [0-2].
+            @param value The new value to set to the cell [row][col]. */
+        void SetAt(int x, int y, float value);
+
+
+        /// Sets this matrix to equal the identity.
+        void SetIdentity();
+
+
+        void SwapColumns(int col1, int col2);
+
+        void SwapRows(int row1, int row2);
+
+        float &At(int row, int col);
+        float At(int x, int y) const;
+
+        /// Sets this to be a copy of the matrix rhs.
+        void Set(const Matrix3x3 &rhs);
+        /// Sets all values of this matrix/
+        void Set(float _00, float _01, float _02,
+                 float _10, float _11, float _12,
+                 float _20, float _21, float _22);
+        /// 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);
+
+        /// Orthonormalizes the basis formed by the column vectors of this matrix.
+        void Orthonormalize(int c0, int c1, int c2);
+
+
+        /// 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;
+        /// 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;
+
 
         /// Returns the main diagonal.
+        /// The main diagonal consists of the elements at m[0][0], m[1][1], m[2][2]
         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.
@@ -114,47 +315,166 @@ namespace J3ML::LinearAlgebra {
         // @note This function computes 9 LOADs, 9 MULs and 5 ADDs. */
         float Determinant() const;
 
-        // Returns an inverted copy of this matrix. This
-        Matrix3x3 Inverse() 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;
+
+        // Returns an inverted copy of this matrix.
+        Matrix3x3 Inverted() const;
 
         // Returns a transposed copy of this matrix.
-        Matrix3x3 Transpose() const;
+        Matrix3x3 Transposed() const;
+
+        /// Returns the inverse transpose of this matrix.
+        Matrix3x3 InverseTransposed() const;
+
+        /// Inverts this matrix using numerically stable Gaussian elimination.
+        /// @return Returns true on success, false otherwise;
+        bool Inverse(float epsilon = 1e-6f);
+
+        /// Inverts this matrix using Cramer's rule.
+        /// @return Returns true on success, false otherwise.
+        bool InverseFast(float epsilon = 1e-6f);
+
+
+        /// Solves the linear equation Ax=b.
+        /** The matrix A in the equations is this matrix. */
+        bool SolveAxb(Vector3 b, Vector3& x) const;
+
+        /// Inverts a column-orthogonal matrix.
+        /** 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.\
+            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.
+            The returned matrix will be column-orthogonal if the original matrix M was row-orthogonal as well.
+            (in which case S had uniform scale, InverseOrthogonalUniformScale() could have been used instead)*/
+        bool InverseColOrthogonal();
+
+        /// 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
+            Inverse().
+            This function may not be called if this matrix contains any scaling or shearing, but it may contain mirroring.*/
+        bool InverseOrthogonalUniformScale();
+
+        void InverseOrthonormal();
+
+        bool InverseSymmetric();
+
+        void Transpose();
+
+        bool InverseTranspose();
+
+        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;
 
+        /// Performs a batch transformation of the given array.
+        void BatchTransform(Vector3 *pointArray, int numPoints) const;
+        void BatchTransform(Vector3 *pointArray, int numPoints, int stride) const;
+        void BatchTransform(Vector4 *vectorArray, int numVectors) const;
+        void BatchTransform(Vector4 *vectorArray, int numVectors, int stride) const;
+
+        /// Returns the sum of the diagonal elements of this matrix.
+        float Trace() const;
+
 
         Matrix3x3 ScaleBy(const Vector3& rhs);
         Vector3 GetScale() const;
 
         Vector3 operator[](int row) const;
 
+        /// Transforms the given vector by this matrix (in the order M * v).
         Vector2 operator * (const Vector2& rhs) const;
         Vector3 operator * (const Vector3& rhs) const;
+        /// Transforms the given vector by this matrix (in the order M * v).
+        /// 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;
+
+        /// Multiplies the two matrices.
         Matrix3x3 operator * (const Matrix3x3& rhs) const;
-        Matrix4x4 operator * (const Matrix4x4& rhs) const;
         Matrix3x3 Mul(const Matrix3x3& rhs) const;
+        /// Multiplies the two matrices.
+        Matrix4x4 operator * (const Matrix4x4& rhs) const;
         Matrix4x4 Mul(const Matrix4x4& rhs) const;
+
         Vector2 Mul(const Vector2& rhs) const;
         Vector3 Mul(const Vector3& rhs) const;
         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;
 
         // 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);}
         // Returns true if the row vectors of this matrix are all perpendicular to each other.
         bool IsRowOrthogonal(float epsilon = 1e-3f) const;
 
 
-
         bool HasUniformScale(float epsilon  = 1e-3f) const;
 
-        Vector3 ExtractScale() const {
-            return {GetColumn(0).Length(), GetColumn(1).Length(), GetColumn(2).Length()};
-        }
+        Vector3 ExtractScale() const;
 
-    protected:
+
+        /// 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;
+
+        /// 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;
+        /// 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;
+        /// 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;
+        /// 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;
+        /// 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;
+        /// 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;
+        /// 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;
+        /// 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;
+
+        /// 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;
+
+
+    protected: /// Member values
         float elems[3][3];
     };
 }

include/J3ML/LinearAlgebra/Matrix4x4.h

@@ -11,6 +11,115 @@
 
 namespace J3ML::LinearAlgebra {
 
+    template <typename Matrix>
+    bool InverseMatrix(Matrix &mat, float epsilon)
+    {
+        Matrix inversed = Matrix::Identity;
+
+        const int nc = std::min<int>(Matrix::Rows, Matrix::Cols);
+
+        for (int column = 0; column < nc; ++column)
+        {
+            // find the row i with i >= j such that M has the largest absolute value.
+            int greatest = column;
+            float greatestVal = std::abs(mat[greatest][column]);
+            for (int i = column+1; i < Matrix::Rows; i++)
+            {
+                float val = std::abs(mat[i][column]);
+                if (val > greatestVal) {
+                    greatest = i;
+                    greatestVal = val;
+                }
+            }
+
+            if (greatestVal < epsilon) {
+                mat = inversed;
+                return false;
+            }
+
+            // exchange rows
+            if (greatest != column) {
+                inversed.SwapRows(greatest, column);
+                mat.SwapRows(greatest, column);
+            }
+
+            // multiply rows
+            assert(!Math::EqualAbs(mat[column][column], 0.f, epsilon));
+            float scale = 1.f / mat[column][column];
+            inversed.ScaleRow(column, scale);
+            mat.ScaleRow(column, scale);
+
+            // add rows
+            for (int i = 0; i < column; i++) {
+                inversed.SetRow(i, inversed.Row(i) - inversed.Row(column) * mat[i][column]);
+                mat.SetRow(i, mat.Row(i) - mat.Row(column) * mat[i][column]);
+            }
+
+            for (int i = column + 1; i < Matrix::Rows; i++) {
+                inversed.SetRow(i, inversed.Row(i) - inversed.Row(column) * mat[i][column]);
+                mat.SetRow(i, mat.Row(i) - mat.Row(column) * mat[i][column]);
+            }
+        }
+        mat = inversed;
+        return true;
+    }
+
+
+    /// Computes the LU-decomposition on the given square matrix.
+    /// @return True if the composition was successful, false otherwise. If the return value is false, the contents of the output matrix are unspecified.
+    template <typename Matrix>
+    bool LUDecomposeMatrix(const Matrix &mat, Matrix &lower, Matrix &upper)
+    {
+        lower = Matrix::Identity;
+        upper = Matrix::Zero;
+
+        for (int i = 0; i < Matrix::Rows; ++i)
+        {
+            for (int col = i; col < Matrix::Cols; ++col)
+            {
+                upper[i][col] = mat[i][col];
+                for (int k = 0; k < i; ++k)
+                    upper[i][col] -= lower[i][k] * upper[k][col];
+            }
+            for (int row = i+1; row < Matrix::Rows; ++row)
+            {
+                lower[row][i] = mat[row][i];
+                for (int k = 0; k < i; ++k)
+                    lower[row][i] -= lower[row][k] * upper[k][i];
+                if (Math::EqualAbs(upper[i][i], 0.f))
+                    return false;
+                lower[row][i] /= upper[i][i];
+            }
+        }
+        return true;
+    }
+
+    /// Computes the Cholesky decomposition on the given square matrix *on the real domain*.
+    /// @return True if successful, false otherwise. If the return value is false, the contents of the output matrix are uspecified.
+    template <typename Matrix>
+    bool CholeskyDecomposeMatrix(const Matrix &mat, Matrix& lower)
+    {
+        lower = Matrix::Zero;
+        for (int i = 0; i < Matrix::Rows; ++i)
+        {
+            for (int j = 0; j < i; ++i)
+            {
+                lower[i][j] = mat[i][j];
+                for (int k = 0; k < j; ++k)
+                    lower[i][j] -= lower[i][j] * lower[j][k];
+                if (Math::EqualAbs(lower[j][j], 0.f))
+                    return false;
+                lower[i][j] /= lower[j][j];
+            }
+            lower[i][i] = mat[i][i];
+            if (lower[i][i])
+                return false;
+            for (int k = 0; k < i; ++k)
+                lower[i][i] -= lower[i][k] * lower[i][k];
+            lower[i][i] = std::sqrt(lower[i][i]);
+        }
+    }
+
     /// @brief A 4-by-4 matrix for affine transformations and perspective projections of 3D geometry.
     /// This matrix can represent the most generic form of transformations for 3D objects,
     /// including perspective projections, which a 4-by-3 cannot store, and translations, which a 3-by-3 cannot represent.
@@ -24,25 +133,30 @@ namespace J3ML::LinearAlgebra {
      *  You can access m_yx using the double-bracket notation m[y][x]
      */
     class Matrix4x4 {
-    public:
-        // TODO: Implement assertions to ensure matrix bounds are not violated!
+    public: /// Constant Values
         enum { Rows = 4 };
         enum { Cols = 4 };
-
+    public: /// Constant Members
         /// A constant matrix that has zeroes in all its entries
         static const Matrix4x4 Zero;
         /// A constant matrix that is the identity.
+        /** The identity matrix looks like the following:
+            1 0 0 0
+            0 1 0 0
+            0 0 1 0
+            0 0 0 1
+          Transforming a vector by the identity matrix is like multiplying a number by one, i.e. the vector is not changed */
         static const Matrix4x4 Identity;
-
         /// A compile-time constant float4x4 which has NaN in each element.
         /// For this constant, each element has the value of quet NaN, or Not-A-Number.
-        /// Never compare a matrix to this value. Due to how IEEE floats work, "nan == nan" returns false!
+        /// @note Never compare a matrix to this value. Due to how IEEE floats work, "nan == nan" returns false!
         static const Matrix4x4 NaN;
-
-        /// Creates a new float4x4 with uninitialized member values.
+    public: /// Constructors
+        /// Creates a new Matrix4x4 with uninitialized member values.
         Matrix4x4() {}
+        Matrix4x4(const Matrix4x4 &rhs) = default; // {Set(rhs);}
         Matrix4x4(float val);
-        /// Constructs this float4x4 to represent the same transformation as the given float3x3.
+        /// 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&);
         explicit Matrix4x4(const float* data);
@@ -65,6 +179,7 @@ namespace J3ML::LinearAlgebra {
             position of the local space pivot. */
         Matrix4x4(const Vector4& r1, const Vector4& r2, const Vector4& r3, const Vector4& r4);
 
+        /// Constructs this Matrix4x4 from the given quaternion.
         explicit Matrix4x4(const Quaternion& orientation);
 
         /// Constructs this float4x4 from the given quaternion and translation.
@@ -102,6 +217,64 @@ namespace J3ML::LinearAlgebra {
             @see RotateFromTo(). */
         static Matrix4x4 LookAt(const Vector3& localFwd, const Vector3& targetDir, const Vector3& localUp, const Vector3& worldUp);
 
+        /// 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.
+         */
+        static Matrix4x4 RotateX(float radians, const Vector3 &pointOnAxis);
+        /// [similarOverload: RotateX] [hideIndex]
+        static Matrix4x4 RotateX(float radians);
+        /// [similarOverload: RotateX] [hideIndex]
+        static Matrix4x4 RotateY(float radians, const Vector3 &pointOnAxis);
+        /// [similarOverload: RotateX] [hideIndex]
+        static Matrix4x4 RotateY(float radians);
+        /// [similarOverload: RotateX] [hideIndex]
+        static Matrix4x4 RotateZ(float radians, const Vector3 &pointOnAxis);
+        /// [similarOverload: RotateX] [hideIndex]
+        static Matrix4x4 RotateZ(float radians);
+
+        /// Creates a new Matrix4x4 that rotates about the given axis.
+        /** @param axisDirection The axis to rotate about. This vector must be normalized.
+            @param angleRadians The angle to rotate by, in radians.
+            @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. */
+        static Matrix4x4 RotateAxisAngle(const Vector3 &axisDirection, float angleRadians, const Vector3& pointOnAxis);
+        static Matrix4x4 RotateAxisAngle(const Vector3 &axisDirection, float angleRadians);
+
+        /// Creates a new Matrix4x4 that rotates sourceDirection vector to coincide with the targetDirection vector.
+        /** @note There are infinite such rotations - this function returns the rotation that has the shortest angle
+            (when decomposed to axis-angle notation)
+            @param sourceDirection The 'from' direction vector. This vector must be normalized.
+            @param targetDirection The 'to' direction vector. This vector must be normalized.
+            @param centerPoint If specified, rotation is performed using this point as the coordinate space origin.
+            If omitted, the rotation is performed about the coordinate system origin (0,0,0).
+            @return A new rotation matrix R for which R*sourceDirection == targetDirection */
+        static Matrix4x4 RotateFromTo(const Vector3 &sourceDirection, const Vector3 &targetDirection, const Vector3 &centerPoint);
+        static Matrix4x4 RotateFromTo(const Vector3 &sourceDirection, const Vector3 &targetDirection);
+        static Matrix4x4 RotateFromTo(const Vector4 &sourceDirection, const Vector4 &targetDirection);
+
+        /// Creates a new Matrix4x4 that rotates one coordinate system to coincide with another.
+        /** This function rotates the sourceDirection vector to coincide with the targetDirection vector, and then
+            rotates sourceDirection2 (which was transformed by 1.) to targetDirection2, but keeping the constraint that
+            sourceDirection must look at targetDirection. */
+        /** @param sourceDirection The first 'from' direction. This vector must be normalized.
+            @param targetDirection The first 'to' direction. This vector must be normalized.
+            @param sourceDirection2 The second 'from' direction. This vector must be normalized.
+            @param targetDirection2 The second 'to' direction. This vector must be normalized.
+            @param centerPoint If specified, rotation is performed using this point as the coordinate space origin.
+            @return The returned matrix maps sourceDirection to targetDirection. Additionally, the returned matrix
+            rotates sourceDirection2 to point towards targetDirection2 as closely as possible, under the previous constriant.
+            The returned matrix is a rotation matrix, i.e. it is orthonormal with a determinant of +1, and optionally
+            has a translation component if the rotation is not performed w.r.t. the coordinate system origin */
+        static Matrix4x4 RotateFromTo(const Vector3& sourceDirection, const Vector3 &targetDirection,
+                                      const Vector3 &sourceDirection2, const Vector3 &targetDirection2,
+                                      const Vector3 &centerPoint);
+        static Matrix4x4 RotateFromTo(const Vector3& sourceDirection, const Vector3 &targetDirection,
+                                      const Vector3 &sourceDirection2, const Vector3 &targetDirection2);
+
+
         /// Returns the translation part.
         /** The translation part is stored in the fourth column of this matrix.
             This is equivalent to decomposing this matrix in the form M = T * M', i.e. this translation is applied last,
@@ -111,9 +284,29 @@ namespace J3ML::LinearAlgebra {
         Vector3 GetTranslatePart() const;
         /// Returns the top-left 3x3 part of this matrix. This stores the rotation part of this matrix (if this matrix represents a rotation).
         Matrix3x3 GetRotatePart() const;
+
+        /// Sets the translation part of this matrix.
+        /** This function sets the translation part of this matrix. These are the first three elements of the fourth column. All other entries are left untouched. */
         void SetTranslatePart(float translateX, float translateY, float translateZ);
         void SetTranslatePart(const Vector3& offset);
+        /// Sets the 3-by-3 part of this matrix to perform rotation about the given axis and angle (in radians). Leaves all other
+        /// entries of this matrix untouched.
         void SetRotatePart(const Quaternion& q);
+        void SetRotatePart(const Vector3& axisDirection, float angleRadians);
+
+        /// Sets the 3-by-3 part of this matrix.
+        /// @note This is a convenience function which calls Set3x3Part.
+        /// @note This function erases the previous top-left 3x3 part of this matrix (any previous rotation, scaling and shearing, etc.) Translation is unaffecte.d
+        void SetRotatePart(const Matrix3x3& rotation) { Set3x3Part(rotation); }
+        /// Sets the 3-by-3 part of this matrix to perform rotation about the positive X axis which passes through the origin.
+        /// Leaves all other entries of this matrix untouched.
+        void SetRotatePartX(float angleRadians);
+        /// Sets the 3-by-3 part of this matrix to perform the rotation about the positive Y axis.
+        /// Leaves all other entries of the matrix untouched.
+        void SetRotatePartY(float angleRadians);
+        /// Sets the 3-by-3 part of this matrix to perform the rotation about the positive Z axis.
+        /// Leaves all other entries of the matrix untouched.
+        void SetRotatePartZ(float angleRadians);
         void Set3x3Part(const Matrix3x3& r);
 
         void SetRow(int row, const Vector3& rowVector, float m_r3);
@@ -283,8 +476,101 @@ namespace J3ML::LinearAlgebra {
         /// i.e. whether the last row of this matrix differs from [0 0 0 1]
         bool ContainsProjection(float epsilon = 1e-3f) const;
 
+
+        /// Sets all values of this matrix.
+        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);
+
+        /// Sets this to be a copy of the matrix rhs.
+        void Set(const Matrix4x4 &rhs);
+
+        /// 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);
+
+        /// Sets this matrix to equal the identity.
+        void SetIdentity();
+        /// Returns the adjugate of this matrix.
+        Matrix4x4 Adjugate() const;
+
+        /// Computes the Cholesky decomposition of this matrix.
+        /// The returned matrix L satisfies L * transpose(L) = this;
+        /// Returns true on success.
+        bool ColeskyDecompose(Matrix4x4 &outL) const;
+
+        /// Computes the LU decomposition of this matrix.
+        /// This decomposition has the form 'this = L * U'
+        /// Returns true on success.
+        bool LUDecompose(Matrix4x4& outLower, Matrix4x4& outUpper) const;
+
+        /// Inverts this matrix using the generic Gauss's method.
+        /// @return Returns true on success, false otherwise.
+        bool Inverse(float epsilon = 1e-6f)
+        {
+            return InverseMatrix(*this, epsilon);
+        }
+
+        /// Returns an inverted copy of this matrix.
+        /// If this matrix does not have an inverse, returns the matrix that was the result of running
+        /// Gauss's method on the matrix.
+        Matrix4x4 Inverted() const;
+
+        /// Inverts a column-orthogonal matrix.
+        /// 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.
+        /// 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)).
+        /// @note The returned matrix will be row-orthogonal, but not column-orthogonal in general.
+        /// The returned matrix will be column-orthogonal if the original matrix M was row-orthogonal as well.
+        /// (in which case S had uniform scale, InverseOrthogonalUniformScale() could have been used instead).
+        bool InverseColOrthogonal();
+
+
+        /// Inverts a matrix that is a concatenation of only translate, rotate, and uniform scale 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 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().
+        /// 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.
+        /// This function may not be called if this matrix contains any projection (last row differs from (0 0 0 1)).
+        bool InverseOrthogonalUniformScale();
+
+        /// 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 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();
 
+        /// Transposes this matrix.
+        /// This operation swaps all elements with respect to the diagonal.
+        void Transpose();
+        /// Returns a transposed copy of this matrix.
+        Matrix4x4 Transposed() const;
+        /// Computes the inverse transpose of this matrix in-place.
+        /// Use the inverse transpose to transform covariant vectors (normal vectors).
+        bool InverseTranspose();
+        /// Returns the inverse transpose of this matrix.
+        /// Use that matrix to transform covariant vectors (normal vectors).
+        Matrix4x4 InverseTransposed() const
+        {
+            Matrix4x4 copy = *this;
+            copy.Transpose();
+            copy.Inverse();
+            return copy;
+        }
+        /// Returns the sum of the diagonal elements of this matrix.
+        float Trace() const;
+
     protected:
         float elems[4][4];
 

include/J3ML/LinearAlgebra/Quaternion.h

@@ -35,7 +35,7 @@ namespace J3ML::LinearAlgebra
         Quaternion(const Vector3 &rotationAxis, float rotationAngleBetween);
 
         Quaternion(const Vector4 &rotationAxis, float rotationAngleBetween);
-        //void Inverse();
+        //void Inverted();
 
         explicit Quaternion(Vector4 vector4);
         explicit Quaternion(const EulerAngle& angle);

include/J3ML/LinearAlgebra/Vector2.h

@@ -1,5 +1,3 @@
-#pragma clang diagnostic push
-#pragma ide diagnostic ignored "modernize-use-nodiscard"
 #pragma once
 #include <J3ML/J3ML.h>
 #include <cstddef>
@@ -191,5 +189,4 @@ namespace J3ML::LinearAlgebra {
     {
         return rhs * lhs;
     }
-}
-#pragma clang diagnostic pop
+}

include/J3ML/LinearAlgebra/Vector3.h

@@ -17,16 +17,66 @@ public:
 public:
     enum {Dimensions = 3};
 public:
+    /// Specifies a compile-time constant Vector3 with value (0,0,0).
+    /** @note Due to static data initialization order being undefined in C++, do NOT use this
+        member to initialize other static data in other compilation units! */
     static const Vector3 Zero;
+    /// Specifies a compile-time constant Vector3 with value (1,1,1).
+    /** @note Due to static data initialization order being undefined in C++, do NOT use this
+        member to initialize other static data in other compilation units! */
+    static const Vector3 One;
+    /// Specifies a compile-time constant Vector3 with value (0,1,0).
+    /** @note Due to static data initialization order being undefined in C++, do NOT use this
+        member to initialize other static data in other compilation units! */
     static const Vector3 Up;
+    /// Specifies a compile-time constant Vector3 with value (0,-1,0).
+    /** @note Due to static data initialization order being undefined in C++, do NOT use this
+        member to initialize other static data in other compilation units! */
     static const Vector3 Down;
+    /// Specifies a compile-time constant Vector3 with value (-1,0,0).
+    /** @note Due to static data initialization order being undefined in C++, do NOT use this
+        member to initialize other static data in other compilation units! */
     static const Vector3 Left;
+    /// Specifies a compile-time constant Vector3 with value (1,0,0).
+    /** @note Due to static data initialization order being undefined in C++, do NOT use this
+        member to initialize other static data in other compilation units! */
     static const Vector3 Right;
+    /// Specifies a compile-time constant Vector3 with value (0,0,-1).
+    /** @note Due to static data initialization order being undefined in C++, do NOT use this
+        member to initialize other static data in other compilation units! */
     static const Vector3 Forward;
+    /// Specifies a compile-time constant Vector3 with value (0,0,1).
+    /** @note Due to static data initialization order being undefined in C++, do NOT use this
+        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.
+        @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! */
     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
+        member to initialize other static data in other compilation units! */
     static const Vector3 Infinity;
+    /// 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
+        member to initialize other static data in other compilation units! */
     static const Vector3 NegativeInfinity;
+    /// Specifies a compile-time constant Vector3 with value (1,1,1).
+    /** @note Due to static data initialization order being undefined in C++, do NOT use this
+        member to initialize other static data in other compilation units! */
+    static const Vector3 UnitX;
+    /// Specifies a compile-time constant Vector3 with value (1,1,1).
+    /** @note Due to static data initialization order being undefined in C++, do NOT use this
+        member to initialize other static data in other compilation units! */
+    static const Vector3 UnitY;
+    /// Specifies a compile-time constant Vector3 with value (1,1,1).
+    /** @note Due to static data initialization order being undefined in C++, do NOT use this
+        member to initialize other static data in other compilation units! */
+    static const Vector3 UnitZ;
+
 public:
 
     /// The default constructor does not initialize any members of this class.

src/J3ML/Geometry/AABB.cpp

@@ -296,6 +296,8 @@ namespace J3ML::Geometry {
             result.z = this->minPoint.z;
         else
             result.z = point.z;
+
+        return result;
     }
 
     AABB::AABB(const Vector3 &min, const Vector3 &max) : Shape(), minPoint(min), maxPoint(max)
@@ -613,20 +615,21 @@ namespace J3ML::Geometry {
         );
     }
 
-    AABB AABB::TransformAABB(const Matrix3x3 &transform) {
+    void AABB::TransformAABB(const Matrix3x3 &transform) {
         // TODO: assert(transform.IsColOrthogonal());
         // TODO: assert(transform.HasUniformScale());
         AABBTransformAsAABB(*this, transform);
     }
 
-    AABB AABB::TransformAABB(const Matrix4x4 &transform) {
+    void AABB::TransformAABB(const Matrix4x4 &transform) {
         // TODO: assert(transform.IsColOrthogonal());
         // TODO: assert(transform.HasUniformScale());
         // TODO: assert(transform.Row(3).Equals(0,0,0,1));
         AABBTransformAsAABB(*this, transform);
+
     }
 
-    AABB AABB::TransformAABB(const Quaternion &transform) {
+    void AABB::TransformAABB(const Quaternion &transform) {
         Vector3 newCenter = transform.Transform(Centroid());
         Vector3 newDir = Vector3::Abs((transform.Transform(Size())*0.5f));
         minPoint = newCenter - newDir;

src/J3ML/Geometry/Capsule.cpp

@@ -1,3 +1,4 @@
+#include <J3ML/Algorithm/GJK.h>
 #include <J3ML/Geometry/Capsule.h>
 #include <J3ML/Geometry/AABB.h>
 #include <J3ML/Geometry/Sphere.h>
@@ -8,6 +9,17 @@ namespace J3ML::Geometry
 
     Capsule::Capsule() : l() {}
 
+    Capsule::Capsule(const LineSegment &endPoints, float radius)
+    :l(endPoints), r(radius)
+    {
+    }
+
+    Capsule::Capsule(const Vector3 &bottomPoint, const Vector3 &topPoint, float radius)
+    :l(bottomPoint, topPoint), r(radius)
+    {
+    }
+
+
     AABB Capsule::MinimalEnclosingAABB() const
     {
         Vector3 d = Vector3(r, r, r);
@@ -49,12 +61,13 @@ namespace J3ML::Geometry
 
     bool Capsule::Intersects(const AABB &aabb) const
     {
-        //return FloatingPointOffsetedGJKIntersect(*this, aabb);
+        return Algorithms::FloatingPointOffsetedGJKIntersect(*this, aabb);
+        return false;
     }
 
     bool Capsule::Intersects(const OBB &obb) const
     {
-        //return GJKIntersect(*this, obb);
+        return Algorithms::GJKIntersect(*this, obb);
     }
 
 /// [groupSyntax]
@@ -104,4 +117,9 @@ namespace J3ML::Geometry
         projectionDistance = extremePoint.Dot(direction);
         return extremePoint;
     }
+
+    Capsule Capsule::Translated(const Vector3 &offset) const
+    {
+        return Capsule(l.A + offset, l.B + offset, r);
+    }
 }

src/J3ML/Geometry/OBB.cpp

@@ -438,6 +438,14 @@ namespace J3ML::Geometry
         return m;
     }
 
+    OBB::OBB(const Vector3 &pos, const Vector3 &radii, const Vector3 &axis0, const Vector3 &axis1, const Vector3 &axis2) {
+        this->pos = pos;
+        this->r = radii;
+        this->axis[0] = axis0;
+        this->axis[1] = axis1;
+        this->axis[2] = axis2;
+    }
+
 
 }
 

src/J3ML/Geometry/Plane.cpp

@@ -87,7 +87,7 @@ namespace J3ML::Geometry
     float Plane::SignedDistance(const Triangle &triangle) const { return Plane_SignedDistance(*this, triangle); }
 
     float Plane::Distance(const Vector3 &point) const {
-        std::abs(SignedDistance(point));
+        return std::abs(SignedDistance(point));
     }
 
     float Plane::Distance(const LineSegment &lineSegment) const

src/J3ML/Geometry/Polyhedron.cpp

@@ -333,7 +333,8 @@ namespace J3ML::Geometry
     }
 
     bool Polyhedron::IsClosed() const {
-
+        // TODO: Implement
+        return false;
     }
 
     Plane Polyhedron::FacePlane(int faceIndex) const

src/J3ML/Geometry/Sphere.cpp

@@ -4,7 +4,7 @@ namespace J3ML::Geometry
 {
 
     bool Sphere::Contains(const LineSegment &lineseg) const {
-
+        return Contains(lineseg.A) && Contains(lineseg.B);
     }
 
     void Sphere::ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const

src/J3ML/J3ML.cpp

@@ -38,7 +38,7 @@ namespace J3ML
     Math::Rotation::Rotation(float value) : valueInRadians(value) {}
 
     Math::Rotation Math::Rotation::operator+(const Math::Rotation &rhs) {
-        valueInRadians += rhs.valueInRadians;
+        return {valueInRadians + rhs.valueInRadians};
     }
 
     float Math::Interp::SmoothStart(float t) {

src/J3ML/LinearAlgebra/Matrix3x3.cpp

@@ -87,18 +87,22 @@ namespace J3ML::LinearAlgebra {
 
     }
 
-    Matrix3x3::Matrix3x3(const Vector3 &r1, const Vector3 &r2, const Vector3 &r3) {
-        this->elems[0][0] = r1.x;
-        this->elems[0][1] = r1.y;
-        this->elems[0][2] = r1.z;
+    Matrix3x3::Matrix3x3(const Vector3 &col0, const Vector3 &col1, const Vector3 &col2) {
+        SetColumn(0, col0);
+        SetColumn(1, col1);
+        SetColumn(2, col2);
 
-        this->elems[1][0] = r2.x;
-        this->elems[1][1] = r2.y;
-        this->elems[1][2] = r2.z;
+        //this->elems[0][0] = r1.x;
+        //this->elems[0][1] = r1.y;
+        //this->elems[0][2] = r1.z;
 
-        this->elems[2][0] = r3.x;
-        this->elems[2][1] = r3.y;
-        this->elems[2][2] = r3.z;
+        //this->elems[1][0] = r2.x;
+        //this->elems[1][1] = r2.y;
+        //this->elems[1][2] = r2.z;
+
+        //this->elems[2][0] = r3.x;
+        //this->elems[2][1] = r3.y;
+        //this->elems[2][2] = r3.z;
     }
 
     Matrix3x3::Matrix3x3(const Quaternion &orientation) {
@@ -120,7 +124,7 @@ namespace J3ML::LinearAlgebra {
         return a*(e*i - f*h) + b*(f*g - d*i) + c*(d*h - e*g);
     }
 
-    Matrix3x3 Matrix3x3::Inverse() const {
+    Matrix3x3 Matrix3x3::Inverted() const {
         // Compute the inverse directly using Cramer's rule
         // Warning: This method is numerically very unstable!
         float d = Determinant();
@@ -144,7 +148,7 @@ namespace J3ML::LinearAlgebra {
         return i;
     }
 
-    Matrix3x3 Matrix3x3::Transpose() const {
+    Matrix3x3 Matrix3x3::Transposed() const {
         auto m00 = this->elems[0][0];
         auto m01 = this->elems[0][1];
         auto m02 = this->elems[0][2];
@@ -456,5 +460,159 @@ namespace J3ML::LinearAlgebra {
         return Transform(rhs);
     }
 
+    Matrix3x3 Matrix3x3::RotateX(float radians) {
+        Matrix3x3 r;
+        r.SetRotatePartX(radians);
+        return r;
+    }
+
+    Matrix3x3 Matrix3x3::RotateY(float radians) {
+        Matrix3x3 r;
+        r.SetRotatePartY(radians);
+        return r;
+    }
+
+    Matrix3x3 Matrix3x3::RotateZ(float radians) {
+        Matrix3x3 r;
+        r.SetRotatePartZ(radians);
+        return r;
+    }
+
+    void Matrix3x3::SetRotatePartX(float angle) {
+        Set3x3PartRotateX(*this, angle);
+    }
+
+    void Matrix3x3::SetRotatePartY(float angle) {
+        Set3x3PartRotateY(*this, angle);
+    }
+
+    void Matrix3x3::SetRotatePartZ(float angle) {
+        Set3x3RotatePartZ(*this, angle);
+    }
+
+    Vector3 Matrix3x3::ExtractScale() const {
+        return {GetColumn(0).Length(), GetColumn(1).Length(), GetColumn(2).Length()};
+    }
+
+    // TODO: Finish implementation
+    Matrix3x3 Matrix3x3::RotateFromTo(const Vector3 &source, const Vector3 &direction) {
+        assert(source.IsNormalized());
+        assert(source.IsNormalized());
+
+        // http://cs.brown.edu/research/pubs/pdfs/1999/Moller-1999-EBA.pdf
+        Matrix3x3 r;
+        float dot = source.Dot(direction);
+        if (std::abs(dot) > 0.999f)
+        {
+            Vector3 s = source.Abs();
+            Vector3 unit = s.x < s.y && s.x < s.z ? Vector3::UnitX : (s.y < s.z ? Vector3::UnitY : Vector3::UnitZ);
+        }
+
+        return Matrix3x3::Identity;
+    }
+
+    Vector3 &Matrix3x3::Row(int row) {
+        assert(row >= 0);
+        assert(row < Rows);
+        return reinterpret_cast<Vector3 &> (elems[row]);
+    }
+
+    Vector3 Matrix3x3::Column(int index) const { return GetColumn(index);}
+
+    Vector3 Matrix3x3::Col(int index) const { return Column(index);}
+
+    Vector3 &Matrix3x3::Row3(int index) {
+        return reinterpret_cast<Vector3 &>(elems[index]);
+    }
+
+    Vector3 Matrix3x3::Row3(int index) const { return GetRow3(index);}
+
+    void Matrix3x3::Set(const Matrix3x3 &x3) {
+
+    }
+
+    bool Matrix3x3::IsFinite() const {
+        for (int y = 0; y < Rows; y++)
+            for (int x = 0; x < Cols; ++x)
+                if (!std::isfinite(elems[y][x]))
+                    return false;
+        return true;
+    }
+
+    /** Compares the two values for equality, allowing the given amount of absolute error. */
+    bool EqualAbs(float a, float b, float epsilon)
+    {
+        return std::abs(a-b) < epsilon;
+    }
+
+    bool Matrix3x3::IsIdentity(float epsilon) const
+    {
+        for (int y = 0; y < Rows; ++y)
+            for (int x = 0; x < Cols; ++x)
+                if (!EqualAbs(elems[y][x], (x == y) ? 1.f : 0.f, epsilon))
+                    return false;
+        return true;
+    }
+
+
+    bool Matrix3x3::IsLowerTriangular(float epsilon) const
+    {
+        return EqualAbs(elems[0][1], 0.f, epsilon)
+            && EqualAbs(elems[0][2], 0.f, epsilon)
+            && EqualAbs(elems[1][2], 0.f, epsilon);
+    }
+
+
+    bool Matrix3x3::IsUpperTriangular(float epsilon) const
+    {
+        return EqualAbs(elems[1][0], 0.f, epsilon)
+            && EqualAbs(elems[2][0], 0.f, epsilon)
+            && EqualAbs(elems[2][1], 0.f, epsilon);
+    }
+
+    bool Matrix3x3::IsInvertible(float epsilon) const
+    {
+        float d = Determinant();
+        bool isSingular = EqualAbs(d, 0.f, epsilon);
+        //assert(Inverse(epsilon) == isSingular);
+        return !isSingular;
+    }
+
+    bool Matrix3x3::IsSymmetric(float epsilon) const
+    {
+        return EqualAbs(elems[0][1], elems[1][0], epsilon)
+            && EqualAbs(elems[0][2], elems[2][0], epsilon)
+            && EqualAbs(elems[1][2], elems[2][1], epsilon);
+    }
+
+    bool Matrix3x3::IsSkewSymmetric(float epsilon) const
+    {
+        return EqualAbs(elems[0][0], 0.f, epsilon)
+            && EqualAbs(elems[1][1], 0.f, epsilon)
+            && EqualAbs(elems[2][2], 0.f, epsilon)
+            && EqualAbs(elems[0][1], -elems[1][0], epsilon)
+            && EqualAbs(elems[0][2], -elems[2][0], epsilon)
+            && EqualAbs(elems[1][2], -elems[2][1], epsilon);
+    }
+
+
+    bool Matrix3x3::HasUnitaryScale(float epsilon) const {
+        Vector3 scale = ExtractScale();
+        return scale.Equals(1.f, 1.f, 1.f, epsilon);
+    }
+
+    bool Matrix3x3::HasNegativeScale() const
+    {
+        return Determinant() < 0.f;
+    }
+
+
+    bool Matrix3x3::IsOrthonormal(float epsilon) const
+    {
+        ///@todo Epsilon magnitudes don't match.
+        return IsColOrthogonal(epsilon) && Row(0).IsNormalized(epsilon) && Row(1).IsNormalized(epsilon) && Row(2).IsNormalized(epsilon);
+    }
+
+
 }
 

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -174,6 +174,7 @@ namespace J3ML::LinearAlgebra {
         float p10 = 0;       float p11 = 2.f / v; float p12 = 0;           float p13 = 0.f;
         float p20 = 0;       float p21 = 0;       float p22 = 1.f / (n-f); float p23 = n / (n-f);
         float p30 = 0;       float p31 = 0;       float p32 = 0.f;         float p33 = 1.f;
+        return {p00,p01,p02,p03, p10, p11, p12, p13, p20,p21,p22,p23, p30,p31,p32,p33};
     }
 
     float Matrix4x4::At(int x, int y) const {
@@ -667,7 +668,7 @@ namespace J3ML::LinearAlgebra {
     {
         assert(!ContainsProjection());
 
-        // a) Transpose the top-left 3x3 part in-place to produce R^t.
+        // a) Transposed the top-left 3x3 part in-place to produce R^t.
         Swap(elems[0][1], elems[1][0]);
         Swap(elems[0][2], elems[2][0]);
         Swap(elems[1][2], elems[2][1]);
@@ -706,4 +707,61 @@ namespace J3ML::LinearAlgebra {
         SetCol(column, columnVector.x, columnVector.y, columnVector.z, columnVector.w);
     }
 
+    void Matrix4x4::Transpose() {
+        Swap(elems[0][1], elems[1][0]);
+        Swap(elems[0][2], elems[2][0]);
+        Swap(elems[0][3], elems[3][0]);
+
+        Swap(elems[1][2], elems[2][1]);
+        Swap(elems[1][3], elems[3][1]);
+        Swap(elems[2][3], elems[3][2]);
+    }
+
+    Matrix4x4 Matrix4x4::Transposed() const {
+        Matrix4x4 copy;
+        copy.elems[0][0] = elems[0][0]; copy.elems[0][1] = elems[1][0]; copy.elems[0][2] = elems[2][0]; copy.elems[0][3] = elems[3][0];
+        copy.elems[1][0] = elems[0][1]; copy.elems[1][1] = elems[1][1]; copy.elems[1][2] = elems[2][1]; copy.elems[1][3] = elems[3][1];
+        copy.elems[2][0] = elems[0][2]; copy.elems[2][1] = elems[1][2]; copy.elems[2][2] = elems[2][2]; copy.elems[2][3] = elems[3][2];
+        copy.elems[3][0] = elems[0][3]; copy.elems[3][1] = elems[1][3]; copy.elems[3][2] = elems[2][3]; copy.elems[3][3] = elems[3][3];
+        return copy;
+    }
+
+    bool Matrix4x4::InverseTranspose() {
+        bool success = Inverse();
+        Transpose();
+        return success;
+    }
+
+    float Matrix4x4::Trace() const {
+        assert(IsFinite());
+        return elems[0][0] + elems[1][1] + elems[2][2] + elems[3][3];
+    }
+
+    bool Matrix4x4::InverseOrthogonalUniformScale() {
+        assert(!ContainsProjection());
+        assert(IsColOrthogonal(1e-3f));
+        assert(HasUniformScale());
+
+        Swap(At(0, 1), At(1, 0));
+        Swap(At(0, 2), At(2, 0));
+        Swap(At(1, 2), At(2, 1));
+        float scale = Vector3(At(0,0), At(1, 0), At(2, 0)).LengthSquared();
+        if (scale == 0.f)
+            return false;
+        scale = 1.f / scale;
+
+        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;
+
+        SetTranslatePart(TransformDir(-At(0, 3), -At(1, 3), -At(2, 3)));
+        return true;
+    }
+
+    Matrix4x4 Matrix4x4::Inverted() const {
+        Matrix4x4 copy = *this;
+        copy.Inverse();
+        return copy;
+    }
+
 }

src/J3ML/LinearAlgebra/Quaternion.cpp

@@ -137,7 +137,7 @@ namespace J3ML::LinearAlgebra {
     AxisAngle Quaternion::ToAxisAngle() const {
         float halfAngle = std::acos(w);
         float angle = halfAngle * 2.f;
-        // TODO: Can Implement Fast Inverse Sqrt Here
+        // TODO: Can Implement Fast Inverted Sqrt Here
         float reciprocalSinAngle = 1.f / std::sqrt(1.f - w*w);
 
         Vector3 axis = {

src/J3ML/LinearAlgebra/Vector3.cpp

@@ -15,6 +15,9 @@ namespace J3ML::LinearAlgebra {
     const Vector3 Vector3::NaN      = {NAN, NAN, NAN};
     const Vector3 Vector3::Infinity = {INFINITY, INFINITY, INFINITY};
     const Vector3 Vector3::NegativeInfinity = {-INFINITY, -INFINITY, -INFINITY};
+    const Vector3 Vector3::UnitX = {1,0,0};
+    const Vector3 Vector3::UnitY = {0,1,0};
+    const Vector3 Vector3::UnitZ = {0,0,1};
 
     Vector3 Vector3::operator+(const Vector3& rhs) const
     {
@@ -86,6 +89,7 @@ namespace J3ML::LinearAlgebra {
         if (index == 0) return x;
         if (index == 1) return y;
         if (index == 2) return z;
+        throw;
     }
 
     bool Vector3::IsWithinMarginOfError(const Vector3& rhs, float margin) const
@@ -437,6 +441,7 @@ namespace J3ML::LinearAlgebra {
         };
     }
 
+    // TODO: Implement
     Vector3 Vector3::Perpendicular(const Vector3 &hint, const Vector3 &hint2) const {
         assert(!this->IsZero());
         assert(hint.IsNormalized());
@@ -444,6 +449,8 @@ namespace J3ML::LinearAlgebra {
         Vector3 v = this->Cross(hint);
         float len = v.TryNormalize();
 
+
+        return Vector3::Zero;
     }
 
     float Vector3::TryNormalize() {