Update OBB.cpp

Constructor

.gitea/workflows/doccy.yaml

@@ -0,0 +1,25 @@
+name: Build Docs With Doxygen
+run-name: Building documentation for ${{ gitea.repository }}.
+on: [push]
+
+jobs:
+  Explore-Gitea-Actions:
+    runs-on: ubuntu-22.04
+    steps:
+      - run: echo "The job was automatically triggered by a ${{ gitea.event_name }} event."
+      - run: echo "This job is now running on a ${{ runner.os }} server hosted by Gitea!"
+      - run: echo "The name of your branch is ${{ gitea.ref }} and your repository is ${{ gitea.repository }}."
+      - name: Check out repository code
+        uses: actions/checkout@v3
+      - run: echo "The ${{ gitea.repository }} repository has been cloned to the runner."
+      - run: echo "The workflow is now ready to build your docs on the runner."
+      - run: echo "Copying SSH key for file transfer into runner."
+      - run: echo "${{ secrets.SSHKEY }}" > /id_rsa
+      - run: chmod 600 /id_rsa
+      - run: echo "Installing doxygen on runner."
+      - run: apt-get update && apt-get install -y doxygen
+      - run: echo "Building documentation."
+      - run: doxygen Doxyfile
+      - run: echo "Copying built documentation to doc site."
+      - run: scp -o "IdentitiesOnly=yes" -o "StrictHostKeyChecking=no" -i /id_rsa -P ${{ secrets.SSHPORT }} -r html ${{ secrets.SSHUSER }}@${{ secrets.SSHIP }}:/var/www/html/$(echo "${{ gitea.repository }}" | cut -f 2 -d '/')
+      - run: echo "This job's status is ${{ job.status }}."

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()

README.md

@@ -1,17 +1,60 @@
 # Josh's 3D Math Library - J3ML
 
+
 Yet Another C++ Math Standard
 
-## Motivation
-This project was sparked by a desire to gain a deeper understanding into the math underlying computer graphics. While packages such as glm, eigen, etc. are amazing libraries, it removes the fun of having to learn it the hard way.
+J3ML is a "Modern C++" C++ library designed to provide comprehensive support for 3D mathematical operations commonly used in computer graphics, game development, physics simulations, and related fields. It offers a wide range of functionalities to simplify the implementation of complex mathematical operations in your projects.
+
+![Static Badge](https://img.shields.io/badge/Lit-Based-%20)
 
-## Use Cases
 ## Features
-### LinearAlgebra
-#### Vectors
-#### Matrices
-#### Conversion Types
-### Geometry
-## Samples
-## Bugs / Issues
-## Compilation
+
+* <b>Vector Operations:</b> Comprehensive support for 3D vector operations including addition, subtraction, scalar multiplication, dot product, cross product, normalization, and more. 
+* **Matrix Operations:** Efficient implementation of 3x3 and 4x4 matrices with support for common operations such as multiplication, transpose, determinant calculation, and inverse calculation.
+* **Quaternion Operations:** Quaternion manipulation functions including conversion to/from axis-angle representation, quaternion multiplication, normalization, and interpolation (slerp).
+* **Transformation Functions:** Functions for transforming points, vectors, and normals using matrices and quaternions.
+* **Geometric Types:** Support for geometric types such as points, lines, rays, planes, spheres, axis-aligned bounding boxes (AABB), and oriented bounding boxes (OBB).
+* **Algorithms:** Implementation of various algorithms including Gilbert-Johnson-Keerthi (GJK) algorithm for collision detection, random number generator, and more.
+* **Utility Functions:** Additional utilities such as conversion between degrees and radians, random number generation, and common constants.
+
+# Usage
+
+To use J3ML in your C++ project, simply include the necessary header files and link against the library. Here's a basic example of how to use the library to perform vector addition:
+
+
+```cpp
+
+#include <iostream>
+#include <j3ml/LinearAlgebra.h>
+
+int main() {
+    // Create two 3D vectors
+    Vector3 v1(1.0, 2.0, 3.0);
+    Vector3 v2(4.0, 5.0, 6.0);
+
+    // Perform vector addition
+    Vector3 result = v1 + v2;
+    
+    // Output the result
+    std::cout << "Result: " << result << std::endl;
+    
+    return 0;
+}
+```
+
+For more detailed usage instructions and examples, please refer to the documentation.
+# Documentation
+
+Documentation is automatically generated from latest commit and is hosted at https://doc.redacted.cc/j3ml .
+
+# Contributing
+
+Contributions to J3ML are welcome! If you find a bug, have a feature request, or would like to contribute code, please submit an issue or pull request to the GitHub repository.
+
+# License
+
+J3ML is licensed under the Public Domain. See the LICENSE file for details.
+
+# Acknowledgements
+
+J3ML is developed and maintained by Joshua O'Leary from Redacted Software and contributors. Special thanks to William J Tomasine II.

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/Algorithm/SAT.h

@@ -0,0 +1,7 @@
+#pragma once
+
+
+namespace J3ML::Algorithm
+{
+
+}

include/J3ML/Geometry/AABB.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <optional>
+
 #include <J3ML/LinearAlgebra.h>
 #include <J3ML/Geometry/Common.h>
 #include <J3ML/Geometry/Shape.h>
@@ -8,96 +10,199 @@
 
 namespace J3ML::Geometry
 {
-
-
     using namespace J3ML::LinearAlgebra;
     using J3ML::Algorithm::RNG;
-    // A 3D axis-aligned bounding box
-    // This data structure can be used to represent coarse bounds of objects, in situations where detailed triangle-level
-    // computations can be avoided. In physics systems, bounding boxes are used as an efficient early-out test for geometry
-    // intersection queries.
-    // the 'Axis-aligned' part in the name means that the local axes of this bounding box are restricted to align with the
-    // axes of the world space coordinate system. This makes computation involving AABB's very fast, since AABB's cannot
-    // be arbitrarily oriented in the space with respect to each other.
-    // If you need to represent a box in 3D space with arbitrary orientation, see the class OBB. */
 
+    /// @brief A 3D axis-aligned bounding box.
+    /// This data structure can be used to represent coarse bounds of objects, in situations where detailed triangle-level
+    /// computations can be avoided. In physics systems, bounding boxes are used as an efficient early-out test for geometry
+    /// intersection queries.
+    /// the 'Axis-aligned' part in the name means that the local axes of this bounding box are restricted to align with the
+    /// axes of the world space coordinate system. This makes computation involving AABB's very fast, since AABB's cannot
+    /// be arbitrarily oriented in the space with respect to each other.
+    /// If you need to represent a box in 3D space with arbitrary orientation, see the class OBB. */
     class AABB : public Shape {
     public:
+        /// Specifies the minimum extent of this AABB in the world space x, y and z axes.
         Vector3 minPoint;
+        /// Specifies the maximum extent of this AABB in the world space x, y and z axes. [similarOverload: minPoint]
         Vector3 maxPoint;
     public:
         static int NumFaces() { return 6; }
         static int NumEdges() { return 12; }
         static int NumVertices() { return 8; }
     public:
+        /// The default constructor does not initialize any members of this class.
+        /** This means that the values of the members minPoint and maxPoint are undefined after creating a new AABB using this
+            default constructor. Remember to assign to them before use.
+            @see minPoint, maxPoint. */
         AABB();
+
+        /// Constructs this AABB by specifying the minimum and maximum extending corners of the box.
+        /** @see minPoint, maxPoint. */
         AABB(const Vector3& min, const Vector3& max);
 
+        /// Constructs this AABB to enclose the given OBB.
+        /** This constructor computes the optimal minimum volume AABB that encloses the given OBB.
+            @note Since an AABB cannot generally represent an OBB, this conversion is not exact, but the returned AABB
+                specifies a larger volume.
+            @see class OBB. */
+        explicit AABB(const OBB &obb);
+
+        /// Constructs this AABB to enclose the given Sphere.
+        /** @see class Sphere. */
+        explicit AABB(const Sphere &s);
+
         Vector3 HalfDiagonal() const { return HalfSize(); }
 
         static AABB FromCenterAndSize(const Vector3 &center, const Vector3 &size);
 
+
+        /// Returns the minimum world-space coordinate along the given axis.
         float MinX() const;
-        float MinY() const;
-        float MinZ() const;
+        float MinY() const; ///< [similarOverload: MinX]
+        float MinZ() const; ///< [similarOverload: MinX]
+
+        /// Returns the maximum world-space coordinate along the given axis.
         float MaxX() const;
-        float MaxY() const;
-        float MaxZ() const;
+        float MaxY() const; ///< [similarOverload: MaxX]
+        float MaxZ() const; ///< [similarOverload: MaxX]
 
         /// 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;
 
+        /// [similarOverload: Size]
+        /** Returns Size()/2.
+            @see Size(), HalfDiagonal(). */
         Vector3 HalfSize() const;
 
         /// Returns the largest sphere that can fit inside this AABB
         /// This function computes the largest sphere that can fit inside this AABB.
         Sphere MaximalContainedSphere() const;
 
+        /// Tests if this AABB is finite.
+        /** @return True if the member variables of this AABB are valid floats and do not contain NaNs or infs, and false otherwise.
+            @see IsDegenerate(), minPoint, maxPoint. */
         bool IsFinite() const;
 
+        /// @return The center point of this AABB.
         Vector3 Centroid() const;
 
+        /// Returns the side lengths of this AABB in x, y and z directions.
+        /** The returned vector is equal to the diagonal vector of this AABB, i.e. it spans from the
+            minimum corner of the AABB to the maximum corner of the AABB.
+            @see HalfSize(), Diagonal(). */
         Vector3 Size() const;
 
         // Quickly returns an arbitrary point inside this AABB
         Vector3 AnyPointFast() const;
 
+        /// Generates a point inside this AABB.
+        /** @param x A normalized value between [0,1]. This specifies the point position along the world x axis.
+            @param y A normalized value between [0,1]. This specifies the point position along the world y axis.
+            @param z A normalized value between [0,1]. This specifies the point position along the world z axis.
+            @return A point inside this AABB at point specified by given parameters.
+            @see Edge(), CornerPoint(), PointOnEdge(), FaceCenterPoint(), FacePoint(). */
         Vector3 PointInside(float x, float y, float z) const;
 
         // Returns an edge of this AABB
         LineSegment Edge(int edgeIndex) const;
 
+        /// Returns a corner point of this AABB.
+        /** This function generates one of the eight corner points of this AABB.
+            @param cornerIndex The index of the corner point to generate, in the range [0, 7].
+                The points are returned in the order 0: ---, 1: --+, 2: -+-, 3: -++, 4: +--, 5: +-+, 6: ++-, 7: +++. (corresponding the XYZ axis directions).
+            @todo Draw which index generates which corner point.
+            @see PointInside(), Edge(), PointOnEdge(), FaceCenterPoint(), FacePoint(), GetCornerPoints(). */
         Vector3 CornerPoint(int cornerIndex) const;
 
+        /// Computes an extreme point of this AABB in the given direction.
+        /** An extreme point is a farthest point of this AABB in the given direction. Given a direction,
+            this point is not necessarily unique.
+            @param direction The direction vector of the direction to find the extreme point. This vector may
+                be unnormalized, but may not be null.
+            @return An extreme point of this AABB in the given direction. The returned point is always a
+                corner point of this AABB.
+            @see CornerPoint(). */
         Vector3 ExtremePoint(const Vector3 &direction) const;
         Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
 
+        /// Returns a point on an edge of this AABB.
+        /** @param edgeIndex The index of the edge to generate a point to, in the range [0, 11]. @todo Document which index generates which one.
+            @param u A normalized value between [0,1]. This specifies the relative distance of the point along the edge.
+            @see PointInside(), CornerPoint(), CornerPoint(), FaceCenterPoint(), FacePoint(). */
         Vector3 PointOnEdge(int edgeIndex, float u) const;
 
+        /// Returns the point at the center of the given face of this AABB.
+        /** @param faceIndex The index of the AABB face to generate the point at. The valid range is [0, 5].
+                This index corresponds to the planes in the order (-X, +X, -Y, +Y, -Z, +Z).
+            @see PointInside(), CornerPoint(), PointOnEdge(), PointOnEdge(), FacePoint(). */
         Vector3 FaceCenterPoint(int faceIndex) const;
 
+        /// Generates a point at the surface of the given face of this AABB.
+        /** @param faceIndex The index of the AABB face to generate the point at. The valid range is [0, 5].
+                This index corresponds to the planes in the order (-X, +X, -Y, +Y, -Z, +Z).
+            @param u A normalized value between [0, 1].
+            @param v A normalized value between [0, 1].
+            @see PointInside(), CornerPoint(), PointOnEdge(), PointOnEdge(), FaceCenterPoint(). */
         Vector3 FacePoint(int faceIndex, float u, float v) const;
 
+        /// Returns the surface normal direction vector the given face points towards.
+        /** @param faceIndex The index of the AABB face to generate the point at. The valid range is [0, 5].
+                This index corresponds to the planes in the order (-X, +X, -Y, +Y, -Z, +Z).
+            @see FacePoint(), FacePlane(). */
         Vector3 FaceNormal(int faceIndex) const;
 
+        /// Computes the plane equation of the given face of this AABB.
+        /** @param faceIndex The index of the AABB face. The valid range is [0, 5].
+                This index corresponds to the planes in the order (-X, +X, -Y, +Y, -Z, +Z).
+            @return The plane equation the specified face lies on. The normal of this plane points outwards from this AABB.
+            @see FacePoint(), FaceNormal(), GetFacePlanes(). */
         Plane FacePlane(int faceIndex) const;
 
+
         void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
 
+        /// Generates an AABB that encloses the given point set.
+        /** This function finds the smallest AABB that contains the given set of points.
+            @param pointArray A pointer to an array of points to enclose inside an AABB.
+            @param numPoints The number of elements in the pointArray list.
+            @see SetFrom(). */
         static AABB MinimalEnclosingAABB(const Vector3 *pointArray, int numPoints);
-        float GetVolume() const;
-        float GetSurfaceArea() const;
+        AABB MinimalEnclosingAABB() const { return *this;}
+        /// Computes the volume of this AABB.
+        /** @see SurfaceArea(), IsDegenerate(). */
+        float Volume() const;
+        /// Computes the surface area of the faces of this AABB.
+        /** @see Volume(). */
+        float SurfaceArea() const;
         Vector3 GetClosestPoint(const Vector3& point) const;
 
         void Translate(const Vector3& offset);
         AABB Translated(const Vector3& offset) const;
-        AABB TransformAABB(const Matrix3x3& transform);
-        AABB TransformAABB(const Matrix4x4& transform);
-        AABB TransformAABB(const Quaternion& transform);
+        void Scale(const Vector3& scale);
+        AABB Scaled(const Vector3& scale) const;
+        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.
+            @param transform The transformation to apply to this AABB. This function assumes that this
+                transformation does not contain shear, nonuniform scaling or perspective properties, i.e. that the fourth
+                row of the float4x4 is [0 0 0 1].
+            @see Translate(), Scale(), TransformAsAABB(), classes float3x3, float3x4, float4x4, Quat. */
         OBB Transform(const Matrix3x3& transform) const;
         OBB Transform(const Matrix4x4& transform) const;
         OBB Transform(const Quaternion& transform) const;
+
+        /// Tests if the given object is fully contained inside this AABB.
+        /** This function returns true if the given object lies inside this AABB, and false otherwise.
+            @note The comparison is performed using less-or-equal, so the faces of this AABB count as being inside, but
+                due to float inaccuracies, this cannot generally be relied upon.
+            @todo Add Contains(Circle/Disc/Sphere/Capsule).
+            @see Distance(), Intersects(), ClosestPoint(). */
         bool Contains(const Vector3& point) const;
         bool Contains(const Vector3& aabbMinPoint, const Vector3& aabbMaxPoint) const;
         bool Contains(const LineSegment& lineSegment) const;
@@ -109,27 +214,94 @@ namespace J3ML::Geometry
         bool Contains(const Frustum& frustum) const;
         bool Contains(const Polyhedron& polyhedron) const;
         bool Contains(const Capsule& capsule) const;
-        // Tests whether this AABB and the given object intersect.
+
+        /// Tests whether this AABB and the given object intersect.
+        /** Both objects are treated as "solid", meaning that if one of the objects is fully contained inside
+            another, this function still returns true. (e.g. in case a line segment is contained inside this AABB,
+            or this AABB is contained inside a Sphere, etc.)
+            @param ray The first parameter of this function specifies the other object to test against.
+            @param dNear [out] If specified, receives the parametric distance along the line denoting where the
+                line entered this AABB.
+            @param dFar [out] If specified, receives the parametric distance along the line denoting where the
+                line exited this AABB.
+            @see Contains(), Distance(), ClosestPoint().
+            @note If you do not need the intersection intervals, you should call the functions without these
+                parameters in the function signature for optimal performance.
+            @todo Add Intersects(Circle/Disc). */
         bool Intersects(const Ray& ray, float dNear, float dFar) const;
         bool Intersects(const Capsule& capsule) const;
         bool Intersects(const Triangle& triangle) const;
         bool Intersects(const Polygon& polygon) const;
         bool Intersects(const Frustum& frustum) const;
         bool Intersects(const Polyhedron& polyhedron) const;
+        bool Intersects(const AABB& aabb) const;
+
+        /** For reference documentation on the Sphere-AABB intersection test, see Christer Ericson's Real-Time Collision Detection, p. 165. [groupSyntax]
+		@param sphere The first parameter of this function specifies the other object to test against.
+		@param closestPointOnAABB [out] Returns the closest point on this AABB to the given sphere. This pointer
+			may be null. */
+        bool Intersects(const Sphere &sphere, Vector3 *closestPointOnAABB = 0) const;
+
+        /// Generates an unindexed triangle mesh representation of this AABB.
+        /** @param numFacesX The number of faces to generate along the X axis. This value must be >= 1.
+            @param numFacesY The number of faces to generate along the Y axis. This value must be >= 1.
+            @param numFacesZ The number of faces to generate along the Z axis. This value must be >= 1.
+            @param outPos [out] An array of size numVertices which will receive a triangle list
+                of vertex positions. Cannot be null.
+            @param outNormal [out] An array of size numVertices which will receive vertex normals.
+                If this parameter is null, vertex normals are not returned.
+            @param outUV [out] An array of size numVertices which will receive vertex UV coordinates.
+                If this parameter is null, a UV mapping is not generated.
+            @param ccwIsFrontFacing If true, then the front-facing direction of the faces will be the sides
+                with counterclockwise winding order. Otherwise, the faces are generated in clockwise winding order.
+            The number of vertices that outPos, outNormal and outUV must be able to contain is
+            (x*y + x*z + y*z)*2*6. If x==y==z==1, then a total of 36 vertices are required. Call
+            NumVerticesInTriangulation to obtain this value.
+            @see ToPolyhedron(), ToEdgeList(), NumVerticesInTriangulation(). */
         TriangleMesh Triangulate(int numFacesX, int numFacesY, int numFacesZ, bool ccwIsFrontFacing) const;
 
+        /// Returns the number of vertices that the Triangulate() function will output with the given subdivision parameters.
+        /** @see Triangulate(). */
+        static int NumVerticesInTriangulation(int numFacesX, int numFacesY, int numFacesZ)
+        {
+            return (numFacesX*numFacesY + numFacesX*numFacesZ + numFacesY*numFacesZ)*2*6;
+        }
+
+        /// Returns the number of vertices that the ToEdgeList() function will output.
+        /** @see ToEdgeList(). */
+        static int NumVerticesInEdgeList()
+        {
+            return 4*3*2;
+        }
+
         /// Finds the set intersection of this and the given AABB.
-        /** @return This function returns the AABB that is contained in both this and the given AABB.
+        /** @return This function returns an intersection that is contained in both this and the given AABB if there is one.
             @todo Add Intersection(OBB/Polyhedron). */
-        AABB Intersection(const AABB& rhs) const;
+        std::optional<AABB> Intersection(const AABB& rhs) const;
 
 
+        /// Sets this AABB to enclose the given set of points.
+        /** @param pointArray A pointer to an array of points to enclose inside an AABB.
+            @param numPoints The number of elements in the pointArray list.
+            @see MinimalEnclosingAABB(). */
         void SetFrom(const Vector3 *pVector3, int i);
 
+        /// Sets this AABB by specifying its center and size.
+        /** @param center The center point of this AABB.
+            @param size A vector that specifies the size of this AABB in x, y and z directions.
+            @see SetFrom(), FromCenterAndSize(). */
         void SetFromCenterAndSize(const Vector3 &center, const Vector3 &size);
 
+        /// Sets this AABB to enclose the given OBB.
+        /** This function computes the minimal axis-aligned bounding box for the given oriented bounding box. If the orientation
+            of the OBB is not aligned with the world axes, this conversion is not exact and loosens the volume of the bounding box.
+            @param obb The oriented bounding box to convert into this AABB.
+            @todo Implement SetFrom(Polyhedron).
+            @see SetCenter(), class OBB. */
         void SetFrom(const OBB &obb);
 
+        /// Sets this AABB to enclose the given sphere.
+        /** 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;
@@ -137,15 +309,28 @@ namespace J3ML::Geometry
         Vector3 GetRandomPointOnEdge(RNG& rng) const;
         Vector3 GetRandomCornerPoint(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(),
+            which incrementally expands the bounds of this AABB to enclose the given objects.
+            @see Enclose(). */
         void SetNegativeInfinity();
 
+        /// Expands this AABB to enclose the given object.
+        /** This function computes an AABB that encloses both this AABB and the specified object, and stores the resulting
+            AABB into this.
+            @note The generated AABB is not necessarily the optimal enclosing AABB for this AABB and the given object. */
         void Enclose(const Vector3 &point);
-
         void Enclose(const Vector3 &aabbMinPt, const Vector3 &aabbMaxPt);
-
         void Enclose(const LineSegment &lineSegment);
-
         void Enclose(const OBB &obb);
+        void Enclose(const Sphere &sphere);
+        void Enclose(const Triangle &triangle);
+        void Enclose(const Capsule &capsule);
+        void Enclose(const Frustum &frustum);
+        void Enclose(const Polygon &polygon);
+        void Enclose(const Polyhedron &polyhedron);
+        void Enclose(const Vector3 *pointArray, int numPoints);
+
 
         bool TestAxis(const Vector3& axis, const Vector3& v0, const Vector3& v1, const Vector3& v2) const;
 

include/J3ML/Geometry/Capsule.h

@@ -2,7 +2,7 @@
 
 #include "LineSegment.h"
 #include "Shape.h"
-#include <J3ML/LinearAlgebra/Vector3.h>
+#include <J3ML/LinearAlgebra.h>
 #include <J3ML/Geometry/Common.h>
 
 namespace J3ML::Geometry
@@ -16,7 +16,7 @@ namespace J3ML::Geometry
         b = std::move(temp);
     }
 
-    using namespace LinearAlgebra;
+    /// A 3D cylinder with spherical ends.
     class Capsule : public Shape
     {
     public:
@@ -25,13 +25,39 @@ namespace J3ML::Geometry
         // Specifies the radius of this capsule
         float r;
     public:
+        /// The default constructor does not initialize any members of this class.
+        /** This means that the values of the members l and r are both undefined after creating a new capsule using
+            this default constructor. Remember to assign to them before use.
+            @see l, r. */
         Capsule();
+        /// Constructs a new capsule by explicitly specifying the member variables.
+        /** @param endPoints Specifies the line segment of the capsule.
+            @param radius Specifies the size of this capsule.
+            @see l, r. */
         Capsule(const LineSegment& endPoints, float radius);
+        /// Constructs a new capsule by explicitly specifying the member variables.
+        /** This constructor is equivalent to calling Capsule(LineSegment(bottomPoint, topPoint), radius), but provided
+            here for conveniency.
+            @see l, r. */
         Capsule(const Vector3& bottomPt, const Vector3& topPt, float radius);
 
         /// Quickly returns an arbitrary point inside this Capsule. Used in GJK intersection test.
         inline Vector3 AnyPointFast() const { return l.A; }
 
+        /// Generates a point that perhaps lies inside this capsule.
+        /** @param height A normalized value between [0,1]. This specifies the point position along the height line of this capsule.
+            @param x A normalized value between [0,1]. This specifies the x coordinate on the plane of the circle cross-section specified by l.
+            @param y A normalized value between [0,1]. This specifies the y coordinate on the plane of the circle cross-section specified by l.
+            @note This function will generate points uniformly, but they do not necessarily lie inside the capsule.
+            @see PointInside(). */
+        Vector3 UniformPointPerhapsInside(float height, float x, float y) const;
+
+        /// Returns the Sphere defining the 'bottom' section of this Capsule (corresponding to the endpoint l.a)
+        Sphere SphereA() const;
+
+        /// Returns the Sphere defining the 'top' section of this Capsule (corresponding to the endpoint l.b)
+        Sphere SphereB() const;
+
         /// Computes the extreme point of this Capsule in the given direction.
         /** An extreme point is a farthest point of this Capsule in the given direction. Given a direction,
             this point is not necessarily unique.
@@ -41,39 +67,137 @@ namespace J3ML::Geometry
         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;
+        /// Computes the total height of this capsule, i.e. LineLength() + Diameter().
+        /** <img src="CapsuleFunctions.png" />
+            @see LineLength(). */
         float Height() const;
+        /// Computes the diameter of this capsule.
         float Diameter() const;
+        /// Returns the bottom-most point of this Capsule.
+        /** <img src="CapsuleFunctions.png" />
+            @note The bottom-most point is only a naming convention, and does not correspond to the bottom-most point along any world axis. The returned
+                point is simply the point at the far end of this Capsule where the point l.a resides.
+            @note The bottom-most point of the capsule is different than the point l.a. The returned point is the point at the very far
+                edge of this capsule, and does not lie on the internal line. See the attached diagram.
+            @see Top(), l. */
         Vector3 Bottom() const;
+        /// Returns the center point of this Capsule.
+        /** <img src="doc/static/docs/CapsuleFunctions.png" />
+            @return The point (l.a + l.b) / 2. This point is the center of mass for this capsule.
+            @see l, Bottom(), Top(). */
         Vector3 Center() const;
-        Vector3 Centroid() const;
+        Vector3 Centroid() const; ///< [similarOverload: Center]
+
+        /// Returns the direction from the bottommost point towards the topmost point of this Capsule.
+        /** <img src="CapsuleFunctions.png" />
+            @return The normalized direction vector from l.a to l.b.
+            @see l. */
+        Vector3 UpDirection() const;
+
+        /// Computes the volume of this Capsule.
+        /** @return pi * r^2 * |b-a| + 4 * pi * r^2 / 3.
+            @see SurfaceArea(). */
+        float Volume() const;
+
+        /// Computes the surface area of this Capsule.
+        /** @return 2 * pi * r * |b-a| + 4 * pi * r^2.
+            @see Volume(). */
+        float SurfaceArea() const;
+
+        /// Returns the cross-section circle at the given height of this Capsule.
+        /** <img src="CapsuleFunctions.png" />
+            @param l A normalized parameter between [0,1]. l == 0 returns a degenerate circle of radius 0 at the bottom of this Capsule, and l == 1
+                    will return a degenerate circle of radius 0 at the top of this Capsule. */
+        //Circle CrossSection(float l) const;
+
         Vector3 ExtremePoint(const Vector3& direction);
+
+        /// Returns the smallest AABB that encloses this capsule.
+        /** @see MinimalEnclosingOBB(). */
         AABB MinimalEnclosingAABB() const;
 
+        /// Returns the smallest OBB that encloses this capsule.
+        /** @see MinimalEnclosingAABB(). */
+        OBB MinimalEnclosingOBB() const;
+
+        /// Projects this Capsule onto the given 1D axis direction vector.
+        /** This function collapses this Capsule onto an 1D axis for the purposes of e.g. separate axis test computations.
+            The function returns a 1D range [outMin, outMax] denoting the interval of the projection.
+            @param direction The 1D axis to project to. This vector may be unnormalized, in which case the output
+                of this function gets scaled by the length of this vector.
+            @param outMin [out] Returns the minimum extent of this object along the projection axis.
+            @param outMax [out] Returns the maximum extent of this object along the projection axis. */
         void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
 
+        /// Returns the topmost point of this Capsule.
+        /** <img src="CapsuleFunctions.png" />
+            @note The topmost point is only a naming convention, and does not correspond to the topmost point along any world axis. The returned
+                point is simply the point at the far end of this Capsule where the point l.b resides.
+            @note The topmost point of the capsule is different than the point l.b. The returned point is the point at the very far
+                edge of this capsule, and does not lie on the internal line. See the attached diagram.
+            @see Bottom(), l. */
+        Vector3 Top() const;
+
+        /// Applies a transformation to this capsule.
+        /** @param transform The transformation to apply to this capsule. This transformation must be
+            affine, and must contain an orthogonal set of column vectors (may not contain shear or projection).
+            The transformation can only contain uniform scale, and may not contain mirroring.
+            @see Translate(), Scale(), classes Matrix3x3, Matrix4x4, Quaternion. */
+        void Transform(const Matrix3x3 &transform);
+        void Transform(const Matrix4x4 &transform);
+        void Transform(const Quaternion &transform);
+
+        /// Computes the closest point inside this capsule to the given point.
+        /** If the target point lies inside this capsule, then that point is returned.
+            @see Distance(), Contains(), Intersects().
+            @todo Add ClosestPoint(Line/Ray/LineSegment/Plane/Triangle/Polygon/Circle/Disc/AABB/OBB/Sphere/Capsule/Frustum/Polyhedron). */
+        Vector3 ClosestPoint(const Vector3 &targetPoint) const;
+
+
+        /// Computes the distance between this capsule and the given object.
+        /** This function finds the nearest pair of points on this and the given object, and computes their distance.
+            If the two objects intersect, or one object is contained inside the other, the returned distance is zero.
+            @todo Add Distance(Triangle/Polygon/Circle/Disc/Capsule).
+            @see Contains(), Intersects(), ClosestPoint(). */
+        float Distance(const Vector3 &point) const;
+        float Distance(const Plane &plane) const;
+        float Distance(const Sphere &sphere) const;
+        float Distance(const Ray &ray) const;
+        float Distance(const Line &line) const;
+        float Distance(const LineSegment &lineSegment) const;
+        float Distance(const Capsule &capsule) const;
+
+        /// Tests if the given object is fully contained inside this capsule.
+        /** This function returns true if the given object lies inside this capsule, and false otherwise.
+            @note The comparison is performed using less-or-equal, so the surface of this capsule count as being inside, but
+                due to float inaccuracies, this cannot generally be relied upon.
+            @todo Add Contains(Circle/Disc/Sphere/Capsule).
+            @see Distance(), Intersects(), ClosestPoint(). */
+        bool Contains(const Vector3 &point) const;
+        bool Contains(const LineSegment &lineSegment) const;
+        bool Contains(const Triangle &triangle) const;
+        bool Contains(const Polygon &polygon) const;
+        bool Contains(const AABB &aabb) const;
+        bool Contains(const OBB &obb) const;
+        bool Contains(const Frustum &frustum) const;
+        bool Contains(const Polyhedron &polyhedron) const;
+
         bool Intersects(const Plane &plane) const;
-
         bool Intersects(const Ray &ray) const;
-
         bool Intersects(const Line &line) const;
-
         bool Intersects(const LineSegment &lineSegment) const;
-
         bool Intersects(const AABB &aabb) const;
-
         bool Intersects(const OBB &obb) const;
-
         bool Intersects(const Sphere &sphere) const;
-
         bool Intersects(const Capsule &capsule) const;
-
         bool Intersects(const Triangle &triangle) const;
-
         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/Common.h

@@ -27,5 +27,13 @@ namespace J3ML::Geometry
 // Methods required by Geometry types
 namespace J3ML::Geometry
 {
+    // Represents a segment along an axis, with the axis as a unit
+    struct Interval {
+        float min;
+        float max;
 
+        bool Intersects(const Interval& rhs) const;
+
+        bool operator==(const Interval& rhs) const = default;
+    };
 }

include/J3ML/Geometry/Frustum.h

@@ -51,7 +51,7 @@ namespace J3ML::Geometry
         D3D,
     };
 
-    /// The handedness rule in J3ML bundles together two different conventions related to the camera:
+    /// @brief The handedness rule in J3ML bundles together two different conventions related to the camera:
     /// * the chirality of the world and view spaces,
     /// * the fixed local front direction of the Frustum.
     /// @note The world and view spaces are always assumed to the same chirality, meaning that Frustum::ViewMatrix()
@@ -74,10 +74,12 @@ namespace J3ML::Geometry
         Right
     };
 
-    /// Represents either an orthographic or a perspective viewing frustum.
+    /// @brief Represents either an orthographic or a perspective viewing frustum.
+    /// @see FrustumType
+    /// @see FrustumProjectiveSpace
+    /// @see FrustumHandedness
     class Frustum : public Shape {
-    public: /// Members
-
+    public: // Members
 
         /// Specifies whether this frustum is a perspective or an orthographic frustum.
         FrustumType type;
@@ -138,85 +140,228 @@ namespace J3ML::Geometry
         Matrix4x4 worldMatrix;
         Matrix4x4 projectionMatrix;
         Matrix4x4 viewProjectionMatrix;
-    public: /// Methods
-        Frustum()
-        : type(FrustumType::Invalid),
-        pos(Vector3::NaN),
-        front(Vector3::NaN),
-        up(Vector3::NaN),
-        nearPlaneDistance(NAN),
-        farPlaneDistance(NAN),
-        worldMatrix(Matrix4x4::NaN),
-        viewProjectionMatrix(Matrix4x4::NaN)
-        {
-            // For conveniency, allow automatic initialization of the graphics API and handedness in use.
-            // If neither of the #defines are set, user must specify per-instance.
-        }
+    public:
 
+    /// The default constructor creates an uninitialized Frustum object.
+    /** This means that the values of the members type, projectiveSpace, handedness, pos, front, up, nearPlaneDistance, farPlaneDistance, horizontalFov/orthographicWidth and
+    verticalFov/orthographicHeight are all NaN after creating a new Frustum using this
+    default constructor. Remember to assign to them before use.
+    @note As an exception to other classes in MathGeoLib, this class initializes its members to NaNs, whereas the other classes leave the members uninitialized. This difference
+        is because the Frustum class implements a caching mechanism where world, projection and viewProj matrices are recomputed on demand, which does not work nicely together
+        if the defaults were uninitialized.
+        */
+        Frustum();
 
         /// Quickly returns an arbitrary point inside this Frustum. Used in GJK intersection test.
         inline Vector3 AnyPointFast() const { return CornerPoint(0); }
 
         static Frustum CreateFrustumFromCamera(const CoordinateFrame& cam, float aspect, float fovY, float zNear, float zFar);
+        /// Returns the tightest AABB that contains this Frustum.
+        /** This function computes the optimal minimum volume AABB that encloses this Frustum.
+            @note Since an AABB cannot generally represent a Frustum, this conversion is not exact, but the returned AABB
+                specifies a larger volume.
+            @see MinimalEnclosingOBB(), ToPolyhedron(). */
         AABB MinimalEnclosingAABB() const;
+        /// Returns the tightest OBB that encloses this Frustum.
+        /** This function computes the optimal minimum volume OBB that encloses this Frustum.
+            @note If the type of this frustum is Perspective, this conversion is not exact, but the returned OBB specifies
+                a larger volume. If the type of this Frustum is orthographic, this conversion is exact, since the shape of an
+                orthographic Frustum is an OBB.
+            @see MinimalEnclosingAABB(), ToPolyhedron(). */
         OBB MinimalEnclosingOBB() const;
+        /// Sets the type of this Frustum.
+        /** @note Calling this function recomputes the cached view and projection matrices of this Frustum.
+            @see SetViewPlaneDistances(), SetFrame(), SetPos(), SetFront(), SetUp(), SetPerspective(), SetOrthographic(), ProjectiveSpace(), Handedness(). */
         void SetKind(FrustumProjectiveSpace projectiveSpace, FrustumHandedness handedness);
+        /// Sets the depth clip distances of this Frustum.
+        /** @param nearPlaneDistance The z distance from the eye point to the position of the Frustum near clip plane. Always pass a positive value here.
+            @param farPlaneDistance The z distance from the eye point to the position of the Frustum far clip plane. Always pass a value that is larger than nearClipDistance.
+            @note Calling this function recomputes the cached projection matrix of this Frustum.
+            @see SetKind(), SetFrame(), SetPos(), SetFront(), SetUp(), SetPerspective(), SetOrthographic(), NearPlaneDistance(), FarPlaneDistance(). */
         void SetViewPlaneDistances(float nearPlaneDistance, float farPlaneDistance);
+        /// Specifies the full coordinate space of this Frustum in one call.
+        /** @note Calling this function recomputes the cached world matrix of this Frustum.
+            @note As a micro-optimization, prefer this function over the individual SetPos/SetFront/SetUp functions if you need to do a batch of two or more changes, to avoid
+            redundant recomputation of the world matrix.
+            @see SetKind(), SetViewPlaneDistances(), SetPos(), SetFront(), SetUp(), SetPerspective(), SetOrthographic(), Pos(), Front(), Up(). */
         void SetFrame(const Vector3& pos, const Vector3& front, const Vector3& up);
+        /// Sets the world-space position of this Frustum.
+        /** @note Calling this function recomputes the cached world matrix of this Frustum.
+            @see SetKind(), SetViewPlaneDistances(), SetFrame(), SetFront(), SetUp(), SetPerspective(), SetOrthographic(), Pos(). */
         void SetPos(const Vector3& pos);
+        /// Sets the world-space direction the Frustum eye is looking towards.
+        /** @note Calling this function recomputes the cached world matrix of this Frustum.
+            @see SetKind(), SetViewPlaneDistances(), SetFrame(), SetPos(), SetUp(), SetPerspective(), SetOrthographic(), Front(). */
         void SetFront(const Vector3& front);
+        /// Sets the world-space camera up direction vector of this Frustum.
+        /** @note Calling this function recomputes the cached world matrix of this Frustum.
+            @see SetKind(), SetViewPlaneDistances(), SetFrame(), SetPos(), SetFront(), SetPerspective(), SetOrthographic(), Up(). */
         void SetUp(const Vector3& up);
+        /// Makes this Frustum use a perspective projection formula with the given FOV parameters.
+        /** A Frustum that uses the perspective projection is shaped like a pyramid that is cut from the top, and has a
+            base with a rectangular area.
+            @note Calling this function recomputes the cached projection matrix of this Frustum.
+            @see SetKind(), SetViewPlaneDistances(), SetFrame(), SetPos(), SetFront(), SetUp(), SetOrthographic(), HorizontalFov(), VerticalFov(), SetHorizontalFovAndAspectRatio(), SetVerticalFovAndAspectRatio(). */
         void SetPerspective(float horizontalFov, float verticalFov);
+        /// Makes this Frustum use an orthographic projection formula with the given FOV parameters.
+        /** A Frustum that uses the orthographic projection is shaded like a cube (an OBB).
+            @note Calling this function recomputes the cached projection matrix of this Frustum.
+            @see SetKind(), SetViewPlaneDistances(), SetFrame(), SetPos(), SetFront(), SetUp(), SetOrthographic(), OrthographicWidth(), OrthographicHeight(). */
         void SetOrthographic(float orthographicWidth, float orthographicHeight);
+        /// Returns the handedness of the projection formula used by this Frustum.
+        /** @see SetKind(), FrustumHandedness. */
         FrustumHandedness Handedness() const { return handedness; }
+        /// Returns the type of the projection formula used by this Frustum.
+        /** @see SetPerspective(), SetOrthographic(), FrustumType. */
         FrustumType Type() const { return type; }
+        /// Returns the convention of the post-projective space used by this Frustum.
+        /** @see SetKind(), FrustumProjectiveSpace. */
         FrustumProjectiveSpace ProjectiveSpace() const { return projectiveSpace;}
+        /// Returns the world-space position of this Frustum.
+        /** @see SetPos(), Front(), Up(). */
         const Vector3 &Pos() const {return pos;}
+        /// Returns the world-space camera look-at direction of this Frustum.
+        /** @see Pos(), SetFront(), Up(). */
         const Vector3 &Front() const { return front; }
+        /// Returns the world-space camera up direction of this Frustum.
+        /** @see Pos(), Front(), SetUp(). */
         const Vector3 &Up() const { return up; }
+        /// Returns the distance from the Frustum eye to the near clip plane.
+        /** @see SetViewPlaneDistances(), FarPlaneDistance(). */
         float NearPlaneDistance() const { return nearPlaneDistance; }
+        /// Returns the distance from the Frustum eye to the far clip plane.
+        /** @see SetViewPlaneDistances(), NearPlaneDistance(). */
         float FarPlaneDistance() const { return farPlaneDistance;}
+        /// Returns the horizontal field-of-view used by this Frustum, in radians.
+        /** @note Calling this function when the Frustum is not set to use perspective projection will return values that are meaningless.
+            @see SetPerspective(), Type(), VerticalFov(). */
         float HorizontalFov() const { return horizontalFov;}
+        /// Returns the vertical field-of-view used by this Frustum, in radians.
+        /** @note Calling this function when the Frustum is not set to use perspective projection will return values that are meaningless.
+            @see SetPerspective(), Type(), HorizontalFov(). */
         float VerticalFov() const { return verticalFov;}
+        /// Returns the world-space width of this Frustum.
+        /** @note Calling this function when the Frustum is not set to use orthographic projection will return values that are meaningless.
+            @see SetOrthographic(), Type(), OrthographicHeight(). */
         float OrthographicWidth() const { return orthographicWidth; }
+        /// Returns the world-space height of this Frustum.
+        /** @note Calling this function when the Frustum is not set to use orthographic projection will return values that are meaningless.
+            @see SetOrthographic(), Type(), OrthographicWidth(). */
         float OrthograhpicHeight() const { return orthographicHeight; }
+        /// Returns the number of line segment edges that this Frustum is made up of, which is always 12.
+        /** This function is used in template-based algorithms to provide an unified API for iterating over the features of a Polyhedron. */
         int NumEdges() const { return 12; }
+        /// Returns the aspect ratio of the view rectangle on the near plane.
+        /** The aspect ratio is the ratio of the width of the viewing rectangle to its height. This can also be computed by
+            the expression horizontalFov / verticalFov. To produce a proper non-stretched image when rendering, this
+            aspect ratio should match the aspect ratio of the actual render target (e.g. 4:3, 16:9 or 16:10 in full screen mode).
+            @see horizontalFov, verticalFov. */
         float AspectRatio() const;
-
+        /// Makes this Frustum use a perspective projection formula with the given horizontal FOV parameter and aspect ratio.
+        /** Specifies the horizontal and vertical field-of-view values for this Frustum based on the given horizontal FOV
+            and the screen size aspect ratio.
+            @note Calling this function recomputes the cached projection matrix of this Frustum.
+            @see SetPerspective(), SetVerticalFovAndAspectRatio(). */
         void SetHorizontalFovAndAspectRatio(float horizontalFov, float aspectRatio);
-
+        /// Makes this Frustum use a perspective projection formula with the given vertical FOV parameter and aspect ratio.
+        /** Specifies the horizontal and vertical field-of-view values for this Frustum based on the given vertical FOV
+            and the screen size aspect ratio.
+            @note Calling this function recomputes the cached projection matrix of this Frustum.
+            @see SetPerspective(), SetHorizontalFovAndAspectRatio(). */
+        void SetVerticalFovAndAspectRatio(float verticalFov, float aspectRatio);
 
         Vector3 CornerPoint(int cornerIndex) const;
 
         Vector3 NearPlanePos(float x, float y) const;
         Vector3 FarPlanePos(float x, float y) const;
 
-        Vector3 WorldRight() const
-        {
-            if (handedness == FrustumHandedness::Right)
-                return Vector3::Cross(front, up);
-            else
-                return Vector3::Cross(up, front);
-        }
+        /// Computes the direction vector that points logically to the right-hand side of the Frustum.
+        /** This vector together with the member variables 'front' and 'up' form the orthonormal basis of the view frustum.
+            @see pos, front. */
+        Vector3 WorldRight() const;
 
-        Plane TopPlane() const;
-        Plane BottomPlane() const;
-        Plane RightPlane() const;
+        Plane TopPlane() const;     ///< [similarOverload: LeftPlane] [hideIndex]
+        Plane BottomPlane() const;  ///< [similarOverload: LeftPlane] [hideIndex]
+        Plane RightPlane() const;   ///< [similarOverload: LeftPlane] [hideIndex]
+        /// Returns the plane equation of the specified side of this Frustum.
+        /** The normal vector of the returned plane points outwards from the volume inside the frustum.
+            This means the negative half-space of the Frustum is the space inside the Frustum.
+            [indexTitle: Left/Right/Top/BottomPlane]
+            @see NearPlane(), FarPlane(), GetPlane(), GetPlanes(). */
         Plane LeftPlane() const;
+        /// Computes the plane equation of the far plane of this Frustum. [similarOverload: NearPlane]
+        /** The normal vector of the returned plane points outwards from the volume inside the frustum, i.e. away from the eye point.
+            (towards front). This means the negative half-space of the Frustum is the space inside the Frustum.
+            @see front, FarPlane(), LeftPlane(), RightPlane(), TopPlane(), BottomPlane(), GetPlane(), GetPlanes(). */
         Plane FarPlane() const;
+        /// Computes the plane equation of the near plane of this Frustum.
+        /** The normal vector of the returned plane points outwards from the volume inside the frustum, i.e. towards the eye point
+            (towards -front). This means the negative half-space of the Frustum is the space inside the Frustum.
+            @see front, FarPlane(), LeftPlane(), RightPlane(), TopPlane(), BottomPlane(), GetPlane(), GetPlanes(). */
         Plane NearPlane() const;
+        /// Computes the width of the near plane quad in world space units.
+        /** @see NearPlaneHeight(). */
         float NearPlaneWidth() const;
+        /// Computes the height of the near plane quad in world space units.
+        /** @see NearPlaneHeight(). */
         float NearPlaneHeight() const;
 
-
+        /// Moves this Frustum by the given offset vector.
+        /** @note This function operates in-place.
+            @param offset The world space offset to apply to the position of this Frustum.
+            @see Transform(). */
         void Translate(const Vector3& offset);
+        /// Applies a transformation to this Frustum.
+        /** @param transform The transformation to apply to this Frustum. This transformation must be
+         * affine, and must contain an orthogoal set of column vectors (may not contain shear or projection).
+         * The transformation can only contain uniform
+         * @see Translate(), Scale(), classes Matrix3x3, Matrix4x4, Quaternion
+         */
         void Transform(const Matrix3x3& transform);
         void Transform(const Matrix4x4& transform);
         void Transform(const Quaternion& transform);
 
-
+        /// Converts this Frustum to a Polyhedron.
+        /** This function returns a Polyhedron representation of this Frustum. This conversion is exact, meaning that the returned
+            Polyhedron represents exactly the same set of points that this Frustum does.
+            @see MinimalEnclosingAABB(), MinimalEnclosingOBB(). */
         Polyhedron ToPolyhedron() const;
 
+        /// Converts this Frustum to a PBVolume.
+        /** This function returns a plane-bounded volume representation of this Frustum. The conversion is exact, meaning that the
+            returned PBVolume<6> represents exactly the same set of points that this Frustum does.
+            @see ToPolyhedron(). */
+        //PBVolume<6> ToPBVolume() const;
+
+        /// Tests if the given object is fully contained inside this Frustum.
+        /** This function returns true if the given object lies inside this Frustum, and false otherwise.
+            @note The comparison is performed using less-or-equal, so the faces of this Frustum count as being inside, but
+                due to float inaccuracies, this cannot generally be relied upon.
+            @todo Add Contains(Circle/Disc/Sphere/Capsule).
+            @see Distance(), Intersects(), ClosestPoint(). */
+        bool Contains(const Vector3 &point) const;
+        bool Contains(const LineSegment &lineSegment) const;
+        bool Contains(const Triangle &triangle) const;
+        bool Contains(const Polygon &polygon) const;
+        bool Contains(const AABB &aabb) const;
+        bool Contains(const OBB &obb) const;
+        bool Contains(const Frustum &frustum) const;
+        bool Contains(const Polyhedron &polyhedron) const;
+
+        /// Computes the distance between this Frustum and the given object.
+        /** This function finds the nearest pair of points on this and the given object, and computes their distance.
+            If the two objects intersect, or one object is contained inside the other, the returned distance is zero.
+            @todo Add Frustum::Distance(Line/Ray/LineSegment/Plane/Triangle/Polygon/Circle/Disc/AABB/OBB/Capsule/Frustum/Polyhedron).
+            @see Contains(), Intersects(), ClosestPoint(). */
+        float Distance(const Vector3 &point) const;
+
+        /// Tests whether this Frustum and the given object intersect.
+        /** Both objects are treated as "solid", meaning that if one of the objects is fully contained inside
+            another, this function still returns true. (e.g. in case a line segment is contained inside this Frustum,
+            or this Frustum is contained inside a Sphere, etc.)
+            The first parameter of this function specifies the other object to test against.
+            @see Contains(), Distance(), ClosestPoint().
+            @todo Add Intersects(Circle/Disc). */
         bool Intersects(const Ray& ray) const;
         //bool Intersects(const Line& line) const;
         bool Intersects(const LineSegment& lineSegment) const;
@@ -229,7 +374,13 @@ namespace J3ML::Geometry
         bool Intersects(const Capsule& obb) const;
         bool Intersects(const Frustum& plane) const;
         bool Intersects(const Polyhedron& triangle) const;
-
+        /// Projects this Frustum onto the given 1D axis direction vector.
+        /** This function collapses this Frustum onto an 1D axis for the purposes of e.g. separate axis test computations.
+            The function returns a 1D range [outMin, outMax] denoting the interval of the projection.
+            @param direction The 1D axis to project to. This vector may be unnormalized, in which case the output
+                of this function gets scaled by the length of this vector.
+            @param outMin [out] Returns the minimum extent of this object along the projection axis.
+            @param outMax [out] Returns the maximum extent of this object along the projection axis. */
         void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
 
         void GetCornerPoints(Vector3 *outPointArray) const;
@@ -240,4 +391,8 @@ namespace J3ML::Geometry
 
         bool Intersects(const Line &line) const;
     };
+
+    Frustum operator * (const Matrix3x3& transform,  const Frustum& frustum);
+    Frustum operator * (const Matrix4x4& transform,  const Frustum& frustum);
+    Frustum operator * (const Quaternion& transform, const Frustum& frustum);
 }

include/J3ML/Geometry/KDTree.h

@@ -0,0 +1,12 @@
+#pragma once
+
+
+namespace J3ML::Geometry
+{
+
+    /// A KD-tree accelleration structure for static geometry.
+    class KdTree
+    {
+
+    };
+}

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/Geometry/Triangle.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "J3ML/LinearAlgebra/Vector3.h"
 #include <J3ML/Geometry/Common.h>
 #include <J3ML/LinearAlgebra.h>
 #include <cfloat>
@@ -13,11 +14,18 @@ namespace J3ML::Geometry
         Vector3 V1;
         Vector3 V2;
     public:
-
         float DistanceSq(const Vector3 &point) const;
 
+        /// Returns a new triangle, translated with a direction vector
+        Triangle Translated(const Vector3& translation) const;
+        /// Returns a new triangle, scaled from 3D factors
+        Triangle Scaled(const Vector3& scaled) const;
+
         bool Intersects(const AABB& aabb) const;
         bool Intersects(const Capsule& capsule) const;
+        bool Intersects(const Triangle& rhs) const;
+        friend bool Intersects(const Triangle& lhs, const Triangle &rhs);
+
         AABB BoundingAABB() const;
 
         /// Tests if the given object is fully contained inside this triangle.
@@ -29,6 +37,8 @@ namespace J3ML::Geometry
         bool Contains(const LineSegment& lineSeg, float triangleThickness = 1e-3f) const;
         bool Contains(const Triangle& triangle, float triangleThickness = 1e-3f) const;
 
+        /// Project the triangle onto an axis, and returns the min and max value with the axis as a unit
+        Interval ProjectionInterval(const Vector3& axis) const;
         void ProjectToAxis(const Vector3 &axis, float &dMin, float &dMax) const;
 
         /// Quickly returns an arbitrary point inside this Triangle. Used in GJK intersection test.
@@ -108,6 +118,8 @@ namespace J3ML::Geometry
 
         Plane PlaneCW() const;
 
+        Vector3 FaceNormal() const;
+
         Vector3 Vertex(int i) const;
 
         LineSegment Edge(int i) const;

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,11 +36,18 @@ 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
 {
 
     bool EqualAbs(float a, float b, float epsilon = 1e-3f);
     float RecipFast(float x);
+
+
     // Coming soon: Units Namespace
     // For Dimensional Analysis
     /*

include/J3ML/LinearAlgebra/AxisAngle.h

@@ -11,10 +11,13 @@ namespace J3ML::LinearAlgebra
     /// Transitional datatype, not useful for internal representation of rotation
     /// But has uses for conversion and manipulation.
     class AxisAngle {
+    public:
         Vector3 axis;
         float angle;
     public:
         AxisAngle();
+        explicit AxisAngle(const Quaternion& q);
+        explicit AxisAngle(const EulerAngle& e);
 
         AxisAngle(const Vector3 &axis, float angle);
 

include/J3ML/LinearAlgebra/EulerAngle.h

@@ -19,8 +19,8 @@ public:
     AxisAngle ToAxisAngle() const;
 
 
-    explicit EulerAngle(const Quaternion& orientation);
-    explicit EulerAngle(const AxisAngle& orientation);
+    explicit EulerAngle(const Quaternion& rhs);
+    explicit EulerAngle(const AxisAngle& rhs);
 
     /// TODO: Implement separate upper and lower bounds
     /// Preserves internal value of euler angles, normalizes and clamps the output.

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,11 +11,119 @@
 
 namespace J3ML::LinearAlgebra {
 
-    /// 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.
-     * The elements of this matrix are
+    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.
+    /* The elements of this matrix are
      *  m_00, m_01, m_02, m_03
      *  m_10, m_11, m_12, m_13
      *  m_20, m_21, m_22, m_23,
@@ -25,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);
@@ -66,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.
@@ -103,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,
@@ -112,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);
@@ -227,9 +419,19 @@ namespace J3ML::LinearAlgebra {
         static Matrix4x4 D3DPerspProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
         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);
+        /// 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);
+
+        /// Computes a left-handed perspective projection matrix for OpenGL.
+        /// @note Use the M*v multiplication order to project points with this matrix.
         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.
         static Matrix4x4 OpenGLPerspProjRH(float n, float f, float h, float v);
 
         Vector4 operator[](int row);
@@ -274,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,13 +35,16 @@ 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);
+        explicit Quaternion(const AxisAngle& angle);
 
         void SetFromAxisAngle(const Vector3 &vector3, float between);
 
         void SetFromAxisAngle(const Vector4 &vector4, float between);
+        void SetFrom(const AxisAngle& angle);
 
         Quaternion Inverse() const;
 
@@ -58,6 +61,8 @@ namespace J3ML::LinearAlgebra
 
         float GetAngle() const;
 
+        EulerAngle ToEulerAngle() const;
+
 
         Matrix3x3 ToMatrix3x3() const;
 

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

@@ -8,27 +8,75 @@
 
 namespace J3ML::LinearAlgebra {
 
-// A 3D (x, y, z) ordered pair.
+/// A 3D (x, y, z) ordered pair.
 class Vector3 {
 public:
-
-
     float x = 0;
     float y = 0;
     float z = 0;
 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

@@ -197,12 +197,12 @@ namespace J3ML::Geometry {
         return aabb;
     }
 
-    float AABB::GetVolume() const {
+    float AABB::Volume() const {
         Vector3 sz = Size();
         return sz.x * sz.y * sz.z;
     }
 
-    float AABB::GetSurfaceArea() const {
+    float AABB::SurfaceArea() const {
         Vector3 size = Size();
         return 2.f * (size.x*size.y + size.x*size.z + size.y*size.z);
     }
@@ -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)
@@ -454,6 +456,51 @@ namespace J3ML::Geometry {
 #endif
     }
 
+    bool AABB::Intersects(const AABB& aabb) const {
+        return Intersection(aabb).has_value();
+    }
+
+    std::optional<AABB> AABB::Intersection(const AABB& rhs) const {
+        // Here we do SAT, except that due to both objects being AABBs, they are "already projected" onto the same axis
+        constexpr auto test = [](float a, float b, float c, float d) -> std::optional<Vector2> {
+            // Overlap Test
+            // Points go:
+            //       +-------------+
+            // +-----|-----+       |
+            // |  1  |     |     2 |
+            // |     +-----|-------+
+            // +-----------+
+            //
+            // A-----C-----B-------D
+            //
+            // IF A < C AND B > C ( Overlap in order object 1 -> object 2)
+            // IF C < A AND D > A ( Overlap in order object 2 -> object 1)
+            if (a < c && b > c) {
+                return Vector2{c, b};
+            }
+            if (c < a && d > a) {
+                return Vector2{a, d};
+            }
+            return std::nullopt;
+        };
+
+        // This is SAT, so we need all axes to collide
+        std::optional<Vector2> xCollision = test(MinX(), MaxX(), rhs.MinX(), rhs.MaxX());
+        if (!xCollision.has_value()) return std::nullopt;
+
+        std::optional<Vector2> yCollision = test(MinY(), MaxY(), rhs.MinY(), rhs.MaxY());
+        if (!yCollision.has_value()) return std::nullopt;
+
+        std::optional<Vector2> zCollision = test(MinZ(), MaxZ(), rhs.MinZ(), rhs.MaxZ());
+        if (!zCollision.has_value()) return std::nullopt;
+
+        // At this point all 3 optionals have a value ; x of each is the "min" value, y of each is the "max" value
+        return AABB{
+            Vector3{xCollision->x, yCollision->x, zCollision->x},
+            Vector3{xCollision->y, yCollision->y, zCollision->y}
+        };
+    }
+
     bool AABB::IntersectLineAABB_CPP(const Vector3 &linePos, const Vector3 &lineDir, float &tNear, float &tFar) const
     {
         assert(lineDir.IsNormalized());
@@ -552,20 +599,37 @@ namespace J3ML::Geometry {
         return AABB(minPoint+offset, maxPoint+offset);
     }
 
-    AABB AABB::TransformAABB(const Matrix3x3 &transform) {
+    void AABB::Scale(const Vector3 &scale) {
+        minPoint.x *= scale.x;
+        minPoint.y *= scale.y;
+        minPoint.z *= scale.z;
+        maxPoint.x *= scale.x;
+        maxPoint.y *= scale.y;
+        maxPoint.z *= scale.z;
+    }
+
+    AABB AABB::Scaled(const Vector3 &scale) const {
+        return AABB(
+            Vector3(minPoint.x*scale.y, minPoint.y*scale.y, minPoint.z*scale.z),
+            Vector3(maxPoint.x*scale.y, maxPoint.y*scale.y, maxPoint.z*scale.z)
+        );
+    }
+
+    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/Common.cpp

@@ -0,0 +1,10 @@
+#include <J3ML/Geometry/Common.h>
+
+namespace J3ML::Geometry {
+
+    bool Interval::Intersects(const Interval& rhs) const {
+        return *this == rhs || this->min > rhs.max != this->max >= rhs.min;
+    }
+
+}
+

src/J3ML/Geometry/Frustum.cpp

@@ -293,4 +293,25 @@ namespace J3ML::Geometry
     {
         return this->ToPolyhedron().Intersects(polyhedron);
     }
+
+    Frustum::Frustum()
+            : type(FrustumType::Invalid),
+              pos(Vector3::NaN),
+              front(Vector3::NaN),
+              up(Vector3::NaN),
+              nearPlaneDistance(NAN),
+              farPlaneDistance(NAN),
+              worldMatrix(Matrix4x4::NaN),
+              viewProjectionMatrix(Matrix4x4::NaN)
+    {
+        // For conveniency, allow automatic initialization of the graphics API and handedness in use.
+        // If neither of the #defines are set, user must specify per-instance.
+    }
+
+    Vector3 Frustum::WorldRight() const {
+        if (handedness == FrustumHandedness::Right)
+            return Vector3::Cross(front, up);
+        else
+            return Vector3::Cross(up, front);
+    }
 }

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/Geometry/Triangle.cpp

@@ -5,9 +5,42 @@
 #include <J3ML/Geometry/Line.h>
 #include <J3ML/Geometry/Capsule.h>
 
-
 namespace J3ML::Geometry
 {
+    Interval Triangle::ProjectionInterval(const Vector3& axis) const {
+        // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter4/generic_sat.html
+        float min = axis.Dot(V0);
+        float max = min;
+
+        float value = axis.Dot(V1);
+        if (value < min)
+            min = value;
+        if (value > max)
+            max = value;
+
+        value = axis.Dot(V2);
+        if (value < min)
+            min = value;
+        if (value > max)
+            max = value;
+        return Interval{min, max};
+    }
+
+    Triangle Triangle::Translated(const Vector3& translation) const {
+        return {
+            V0 + translation,
+            V1 + translation,
+            V2 + translation
+        };
+    }
+
+    Triangle Triangle::Scaled(const Vector3& scale) const {
+        return {
+            {V0.x * scale.x, V0.y * scale.y, V0.z * scale.y},
+            {V1.x * scale.x, V1.y * scale.y, V1.z * scale.y},
+            {V2.x * scale.x, V2.y * scale.y, V2.z * scale.y},
+        };
+    }
 
     LineSegment Triangle::Edge(int i) const
     {
@@ -37,6 +70,13 @@ namespace J3ML::Geometry
             return Vector3::NaN;
     }
 
+    Vector3 Triangle::FaceNormal() const {
+        Vector3 edge1 = V1 - V0;
+        Vector3 edge2 = V2 - V1;
+
+        return edge1.Cross(edge2);
+    }
+
     Plane Triangle::PlaneCCW() const
     {
         return Plane(V0, V1, V2);
@@ -351,6 +391,78 @@ namespace J3ML::Geometry
         return capsule.Intersects(*this);
     }
 
+    namespace {
+        bool HaveSeparatingAxis(const Triangle& t1, const Triangle& t2, const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) {
+            // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter4/robust_sat.html
+            Vector3 ab = (a - b);
+            Vector3 axis = ab.Cross(c - d);
+
+            if (axis.IsZero()) {
+                // Axis is zero, they are parallel, try to find the vector orthogonal to both
+                Vector3 n = ab.Cross(c - a);
+
+                if (n.IsZero()) {
+                    // AB and AC are parallel, this means they are both on the same axis, just pick one
+                    axis = ab;
+                } else {
+                    // Parallel but not on the same axis, get the vector that is normal to both edges
+                    axis = ab.Cross(n);
+                }
+            }
+
+            return !t1.ProjectionInterval(axis).Intersects(t2.ProjectionInterval(axis));
+        }
+    }
+
+    bool Intersects(const Triangle& lhs, const Triangle& rhs) {
+        // Triangle v Triangle intersection check using SAT
+        // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter4/generic_sat.html
+        // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter4/triangle-triangle.html
+        // Reminder, we only need to find *one* axis to disprove that they collide, the corrolary is that we need to check ALL axes to prove that they collide
+        
+        Vector3 lEdges[3] = {
+            lhs.V1 - lhs.V0,
+            lhs.V2 - lhs.V1,
+            lhs.V0 - lhs.V2,
+        };
+
+        // First, use lhs's face normal as the separating axis
+        if (HaveSeparatingAxis(lhs, rhs, lhs.V1, lhs.V0, lhs.V2, lhs.V1)) {
+            return false;
+        }
+
+        Vector3 rEdges[3] = {
+            rhs.V1 - rhs.V0,
+            rhs.V2 - rhs.V1,
+            rhs.V0 - rhs.V2,
+        };
+
+        // Second, use rhs's face normal as the separating axis
+        if (HaveSeparatingAxis(lhs, rhs, rhs.V1, rhs.V0, rhs.V2, rhs.V1)) {
+            return false;
+        }
+
+        // Third, check the normals of each edge against each other
+        if (HaveSeparatingAxis(lhs, rhs, lhs.V1, lhs.V0, rhs.V1, rhs.V0) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V1, lhs.V0, rhs.V2, rhs.V1) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V1, lhs.V0, rhs.V0, rhs.V2) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V2, lhs.V1, rhs.V1, rhs.V0) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V2, lhs.V1, rhs.V2, rhs.V1) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V2, lhs.V1, rhs.V0, rhs.V2) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V0, lhs.V2, rhs.V1, rhs.V0) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V0, lhs.V2, rhs.V2, rhs.V1) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V0, lhs.V2, rhs.V0, rhs.V2)) {
+            return false;
+        }
+
+        // No axis is separating, we can safely conclude they intersect
+        return true;
+    }
+
+    bool Triangle::Intersects(const Triangle& rhs) const {
+        return Geometry::Intersects(*this, rhs);
+    }
+
     Vector3 Triangle::ExtremePoint(const Vector3 &direction) const {
         Vector3 mostExtreme = Vector3::NaN;
         float mostExtremeDist = -FLT_MAX;

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/AxisAngle.cpp

@@ -14,4 +14,48 @@ namespace J3ML::LinearAlgebra {
                 std::cos(angle/2)
         };
     }
+
+    AxisAngle::AxisAngle(const Quaternion &q) {
+        auto theta = std::acos(q.w) * 2.f;
+        auto ax = q.x / std::sin(std::acos(theta));
+        auto ay = q.y / std::sin(std::acos(theta));
+        auto az = q.z / std::sin(std::acos(theta));
+    }
+
+    AxisAngle::AxisAngle(const EulerAngle &e) {
+
+        // Assuming the angles are in radians
+
+        float heading = e.pitch;
+        float attitude = e.yaw;
+        float bank = e.roll;
+
+        float c1 = std::cos(heading / 2.f);
+        float s1 = std::sin(heading / 2.f);
+        float c2 = std::cos(attitude / 2.f);
+        float s2 = std::sin(attitude / 2.f);
+        float c3 = std::cos(bank / 2.f);
+        float s3 = std::sin(bank / 2.f);
+
+        float w = c1*c2*c3 - s1*s2*s3;
+        float x = c1*c2*c3 + s1*s2*s3;
+        float y = s1*c2*c3 + c1*s2*s3;
+        float z = c1*s2*c3 - s1*c2*s3;
+
+        angle = 2.f * std::acos(w);
+
+        double norm = x*x + y*y + z*z;
+        if (norm < 0.001) { // when all euler angles are zero angle=0, so
+            // we can set axis to anything to avoid divide by zero
+            x = 1;
+            y = z = 0;
+        } else {
+            norm = std::sqrt(norm);
+            x /= norm;
+            y /= norm;
+            z /= norm;
+        }
+
+        axis = {x, y, z};
+    }
 }

src/J3ML/LinearAlgebra/EulerAngle.cpp

@@ -48,4 +48,64 @@ namespace J3ML::LinearAlgebra {
     }
 
     EulerAngle::EulerAngle() : pitch(0), yaw(0), roll(0) {}
+
+    EulerAngle::EulerAngle(const AxisAngle &rhs) {
+
+        float x = rhs.axis.x;
+        float y = rhs.axis.y;
+        float z = rhs.axis.z;
+        float angle = rhs.angle;
+
+        double s = std::sin(rhs.angle);
+
+        double c = std::cos(rhs.angle);
+
+        double t = 1-c;
+
+        // if axis is not already normalized then uncomment this
+
+        // double magnitude = std::sqrt(x*x + y*y + z*z);
+        // if (magnitude == 0) throw error;
+        // x /= magnitude;
+        // y /= magnitude;
+        // z /= magnitude;
+
+        if ((x*y*t + z*s) > 0.998) { // North pole singularity detected
+            pitch = 2 * std::atan2(x * std::sin(angle/2.f), std::cos(angle/2.f));
+            yaw = M_PI / 2.f;
+            roll = 0;
+            return;
+        }
+
+        if ((x*y*t + z*s) < -0.998) { // South pole singularity detected
+            pitch = -2 * std::atan2(x * std::sin(angle/2.f), std::cos(angle/2.f));
+            yaw = -M_PI / 2.f;
+            roll = 0;
+            return;
+        }
+
+        pitch = std::atan2(y * s-x * z * t, 1 - (y*y + z*z) * t);
+        yaw = std::asin(x * y * t + z * s);
+        roll = std::atan2(x * s - y * z * t, 1 - (x*x + z*z) * t);
+    }
+
+    EulerAngle::EulerAngle(const Quaternion &rhs) {
+        double test = rhs.x * rhs.y + rhs.z * rhs.w;
+        if (test > 0.499) { // Singularity at north pole
+            pitch = 2 * std::atan2(rhs.x, rhs.w);
+            yaw = M_PI / 2.f;
+            roll = 0;
+            return;
+        }
+
+        if (test < -0.499) { // Singularity at south pole
+            pitch = -2 * std::atan2(rhs.x, rhs.y);
+            yaw = - M_PI / 2.f;
+            roll = 0;
+            return;
+        }
+        float sqx = rhs.x * rhs.x;
+        float sqy = rhs.y * rhs.y;
+        float sqz = rhs.z * rhs.z;
+    }
 }

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 = {
@@ -201,4 +201,39 @@ namespace J3ML::LinearAlgebra {
     Quaternion::Quaternion(const Vector4 &rotationAxis, float rotationAngleBetween) {
         SetFromAxisAngle(rotationAxis, rotationAngleBetween);
     }
+
+    Quaternion::Quaternion(const AxisAngle &angle) {
+        double s = std::sin(angle.angle / 2);
+        x = angle.axis.x * s;
+        y = angle.axis.y * s;
+        z = angle.axis.z * s;
+        w = std::cos(angle.angle / 2);
+    }
+
+    Quaternion::Quaternion(const EulerAngle &angle) {
+        // Abbreviations for the various angular functions
+        double cr = std::cos(angle.roll * 0.5);
+        double sr = std::sin(angle.roll * 0.5);
+        double cp = std::cos(angle.pitch * 0.5);
+        double sp = std::sin(angle.pitch * 0.5);
+        double cy = std::cos(angle.yaw * 0.5);
+        double sy = std::sin(angle.yaw * 0.5);
+
+        w = cr * cp * cy + sr * sp * sy;
+        x = sr * cp * cy - cr * sp * sy;
+        y = cr * sp * cy + sr * cp * sy;
+        z = cr * cp * sy - sr * sp * cy;
+    }
+
+    void Quaternion::SetFrom(const AxisAngle &angle) {
+        double s = std::sin(angle.angle / 2);
+        x = angle.axis.x * s;
+        y = angle.axis.y * s;
+        z = angle.axis.z * s;
+        w = std::cos(angle.angle / 2);
+    }
+
+    EulerAngle Quaternion::ToEulerAngle() const {
+        return EulerAngle(*this);
+    }
 }

src/J3ML/LinearAlgebra/Vector2.cpp

@@ -86,10 +86,7 @@ namespace J3ML::LinearAlgebra {
 
     float Vector2::Dot(const Vector2& rhs) const
     {
-        auto a = this->Normalize();
-        auto b = rhs.Normalize();
-
-        return a.x * b.x + a.y * b.y;
+        return this->x * rhs.x + this->y * rhs.y;
     }
 
     Vector2 Vector2::Project(const Vector2& rhs) const

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
@@ -154,11 +158,9 @@ namespace J3ML::LinearAlgebra {
 
     float Vector3::Dot(const Vector3& rhs) const
     {
-        auto a = this->Normalize();
-        auto b = rhs.Normalize();
-        return a.x * b.x +
-               a.y * b.y +
-               a.z * b.z;
+        return x * rhs.x +
+               y * rhs.y +
+               z * rhs.z;
     }
 
     Vector3 Vector3::Project(const Vector3& rhs) const
@@ -439,6 +441,7 @@ namespace J3ML::LinearAlgebra {
         };
     }
 
+    // TODO: Implement
     Vector3 Vector3::Perpendicular(const Vector3 &hint, const Vector3 &hint2) const {
         assert(!this->IsZero());
         assert(hint.IsNormalized());
@@ -446,6 +449,8 @@ namespace J3ML::LinearAlgebra {
         Vector3 v = this->Cross(hint);
         float len = v.TryNormalize();
 
+
+        return Vector3::Zero;
     }
 
     float Vector3::TryNormalize() {

tests/Geometry/Geometry.cpp

@@ -0,0 +1,51 @@
+#include <gtest/gtest.h>
+#include <J3ML/Geometry/Common.h>
+
+using J3ML::Geometry::Interval;
+
+TEST(CommonGeometry, Interval_Intersect) {
+    // <- a ->
+    //             <- b ->
+    EXPECT_EQ((Interval{0, 1}.Intersects({2, 3})), false);
+
+    //             <- a ->
+    // <- b ->
+    EXPECT_EQ((Interval{2, 3}.Intersects({0, 1})), false);
+
+    // <- a ->
+    //    <- b ->
+    EXPECT_EQ((Interval{2, 4}.Intersects({3, 5})), true);
+
+    //    <- a ->
+    // <- b ->
+    EXPECT_EQ((Interval{2, 4}.Intersects({1, 3})), true);
+
+    // <- a ->
+    //      <- b ->
+    EXPECT_EQ((Interval{2, 3}.Intersects({3, 5})), true);
+
+    //      <- a ->
+    // <- b ->
+    EXPECT_EQ((Interval{3, 5}.Intersects({2, 3})), true);
+
+    // <- a ->
+    // <- b     ->
+    EXPECT_EQ((Interval{2, 3}.Intersects({2, 5})), true);\
+
+    // <- a ->
+    // <- b ->
+    EXPECT_EQ((Interval{2, 3}.Intersects({2, 3})), true);
+
+    // . a
+    // . b
+    EXPECT_EQ((Interval{2, 2}.Intersects({2, 2})), true);
+
+    // <-    a    ->
+    //    <- b ->
+    EXPECT_EQ((Interval{2, 5}.Intersects({3, 4})), true);
+
+    //    <- a ->
+    // <-    b    ->
+    EXPECT_EQ((Interval{3, 4}.Intersects({2, 5})), true);
+}
+

tests/Geometry/TriangleTests.cpp

@@ -0,0 +1,89 @@
+#include <gtest/gtest.h>
+#include <J3ML/Geometry/Triangle.h>
+
+using J3ML::Geometry::Interval;
+using J3ML::Geometry::Triangle;
+
+TEST(TriangleTests, FaceNormal)
+{
+    Triangle t{
+        Vector3{-1, -1, -1},
+        Vector3{0,   1,  0},
+        Vector3{1,  -1,  1}
+    };
+
+    EXPECT_EQ(t.FaceNormal(), (Vector3{4, 0, -4}));
+}
+
+TEST(TriangleTests, IntersectTriangle)
+{
+    Triangle xyTriangle{
+        {0.0f, 0.0f, 0.0f},
+        {1.0f, 1.0f, 0.0f},
+        {2.0f, 0.0f, 0.0f}
+    };
+
+    // Triangle collides with itself
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle), true);
+    // Translate 1 towards x -- should collide
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(1.0f, 0.0f, 0.0f))), true);
+    // Translate 2 towards x -- should collide exactly on V1
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(2.0f, 0.0f, 0.0f))), true);
+    // Translate 2 towards negative x -- should collide exactly on V0
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(-2.0f, 0.0f, 0.0f))), true);
+    // Translate 3 towards x -- should not collide
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(3.0f, 0.0f, 0.0f))), false);
+    // Translate 3 towards negative x -- should not collide
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(-3.0f, 0.0f, 0.0f))), false);
+    // Translate 1 towards z -- should not collide
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(0.0f, 0.0f, 1.0f))), false);
+    // Triangle collides with contained smaller triangle
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Scaled(Vector3(0.5f, 0.5f, 0.5f)).Translated(Vector3(0.25f, 0.25f, 0.0f))), true);
+
+    Triangle zxTriangle {
+        {0.0f, 0.0f, 0.0f},
+        {1.0f, 0.0f, 1.0f},
+        {0.0f, 0.0f, 2.0f}
+    };
+
+    // Should collide exactly on V0
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle), true);
+    // Should collide across xyTriangle's edge and  zxTriangle's face
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(0.0f, 0.0f, -1.0))), true);
+    // Should collide exactly on V1
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(0.0f, 0.0f, -2.0))), true);
+    // xyTriangle's face should be poked by zxTriangle's V0
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(1.0f, 1.0f, 0.0f))), true);
+    // xyTriangle's face should be cut by zxTriangle
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(1.0f, 1.0f, -0.5f))), true);
+    // Should not collide
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(1.0f, 1.0f, 1.0f))), false);
+    // Should not collide
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(0.0f, 0.0f, -3.0f))), false);
+
+    Triangle yxTriangle{
+        {0.0f, 0.0f, 0.0f},
+        {1.0f, 1.0f, 0.0f},
+        {0.0f, 2.0f, 0.0f}
+    };
+
+    // Should collide on V0-V1 edge
+    EXPECT_EQ(Intersects(yxTriangle, yxTriangle), true);
+    // Should not collide
+    EXPECT_EQ(Intersects(xyTriangle, yxTriangle.Translated(Vector3(0.0f, 1.0f, 0.0f))), false);
+    // Should not collide
+    EXPECT_EQ(Intersects(yxTriangle, yxTriangle.Translated(Vector3(0.0f, 0.0f, 1.0f))), false);
+
+    Triangle zyInvertedTriangle{
+        {0.0f, 1.0f, -1.0f},
+        {0.0f, 0.0f, 0.0f},
+        {0.0f, 1.0f, 1.0f}
+    };
+    // Should collide exactly on V1
+    EXPECT_EQ(Intersects(xyTriangle, zyInvertedTriangle), true);
+    // Should not collide
+    EXPECT_EQ(Intersects(xyTriangle, zyInvertedTriangle.Translated(Vector3(0.0f, 1.0f, 0.0f))), false);
+    // Should not collide
+    EXPECT_EQ(Intersects(xyTriangle, zyInvertedTriangle.Translated(Vector3(0.25f, 0.75f, 0.0f))), false);
+}
+

tests/LinearAlgebra/Vector2Tests.cpp

@@ -139,15 +139,15 @@ TEST(Vector2Test, V2_DotProduct)
     // TODO: Equality
     Vector2 A {2, 2};
     Vector2 B {1, 1};
-    EXPECT_FLOAT_EQ(A.Dot(B), 1.f);
+    EXPECT_FLOAT_EQ(A.Dot(B), 4.f);
 }
 
 TEST(Vector2Test, V2_Project)
 {
-    Vector2 Base {1, 1};
-    Vector2 Projected {1, 1};
+    Vector2 Base {4, 4};
+    Vector2 Projected {1, 0};
 
-    Vector2 ExpectedResult {0.5, 0.5};
+    Vector2 ExpectedResult {4, 0};
 
     EXPECT_EQ(Base.Project(Projected), ExpectedResult);
 }

tests/LinearAlgebra/Vector3Tests.cpp

@@ -152,7 +152,7 @@ TEST(Vector3Test, V3_DotProduct) {
     Vector3 B{1,1,1};
 
 
-    float ExpectedResult = 1;
+    float ExpectedResult = 18;
 
     EXPECT_FLOAT_EQ(A.Dot(B), ExpectedResult);
 }