Update Vector2.cpp

change == and != to not check epsilon. ~= should check epsilon.

.gitea/workflows/buildtest.yml

@@ -0,0 +1,24 @@
+name: Run ReCI Build Test
+run-name: Run ReCI Build Test For ${{ gitea.repository }}.
+on: [push]
+
+jobs:
+  Explore-Gitea-Actions:
+    runs-on: ubuntu-22.04
+    env:
+      RECI_GIT: https://git.redacted.cc/maxine/ReCI
+      RECI: /RECI
+      JTEST_EXEC: ./cmake-build-debug/tests/J3MLTestSuite
+    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 run your tests on the runner."
+      - run: echo "Install toolchain and run ReCI build test"
+      - run: apt-get update && apt-get install -y lua5.3 git && git clone $RECI_GIT $RECI
+      - run: lua $RECI/reci.lua -f $RECI/scripts/buildtools.reci -f $RECI/scripts/buildtest.reci -r "JTEST_EXEC=$JTEST_EXEC"
+      - run: echo this only exists so I can rerun the action3
+      - run: echo "This job's status is ${{ job.status }}."

.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 }}."

.gitignore

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

CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.18)
+cmake_minimum_required(VERSION 3.18...3.27)
 PROJECT(J3ML
         VERSION 1.1
         LANGUAGES CXX
@@ -10,62 +10,41 @@ 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")
 
 # Enable Package Managers
 include(cmake/CPM.cmake)
-#include(cmake/gtest.cmake)
 
-include(CTest)
 
 file(GLOB_RECURSE J3ML_HEADERS "include/J3ML/*.h" "include/J3ML/*.hpp")
 file(GLOB_RECURSE J3ML_SRC     "src/J3ML/*.c" "src/J3ML/*.cpp")
 
 include_directories("include")
 
-add_library(J3ML SHARED ${J3ML_SRC}
-        src/J3ML/LinearAlgebra/AxisAngle.cpp
-        include/J3ML/LinearAlgebra/Vector.h
-        include/J3ML/Geometry/Plane.h
-        include/J3ML/Geometry/AABB.h
-        include/J3ML/Geometry/Frustum.h
-        include/J3ML/Geometry/OBB.h
-        include/J3ML/Geometry/Capsule.h
-        include/J3ML/Geometry/Sphere.h
-        include/J3ML/Geometry/Ray.h
-        include/J3ML/Geometry/QuadTree.h
-        include/J3ML/Geometry/LineSegment.h
-        include/J3ML/Geometry/TriangleMesh.h
-        include/J3ML/Geometry/Polygon.h
-        include/J3ML/Geometry/Triangle.h
-        include/J3ML/Geometry/Triangle2D.h
-        src/J3ML/Geometry/AABB.cpp
-        src/J3ML/Geometry/Plane.cpp
-        src/J3ML/Geometry/Sphere.cpp
-        src/J3ML/Geometry/Frustum.cpp
-        src/J3ML/Geometry/OBB.cpp
-        src/J3ML/Geometry/Ray.cpp
-        src/J3ML/Geometry/Capsule.cpp
-        src/J3ML/Geometry/TriangleMesh.cpp
-        src/J3ML/Geometry/QuadTree.cpp
-        src/J3ML/Geometry/LineSegment.cpp
-        include/J3ML/Geometry/AABB2D.h
-        src/J3ML/Geometry/Polygon.cpp
-        include/J3ML/Geometry/Polyhedron.h
-        src/J3ML/Geometry/Polyhedron.cpp
-        include/J3ML/Algorithm/RNG.h
-        src/J3ML/Algorithm/RNG.cpp
-        include/J3ML/Algorithm/Spring.h
-        include/J3ML/Algorithm/DifferentialSolvers.h
-        include/J3ML/Units.h
-        src/J3ML/J3ML.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)
 
+CPMAddPackage(
+        NAME jtest
+        URL https://git.redacted.cc/josh/jtest/archive/Release-1.5.zip
+)
+
+target_include_directories(J3ML PUBLIC ${jtest_SOURCE_DIR}/include)
+
+target_link_libraries(J3ML PUBLIC jtest)
+
+if(WIN32)
+    #target_compile_options(J3ML PRIVATE -Wno-multichar)
+endif()
+
 install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME})
 
 install(FILES ${J3ML_HEADERS} DESTINATION include/${PROJECT_NAME})
@@ -73,4 +52,10 @@ 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,63 @@
 # Josh's 3D Math Library - J3ML
 
+![J3ML Logo](logo_light_small.png)
+
 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++" 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 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.
+
+bump :3 :3 :3 :3 :3 :3 :3 :3 :3 :3 :3 :3 :3 :3 :3 :3 :3

cmake/gtest.cmake

@@ -1,68 +0,0 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-
-#-----------------------------------------------------------------------
-# CPM configuration
-#-----------------------------------------------------------------------
-set(CPM_MODULE_NAME google_test)
-set(CPM_LIB_TARGET_NAME ${CPM_MODULE_NAME})
-
-if ((DEFINED CPM_DIR) AND (DEFINED CPM_UNIQUE_ID) AND (DEFINED CPM_TARGET_NAME))
-    set(CPM_LIB_TARGET_NAME ${CPM_TARGET_NAME})
-    set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CPM_DIR})
-    include(CPM)
-else()
-    set(CPM_DIR "${CMAKE_CURRENT_BINARY_DIR}/cpm-packages" CACHE TYPE STRING)
-    find_package(Git)
-    if(NOT GIT_FOUND)
-        message(FATAL_ERROR "CPM requires Git.")
-    endif()
-    if (NOT EXISTS ${CPM_DIR}/CPM.cmake)
-        execute_process(
-                COMMAND "${GIT_EXECUTABLE}" clone https://github.com/iauns/cpm ${CPM_DIR}
-                RESULT_VARIABLE error_code
-                OUTPUT_VARIABLE head_sha)
-        if(error_code)
-            message(FATAL_ERROR "CPM failed to get the hash for HEAD")
-        endif()
-    endif()
-    include(${CPM_DIR}/CPM.cmake)
-endif()
-
-# All externals *must* define this.
-CPM_ForceOnlyOneModuleVersion()
-
-CPM_InitModule(${CPM_MODULE_NAME})
-
-#------------------------------------------------------------------------------
-# Google Test
-#------------------------------------------------------------------------------
-# Google test as an external project is a bad idea! You really should add it
-# as a subdirectory so that it can capture your compiler flags
-set(GOOGLE_TEST_DIR ${CMAKE_CURRENT_SOURCE_DIR}/3rdParty/gtest)
-CPM_EnsureRepoIsCurrent(
-        TARGET_DIR ${GOOGLE_TEST_DIR}
-        SVN_REPOSITORY "http://googletest.googlecode.com/svn/trunk"
-        SVN_REVISION "664"
-        USE_CACHING TRUE
-)
-
-# Compiler flags for MSVC 2010
-if (MSVC_VERSION EQUAL 1600)
-    add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0)
-    CPM_ExportAdditionalDefinition("-DGTEST_USE_OWN_TR1_TUPLE=0")
-endif()
-
-# Compiler flags for MSVC 2012
-if(MSVC_VERSION EQUAL 1700)
-    add_definitions(-D_VARIADIC_MAX=10)
-endif()
-
-# For All versions of gtest, we force shared CRT.
-set(gtest_force_shared_crt ON)
-set(gtest_force_shared_crt ON)
-
-# Add gtest now that we have the appropriate flags set.
-add_subdirectory(${GOOGLE_TEST_DIR})
-
-CPM_ExportAdditionalIncludeDir("${GOOGLE_TEST_DIR}/include")
-CPM_ExportAdditionalLibraryTarget("gtest")

include/J3ML/Algorithm/Bezier.hpp

@@ -0,0 +1,89 @@
+/// @file       Bezier.hpp
+/// @desc       Cubic Bezier Curve impl. using De Casteljau's Method
+/// @author     Joshua O'Leary
+/// @edited     20 June 2024
+/// @version    2
+/// @copyright  (c) Redacted Software 2024
+/// @license    Unlicense - Public Domain
+
+#pragma once
+
+// Bezier Method:
+// p = (1-t)^3 * P0 + 3*t*(1-t)^2*P1 + 3*t^2*(1-t)*P2 + t^3*P3;
+
+// For cubics:
+// p = (1-t)^2 * P0 + 2*(1-t)*t*P1 + t*t*P2;
+
+// Transcribed from here: explicit form and derivative
+// https://en.wikipedia.org/wiki/B%C3%A9zier_curve#Cubic_B%C3%A9zier_curves
+
+#include <J3ML/LinearAlgebra/Vector2.hpp>
+
+namespace J3ML::Algorithm
+{
+    using namespace J3ML::LinearAlgebra;
+
+    template <typename T>
+    inline T Square(T f) { return f * f; }
+
+    template <typename T>
+    inline T Cube(T f) { return f * f * f; }
+
+    /// Four points P0, P1, P2, P3 in the plane space define a cubic Bezier curve.
+    /// The curve can be modeled as a polynomial of third order.
+    /// The curve starts at P0, going toward P1, and arrives at P3 coming from the direction of P2.
+    /// Usually, it will not pass through P1, or P2, these points are only there to provide directional information.
+    template <typename T>
+    inline T Bezier(float t, const T& p0, const T& p1, const T& p2, const T& p3)
+    {
+        return Cube(1 - t) * p0 + 3 * Square(1 - t) * t * p1 + 3 * (1 - t) * Square(t) * p2 + Cube(t) * p3;
+    }
+
+    /// Computes a point along a 2-dimensional Cubic Bezier curve.
+    /// @param t The normalized distance along the curve to compute, with range of [0, 1].
+    /// @param p0 The start-point of the curve.
+    /// @param p1
+    /// @param p2
+    /// @param p3 The end-point of the curve.
+    Vector2 Bezier(float t, const Vector2& p0, const Vector2& p1, const Vector2& p2, const Vector2& p3);
+
+    /// Computes a point along the tangent of a 2-dimensional Cubic Bezier Curve.
+    /// @param t The normalized distance along the curve to compute, with range of [0, 1].
+    /// @param p0 The start-point of the curve.
+    /// @param p1
+    /// @param p2
+    /// @param p3 The end-point of the curve.
+    Vector2 BezierDerivative(float t, const Vector2& p0, const Vector2& p1, const Vector2& p2, const Vector2& p3);
+
+    /// Computes a point along the normal of a 2-dimensional Cubic Bezier Curve.
+    /// @param t The normalized distance along the curve to compute, with range of [0, 1].
+    /// @param p0 The start-point of the curve.
+    /// @param p1 The first control point,  which determines the direction at which the curve meets point 0.
+    /// @param p2 The second control point, which determines the direction at which the curve meets point 3.
+    /// @param p3 The end-point of the curve.
+    Vector2 BezierNormal(float t, const Vector2& p0, const Vector2& p1, const Vector2& p2, const Vector2& p3);
+
+    /// Computes a point along a 3-dimensional Cubic Bezier curve.
+    /// @param t The normalized distance along the curve to compute, with range of [0, 1].
+    /// @param p0 The start-point of the curve.
+    /// @param p1 The first control point,  which determines the direction at which the curve meets point 0.
+    /// @param p2 The second control point, which determines the direction at which the curve meets point 3.
+    /// @param p3 The end-point of the curve.
+    Vector3 Bezier(float t, const Vector3& p0, const Vector3& p1, const Vector3& p2, const Vector3& p3);
+
+    /// Computes a point along the tangent of a 3-dimensional Cubic Bezier Curve.
+    /// @param t The normalized distance along the curve to compute, with range of [0, 1].
+    /// @param p0 The start-point of the curve.
+    /// @param p1 The first control point,  which determines the direction at which the curve meets point 0.
+    /// @param p2 The second control point, which determines the direction at which the curve meets point 3.
+    /// @param p3 The end-point of the curve.
+    Vector3 BezierDerivative(float t, const Vector3& p0, const Vector3& p1, const Vector3& p2, const Vector3& p3);
+
+    /// Computes a point along the normal of a 3-dimensional Cubic Bezier Curve.
+    /// @param t The normalized distance along the curve to compute, with range of [0, 1].
+    /// @param p0 The start-point of the curve.
+    /// @param p1 The first control point,  which determines the direction at which the curve meets point 0.
+    /// @param p2 The second control point, which determines the direction at which the curve meets point 3.
+    /// @param p3 The end-point of the curve.
+    Vector3 BezierNormal(float t, const Vector3& p0, const Vector3& p1, const Vector3& p2, const Vector3& p3);
+}

include/J3ML/Algorithm/Clock.hpp

@@ -0,0 +1,3 @@
+#pragma once
+
+

include/J3ML/Algorithm/GJK.hpp

@@ -0,0 +1,61 @@
+/// @file GJK.hpp
+/// Implementation of the Gilbert-Johnson-Keerthi (GJK) convex polyhedron intersection test
+
+#pragma once
+
+#include <J3ML/LinearAlgebra.hpp>
+#include <J3ML/Geometry.hpp>
+
+namespace J3ML::Algorithms
+{
+
+    Vector3 UpdateSimplex(Vector3 *s, int &n);
+
+    #define SUPPORT(dir, minS, maxS) (a.ExtremePoint(dir, maxS) - b.ExtremePoint(-dir, minS));
+
+    template <typename A, typename B>
+    bool GJKIntersect(const A &a, const B &b)
+    {
+        Vector3 support[4];
+        // Start with an arbitrary point in the Minkowski set shape.
+        support[0] = a.AnyPointFast() - b.AnyPointFast();
+        if (support[0].LengthSquared() < 1e-7f) // Robustness check: Test if the first arbitrary point we guessed produced the zero vector we are looking for!
+            return true;
+        Vector3 d = -support[0]; // First search direction is straight toward the origin from the found point.
+        int n = 1; // Stores the current number of points in the search simplex.
+        int nIterations = 50; // Robustness check: Limit the maximum number of iterations to perform to avoid infinite loop if types A or B are buggy!
+        while (nIterations-- > 0)
+        {
+            // Compute the extreme point to the direction d in the Minkowski set shape.
+            float maxS, minS;
+            Vector3 newSupport = SUPPORT(d, minS, maxS);
+            // If the most extreme point in that search direction did not walk past the origin, then the origin cannot be contained in the Minkowski
+            // convex shape, and the two convex objects a and b do not share a common point - no intersection!
+            if (minS + maxS < 0.f)
+                return false;
+
+            // Add the newly evaluated point to the search simplex
+            assert(n < 4);
+            support[n++] = newSupport;
+            // Examine the current simplex, prune a redundant part of it, and produce the next search direction.
+            d = UpdateSimplex(support, n);
+
+            if (n == 0) // Was the origin contained in the current simplex? If so, then the convex shapes a and b do share a common point - intersection!
+                return true;
+        }
+        return false;
+    }
+
+    // This computes GJL intersection, but by first translating both objects to a coordinate frame that is as closely
+    // centered around world origin as possible, to gain floating point precision.
+    template <typename A, typename B>
+    bool FloatingPointOffsetedGJKIntersect(const A &a, const B &b)
+    {
+        AABB ab = a.MinimalEnclosingAABB();
+        AABB bb = b.MinimalEnclosingAABB();
+
+        Vector3 offset = (Vector3::Min(ab.minPoint, bb.minPoint) + Vector3::Max(ab.maxPoint, bb.maxPoint)) * 0.5f;
+        const Vector3 floatingPtPrecisionOffset = -offset;
+        return GJKIntersect(a.Translated(floatingPtPrecisionOffset), b.Translated(floatingPtPrecisionOffset));
+    }
+}

include/J3ML/Algorithm/Parabola.hpp

@@ -0,0 +1,17 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file Parabola.hpp
+/// @desc Algorithm for calculating a parabolic curve to be used in instantaneous bullet raycasting.
+/// @edit 2024-10-22
+
+#pragma once
+
+namespace J3ML
+{
+
+}

include/J3ML/Algorithm/RNG.hpp

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

include/J3ML/Algorithm/Reinterpret.hpp

@@ -0,0 +1,40 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file Reinterpret.h
+/// @desc Reinterpret one type to another using union hackery.
+/// @edit 2024-07-06
+
+#pragma once
+
+
+namespace J3ML
+{
+
+    /// As per C99, union-reinterpret should now be safe: http://stackoverflow.com/questions/8511676/portable-data-reinterpretation
+    template <typename To, typename From>
+    union ReinterpretOp {
+        From from;
+        To to;
+
+
+        ReinterpretOp(From from_) : from(from_) {} // Do not make explicit!
+        operator To() const { return to; } // Do not make explicit!
+    };
+
+
+    template <typename To, typename From>
+    To ReinterpretAs(From input)
+    {
+        //ReinterpretOp<To, From> fi;
+        //fi.to = input;
+        return ReinterpretOp<To, From>(input); //fi.from;
+    }
+
+
+
+}

include/J3ML/Algorithm/SAT.hpp

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

include/J3ML/Algorithm/Triangulate.hpp

@@ -0,0 +1,18 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file Parabola.hpp
+/// @desc Algorithm for calculating a parabolic curve to be used in instantaneous bullet raycasting.
+/// @edit 2024-10-22
+
+#pragma once
+
+
+namespace J3ML
+{
+    /// @see class Polygon::Triangulate for current implementation.
+}

include/J3ML/Geometry.h

@@ -1,32 +0,0 @@
-#include <J3ML/LinearAlgebra/Vector2.h>
-#include <J3ML/LinearAlgebra/Vector3.h>
-
-#pragma once
-
-namespace J3ML::Geometry {
-    using Vector2 = J3ML::LinearAlgebra::Vector2;
-    using Vector3 = J3ML::LinearAlgebra::Vector3;
-
-    class LineSegment2D
-    {
-        Vector2 A;
-        Vector2 B;
-    };
-
-    class Rectangle;
-    class AABB;
-    class OBB;
-    class Capsule;
-    class Frustum;
-    class OBB2D;
-    class Line2D;
-    class Ray2D;
-    class Triangle2D;
-    class Polygon2D;
-
-
-    struct IntersectionResult2D {};
-
-    bool Intersects2D(LineSegment2D seg, Rectangle rect);
-    IntersectionResult2D GetIntersection2D(LineSegment2D seg, Rectangle rect);
-}

include/J3ML/Geometry.hpp

@@ -0,0 +1,25 @@
+#include <J3ML/LinearAlgebra.hpp>
+
+#pragma once
+
+#include <J3ML/Geometry/AABB2D.hpp>
+#include <J3ML/Geometry/Plane.hpp>
+#include <J3ML/Geometry/Sphere.hpp>
+#include <J3ML/Geometry/Line.hpp>
+#include <J3ML/Geometry/LineSegment.hpp>
+#include <J3ML/Geometry/Frustum.hpp>
+#include <J3ML/Geometry/OBB.hpp>
+#include <J3ML/Geometry/Capsule.hpp>
+#include <J3ML/Geometry/AABB.hpp>
+#include <J3ML/Geometry/Polyhedron.hpp>
+#include <J3ML/Geometry/QuadTree.hpp>
+#include <J3ML/Geometry/Ray.hpp>
+#include <J3ML/Geometry/Sphere.hpp>
+#include <J3ML/Geometry/Triangle.hpp>
+#include <J3ML/Geometry/Triangle2D.hpp>
+#include <J3ML/Geometry/TriangleMesh.hpp>
+#include <J3ML/Geometry/PBVolume.hpp>
+
+#include <J3ML/Geometry/KDTree.hpp>
+
+using namespace J3ML::Geometry;

include/J3ML/Geometry/AABB.h

@@ -1,158 +0,0 @@
-#pragma once
-
-#include <J3ML/LinearAlgebra/Vector3.h>
-
-#include "J3ML/LinearAlgebra.h"
-#include <J3ML/Geometry.h>
-
-#include <J3ML/Geometry/Plane.h>
-#include <J3ML/Geometry/Sphere.h>
-#include <J3ML/Geometry/OBB.h>
-#include <J3ML/Geometry/LineSegment.h>
-#include <J3ML/Geometry/Triangle.h>
-#include <J3ML/Geometry/Polygon.h>
-#include <J3ML/Geometry/Frustum.h>
-#include <J3ML/Geometry/Capsule.h>
-#include <J3ML/Geometry/Ray.h>
-#include <J3ML/Geometry/TriangleMesh.h>
-#include <J3ML/Geometry/Polyhedron.h>
-
-
-// TODO: Fix circular include between AABB and OBB
-
-
-
-namespace J3ML::Geometry
-{
-    using namespace LinearAlgebra;
-    // 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:
-        Vector3 minPoint;
-        Vector3 maxPoint;
-
-        static int NumFaces() { return 6; }
-
-        static int NumEdges() { return 12; }
-
-        static int NumVertices() { return 8; }
-
-        static AABB FromCenterAndSize(const Vector3 &center, const Vector3 &size);
-
-        float MinX() const;
-
-        float MinY() const;
-
-        float MinZ() const;
-
-        float MaxX() const;
-
-        float MaxY() const;
-
-        float MaxZ() const;
-
-        /// Returns the smallest sphere that contains this AABB.
-        /// This function computes the minimal volume sphere that contains all the points inside this AABB
-        Sphere MinimalEnclosingSphere() const;
-
-        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;
-
-        bool IsFinite() const;
-
-        Vector3 Centroid() const;
-
-        Vector3 Size() const;
-
-        // Quickly returns an arbitrary point inside this AABB
-        Vector3 AnyPointFast() const;
-
-        Vector3 PointInside(float x, float y, float z) const;
-
-        // Returns an edge of this AABB
-        LineSegment Edge(int edgeIndex) const;
-
-        Vector3 CornerPoint(int cornerIndex) const;
-
-        Vector3 ExtremePoint(const Vector3 &direction) const;
-
-        Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance);
-
-        Vector3 PointOnEdge(int edgeIndex, float u) const;
-
-        Vector3 FaceCenterPoint(int faceIndex) const;
-
-        Vector3 FacePoint(int faceIndex, float u, float v) const;
-
-        Vector3 FaceNormal(int faceIndex) const;
-
-        Plane FacePlane(int faceIndex) const;
-
-        static AABB MinimalEnclosingAABB(const Vector3 *pointArray, int numPoints);
-        float GetVolume() const;
-        float GetSurfaceArea() const;
-        Vector3 GetRandomPointInside();
-        Vector3 GetRandomPointOnSurface();
-        Vector3 GetRandomPointOnEdge();
-        Vector3 GetRandomCornerPoint();
-        AABB Translated(const Vector3& offset) const;
-        AABB TransformAABB(const Matrix3x3& transform);
-        AABB TransformAABB(const Matrix4x4& transform);
-        AABB TransformAABB(const Quaternion& transform);
-        OBB Transform(const Matrix3x3& transform);
-        OBB Transform(const Matrix4x4& transform);
-        OBB Transform(const Quaternion& transform);
-        bool Contains(const Vector3& point) const;
-        bool Contains(const LineSegment& lineSegment) const;
-        bool Contains(const AABB& aabb) const;
-        bool Contains(const OBB& obb) const;
-        bool Contains(const Sphere& sphere) const;
-        bool Contains(const Triangle& triange) const;
-        bool Contains(const Polygon& polygon) const;
-        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.
-        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;
-        TriangleMesh Triangulate(int numFacesX, int numFacesY, int numFacesZ, bool ccwIsFrontFacing) const;
-        AABB Intersection(const AABB& rhs) const;
-        bool IntersectLineAABB(const Vector3& linePos, const Vector3& lineDir, float tNear, float tFar) const;
-
-
-        void SetFrom(const Vector3 *pVector3, int i);
-
-        void SetFromCenterAndSize(const Vector3 &center, const Vector3 &size);
-
-        void SetFrom(const OBB &obb);
-
-        void SetFrom(const Sphere &s);
-
-        Vector3 GetRandomPointInside() const;
-
-        void SetNegativeInfinity();
-
-        void Enclose(const Vector3 &point);
-
-        void Enclose(const Vector3 &aabbMinPt, const Vector3 &aabbMaxPt);
-
-        void Enclose(const LineSegment &lineSegment);
-
-        void Enclose(const OBB &obb);
-    };
-}

include/J3ML/Geometry/AABB.hpp

@@ -0,0 +1,415 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file AABB.hpp
+/// @desc The Axis-Aligned Bounding Box geometry object.
+/// @edit 2024-08-01
+
+#pragma once
+
+#include <format>
+#include <optional>
+
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/Geometry/Forward.hpp>
+#include <J3ML/LinearAlgebra.hpp>
+#include <J3ML/Algorithm/RNG.hpp>
+
+
+namespace J3ML::Geometry
+{
+
+
+    // TODO: Move this somewhere else to do goofy experiments!
+    template <typename T, typename = std::enable_if_t<std::is_arithmetic_v<T>, T>>
+    float fdiv(T a, T b) {
+        return (float)a/(b);
+    }
+
+    using namespace J3ML::LinearAlgebra;
+    using J3ML::Algorithm::RNG;
+
+    /// @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) {
+            SetFrom(obb);
+        }
+
+        /// Constructs this AABB to enclose the given Sphere.
+        /** @see class Sphere. */
+        explicit AABB(const Sphere &s) {
+            SetFrom(s);
+        }
+
+
+
+        /// Returns the diameter vector of this AABB.
+        /** @note For AABB, Diagonal() and Size() are the same concept. These functions are provided for symmetry with the OBB class.
+            @see Size(), HalfDiagonal() */
+        Vector3 Diagonal() const { return Size(); }
+
+        /// Returns Diagonal()/2.
+        /// @see Diagonal(), HalfSize()
+        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; ///< [similarOverload: MinX]
+        float MinZ() const; ///< [similarOverload: MinX]
+
+        /// Returns the maximum world-space coordinate along the given axis.
+        float MaxX() const;
+        float MaxY() const; ///< [similarOverload: MaxX]
+        float MaxZ() const; ///< [similarOverload: MaxX]
+
+
+        Vector3 MinPoint() const;
+
+        Vector3 MaxPoint() const;
+
+        /// Returns the smallest sphere that contains this AABB.
+        /// This function computes the minimal volume sphere that contains all the points inside this AABB
+        Sphere MinimalEnclosingSphere() const;
+
+        /// [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;
+        Vector3 CenterPoint() 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);
+        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;
+        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;
+        bool Contains(const AABB& aabb) const;
+        bool Contains(const OBB& obb) const;
+        bool Contains(const Sphere& sphere) const;
+        bool Contains(const Triangle& triangle) const;
+        bool Contains(const Polygon& polygon) const;
+        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.
+        /** 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 an intersection that is contained in both this and the given AABB if there is one.
+            @todo Add Intersection(OBB/Polyhedron). */
+        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 RandomPointInside(RNG& rng) const;
+        Vector3 RandomPointOnSurface(RNG& rng) const;
+        Vector3 RandomPointOnEdge(RNG& rng) const;
+        Vector3 RandomCornerPoint(RNG& rng) const;
+
+        /// Sets this structure to a degenerate AABB that does not have any volume.
+        /** This function is useful for initializing the AABB to "null" before a loop of calls to Enclose(),
+            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) {
+            //Enclose(frustum.MinimalEnclosingAABB());
+        }
+        void Enclose(const Polygon &polygon) {
+            //Enclose(polygon.MinimalEnclosingAABB());
+        }
+        void Enclose(const Polyhedron &polyhedron) {
+            //Enclose(polyhedron.MinimalEnclosingAABB());
+        }
+        void Enclose(const Vector3 *pointArray, int numPoints) {
+            assert(pointArray || numPoints == 0);
+            if (!pointArray)
+                return;
+            for (int i = 0; i < numPoints; ++i)
+                Enclose(pointArray[i]);
+        }
+
+
+        bool TestAxis(const Vector3& axis, const Vector3& v0, const Vector3& v1, const Vector3& v2) const;
+
+        bool Intersects(const LineSegment &lineSegment) const;
+        /// Computes the intersection of a line, ray or line segment and an AABB.
+        /** Based on "T. Kay, J. Kajiya. Ray Tracing Complex Scenes. SIGGRAPH 1986 vol 20, number 4. pp. 269-"
+            http://www.siggraph.org/education/materials/HyperGraph/raytrace/rtinter3.htm
+            @param linePos The starting position of the line.
+            @param lineDir The direction of the line. This direction vector must be normalized!
+            @param tNear [in, out] For the test, the input line is treated as a line segment. Pass in the signed distance
+                from the line origin to the start of the line. For a Line-AABB test, -FLOAT_INF is typically passed here.
+                For a Ray-AABB test, 0.0f should be inputted. If intersection occurs, the signed distance from line origin
+                to the line entry point in the AABB is returned here.
+            @param tFar [in, out] Pass in the signed distance from the line origin to the end of the line. For Line-AABB and
+                Ray-AABB tests, pass in FLOAT_INF. For a LineSegment-AABB test, pass in the length of the line segment here.
+                If intersection occurs, the signed distance from line origin to the line exit point in the AABB
+                is returned here.
+            @return True if an intersection occurs, false otherwise.
+            @note This is a low level utility function. It may be more convenient to use one of the AABB::Intersects()
+                functions instead.
+            @see Intersects(). */
+        bool IntersectLineAABB(const Vector3& linePos, const Vector3& lineDir, float tNear, float tFar) const;
+        bool IntersectLineAABB_CPP(const Vector3 &linePos, const Vector3 &lineDir, float &tNear, float &tFar) const;
+    };
+
+
+}

include/J3ML/Geometry/AABB2D.h

@@ -1,106 +0,0 @@
-#pragma once
-
-#include <J3ML/LinearAlgebra/Vector2.h>
-
-namespace J3ML::Geometry
-{
-    using LinearAlgebra::Vector2;
-    // CaveGame AABB
-    class AABB2D
-    {
-    public:
-
-        Vector2 minPoint;
-        Vector2 maxPoint;
-        AABB2D() {}
-        AABB2D(const Vector2& min, const Vector2& max):
-        minPoint(min), maxPoint(max)
-        {}
-
-        float Width() const { return maxPoint.x - minPoint.x;  }
-        float Height() const { return maxPoint.y - minPoint.y; }
-
-        float DistanceSq(const Vector2& pt) const
-        {
-            Vector2 cp = pt.Clamp(minPoint, maxPoint);
-            return cp.DistanceSq(pt);
-        }
-
-        void SetNegativeInfinity();
-
-        void Enclose(const Vector2& point)
-        {
-            minPoint = Vector2::Min(minPoint, point);
-            maxPoint = Vector2::Max(maxPoint, point);
-        }
-
-        bool Intersects(const AABB2D& rhs) const
-        {
-            return maxPoint.x >= rhs.minPoint.x &&
-                   maxPoint.y >= rhs.minPoint.y &&
-                   rhs.maxPoint.x >= minPoint.x &&
-                   rhs.maxPoint.y >= minPoint.y;
-        }
-
-        bool Contains(const AABB2D& rhs) const
-        {
-            return rhs.minPoint.x >= minPoint.x && rhs.minPoint.y >= minPoint.y
-                   && rhs.maxPoint.x <= maxPoint.x && rhs.maxPoint.y <= maxPoint.y;
-        }
-
-        bool Contains(const Vector2& pt) const
-        {
-            return pt.x >= minPoint.x && pt.y >= minPoint.y
-                   && pt.x <= maxPoint.x && pt.y <= maxPoint.y;
-        }
-
-        bool Contains(int x, int y) const
-        {
-            return x >= minPoint.x && y >= minPoint.y
-                   && x <= maxPoint.x && y <= maxPoint.y;
-        }
-
-        bool IsDegenerate() const
-        {
-            return minPoint.x >= maxPoint.x || minPoint.y >= maxPoint.y;
-        }
-
-        bool HasNegativeVolume() const
-        {
-            return maxPoint.x < minPoint.x || maxPoint.y < minPoint.y;
-        }
-
-        bool IsFinite() const
-        {
-            return minPoint.IsFinite() && maxPoint.IsFinite() && minPoint.MinElement() > -1e5f && maxPoint.MaxElement() < 1e5f;
-        }
-
-        Vector2 PosInside(const Vector2 &normalizedPos) const
-        {
-            return minPoint + normalizedPos.Mul(maxPoint - minPoint);
-        }
-
-        Vector2 ToNormalizedLocalSpace(const Vector2 &pt) const
-        {
-            return (pt - minPoint).Div(maxPoint - minPoint);
-        }
-
-
-        AABB2D operator+(const Vector2& pt) const
-        {
-            AABB2D a;
-            a.minPoint = minPoint + pt;
-            a.maxPoint = maxPoint + pt;
-            return a;
-        }
-
-        AABB2D operator-(const Vector2& pt) const
-        {
-            AABB2D a;
-            a.minPoint = minPoint - pt;
-            a.maxPoint = maxPoint - pt;
-            return a;
-        }
-
-    };
-}

include/J3ML/Geometry/AABB2D.hpp

@@ -0,0 +1,105 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file AABB2D.hpp
+/// @desc A 2D Axis-Aligned Bounding Box structure.
+/// @edit 2024-08-01
+/// @note On backlog, low-priority.
+
+#pragma once
+
+#include <J3ML/LinearAlgebra.hpp>
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/Geometry/Forward.hpp>
+
+
+/// See Christer Ericson's Real-time Collision Detection, p. 87, or
+/// James Arvo's "Transforming Axis-aligned Bounding Boxes" in Graphics Gems 1, pp. 548-550.
+/// http://www.graphicsgems.org/
+template <typename Matrix>
+void AABB2DTransformAsAABB2D(AABB2D& aabb, Matrix& m);
+
+
+
+namespace J3ML::Geometry
+{
+    using LinearAlgebra::Vector2;
+
+    // TODO: Integer AABB2D for even leaner box computation.
+
+    // CaveGame AABB
+    class AABB2D : public Shape2D
+    {
+    public:
+
+        Vector2 minPoint;
+        Vector2 maxPoint;
+        AABB2D() {}
+        AABB2D(const Vector2& min, const Vector2& max):
+        minPoint(min), maxPoint(max)
+        {}
+
+        [[nodiscard]] float Width() const;
+        [[nodiscard]] float Height() const;
+
+        Vector2 Centroid();
+
+        [[nodiscard]] float DistanceSq(const Vector2& pt) const;
+
+        void SetNegativeInfinity();
+
+        void Enclose(const Vector2& point);
+
+        [[nodiscard]] bool Intersects(const AABB2D& rhs) const;
+
+        [[nodiscard]] bool Contains(const AABB2D& rhs) const;
+
+        [[nodiscard]] bool Contains(const Vector2& pt) const;
+
+        [[nodiscard]] bool Contains(int x, int y) const;
+
+        [[nodiscard]] bool IsDegenerate() const;
+
+        [[nodiscard]] bool HasNegativeVolume() const;
+
+        [[nodiscard]] bool IsFinite() const;
+
+        Vector2 PosInside(const Vector2 &normalizedPos) const;
+
+        Vector2 ToNormalizedLocalSpace(const Vector2 &pt) const;
+
+        AABB2D operator+(const Vector2& pt) const;
+
+        AABB2D operator-(const Vector2& pt) const;
+
+        Vector2 CornerPoint(int cornerIndex);
+
+        void TransformAsAABB(const Matrix3x3& transform);
+        void TransformAsAABB(const Matrix4x4& transform);
+
+
+    };
+
+    template<typename Matrix>
+    void AABB2DTransformAsAABB2D(AABB2D &aabb, Matrix &m) {
+        float ax = m[0][0] * aabb.minPoint.x;
+        float bx = m[0][0] * aabb.maxPoint.x;
+        float ay = m[0][1] * aabb.minPoint.y;
+        float by = m[0][1] * aabb.maxPoint.y;
+
+        float ax2 = m[1][0] * aabb.minPoint.x;
+        float bx2 = m[1][0] * aabb.maxPoint.x;
+        float ay2 = m[1][1] * aabb.minPoint.y;
+        float by2 = m[1][1] * aabb.maxPoint.y;
+
+        aabb.minPoint.x = J3ML::Math::Min(ax, bx) + J3ML::Math::Min(ay, by) + m[0][3];
+        aabb.maxPoint.x = J3ML::Math::Max(ax, bx) + J3ML::Math::Max(ay, by) + m[0][3];
+
+        aabb.minPoint.y = J3ML::Math::Min(ax2, bx2) + J3ML::Math::Min(ay2, by2) + m[1][3];
+        aabb.maxPoint.y = J3ML::Math::Max(ax2, bx2) + J3ML::Math::Max(ay2, by2) + m[1][3];
+    }
+}

include/J3ML/Geometry/Capsule.h

@@ -1,27 +0,0 @@
-#pragma once
-
-#include "LineSegment.h"
-#include <J3ML/LinearAlgebra/Vector3.h>
-
-namespace J3ML::Geometry
-{
-    using namespace LinearAlgebra;
-    class Capsule
-    {
-        // Specifies the two inner points of this capsule
-        LineSegment l;
-        // Specifies the radius of this capsule
-        float r;
-
-        Capsule();
-        Capsule(const LineSegment& endPoints, float radius);
-        Capsule(const Vector3& bottomPt, const Vector3& topPt, float radius);
-        bool IsDegenerate() const;
-        float Height() const;
-        float Diameter() const;
-        Vector3 Bottom() const;
-        Vector3 Center() const;
-        Vector3 Centroid() const;
-        Vector3 ExtremePoint(const Vector3& direction);
-    };
-}

include/J3ML/Geometry/Capsule.hpp

@@ -0,0 +1,220 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file Capsule.hpp
+/// @desc The Capsule geometry object.
+/// @edit 2024-08-01
+
+#pragma once
+
+#include <J3ML/LinearAlgebra.hpp>
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/Geometry/Forward.hpp>
+#include <J3ML/Geometry/LineSegment.hpp>
+
+namespace J3ML::Geometry
+{
+
+    /// A 3D cylinder with spherical ends.
+    class Capsule : public Shape
+    {
+    public:
+        /// Specifies the two inner points of this capsule
+        LineSegment l;
+        /// 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);
+
+        /// Constructs a new capsule from a sphere.
+        /** This conversion results in a capsule which has its both endpoints at the exact same coordinates, and hence the
+            length of the inner line segment is set to 0. */
+        void SetFrom(const Sphere& s);
+
+        /// Sets this Capsule to a degenerate negative-volume state.
+        void SetDegenerate();
+
+
+        /// Quickly returns an arbitrary point inside this Capsule. Used in GJK intersection test.
+        [[nodiscard]] Vector3 AnyPointFast() const;
+
+        /// 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)
+        [[nodiscard]] Sphere SphereA() const;
+
+        /// Returns the Sphere defining the 'top' section of this Capsule (corresponding to the endpoint l.b)
+        [[nodiscard]] 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.
+            @param direction The direction vector of the direction to find the extreme point. This vector may
+                be unnormalized, but may not be null.
+            @return The extreme point of this Capsule in the given direction. */
+        [[nodiscard]] 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. */
+        [[nodiscard]] bool IsDegenerate() const;
+
+        /// Computes the total height of this capsule, i.e. LineLength() + Diameter().
+        /** <img src="CapsuleFunctions.png" />
+            @see LineLength(). */
+        [[nodiscard]] float Height() const;
+
+        /// Computes the distance of the two inner points of this capsule. @see Height.
+        [[nodiscard]] float LineLength() const;
+        /// Computes the diameter of this capsule.
+        [[nodiscard]] 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. */
+        [[nodiscard]] 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(). */
+
+        [[nodiscard]] Vector3 Center() const;
+        [[nodiscard]] 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. */
+        [[nodiscard]] Vector3 UpDirection() const;
+
+        /// Computes the volume of this Capsule.
+        /** @return pi * r^2 * |b-a| + 4 * pi * r^2 / 3.
+            @see SurfaceArea(). */
+        [[nodiscard]] float Volume() const;
+
+        /// Computes the surface area of this Capsule.
+        /** @return 2 * pi * r * |b-a| + 4 * pi * r^2.
+            @see Volume(). */
+        [[nodiscard]] 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(). */
+        [[nodiscard]] 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. */
+        [[nodiscard]] 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/Circle.hpp

@@ -0,0 +1,164 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file Circle.hpp
+/// @desc The Circle geometry object.
+/// @edit 2024-08-01
+
+#pragma once
+
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/Geometry/Forward.hpp>
+#include <J3ML/LinearAlgebra.hpp>
+
+namespace J3ML
+{
+
+    /// A two-dimensional circle in 3D space.
+    /// This class represents both a hollow circle (only edge) and a solid circle (disc).
+    class Circle : public Shape
+    {
+    public:
+        /// The center position of this circle.
+        Vector3 Position;
+        /// The normal direction of this circle.
+        /** A circle is a two-dimensional object in 3D space. This normal vector (together with the Position)
+            specifies the plane in which this circle lies in.
+            This vector is always normalized. If you assign to this member directly, be sure to only assign normalized vectors. */
+        Vector3 Normal;
+        /// The radius of the circle.
+        /** This parameter must be strictly positive to specify a non-degenerate circle. If zero is specified, this circle
+            is considered to be degenerate. */
+        float Radius;
+        /// The default constructor does not initialize any members of this class.
+        /** This means that the values of members Position, Normal, and Radius are all undefined after creating
+            a new circle using this default constructor. Remember to assign them before use.
+            @see Position, Normal, Radius. */
+        Circle() {}
+        /// Constructs a new circle by explicitly specifying the member variables.
+        /** @param center The center point of the circle.
+            @param normal The direction vector that specifies the orientation of this circle.
+               This vector must be normalized, the constructor will not normalize the vector for you (for performance reasons).
+            @param radius The radius of the circle.
+            @see Position, Normal, Radius. */
+        Circle(const Vector3& center, const Vector3& normal, float radius);
+
+        /// Returns a normalized direction vector to the 'U direction' of the circle.
+        /** This vector lies on the plane of this circle.
+            The U direction specifies the first basis vector of a local space of this circle. */
+        Vector3 BasisU() const;
+        /// Returns a normalized direction vector to the 'V direction' of the circle.
+        /** This vector lies on the plane of this circle.
+            The V direction specifies the second basis vector of a local space of this circle. */
+        Vector3 BasisV() const;
+        /// Returns a point at the edge of this circle.
+        /** @param angleRadians The direction of the point to get. A full circle is generated by the range [0, 2*pi],
+                but it is ok to pass in values outside this range.
+            @note This function is equivalent to calling GetPoint(float angleRadians, float d) with a value of d == 1.
+            @return A point in world space at the edge of this circle. */
+        Vector3 GetPoint(float angleRadians) const;
+        /// Returns a point inside this circle.
+        /** @param angleRadians The direction of the point to get. A full circle is generated by the range [0, 2*pi],
+                but it is ok to pass in values outside this range.
+            @param d A value in the range [0, 1] that specifies the normalized distance of the point from the center of the circle.
+                A value of 0 returns the center point of this circle. A value of 1 returns a point at the edge of this circle.
+                The range of d is not restricted, so it is ok to pass in values larger than 1 to generate a point lying completely
+                outside the circle. */
+        Vector3 GetPoint(float angleRadians, float d) const;
+
+        /// Returns the center point of this circle.
+        /** This point is also the center of mass for this circle. The functions CenterPoint() and Centroid() are equivalent.
+            @see Position. */
+        Vector3 CenterPoint() const { return Position; }
+        Vector3 Centroid() const { return Position;}
+
+        /// Computes an extreme point of this Circle/Disc in the given direction.
+        /** An extreme point is a farthest point of this Circle/Disc 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 Circle/Disc in the given direction. The returned point is always at
+                the edge of this Circle. */
+        Vector3 ExtremePoint(const Vector3& direction) const;
+
+        /// Computes the plane this circle is contained in.
+        /** All of the points of this circle lie inside this plane.
+            @see class Plane. */
+        Plane ContainingPlane() const;
+
+        /// Translates this Circle in world space.
+        /** @param offset The amount of displacement to apply to this circle, in world space coordinates.
+            @see Transform(). */
+        void Translate(const Vector3& offset);
+
+        /// Applies a transformation to this Circle.
+        /** @param transform The transformation to apply to this Circle. 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);
+
+        /// Tests if the given point is contained at the edge of this circle.
+        /** @param point The target point to test.
+            @param maxDistance The epsilon threshold to test the distance against. This effectively turns the circle into a torus
+                for this test.
+            @see DistanceToEdge(), DistanceToDisc(), ClosestPointToEdge(), ClosestPointToDisc().
+            @todo Implement DiscContains(Vector3/LineSegment/Triangle). */
+        bool EdgeContains(const Vector3& point, float maxDistance = 1e-6f) const;
+
+        /// Computes the distance of the given object to the edge of this circle.
+        /** @todo Implement DistanceToEdge(Ray/LineSegment/Line).
+            @return The distance of the given point to the edge of this circle. If the point is contained on this circle,
+                the value 0 is returned.
+            @see DistanceToEdge(), DistanceToDisc(),  ClosestPointToDisc().*/
+        float DistanceToEdge(const Vector3& point) const;
+
+        /// Computes the distance of the given object to this disc (filled circle).
+        /** If the point is contained inside this disc, the value 0 is returned.
+            @see DistanceToEdge(), ClosestPointToEdge(), ClosestPointToDisc().
+            @todo Implement DistanceToDisc(Ray/LineSegment/Line). */
+        float DistanceToDisc(const Vector3& point) const;
+
+        /// Computes the closest point on the edge of this circle to the given object.
+        /** @todo Implement ClosestPointToEdge(Ray/LineSegment/Line).
+            @see DistanceToEdge(), DistanceToDisc(), ClosestPointToDisc(). */
+        Vector3 ClosestPointToEdge(const Vector3& point) const;
+
+        /// Computes the closest point on the disc of this circle to the given object.
+        /** @todo Implement ClosestPointToDisc(Ray/LineSegment/Line).
+            @see DistanceToEdge(), DistanceToDisc(), ClosestPointToEdge(). */
+        Vector3 ClosestPointToDisc(const Vector3& point) const;
+
+        /// Tests this circle for an intersection against the given plane.
+        /** @note For Circle-Plane intersection, there is no need to differentiate between a hollow or filled circle(disc).
+            @return The number of intersection points found for this circle and the given plane.
+            @see IntersectsDisc(). */
+        int Intersects(const Plane& plane, Vector3* pt1, Vector3* pt2) const;
+        int Intersects(const Plane& plane) const;
+
+        /// Tests this disc for an intersection against the given object.
+        /** @see Intersects(). */
+        bool IntersectsDisc(const Line& line) const;
+        bool IntersectsDisc(const LineSegment& lineSegment) const;
+        bool IntersectsDisc(const Ray& ray) const;
+
+        /// Tests if this circle intersects the faces of the given OBB.
+        /** @param obb The bounding box to test against. This box is treated as "hollow", i.e. only the faces of the OBB are considered to be
+                a part of the OBB.
+            @return A vector that contains all the detected points of intersection for this circle and the given OBB. If the circle is fully
+                contained inside the OBB, or is fully outside the OBB, no intersection occurs, and the returned vector has zero elements.
+            @see Intersects(), IntersectsDisc(). */
+        std::vector<Vector3> IntersectsFaces(const OBB& obb) const;
+        std::vector<Vector3> IntersectsFaces(const AABB& aabb) const;
+    };
+    Circle operator *(const Matrix3x3& transform,  const Circle& circle);
+    Circle operator *(const Matrix4x4& transform,  const Circle& circle);
+    Circle operator *(const Quaternion& transform, const Circle& circle);
+    std::ostream& operator << (std::ostream& o, const Circle& circle);
+}

include/J3ML/Geometry/Forward.hpp

@@ -0,0 +1,44 @@
+#pragma once
+
+
+// Forward declarations for classes that include each other
+namespace J3ML::Geometry
+{
+    class AABB2D;
+    class AABB;
+    class Capsule;
+    class Frustum;
+    class LineSegment;
+    class LineSegment2D;
+    class Line;
+    class OBB;
+    class OBB2D;
+    class Plane;
+    class Polygon;
+    class Polyhedron;
+    template<typename T> class QuadTree;
+    class Ray;
+    class Sphere;
+    class Circle;
+    class Triangle;
+    class Triangle2D;
+    class TriangleMesh;
+    template <int N> class PBVolume;
+
+}
+
+// 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;
+    };
+}
+
+using namespace J3ML::Geometry;

include/J3ML/Geometry/Frustum.h

@@ -1,42 +0,0 @@
-//
-// Created by dawsh on 1/25/24.
-//
-#pragma once
-
-#include "Plane.h"
-#include <J3ML/LinearAlgebra/CoordinateFrame.h>
-
-namespace J3ML::Geometry
-{
-
-    using J3ML::LinearAlgebra::CoordinateFrame;
-
-    enum class FrustumType
-    {
-        Invalid,
-        /// Set the Frustum type to this value to define the orthographic projection formula. In orthographic projection,
-        /// 3D images are projected onto a 2D plane essentially by flattening the object along one direction (the plane normal).
-        /// The size of the projected images appear the same independent of their distance to the camera, and distant objects will
-        /// not appear smaller. The shape of the Frustum is identical to an oriented bounding box (OBB).
-        Orthographic,
-        /// Set the Frustum type to this value to use the perspective projection formula. With perspective projection, the 2D
-        /// image is formed by projecting 3D points towards a single point (the eye point/tip) of the Frustum, and computing the
-        /// point of intersection of the line of the projection and the near plane of the Frustum.
-        /// This corresponds to the optics in the real-world, and objects become smaller as they move to the distance.
-        /// The shape of the Frustum is a rectangular pyramid capped from the tip.
-        Perspective
-    };
-
-    class Frustum {
-    public:
-        Plane TopFace;
-        Plane BottomFace;
-        Plane RightFace;
-        Plane LeftFace;
-        Plane FarFace;
-        Plane NearFace;
-        static Frustum CreateFrustumFromCamera(const CoordinateFrame& cam, float aspect, float fovY, float zNear, float zFar);
-    };
-
-
-}

include/J3ML/Geometry/Frustum.hpp

@@ -0,0 +1,536 @@
+//
+// Created by dawsh on 1/25/24.
+//
+#pragma once
+
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/Geometry/Forward.hpp>
+#include <J3ML/LinearAlgebra.hpp>
+
+namespace J3ML::Geometry
+{
+    /// A frustum can be set to one of the two common different forms.
+    enum class FrustumType
+    {
+        Invalid = 0,
+
+        /// Set the Frustum type to this value to define the orthographic projection formula. In orthographic projection,
+        /// 3D images are projected onto a 2D plane essentially by flattening the object along one direction (the plane normal).
+        /// The size of the projected images appear the same independent of their distance to the camera, and distant objects will
+        /// not appear smaller. The shape of the Frustum is identical to an oriented bounding box (OBB).
+        Orthographic,
+
+        /// Set the Frustum type to this value to use the perspective projection formula. With perspective projection, the 2D
+        /// image is formed by projecting 3D points towards a single point (the eye point/tip) of the Frustum, and computing the
+        /// point of intersection of the line of the projection and the near plane of the Frustum.
+        /// This corresponds to the optics in the real-world, and objects become smaller as they move to the distance.
+        /// The shape of the Frustum is a rectangular pyramid capped from the tip.
+        Perspective
+    };
+
+    /// The Frustum class offers choosing between the two common conventions for the value ranges in
+    /// post-projective space. If you are using either the OpenGL or Direct3D API, you must feed the API data that matches
+    /// the correct convention.
+    enum class FrustumProjectiveSpace
+    {
+        Invalid = 0,
+        /// If this option is chosen, the post-projective unit cube of the Frustum
+        /// is modelled after the OpenGL API convention, meaning that in projected space,
+        /// points inside the Frustum have the X and Y range in [-1, 1] and Z ranges in [-1, 1],
+        /// where the near plane maps to Z=-1 and the far plane maps to Z=1.
+        /// @note If you are submitting projection matrices to GPU hardware using the OpenGL API,
+        ///       you **must** use this convention. (or otherwise more than half of the precision of the GL depth buffer is wasted)
+        GL,
+
+        /// If this option is chosen, the post-projective unit cube is modelled after the
+        /// Direct3D API convention, which differs from the GL convention that Z ranges in [0, 1] instead.
+        /// Near plane maps to Z=0, and far plane maps to Z=1. The X and Y ranges in [-1, 1] as is with GL.
+        /// @note If you are submitting projection matrices to GPU hardware using the Direct3D API,
+        ///       you **must** use this convention.
+        D3D,
+    };
+
+    /// @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()
+    ///       (and hence Frustum::WorldMatrix()) always returns a matrix with a positive determinant, i.e. it does not mirror.
+    ///       If FrustumRightHanded is chosen, then Frustum::ProjectionMatrix() is a mirroring matrix, since the post-projective space
+    ///       is always left-handed.
+    /// @note Even though in the local space of the camera +Y is always up, in the world space one can use any 'world up' direction
+    ///       as one pleases, by orienting the camera via the Frustum::up vector.
+    enum class FrustumHandedness
+    {
+        Invalid = 0,
+
+        /// If a Frustum is left-handed, then in the local space of the Frustum (the view space), the camera looks towards +Z,
+        /// while +Y goes towards up, and +X goes towards right.
+        /// @note The fixed-pipeline D3D9 API traditionally used the FrustumLeftHanded convention.
+        Left,
+
+        /// If a Frustum is right-handed, then the camera looks towards -Z, +Y is up, and +X is right.
+        /// @note The fixed-pipeline OpenGL API traditionally used the FrustumRightHanded convention.
+        Right
+    };
+
+    /// @brief Represents either an orthographic or a perspective viewing frustum.
+    /// @see FrustumType
+    /// @see FrustumProjectiveSpace
+    /// @see FrustumHandedness
+    class Frustum : public Shape {
+    public: // Members
+
+        /// Specifies whether this frustum is a perspective or an orthographic frustum.
+        FrustumType type;
+        /// Specifies whether the [-1, 1] or [0, 1] range is used for the post-projective depth range.
+        FrustumProjectiveSpace projectiveSpace ;
+        /// Specifies the chirality of world and view spaces
+        FrustumHandedness handedness;
+        /// The eye point of this frustum
+        Vector3 pos;
+        /// The normalized direction this frustum is watching towards.
+        Vector3 front;
+        /// The normalized up direction for this frustum.
+        /// This vector is specified in world (global) space. This vector is always normalized.
+        /// @note The vectors front and up must always be perpendicular to each other. This means that this up vector is not
+        /// a static/constant up vector, e.g. (0, 1, 0), but changes according to when the camera pitches up and down to
+        /// preserve the condition that front and up are always perpendicular
+        /// @note In the _local_ space of the Frustum, the direction +y is _always_ the up direction and cannot be changed. This
+        /// coincides to how Direct3D and OpenGL view and projection matrices are constructed
+        Vector3 up;
+        /// Distance from the eye point to the front plane
+        /// This parameter must be positive. If perspective projection is used, this parameter must be strictly positive
+        /// (0 is not allowed). If orthographic projection is used, 0 is possible (but uncommon, and not recommended).
+        /// When using the Frustum class to derive perspective projection matrices for a GPU, it should be noted that too
+        /// small values cause poor resolution of Z values near the back plane in post-perspective space, if non-linear
+        /// depth is used (which is common). The larger this value is, the more resolution there is for the Z values across the
+        /// depth range. Too large values cause clipping of geometry when they come very near the camera. */
+        float nearPlaneDistance;
+        /// Distance from the eye point to the back plane of the projection matrix.
+        /// This parameter must be strictly greater than nearPlaneDistance. The range [nearPlaneDistance, farPlaneDistance]
+        //	specifies the visible range of objects inside the Frustum. When using the Frustum class for deriving perspective
+        //	projection matrix for GPU rendering, it should be remembered that any geometry farther from the camera (in Z value)
+        //	than this distance will be clipped from the view, and not rendered.
+        float farPlaneDistance;
+
+        union {
+            /// Horizontal field-of-view, in radians. This field is only valid if type == PerspectiveFrustum.
+            /** @see type. */
+            float horizontalFov;
+            /// The width of the orthographic frustum. This field is only valid if type == OrthographicFrustum.
+            /** @see type. */
+            float orthographicWidth;
+        };
+
+        union {
+            /// Vertical field-of-view, in radians. This field is only valid if type == PerspectiveFrustum.
+            /** @see type. */
+            float verticalFov;
+            /// The height of the orthographic frustum. This field is only valid if type == OrthographicFrustum.
+            /** @see type. */
+            float orthographicHeight;
+        };
+
+        void WorldMatrixChanged();
+        void ProjectionMatrixChanged();
+
+        /// Frustums are typically used in batch culling operations.
+        /// Therefore the matrices associated with a frustum are cached for immediate access.
+        Matrix4x4 worldMatrix;
+        Matrix4x4 projectionMatrix;
+        Matrix4x4 viewProjectionMatrix;
+    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();
+
+        static Frustum CreateFrustumFromCamera(const CoordinateFrame& cam, float aspect, float fovY, float zNear, float zFar);
+    public:
+
+
+        /// Quickly returns an arbitrary point inside this Frustum. Used in GJK intersection test.
+        [[nodiscard]] Vector3 AnyPointFast() const { return CornerPoint(0); }
+
+
+        /// 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(). */
+        [[nodiscard]] AABB MinimalEnclosingAABB() const;
+
+        [[nodiscard]] bool IsFinite() 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(). */
+        [[nodiscard]] OBB MinimalEnclosingOBB(float expandGuardband = 1e-5f) 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 n The z distance from the eye point to the position of the Frustum near clip plane. Always pass a positive value here.
+            @param f 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 n, float f);
+
+        /// 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 h, float v);
+
+        /// 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 w, float h);
+
+        /// Returns the handedness of the projection formula used by this Frustum.
+        /** @see SetKind(), FrustumHandedness. */
+        [[nodiscard]] FrustumHandedness Handedness() const { return handedness; }
+        /// Returns the type of the projection formula used by this Frustum.
+        /** @see SetPerspective(), SetOrthographic(), FrustumType. */
+        [[nodiscard]] FrustumType Type() const { return type; }
+        /// Returns the convention of the post-projective space used by this Frustum.
+        /** @see SetKind(), FrustumProjectiveSpace. */
+        [[nodiscard]] FrustumProjectiveSpace ProjectiveSpace() const { return projectiveSpace;}
+        /// Returns the world-space position of this Frustum.
+        /** @see SetPos(), Front(), Up(). */
+        [[nodiscard]] const Vector3 &Pos() const {return pos;}
+        /// Returns the world-space camera look-at direction of this Frustum.
+        /** @see Pos(), SetFront(), Up(). */
+        [[nodiscard]] const Vector3 &Front() const { return front; }
+        /// Returns the world-space camera up direction of this Frustum.
+        /** @see Pos(), Front(), SetUp(). */
+        [[nodiscard]] const Vector3 &Up() const { return up; }
+        /// Returns the distance from the Frustum eye to the near clip plane.
+        /** @see SetViewPlaneDistances(), FarPlaneDistance(). */
+        [[nodiscard]] float NearPlaneDistance() const { return nearPlaneDistance; }
+        /// Returns the distance from the Frustum eye to the far clip plane.
+        /** @see SetViewPlaneDistances(), NearPlaneDistance(). */
+        [[nodiscard]] 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(). */
+        [[nodiscard]] 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(). */
+        [[nodiscard]] 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(). */
+        [[nodiscard]] 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(). */
+        [[nodiscard]] 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. */
+        static int NumEdges() { 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. */
+        [[nodiscard]] 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 hFov, 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 vFov, float aspectRatio);
+
+        /// Finds a ray in world space that originates at the eye point and looks in the given direction inside the frustum.
+        /** The (x,y) coordinate specifies the normalized viewport coordinate through which the ray passes.
+            Both x and y must be in the range [-1,1].
+            Specifying (-1, -1) returns the bottom-left corner of the near plane.
+            The point (1, 1) corresponds to the top-right corner of the near plane. */
+        Ray UnProject(float x, float y) const;
+
+        Ray UnProject(const Vector2& xy) const;
+
+        Ray UnProjectFromNearPlane(float x, float y) const;
+
+        [[nodiscard]] LineSegment UnProjectLineSegment(float x, float y) const;
+
+        Vector3 PointInside(float x, float y, float z) const;
+
+        Vector3 PointInside(const Vector3& xyz) const;
+
+
+        [[nodiscard]] Vector3 CenterPoint() const;
+
+        [[nodiscard]] Vector3 CornerPoint(int cornerIndex) const;
+
+        /// Returns a point on the near plane.
+        /** @param x A value in the range [-1, 1].
+            @param y A value in the range [-1, 1].
+            Specifying (-1, -1) returns the bottom-left corner of the near plane.
+            The point (1, 1) corresponds to the top-right corner of the near plane.
+            @note This coordinate space is called the normalized viewport coordinate space.
+            @see FarPlanePos(). */
+        [[nodiscard]] Vector3 NearPlanePos(float x, float y) const;
+        [[nodiscard]] Vector3 NearPlanePos(const Vector2& point) const;
+
+        /// Returns a point on the far plane.
+        /** @param x A value in the range [-1, 1].
+            @param y A value in the range [-1, 1].
+            Specifying (-1, -1) returns the bottom-left corner of the far plane.
+            The point (1, 1) corresponds to the top-right corner of the far plane.
+            @note This coordinate space is called the normalized viewport coordinate space.
+            @see NearPlanePos(). */
+        [[nodiscard]] Vector3 FarPlanePos(float x, float y) const;
+        [[nodiscard]] Vector3 FarPlanePos(const Vector2& point) const;
+
+        /// 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. */
+        [[nodiscard]] Vector3 WorldRight() const;
+
+        [[nodiscard]] Plane TopPlane() const;     ///< [similarOverload: LeftPlane] [hideIndex]
+        [[nodiscard]] Plane BottomPlane() const;  ///< [similarOverload: LeftPlane] [hideIndex]
+        [[nodiscard]] 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(). */
+        [[nodiscard]] 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(). */
+        [[nodiscard]] 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(). */
+        [[nodiscard]] Plane NearPlane() const;
+
+        /// Computes the width of the near plane quad in world space units.
+        /** @see NearPlaneHeight(). */
+        [[nodiscard]] float NearPlaneWidth() const;
+
+        /// Computes the height of the near plane quad in world space units.
+        /** @see NearPlaneHeight(). */
+        [[nodiscard]] float NearPlaneHeight() const;
+
+
+        /// Returns the specified plane of this frustum.
+        /** The normal vector of the returned plane points outwards from the volume inside the frustum.
+            @param faceIndex A number in the range [0,5], which returns the plane at the selected index from
+                the array { near, far, left, right, top, bottom} */
+        Plane GetPlane(int faceIndex) const;
+
+        /// Returns all six planes of this Frustum.
+        /** The planes will be output in the order { near, far, left, right, top, bottom }.
+            @param outArray [out] A pointer to an array of at least 6 elements. This pointer will receive the planes of this Frustum.
+                This pointer may not be null.
+            @see GetPlane(), NearPlane(), FarPlane(), LeftPlane(), RightPlane(), TopPlane(), BottomPlane(). */
+        void GetPlanes(Plane *outArray) 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(). */
+        [[nodiscard]] 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(). */
+        [[nodiscard]] PBVolume<6> ToPBVolume() const;
+
+
+        [[nodiscard]] Vector3 Project(const Vector3& point) const;
+
+        /// Computes the matrix that transforms from the world (global) space to the projection space of this Frustum.
+        /** The matrix computed by this function is simply the concatenation ProjectionMatrix()*ViewMatrix(). This order
+            of concatenation follows the M*v convention of transforming vectors (as opposed to the v*M convention). This
+            multiplication order is used, since the matrices ProjectionMatrix() and ViewMatrix() also follow the M*v convention.
+            @return A matrix that performs the world->view->proj transformation. This matrix is neither invertible or
+                orthonormal. The returned matrix is built to use the convention Matrix * vector
+                to map a point between these spaces. (as opposed to the convention v*M).
+            @see WorldMatrix(), ViewMatrix(), ProjectionMatrix(). */
+        [[nodiscard]] Matrix4x4 ViewProjMatrix() const;
+        [[nodiscard]] Matrix4x4 ComputeViewProjMatrix() const;
+
+
+        /// Computes the matrix that transforms from the view space to the world (global) space of this Frustum.
+        /** @note The returned matrix is the inverse of the matrix returned by ViewMatrix().
+            @return An orthonormal affine matrix that performs the view->world transformation. The returned
+                matrix is built to use the convention Matrix * vector to map a point between these spaces.
+                (as opposed to the convention v*M).
+            @see ViewMatrix(), ProjectionMatrix(), ViewProjMatrix(). */
+        [[nodiscard]] Matrix4x4 WorldMatrix() const;
+        [[nodiscard]] Matrix4x4 ComputeWorldMatrix() const;
+
+        /// Computes the matrix that transforms from the world (global) space to the view space of this Frustum.
+        /** @note The returned matrix is the inverse of the matrix returned by WorldMatrix().
+            @return An orthonormal affine matrix that performs the world->view transformation. The returned
+                matrix is built to use the convention Matrix * vector to map a point between these spaces.
+                (as opposed to the convention v*M).
+            @see WorldMatrix(), ProjectionMatrix(), ViewProjMatrix(). */
+        [[nodiscard]] Matrix4x4 ViewMatrix() const;
+        [[nodiscard]] Matrix4x4 ComputeViewMatrix() const;
+
+
+        /// Computes the matrix that projects from the view space to the projection space of this Frustum.
+        /** @return A projection matrix that performs the view->proj transformation. This matrix is neither
+                invertible or orthonormal. The returned matrix is built to use the convention Matrix * vector
+                to map a point between these spaces. (as opposed to the convention v*M).
+                @see WorldMatrix(), ViewMatrix(), ViewProjMatrix(). */
+        [[nodiscard]] Matrix4x4 ProjectionMatrix() const;
+        [[nodiscard]] Matrix4x4 ComputeProjectionMatrix() 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(). */
+        [[nodiscard]] bool Contains(const Vector3 &point) const;
+        [[nodiscard]] bool Contains(const LineSegment &lineSegment) const;
+        [[nodiscard]] bool Contains(const Triangle &triangle) const;
+        [[nodiscard]] bool Contains(const Polygon &polygon) const;
+        [[nodiscard]] bool Contains(const AABB &aabb) const;
+        [[nodiscard]] bool Contains(const OBB &obb) const;
+        [[nodiscard]] bool Contains(const Frustum &frustum) const;
+        [[nodiscard]] 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(). */
+        [[nodiscard]] float Distance(const Vector3 &point) const;
+
+        /// Computes the closest point inside this frustum to the given point.
+        /** If the target point lies inside this Frustum, then that point is returned.
+            @see Distance(), Contains(), Intersects().
+            @todo Add ClosestPoint(Line/Ray/LineSegment/Plane/Triangle/Polygon/Circle/Disc/AABB/OBB/Capsule/Frustum/Polyhedron). */
+        [[nodiscard]] Vector3 ClosestPoint(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). */
+        [[nodiscard]] bool Intersects(const Ray& ray) const;
+        [[nodiscard]] bool Intersects(const Line &line) const;
+        [[nodiscard]] bool Intersects(const LineSegment& lineSegment) const;
+        [[nodiscard]] bool Intersects(const AABB& aabb) const;
+        [[nodiscard]] bool Intersects(const OBB& obb) const;
+        [[nodiscard]] bool Intersects(const Plane& plane) const;
+        [[nodiscard]] bool Intersects(const Triangle& triangle) const;
+        [[nodiscard]] bool Intersects(const Polygon& lineSegment) const;
+        [[nodiscard]] bool Intersects(const Sphere& aabb) const;
+        [[nodiscard]] bool Intersects(const Capsule& obb) const;
+        [[nodiscard]] bool Intersects(const Frustum& plane) const;
+        [[nodiscard]] 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;
+
+        Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
+
+        [[nodiscard]] LineSegment Edge(int edgeIndex) const;
+
+
+
+        /// Maps a point from the normalized viewport space to the screen space
+        /** In normalized viewport space, top-left: (-1, 1), top-right: (1,1), bottom-left: (-1, -1), bottom-right: (-1, 1)
+            In screen space: top-left: (0, 0), top-right: (0, screenWidth-1), bottom-left: (0, screenHeight-1), bottom-right: (screenWidth-1, screenHeight-1).
+            This mapping is affine.
+            @see ScreenToViewportSpace(). */
+        static Vector2 ViewportToScreenSpace(float x, float y, int screenWidth, int screenHeight);
+        static Vector2 ViewportToScreenSpace(const Vector2& point, int screenWidth, int screenHeight);
+
+        /// Maps a point from screen space to normalized viewport space.
+        /** This function computes the inverse function of ViewportToScreenSpace(). This mapping is affine.
+            @see ViewportToScreenSpace(). */
+        static Vector2 ScreenToViewportSpace(float x, float y, int screenWidth, int screenHeight);
+        static Vector2 ScreenToViewportSpace(const Vector2& point, int screenWidth, int screenHeight);
+    };
+
+    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/Icosahedron.hpp

@@ -0,0 +1,109 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file Icosahedron.hpp
+/// @desc Icosahedron class implementation, borrowed from http://www.songho.ca/opengl/gl_sphere.html#icosphere
+/// @edit 2024-10-22
+
+/** Polyhedron with 12 vertices, 30 edges, and 20 faces (triangles) for OpenGL
+    If the radius is r, then the length of the edge is (r / sin(2pi/5))
+
+    Vertices of icosahedron are constructed with spherical coords by aligning
+    the north pole to (0,0,r) and the south pole to (0,0,-r). Other 10 vertices
+    are computed by rotating 72 degrees along y-axis at the elevation angle
+    +/- 26.565 (=arctan(1/2)).
+
+    The unwrapped (paper model) of icosahedron and texture map look like this:
+    // (S,0)  3S  5S  7S  9S
+    //    /\  /\  /\  /\  /\      : 1st row (5 triangles)       //
+    //   /__\/__\/__\/__\/__\                                   //
+    // T \  /\  /\  /\  /\  /\    : 2nd row (10 triangles)      //
+    //    \/__\/__\/__\/__\/__\                                 //
+    // 2T  \  /\  /\  /\  /\  /   : 3rd row (5 triangles)       //
+    //      \/  \/  \/  \/  \/                                  //
+    //      2S  4S  6S  8S  (10S,3T)
+    // where S = 186/2048 = 0.0908203
+    //       T = 322/1024 = 0.3144531
+    // If a texture size is 2048x1024, S=186, T=322
+
+    AUTHOR: Song Ho Ahn (song.ahn@gmail.com)
+ */
+
+#include <vector>
+#include "Color4.hpp"
+
+#pragma once
+
+namespace J3ML
+{
+
+    class Icosahedron
+    {
+    public:
+        float radius;
+        float edgeLength;
+        Icosahedron(float radius = 1.0f);
+        float Radius() const { return radius; }
+        void Radius(float new_radius) {radius = new_radius;}
+        float EdgeLength() const { return edgeLength;}
+        void EdgeLength(float new_edge_length) { edgeLength = new_edge_length;}
+        // for vertex data
+        unsigned int VertexCount() const;
+        unsigned int NormalCount() const;
+        unsigned int TexCoordCount() const;
+        unsigned int IndexCount() const;
+        unsigned int LineIndexCount() const;
+        unsigned int TriangleCount() const;
+        unsigned int VertexSize() const;
+        unsigned int NormalSize() const;
+        unsigned int TexCoordSize() const;
+        unsigned int IndexSize() const;
+        unsigned int LineIndexSize() const;
+        const float* Vertices() const;
+        const float* Normals() const;
+        const float* TexCoords() const;
+        const unsigned int* Indices() const;
+        const unsigned int* LineIndices() const;
+        // for interleaved vertices: V/N/T
+        unsigned int InterleavedVertexCount() const;
+        unsigned int InterleavedVertexSize() const;
+        int InterleavedStride() const;
+        const float* InterleavedVertices() const;
+
+        // draw in VertexArray mode
+        void draw() const;
+        void drawLines(const Color4& lineColor) const;
+        void drawWithLines(const Color4& lineColor) const;
+
+    protected:
+    private:
+        // static functions
+        static void computeFaceNormal(float v1[3], float v2[3], float v3[3], float n[3]);
+
+        // member functions
+        void updateRadius();
+        std::vector<float> computeVertices();
+        void buildVertices();
+        void buildInterleavedVertices();
+        void addVertices(float v1[3], float v2[3], float v3[3]);
+        void addNormals(float n1[3], float n2[3], float n3[3]);
+        void addTexCoords(float t1[2], float t2[2], float t3[2]);
+        void addIndices(unsigned int i1, unsigned int i2, unsigned int i3);
+        void addLineIndices(unsigned int indexFrom);
+
+        // member vars
+        //float radius;
+        //float edgeLength;
+        std::vector<float> vertices;
+        std::vector<float> normals;
+        std::vector<float> texCoords;
+        std::vector<unsigned int> indices;
+        std::vector<unsigned int> lineIndices;
+
+
+    };
+}

include/J3ML/Geometry/KDTree.hpp

@@ -0,0 +1,83 @@
+#pragma once
+
+
+
+namespace J3ML::Geometry
+{
+
+    enum CardinalAxis
+    {
+        AxisX = 0,
+        AxisY,
+        AxisZ,
+        AxisNone
+    };
+
+
+    struct KdTreeNode
+    {
+        /// If this is an inner node, specifies along which axis this node is split.
+        /// If this is a leaf, has the value AxisNone.
+        unsigned splitAxis  : 2;
+        /// If this is an inner node, specifies the index/offset to the child node pair.
+        /// If this is a leaf, the value is undefined.
+        unsigned childIndex : 30;
+        union
+        {
+            /// If this is an inner node, specifies the position along the cardinal axis of the split.
+            float splitPos;
+            /// If this is a leaf, specifies the index/ofset to the object bucket for this leaf.
+            /// If zero, this leaf does not have a bucket associated with it. (empty leaf)
+            u32 bucketIndex;
+        };
+
+        /// If true, this leaf does not contain any objects.
+        bool IsEmptyLeaf() const { assert(IsLeaf()); return bucketIndex == 0; }
+        bool IsLeaf() const { return splitAxis == AxisNone; }
+        int LeftChildIndex() const { return (int)childIndex; }
+        int RightChildIndex() const { return (int) childIndex+1; }
+        CardinalAxis SplitAxis() const { return (CardinalAxis) splitAxis;}
+    };
+
+    /// A KD-tree accelleration structure for static geometry.
+    template <typename T>
+    class KdTree
+    {
+    public:
+        KdTree() {}
+        //~KDTree() { /* TODO: FILL */}
+        void AddObjects(const T *objects, int numObjects);
+        void Build();
+        void Clear();
+        u32* Bucket(int bucketIndex);
+        const u32* Bucket(int bucketIndex) const;
+        T& Object(int objectIndex);
+        const T& Object(int objectIndex) const;
+
+        int NumObjects() const;
+
+        int NumNodes() const;
+        int NumLeaves() const;
+        int NumInnerNodes() const;
+        int TreeHeight() const;
+        KdTreeNode* Root();
+        const KdTreeNode* Root() const;
+        bool IsPartOfThisTree(const KdTreeNode *node) const;
+        //const AABB& BoundingAABB() const { return rootAABB;}
+
+        /// Traverses a ray through this kD-tree, and calls the given leafCallback function for each leaf of the tree.
+        /// Uses the "recursive B" method from Vlastimil Havran's thesis.
+        template <typename Func>
+        void RayQuery(const Ray& r, Func &leaftCallback);
+
+        template <typename Func>
+        void AABBQuery(const AABB& aabb, Func& leafCallback);
+    private:
+        static const int maxNodes = 256 * 1024;
+        static const int maxTreeDepth = 30;
+
+        std::vector<KdTreeNode> nodes;
+        //std::vector<u8, Aligned
+
+    };
+}

include/J3ML/Geometry/Line.hpp

@@ -0,0 +1,25 @@
+#pragma once
+
+
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/Geometry/LineSegment.hpp>
+
+namespace J3ML::Geometry
+{
+    class Line : public Shape
+    {
+    public:
+        Vector3 Position;  /// Specifies the origin of this line.
+        Vector3 Direction; /// The normalized direction vector of this ray.
+
+        Line(const Vector3& position, const Vector3& direction) : Position(position), Direction(direction) {}
+
+        static void ClosestPointLineLine(const Vector3 &v0, const Vector3 &v10, const Vector3 &v2, const Vector3 &v32, float &d, float &d2);
+
+        Vector3 ClosestPoint(const J3ML::Geometry::LineSegment &other, float &d, float &d2) const;
+
+        Vector3 GetPoint(float d) const;
+
+        Vector3 ClosestPoint(const Vector3 &targetPoint, float &d) const;
+    };
+}

include/J3ML/Geometry/LineSegment.h

@@ -1,16 +0,0 @@
-#pragma once
-
-#include <J3ML/LinearAlgebra/Vector3.h>
-
-namespace J3ML::Geometry
-{
-    using LinearAlgebra::Vector3;
-    class LineSegment
-    {
-    public:
-        LineSegment();
-        LineSegment(const Vector3& a, const Vector3& b);
-        Vector3 A;
-        Vector3 B;
-    };
-}

include/J3ML/Geometry/LineSegment.hpp

@@ -0,0 +1,244 @@
+#pragma once
+
+#include <J3ML/LinearAlgebra.hpp>
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/Geometry/Forward.hpp>
+
+namespace J3ML::Geometry
+{
+
+    /// A line segment in 3D space is a finite line with a start and end point.
+    class LineSegment : public Shape2D
+    {
+    public:
+        Vector3 A; /// The starting point of this line segment.
+        Vector3 B; /// The end point of this line segment.
+    public:
+        /// The default constructor does not initialize any members of this class.
+        /** This means that the values of the members A and B are undefined after creating a new LineSegment using this
+            default constructor. Remember to assign to them before use.
+            @see A, B. */
+        LineSegment();
+        /// Constructs a line segment through the given end points.
+        /** @see a, b. */
+        LineSegment(const Vector3& a, const Vector3& b);
+
+        /// Constructs a line segment from a ray or a line.
+        /** This constructor takes the ray/line origin position as the starting point of this line segment, and defines the end point
+            of the line segment using the given distance parameter.
+            @param d The distance along the ray/line for the end point of this line segment. This will set b = ray.pos + d * ray.dir
+            as the end point. When converting a ray to a line segment, it is possible to pass in a d value < 0, but in that case
+            the resulting line segment will not lie on the ray.
+            @see a, b, classes Ray, Line, Line::GetPoint(), Ray::GetPoint() */
+        explicit LineSegment(const Ray &ray, float d);
+
+        explicit LineSegment(const Line &line, float d);
+
+
+        /// Tests if the given point or line segment is contained on this line segment.
+        /** @param distanceThreshold Because a line segment is an one-dimensional object in 3D space, an epsilon value
+                is used as a threshold for this test. This effectively transforms this line segment to a capsule with
+                the radius indicated by this value.
+            @return True if this line segment contains the given point or line segment.
+            @see Intersects, ClosestPoint(), Distance(). */
+        bool Contains(const Vector3& point, float distanceThreshold = 1e-3f) const;
+        bool Contains(const LineSegment &lineSegment, float distanceThreshold = 1e-3f) const;
+
+        /// Quickly returns an arbitrary point inside this LineSegment. Used in GJK intersection test.
+        Vector3 AnyPointFast() const;
+
+        /// Computes an extreme point of this LineSegment in the given direction.
+        /** An extreme point is a farthest point along this LineSegment 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 LineSegment in the given direction. The returned point is always
+                either a or b.
+            @see a, b.*/
+        [[nodiscard]] Vector3 ExtremePoint(const Vector3 &direction) const;
+        Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
+
+        /// Translates this LineSegment in world space.
+        /** @param offset The amount of displacement to apply to this LineSegment, in world space coordinates.
+            @see Transform(). */
+        void Translate(const Vector3 &offset);
+
+        /// Applies a transformation to this line.
+        /** This function operates in-place.
+            @see Translate(), classes Matrix3x3, Matrix4x4, Quaternion, Transform(). */
+        void Transform(const Matrix3x3 &transform);
+        void Transform(const Matrix4x4 &transform);
+        void Transform(const Quaternion &transform);
+
+        void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
+
+        /// Returns a point on the line.
+        /** @param d The normalized distance along the line segment to compute. If a value in the range [0, 1] is passed, then the
+            returned point lies along this line segment. If some other value is specified, the returned point lies on the
+            line defined by this line segment, but not inside the interval from a to b.
+            @note The meaning of d here differs from Line::GetPoint and Ray::GetPoint. For the class LineSegment,
+                GetPoint(0) returns a, and GetPoint(1) returns b. This means that GetPoint(1) will not generally be exactly one unit
+                away from the starting point of this line segment, as is the case with Line and Ray.
+            @return (1-d)*a + d*b.
+            @see a, b, Line::GetPoint(), Ray::GetPoint(). */
+        [[nodiscard]] Vector3 GetPoint(float d) const;
+
+        /// Returns the center point of this line segment.
+        /** This function is the same as calling GetPoint(0.5f), but provided here as conveniency.
+            @see GetPoint(). */
+        [[nodiscard]] Vector3 CenterPoint() const;
+
+        /// Reverses the direction of this line segment.
+        /** This function swaps the start and end points of this line segment so that it runs from b to a.
+            This does not have an effect on the set of points represented by this line segment, but it reverses
+            the direction of the vector returned by Dir().
+            @note This function operates in-place.
+            @see a, b, Dir(). */
+        void Reverse();
+
+
+        /// Computes the closest point on this line segment to the given object.
+        /** @return The closest point on this line segment to the given object.
+            @see Contains(), Distance(), Intersects(). */
+        Vector3 ClosestPoint(const Vector3& point) const;
+        /// Computes the closest point on this line segment to the given object.
+        /** @param d [out] If specified, this parameter receives the normalized distance along
+                this line segment which specifies the closest point on this line segment to
+                the specified point.
+            @return The closest point on this line segment to the given object.
+            @see Contains(), Distance(), Intersects(). */
+        Vector3 ClosestPoint(const Vector3& point, float& d) const;
+
+        /** @param d2 [out] If specified, this parameter receives the (normalized, in case of line segment)
+                    distance along the other line object which specifies the closest point on that line to
+                    this line segment. */
+
+        /// Computes the closest point on this line segment to the given object.
+        /** @return The closest point on this line segment to the given object.
+            @see Contains(), Distance(), Intersects(). */
+        Vector3 ClosestPoint(const Ray& other)  const;
+        /// Computes the closest point on this line segment to the given object.
+        /** @return The closest point on this line segment to the given object.
+            @see Contains(), Distance(), Intersects(). */
+        Vector3 ClosestPoint(const Ray& other, float &d) const;
+        /// Computes the closest point on this line segment to the given object.
+        /** @return The closest point on this line segment to the given object.
+            @see Contains(), Distance(), Intersects(). */
+        Vector3 ClosestPoint(const Ray& other, float &d, float &d2) const;
+
+        /// Computes the closest point on this line segment to the given object.
+        /** @return The closest point on this line segment to the given object.
+            @see Contains(), Distance(), Intersects(). */
+        Vector3 ClosestPoint(const Line& other) const;
+        /// Computes the closest point on this line segment to the given object.
+        /** @return The closest point on this line segment to the given object.
+            @see Contains(), Distance(), Intersects(). */
+        Vector3 ClosestPoint(const Line& other, float &d) const;
+        /// Computes the closest point on this line segment to the given object.
+        /** @return The closest point on this line segment to the given object.
+            @see Contains(), Distance(), Intersects(). */
+        Vector3 ClosestPoint(const Line &other, float &d, float &d2) const;
+
+        /// Computes the closest point on this line segment to the given object.
+        /** @return The closest point on this line segment to the given object.
+            @see Contains(), Distance(), Intersects(). */
+        Vector3 ClosestPoint(const LineSegment& other) const;
+        /// Computes the closest point on this line segment to the given object.
+        /** @return The closest point on this line segment to the given object.
+            @see Contains(), Distance(), Intersects(). */
+        Vector3 ClosestPoint(const LineSegment& other, float &d) const;
+        /// Computes the closest point on this line segment to the given object.
+        /** @return The closest point on this line segment to the given object.
+            @see Contains(), Distance(), Intersects(). */
+        Vector3 ClosestPoint(const LineSegment& other, float &d, float &d2) const;
+
+
+
+        [[nodiscard]] float Distance(const Vector3 &point) const;
+        [[nodiscard]] float DistanceSq(const Vector3& point) const;
+        [[nodiscard]] float DistanceSq(const LineSegment &other) const;
+        float Distance(const Vector3 &point, float &d) const;
+        [[nodiscard]] float Distance(const Ray &other) const;
+        float Distance(const Ray &other, float &d) const;
+        float Distance(const Ray &other, float &d, float &d2) const;
+        [[nodiscard]] float Distance(const Line &other) const;
+        float Distance(const Line &other, float &d) const;
+        float Distance(const Line &other, float &d, float &d2) const;
+        [[nodiscard]] float Distance(const LineSegment &other) const;
+        float Distance(const LineSegment &other, float &d) const;
+        float Distance(const LineSegment &other, float &d, float &d2) const;
+        [[nodiscard]] float Distance(const Plane& other) const;
+
+        /// Returns the normalized direction vector that points in the direction a->b.
+        /** @note The returned vector is normalized, meaning that its length is 1, not |b-a|.
+            @see a, b. */
+        [[nodiscard]] Vector3 Dir() const;
+
+        /// Computes the length of this line segment.
+        /** @return |b-a|.
+            @see a, b. */
+        [[nodiscard]] float Length() const;
+
+        /// Computes the squared length of this line segment.
+        /** Calling this function is faster than calling Length(), since this function avoids computing a square root.
+            If you only need to compare lengths to each other and are not interested in the actual length values,
+            you can compare by using LengthSq(), instead of Length(), since Sqrt() is an order-preserving
+            (monotonous and non-decreasing) function. [similarOverload: Length] */
+        [[nodiscard]] float LengthSq() const;
+
+        /// Tests if this line segment is finite.
+        /** A line segment is <b><i>finite</i></b> if its endpoints a and b do not contain floating-point NaNs or +/-infs
+            in them.
+            @return True if both a and b have finite floating-point values. */
+        [[nodiscard]] bool IsFinite() const;
+
+
+        /// Tests whether this line segment 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. (for example, if this line segment is contained inside a sphere)
+            @todo Output intersection point. */
+        bool Intersects(const Plane& plane) const;
+        bool Intersects(const Plane& plane, float* d) const;
+        bool Intersects(const Triangle& triangle, float* d, Vector3* intersectionPoint) const;
+        bool Intersects(const Sphere& s, Vector3* intersectionPoint = 0, Vector3* intersectionNormal = 0, float* d = 0) const;
+        bool Intersects(const AABB& aabb, float& dNear, float& dFar) const;
+        bool Intersects(const AABB& aabb) const;
+        bool Intersects(const OBB& obb, float& dNear, float& dFar) const;
+        bool Intersects(const OBB& obb) const;
+        bool Intersects(const Capsule& capsule) const;
+        bool Intersects(const Polygon& polygon) const;
+        bool Intersects(const Frustum& frustum) const;
+        bool Intersects(const Polyhedron& polyhedron) const;
+        /** @param epsilon If testing intersection between two line segments, a distance threshold value is used to account
+                    for floating-point inaccuracies. */
+        bool Intersects(const LineSegment &segment, float epsilon = 1e-3f) const;
+
+        // TODO: Implement Circle class.
+        // TODO: This signature will be moved to bool Intersects(const Disc& disc) const;
+        //bool IntersectsDisc(const Circle& disc) const;
+
+        /// Converts this LineSegment to a Ray.
+        /** The pos member of the returned Ray will be equal to a, and the dir member equal to Dir().
+            @see class Ray, ToLine() */
+        [[nodiscard]] Ray ToRay() const;
+
+        /// Converts this LineSegment to a Line.
+        /** The pos member of the returned Line will be equal to a, and the dir member equal to Dir().
+            @see class Line, ToRay() */
+        [[nodiscard]] Line ToLine() const;
+
+        /// Tests if this line segment represents the same set of points than the given line segment.
+        /** @param distanceThreshold Specifies how much distance threshold to allow in the comparison.
+            @return True if a == rhs.a && b == rhs.b, or, a == rhs.b && b = rhs.a, within the given epsilon. */
+        bool Equals(const LineSegment &rhs, float distanceThreshold = 1e-3f) const;
+
+        /// Compares whether this LineSegment and the given LineSegment are identical bit-by-bit in the underlying representation.
+        /** @note Prefer using this over e.g. memcmp, since there can be SSE-related padding in the structures. */
+        //bool BitEquals(const LineSegment &other) const { return a.BitEquals(other.a) && b.BitEquals(other.b); }
+
+
+        /// Ret
+    };
+
+    LineSegment operator *(const Matrix4x4 &transform, const LineSegment &l);
+}

include/J3ML/Geometry/LineSegment2D.hpp

@@ -0,0 +1,185 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file LineSegment2D.hpp
+/// @desc A 2D representation of a finite line between two points.
+/// @edit 2024-10-22
+
+#pragma once
+
+#include <J3ML/LinearAlgebra/Vector2.hpp>
+#include <J3ML/LinearAlgebra/Matrix3x3.hpp>
+#include <J3ML/LinearAlgebra/Matrix4x4.hpp>
+
+namespace J3ML::Geometry
+{
+    /// A line segment in 2D space is a finite line with a start and end point.
+    class LineSegment2D {
+    public:
+        /// The starting point of this line segment.
+        Vector2 A;
+        /// The end point of this line segment.
+        Vector2 B;
+
+        /// The default constructor does not initialize any members of this class.
+        /** This means that the values of the members a and b are undefined after creating a new LineSegment2D using this
+            default constructor. Remember to assign to them before use.
+            @see A, B. */
+        LineSegment2D() {}
+
+        /// Constructs a line segment through the given end points.
+        LineSegment2D(const Vector2 &a, const Vector2 &b);
+
+        /// Returns a point on the line.
+        /** @param d The normalized distance along the line segment to compute. If a value in the range [0, 1] is passed, then the
+                returned point lies along this line segment. If some other value is specified, the returned point lies on the line
+                defined by this line segment, but *not* inside the interval from a to b.
+            @note The meaning of d here differs from Line2D::GetPoint and Ray2D::GetPoint. For the class LineSegment2D,
+                GetPoint(0) returns a, and getPoint(1) returns b. This means that GetPoint(1) will not generally be exactly one unit
+                away from the starting point of this line segment, as is the case with Line2D and Ray2D.
+            @return (1-d)*a + d*b;
+            @see a, b, Line2D::GetPoint(), Ray2D::GetPoint() */
+        Vector2 GetPoint(float d) const;
+
+        /// Returns the center point of this line segment.
+        /** This function is the same as calling GetPoint(0.5f), but provided here as convenience.
+            @see GetPoint(). */
+        Vector2 CenterPoint() const;
+
+        Vector2 Centroid() const;
+
+        /// Reverses the direction of this line segment.
+        /** This function swaps the start and end points of this line segment so that it runs from b to a.
+            This does not have an effect on the set of points represented by this line segment, but it reverses
+            the direction of the vector returned by Dir().
+            @note This function operates in-place.
+            @see a, b, Dir(). */
+        void Reverse();
+
+        /// Returns the normalized direction vector that points in the direction a->b.
+        /** @note The returned vector is normalized, meaning that its length is 1, not |b-a|.
+            @see a, b. */
+        Vector2 Dir() const;
+
+        /// Quickly returns an arbitrary point inside this LineSegment2D. Used in GJK intersection test.
+        Vector2 AnyPointFast() const;
+
+        /// Computes an extreme point of this LineSegment2D in the given direction.
+        /** An extreme point is a farthest point along this LineSegment2D 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 un-normalized, but may not be null.
+            @return An extreme point of this LineSegment2D in the given direction. The returned point is always
+                either a or b.
+            @see a, b. **/
+        Vector2 ExtremePoint(const Vector2 &direction) const;
+
+        Vector2 ExtremePoint(const Vector2 &direction, float &projectionDistance) const;
+
+        /// Translates this LineSegment2D in world space.
+        /** @param offset The amount of displacement to apply to this LineSegment2D, in world space coordinates.
+            @see Transform(). */
+        void Translate(const Vector2& offset);
+
+        /// Applies a transformation to this line.
+        /** This function operates in-place.
+            @see Translate(), Transform(), classes Matrix3x3, Matrix4x4, Quaternion*/
+        void Transform(const Matrix3x3&  transform);
+        void Transform(const Matrix4x4&  transform);
+        void Transform(const Quaternion& transform);
+
+        /// Computes the length of this line segment.
+        /** @return |b - a|
+            @see a, b. */
+        float Length() const;
+        /// Computes the squared length of this line segment.
+        /** Calling this function is faster than calling Length(), since this function avoids computing a square root.
+            If you only need to compare lengths to each other and are not interested in the actual length values,
+            you can compare using LengthSq(), instead of Length(), since Sqrt() is an order-preserving,
+            (monotonous and non-decreasing) function. */
+        float LengthSq() const;
+        float LengthSquared() const;
+
+        /// Tests if this line segment is finite
+        /** A line segment is finite if its endpoints a and b do not contain floating-point NaNs for +/- infs
+            in them.
+            @return True if both a and b have finite floating-point values. */
+        bool IsFinite() const;
+
+        /// Tests if this line segment represents the same set of points than the the given line segment.
+        /** @param epsilon Specifies how much distance threshold to allow in the comparison.
+            @return True if a == rhs.a && b == rhs.b, or, a == rhs.b && b == rhs.a, within the given epsilon. */
+        bool Equals(const LineSegment2D& rhs, float epsilon = 1e-3f) const;
+
+        /// Tests if the given point or line segment is contained on this line segment.
+        /** @param epsilon Because a line segment is a one-dimensional object in 3D space, an epsilon value
+                is used as a threshold for this test. This effectively transforms this line segment to a capsule with
+                the radius indicated by this value.
+            @return True if this line segment contains the given point or line segment.
+            @see Intersects, ClosestPoint(), Distance(). */
+        bool Contains(const Vector2& point, float epsilon = 1e-3f) const;
+        bool Contains(const LineSegment2D& rhs, float epsilon = 1e-3f) const;
+
+        /// Computes the closest point on this line segment to the given object.
+        /** @param d If specified, this parameter receives the normalized distance along
+                this line segment which specifies the closest point on this line segment to
+                the specified point.
+            @return The closest point on this line segment to the given object.
+            @see Contains(), Distance(), Intersects(). */
+        Vector2 ClosestPoint(const Vector2& point) const;
+        Vector2 ClosestPoint(const Vector2& point, float& d) const;
+
+
+
+        /** @param d2 [out] If specified, this parameter receives the (normalized, in case of line segment)
+                distance along the other line object which specifies the closest point on that line to
+                this line segment. */
+        Vector2 ClosestPoint(const LineSegment2D& other) const;
+        Vector2 ClosestPoint(const LineSegment2D& other, float& d) const;
+        Vector2 ClosestPoint(const LineSegment2D& other, float& d, float& d2) const;
+
+        /// Computes the distance between this line segment and the given object.
+        /** @param d [out] If specified, this parameter receives the normalized distance along
+                this line segment which specifies the closest point on this line segment to
+                the specified point.
+            @return The distance between this line segment and the given object.
+            @see */
+        float Distance(const Vector2& point) const;
+        float Distance(const Vector2& point, float& d) const;
+
+        /** @param d2 [out] If specified, this parameter receives the (normalized, in case of line segment)
+                distance along the other line object which specifies the closest point on that line to
+                this line segment. */
+        float Distance(const LineSegment2D& other) const;
+        float Distance(const LineSegment2D& other, float& d) const;
+        float Distance(const LineSegment2D& other, float& d, float& d2) const;
+
+        float DistanceSq(const Vector2& point) const;
+        float DistanceSq(const LineSegment2D& other) const;
+
+        /** @param epsilon If testing intersection between two line segments, a distance threshold value is used to account
+                for floating point inaccuracies. */
+        bool Intersects(const LineSegment2D& lineSegment, float epsilon = 1e-3f) const;
+
+
+        /// Projects this LineSegment2D onto the given 1D axis direction vector.
+        /** This function collapses this LineSegment2D onto a 1D axis for the purposes of e.g. separate axis test computations.
+            This function returns a 1D range [outMin, outNax] 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 vector along the projection axis.
+            @param outMax [out] Returns the maximum extent of this vector along the projection axis. */
+        void ProjectToAxis(const Vector2& direction, float& outMin, float& outMax) const;
+
+    protected:
+    private:
+    };
+
+    LineSegment2D operator * (const Matrix3x3& transform, const LineSegment2D& line);
+    LineSegment2D operator * (const Matrix4x4& transform, const LineSegment2D& line);
+    LineSegment2D operator * (const Quaternion& transform, const LineSegment2D& line);
+}

include/J3ML/Geometry/OBB.h

@@ -1,48 +0,0 @@
-#pragma once
-
-#include <J3ML/Geometry.h>
-#include <J3ML/Geometry/AABB.h>
-#include <J3ML/Geometry/LineSegment.h>
-#include <J3ML/Geometry/Polyhedron.h>
-
-namespace J3ML::Geometry {
-    class OBB
-    {
-    public:
-        // The center position of this OBB
-        Vector3 pos;
-        // Stores half-sizes to x, y, and z directions in the local space of this OBB.
-        Vector3 r;
-        // Specifies normalized direc tion vectors for the local axes
-        Vector3 axis[3];
-
-        OBB() {}
-        OBB(const Vector3& pos, const Vector3& radii, const Vector3& axis0, const Vector3& axis1, const Vector3& axis2);
-        OBB(const Geometry::AABB& aabb);
-        inline static int NumFaces() { return 6;  }
-        inline static int NumEdges() { return 12; }
-        inline static int NumVertices() { return 8; }
-
-        Polyhedron ToPolyhedron() const;
-
-        Geometry::AABB MinimalEnclosingAABB() const;
-
-        Sphere MinimalEnclosingSphere() const;
-        Sphere MaximalContainedSphere() const;
-        Vector3 Size() const;
-        Vector3 HalfSize() const;
-        Vector3 Diagonal() const;
-        Vector3 HalfDiagonal() const;
-        bool IsFinite() const;
-        bool IsDegenerate() const;
-        Vector3 CenterPoint() const;
-        Vector3 Centroid() const;
-
-        Vector3 AnyPointFast() const;
-
-        float Volume();
-        float SurfaceArea();
-        Geometry::LineSegment Edge(int edgeIndex) const;
-        Vector3 CornerPoint(int cornerIndex) const;
-    };
-}

include/J3ML/Geometry/OBB.hpp

@@ -0,0 +1,137 @@
+#pragma once
+
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/Geometry/Forward.hpp>
+#include <J3ML/LinearAlgebra.hpp>
+
+
+namespace J3ML::Geometry {
+    /// A 3D arbitrarily oriented bounding box
+    // This data structure represents a box in 3D space. The local axes of this box can be arbitrarily oriented/rotated
+    // with respect to the global world coordinate system. This allows OBBs to more tightly bound objects than AABBs do,
+    // which always align with the world space axes. This flexibility has the drawback that the geometry tests and operations
+    // involving OBBs are more costly, and representing an OBB in memory takes more space (15 floats vs 6 floats)
+    class OBB : public Shape
+    {
+    public:
+        // The center position of this OBB
+        Vector3 pos;
+        // Stores half-sizes to x, y, and z directions in the local space of this OBB.
+        Vector3 r;
+        // Specifies normalized direction vectors for the local axes
+        Vector3 axis[3];
+
+        /// Default constructor that does not initialize any member values.
+        OBB() {}
+        // Constructs an OBB by explicitly initializing all member values
+        OBB(const Vector3& pos, const Vector3& radii, const Vector3& axis0, const Vector3& axis1, const Vector3& axis2);
+        OBB(const AABB& aabb);
+        inline static int NumFaces() { return 6;  }
+        inline static int NumEdges() { return 12; }
+        inline static int NumVertices() { return 8; }
+
+
+        float MinX() const;
+        float MinY() const;
+        float MinZ() const;
+        float MaxX() const;
+        float MaxY() const;
+        float MaxZ() const;
+
+        Vector3 MinPoint() const;
+        Vector3 MaxPoint() const;
+
+        Polyhedron ToPolyhedron() const;
+        //PBVolume<6> ToPBVolume() const;
+        AABB MinimalEnclosingAABB() const;
+
+        bool Intersects(const LineSegment &lineSegment) const;
+
+        Sphere MinimalEnclosingSphere() const;
+        Sphere MaximalContainedSphere() const;
+        Vector3 Size() const;
+        Vector3 HalfSize() const;
+        Vector3 Diagonal() const;
+        Vector3 HalfDiagonal() const;
+        void Transform(const Matrix3x3& transform);
+        void Transform(const Matrix4x4& transform);
+        void Transform(const Quaternion& transform);
+        bool IsFinite() const;
+        bool IsDegenerate() const;
+        Vector3 CenterPoint() const;
+        Vector3 Centroid() const;
+
+        Vector3 AnyPointFast() const { return pos; }
+
+        float Volume() const;
+        float SurfaceArea() const;
+        Geometry::LineSegment Edge(int edgeIndex) const;
+        Vector3 CornerPoint(int cornerIndex) const;
+
+        Vector3 PointInside(float x, float y, float z) const;
+        Vector3 PointInside(const Vector3& pt) const;
+
+        void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
+
+        int UniqueFaceNormals(Vector3 *out) const;
+
+        int UniqueEdgeDirections(Vector3 *out) const;
+
+        Vector3 PointOnEdge(int edgeIndex, float u) const;
+
+        Vector3 FaceCenterPoint(int faceIndex) const;
+
+        void GetCornerPoints(Vector3 *outPointArray) const;
+
+        void GetFacePlanes(Plane *outPlaneArray) const;
+
+        Plane FacePlane(int faceIndex) const;
+
+        void ExtremePointsAlongDirection(const Vector3 &dir, const Vector3 *pointArray, int numPoints, int &idxSmallest,
+                                         int &idxLargest, float &smallestD, float &largestD);
+
+        Vector3 FacePoint(int faceIndex, float u, float v) const;
+
+        Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
+
+        Vector3 ExtremePoint(const Vector3 &direction) const;
+
+
+        void SetFrom(const AABB &aabb, const Matrix3x3 &transform);
+
+        void SetFrom(const AABB &aabb, const Matrix4x4 &transform);
+
+        void SetFrom(const AABB &aabb, const Quaternion &transform);
+
+        bool Intersects(const Plane& plane) const;
+
+        Matrix4x4 LocalToWorld() const;
+
+        Matrix4x4 WorldToLocal() const;
+
+        Vector3 ClosestPoint(const Vector3 &targetPoint) const;
+
+        /// Expands this OBB to enclose the given object. The axis directions of this OBB remain intact.
+        /** This function operates in-place. This function does not necessarily result in an OBB that is an
+            optimal fit for the previous OBB and the given point. */
+        void Enclose(const Vector3 & vector3);
+
+        float Distance(const Vector3 &point) const;
+
+
+        /// Tests if the given object is fully contained inside this OBB.
+        /** This function returns true if the given object lies inside this OBB, and false otherwise.
+            @note The comparison is performed using less-or-equal, so the faces of this OBB count as being inside,
+            but due to float inaccuracies, this cannot generally be relied upon. */
+        bool Contains(const Vector3& point) const;
+        bool Contains(const LineSegment&) const;
+        bool Contains(const AABB&) const;
+        bool Contains(const OBB&) const;
+        bool Contains(const Triangle&)  const;
+        bool Contains(const Polygon&)   const;
+        bool Contains(const Frustum&)   const;
+        bool Contains(const Polyhedron&) const;
+
+
+    };
+}

include/J3ML/Geometry/PBVolume.hpp

@@ -0,0 +1,243 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file PBVolume.hpp
+/// @desc Implements a convex polyhedron data structure.
+/// @edit 2024-07-09
+
+#pragma once
+
+#include <J3ML/Geometry/AABB.hpp>
+#include <J3ML/Geometry/Polyhedron.hpp>
+#include <J3ML/Geometry/Plane.hpp>
+#include <J3ML/Geometry/Sphere.hpp>
+
+
+namespace J3ML::Geometry
+{
+
+    enum class CullTestResult
+    {
+        // The tested objects don't intersect - they are fully disjoint.
+        Outside,
+        // The tested object is at least not fully contained inside the other object, but no other information is known.
+        // The objects might intersect or be disjoint.
+        NotContained,
+        // The tested object is fully contained inside the other object.
+        Inside
+    };
+
+    /// PBVolume is a "plane bounded volume", a convex polyhedron represented by a set
+    /// of planes. The number of planes is fixed at compile time so that compilers are able
+    template <int N>
+    class PBVolume
+    {
+    public:
+        Plane p[N];
+
+        int NumPlanes() const { return N; }
+
+        bool Contains(const Vector3& point) const {
+            for (int i = 0; i< N; ++i)
+                if (p[i].SignedDistance(point) > 0.f)
+                    return false;
+            return true;
+        }
+
+        CullTestResult InsideOrIntersects(const AABB& aabb) const {
+            CullTestResult result = CullTestResult::Inside;
+
+            for (int i = 0; i < N; ++i) {
+                Vector3 nPoint;
+                Vector3 pPoint;
+
+                nPoint.x = (p[i].normal.x < 0.f ? aabb.maxPoint.x : aabb.minPoint.x);
+                nPoint.y = (p[i].normal.y < 0.f ? aabb.maxPoint.y : aabb.minPoint.y);
+                nPoint.z = (p[i].normal.z < 0.f ? aabb.maxPoint.z : aabb.minPoint.z);
+
+                // Find the n and p points of the aabb. (The nearest and farthes corners relative to the plane)
+                //const vec &sign = npPointsSignLUT[((p[i].normal.z >= 0.f) ? 4 : 0) +
+                //                                  ((p[i].normal.y >= 0.f) ? 2 : 0) +
+                //                                  ((p[i].normal.x >= 0.f) ? 1 : 0)];
+                //const vec nPoint = c + sign * r;
+                //const vec pPoint = c - sign * r;
+
+                float a = p[i].SignedDistance(nPoint);
+                if (a >= 0.f)
+                    return CullTestResult::Outside; // The AABB is certainly outside this PBVolume.
+                a = p[i].SignedDistance(pPoint);
+                if (a >= 0.f)
+                    result = CullTestResult::NotContained; // At least one vertex is outside this PBVolume. The whole AABB can't possibly be contained in this PBVolume.
+            }
+            // We can return here either TestInside or TestNotContained, but it's possible that the AABB was outside the frustum, and we
+            // just failed to find a separating axis.
+            return result;
+        }
+
+        CullTestResult InsideOrIntersects(const Sphere& sphere) const {
+            CullTestResult result = CullTestResult::Inside;
+            for(int i = 0; i < N; ++i)
+            {
+                float d = p[i].SignedDistance(sphere.Position);
+                if (d >= sphere.Radius)
+                    return CullTestResult::Outside;
+                else if (d >= -sphere.Radius)
+                    result = CullTestResult::NotContained;
+            }
+            return result;
+        }
+    private:
+        /// A helper struct used only internally in ToPolyhedron.
+        struct CornerPt
+        {
+            int ptIndex; // Index to the Polyhedron list of vertices.
+            int j, k;    // The two plane faces in addition to the main plane that make up this point.
+        };
+
+        bool ContainsExcept(const Vector3 &point, int i, int j, int k) const
+        {
+            for(int l = 0; l < N; ++l)
+                if (l != i && l != j && l != k && p[l].SignedDistance(point) > 0.f)
+                    return false;
+            return true;
+        }
+
+    public:
+
+        Polyhedron ToPolyhedron() const {
+            Polyhedron ph;
+            std::vector<CornerPt> faces[N];
+            for (int i = 0; i < N-2; ++i)
+                for (int j = i+1; j < N-1; ++j)
+                    for (int k = j+1; k < N; ++k)
+                    {
+                        Vector3 corner;
+                        bool intersects = p[i].Intersects(p[j], p[k], 0, &corner);
+                        if (intersects && ContainsExcept(corner, i, j, k)) {
+                            CornerPt pt;
+
+                            // Find if this vertex is duplicate of an existing vertex.
+                            bool found = false;
+                            for (size_t l = 0; i < ph.v.size(); ++l)
+                                if (Vector3(ph.v[l]).Equals(corner)) {
+                                    found = true;
+                                    pt.ptIndex = (int)l;
+                                    break;
+                                }
+
+                            if (!found) // New vertex?
+                            {
+                                ph.v.push_back(corner);
+                                pt.ptIndex = (int)ph.v.size()-1;
+                            } // else existing corner point
+
+                            pt.j = j;
+                            pt.k = k;
+                            faces[i].push_back(pt);
+
+                            pt.j = i;
+                            pt.k = k;
+                            faces[j].push_back(pt);
+
+                            pt.j = i;
+                            pt.k = j;
+                            faces[k].push_back(pt);
+
+                        }
+                    }
+
+            // Check if we got a degenerate polyhedron?
+            if (ph.v.size() <= 1)
+                return ph;
+            else if (ph.v.size() == 2) {
+                // Create a degenerate face that's an edge.
+                Polyhedron::Face face;
+                face.v.push_back(0);
+                face.v.push_back(1);
+                ph.f.push_back(face);
+                return ph;
+            } else if (ph.v.size() == 3) {
+                // Create a degenerate face that's a triangle.
+                Polyhedron::Face face;
+                face.v.push_back(0);
+                face.v.push_back(1);
+                face.v.push_back(2);
+                ph.f.push_back(face);
+                return ph;
+            }
+
+            // Connect the edges in each face using selection sort.
+            for(int i = 0; i < N; ++i)
+            {
+                std::vector<CornerPt> &pt = faces[i];
+                if (pt.size() < 3)
+                    continue;
+                for(size_t j = 0; j < pt.size()-1; ++j)
+                {
+                    CornerPt &prev = pt[j];
+                    bool found = false;
+                    for(size_t k = j+1; k < pt.size(); ++k)
+                    {
+                        CornerPt &cur = pt[k];
+                        if (cur.j == prev.k)
+                        {
+                            Swap(cur, pt[j+1]);
+                            found = true;
+                            break;
+                        }
+                        if (cur.k == prev.k)
+                        {
+                            Swap(cur.j, cur.k);
+                            Swap(cur, pt[j+1]);
+                            found = true;
+                            break;
+                        }
+                    }
+                    assert(found);
+                }
+                assert(pt[0].j == pt[pt.size()-1].k);
+                Polyhedron::Face face;
+                for(size_t j = 0; j < pt.size(); ++j)
+                {
+                    face.v.push_back(pt[j].ptIndex);
+                }
+                ph.f.push_back(face);
+            }
+
+            // Fix up winding directions.
+            for(size_t i = 0; i < ph.f.size(); ++i)
+            {
+                // TODO: Why problem?
+                //Plane face = ph.FacePlane((int)i);
+                //for(size_t j = 0; j < ph.v.size(); ++j)
+                //{
+                //    if (face.SignedDistance(ph.v[j]) > 1e-3f)
+                //    {
+                //        ph.f[i].FlipWindingOrder();
+                //        break;
+                //    }
+                //}
+            }
+            return ph;
+        }
+
+        /// Computes the set intersection of this PBVolume and the PBVolume rhs.
+        /// That is, returns the convex set of points that are contained in both this and rhs.
+        /// Set intersection is symmetric, so a.SetIntersection(b) is the same as b.SetIntersection(a).
+        /// @note The returned PBVolume may contain redundant planes, these are not pruned.
+        template<int M>
+        PBVolume<N+M> SetIntersection(const PBVolume<M> &rhs) const
+        {
+            PBVolume<N+M> res;
+            for(int i = 0; i < N; ++i)
+                res.p[i] = p[i];
+            for(int i = 0; i < M; ++i)
+                res.p[N+i] = rhs.p[i];
+            return res;
+        }
+    };
+}

include/J3ML/Geometry/Plane.h

@@ -1,17 +0,0 @@
-#pragma once
-#include <J3ML/LinearAlgebra/Vector3.h>
-
-
-
-namespace J3ML::Geometry
-{
-    using J3ML::LinearAlgebra::Vector3;
-
-    class Plane
-    {
-    public:
-        Vector3 Position;
-        Vector3 Normal;
-        float distance = 0.f;
-    };
-}

include/J3ML/Geometry/Plane.hpp

@@ -0,0 +1,106 @@
+#pragma once
+
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/LinearAlgebra.hpp>
+#include <J3ML/Geometry/Forward.hpp>
+
+
+namespace J3ML::Geometry
+{
+
+    class Plane : public Shape
+    {
+    public:
+        Vector3 Position;
+        Vector3 Normal;
+        float distance = 0.f;
+    public:
+        Plane() {}
+        /// Constructs a plane by directly specifying the normal and distance parameters.
+        Plane(const Vector3& normal, float d);
+
+        Plane(const Vector3& v1, const Vector3 &v2, const Vector3& v3);
+        Plane(const Vector3& pos, const Vector3& norm);
+        Plane(const Line &line, const Vector3 &normal);
+
+        void Set(const Vector3& v1, const Vector3& v2, const Vector3& v3);
+
+        void Set(const Vector3 &point, const Vector3 &normal_);
+
+        bool Intersects(J3ML::Geometry::Ray ray, float *dist) const;
+
+        /// Tests if the given point lies on the positive side of this plane.
+        /** A plane divides the space in three sets: the negative halfspace, the plane itself, and the positive halfspace.
+            The normal vector of the plane points towards the positive halfspace.
+            @return This function returns true if the given point lies either on this plane itself, or in the positive
+                halfspace of this plane.
+            @see IsInPositiveDirection, AreOnSameSide(), Distance(), SignedDistance(). */
+        bool IsOnPositiveSide(const Vector3 &point) const;
+
+        float SignedDistance(const Vector3 &point) const;
+        float SignedDistance(const AABB &aabb) const;
+        float SignedDistance(const OBB &obb) const;
+        float SignedDistance(const Capsule &capsule) const;
+        //float Plane::SignedDistance(const Circle &circle) const { return Plane_SignedDistance(*this, circle); }
+        float SignedDistance(const Frustum &frustum) const;
+        //float SignedDistance(const Line &line) const { return Plane_SignedDistance(*this, line); }
+        float SignedDistance(const LineSegment &lineSegment) const;
+        float SignedDistance(const Ray &ray) const;
+        //float Plane::SignedDistance(const Plane &plane) const { return Plane_SignedDistance(*this, plane); }
+        float SignedDistance(const Polygon &polygon) const;
+        float SignedDistance(const Polyhedron &polyhedron) const;
+        float SignedDistance(const Sphere &sphere) const;
+        float SignedDistance(const Triangle &triangle) const;
+
+        static bool
+        IntersectLinePlane(const Vector3 &planeNormal, float planeD, const Vector3 &linePos, const Vector3 &lineDir,
+                           float &t);
+
+        float Distance(const Vector3 &) const;
+
+        float Distance(const LineSegment &lineSegment) const;
+
+        float Distance(const Sphere &sphere) const;
+
+        float Distance(const Capsule &capsule) const;
+
+        bool Intersects(const Line &line, float *dist) const;
+
+        bool Intersects(const LineSegment &lineSegment, float *dist) const;
+
+        bool Intersects(const Sphere &sphere) const;
+
+        bool Intersects(const Capsule &capsule) const;
+
+        bool Intersects(const AABB &aabb) const;
+
+        bool Intersects(const OBB &obb) const;
+
+        bool Intersects(const Triangle &triangle) const;
+
+        bool Intersects(const Frustum &frustum) const;
+
+        bool Intersects(const Polyhedron &polyhedron) const;
+
+        LineSegment Project(const LineSegment &segment);
+
+        Vector3 Project(const Vector3 &point) const;
+
+
+        /// Projects the given point to the negative half-space of this plane.
+        Vector3 ProjectToNegativeHalf(const Vector3& point) const;
+
+        /// Projects the given point to the positive half-space of this plane.
+        Vector3 ProjectToPositiveHalf(const Vector3& point) const;
+
+
+        bool IsParallel(const Plane& plane, float epsilon = 1e-3f) const {
+            return Normal.Equals(plane.Normal, epsilon);
+        }
+
+        /// Returns true if the two planes are equal, and their normals are oriented to the same direction.
+        bool Equals(const Plane& other, float epsilon = 1e-3f) const {
+            return IsParallel(other, epsilon) && Math::EqualAbs(distance, other.distance, epsilon);
+        }
+    };
+}

include/J3ML/Geometry/Polygon.h

@@ -1,7 +0,0 @@
-#pragma once
-
-namespace J3ML::Geometry {
-    class Polygon {
-
-    };
-}

include/J3ML/Geometry/Polygon.hpp

@@ -0,0 +1,106 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file Polygon.hpp
+/// @desc The Polygon geometry object.
+/// @edit 2024-08-01
+
+#pragma once
+
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/Geometry/Forward.hpp>
+#include <J3ML/LinearAlgebra.hpp>
+#include <vector>
+
+namespace J3ML::Geometry {
+
+    class Polygon : public Shape2D
+    {
+    public:
+        std::vector<Vector3> vertices;
+
+        /// Quickly returns an arbitrary point inside this AABB. Used in GJK intersection test.
+        Vector3 AnyPointFast() const;
+
+        AABB MinimalEnclosingAABB() const;
+        int NumVertices() const;
+        Vector3 Vertex(int vertexIndex) const;
+        Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
+
+        Vector3 ExtremePoint(const Vector3 &direction) const;
+
+        void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
+
+        bool Intersects(const Capsule &capsule) const;
+        bool Intersects(const Line &line) const;
+        bool Intersects(const Ray &ray) const;
+        bool Intersects(const LineSegment &lineSegment) const;
+        bool Intersects(const Plane &plane) const;
+        bool Intersects(const AABB &aabb) const;
+        bool Intersects(const OBB &obb) const;
+        bool Intersects(const Triangle &triangle, float polygonThickness = 1e-3f) const;
+        bool Intersects(const Polygon &polygon, float polygonThickness = 1e-3f) const;
+        bool Intersects(const Frustum &frustum) const;
+        bool Intersects(const Polyhedron &polyhedron) const;
+        bool Intersects(const Sphere &sphere) const;
+        std::vector<Triangle> Triangulate() const;
+        /// Tests if this polygon is planar.
+        /** A polygon is planar if all its vertices lie on the same plane.
+            @note Almost all functions in this class require that the polygon is planar. While you can store vertices of
+                non-planar polygons in this class, they are better avoided. Read the member function documentation carefully
+                to avoid calling for non-planar polygons any functions which assume planarity.
+            @see IsConvex(), IsSimple(), IsNull(), IsFinite(), IsDegenerate(). */
+        bool IsPlanar(float epsilonSq = 1e-4f) const;
+
+        Vector2 MapTo2D(int i) const;
+
+        Vector2 MapTo2D(const Vector3 &point) const;
+
+        Vector3 BasisU() const;
+
+        Vector3 BasisV() const;
+
+        Plane PlaneCCW() const;
+
+        bool Contains(const Polygon &worldSpacePolygon, float polygonThickness) const;
+
+        bool Contains(const Vector3 &worldSpacePoint, float polygonThicknessSq = 1e-2f) const;
+
+        bool Contains(const LineSegment &worldSpaceLineSegment, float polygonThickness) const;
+
+        bool Contains(const Triangle &worldSpaceTriangle, float polygonThickness) const;
+
+        LineSegment Edge(int i) const;
+
+        bool ConvexIntersects(const AABB &aabb) const;
+
+        bool ConvexIntersects(const OBB &obb) const;
+
+        bool ConvexIntersects(const Frustum &frustum) const;
+
+        Vector3 MapFrom2D(const Vector2 &point) const;
+
+        bool Intersects2D(const LineSegment &segment) const;
+
+        Vector3 ClosestPoint(const LineSegment &lineSegment) const;
+
+        Vector3 ClosestPoint(const LineSegment &lineSegment, Vector3 *lineSegmentPt) const;
+
+        Vector3 ClosestPoint(const Vector3 &point) const;
+
+        /// Converts this Polygon to a Polyhedron representation.
+        /** This function will create a Polyhedron with two faces, one for the front face of this Polygon,
+            and one for the back face.
+            @todo Add ToPolyhedron(float polygonThickness)
+            @see Triangulate(), MinimalEnclosingAABB(). */
+        Polyhedron ToPolyhedron() const;
+
+    protected:
+
+
+    };
+}

include/J3ML/Geometry/Polyhedron.h

@@ -1,8 +0,0 @@
-#pragma once
-
-namespace J3ML::Geometry
-{
-    class Polyhedron {
-
-    };
-}

include/J3ML/Geometry/Polyhedron.hpp

@@ -0,0 +1,133 @@
+#pragma once
+
+#include <vector>
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/Geometry/Forward.hpp>
+#include <J3ML/LinearAlgebra.hpp>
+
+
+namespace J3ML::Geometry
+{
+
+    // Represents a three-dimensional closed geometric solid defined by flat polygonal faces.
+    class Polyhedron : public Shape
+    {
+    public:
+        // Stores a list of indices of a single face of a Polygon
+        struct Face
+        {
+            // Specifies the indices of the corner vertices of the polyhedron.
+            // Indices point to the polyhedron vertex array.
+            // The face vertices should all lie on the same plane.
+            // The positive direction of the plane (the direction the face outwards normal points)
+            // is the one where the vertices are wound in counter-clockwise order.
+            std::vector<int> v;
+
+            // Reverses the winding order of this face. This has the effect of reversing the direction
+            // the normal of this face points to.
+            void FlipWindingOrder();
+
+        };
+
+        // Specifies the vertices of this polyhedron.
+        std::vector<Vector3> v;
+        std::vector<Face> f;
+    public:
+        /// The default constructor creates a null polyhedron.
+        /** The null polyhedron has 0 vertices and 0 faces.
+            @see IsNull(). */
+        Polyhedron() = default;
+
+        [[nodiscard]] int NumVertices() const {return (int)v.size();}
+        [[nodiscard]] int NumFaces() const { return (int)f.size();}
+        [[nodiscard]] AABB MinimalEnclosingAABB() const;
+
+        [[nodiscard]] Vector3 Vertex(int vertexIndex) const;
+
+        [[nodiscard]] bool Contains(const Vector3&) const;
+        [[nodiscard]] bool Contains(const LineSegment&) const;
+        [[nodiscard]] bool Contains(const Triangle&) const;
+        [[nodiscard]] bool Contains(const Polygon&) const;
+        [[nodiscard]] bool Contains(const AABB&) const;
+        [[nodiscard]] bool Contains(const OBB&) const;
+        [[nodiscard]] bool Contains(const Frustum&) const;
+        [[nodiscard]] bool Contains(const Polyhedron&) const;
+
+        [[nodiscard]] bool ContainsConvex(const Vector3&, float epsilon = 1e-4f) const;
+        [[nodiscard]] bool ContainsConvex(const LineSegment&) const;
+        [[nodiscard]] bool ContainsConvex(const Triangle&) const;
+
+        /// Tests whether this polyhedron 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 polyhedron,
+            or this polyhedron is contained inside a sphere, etc.)
+            @return True if an intersection occurs or one of the objects is contained inside the other, false otherwise.
+            @note This function assumes that this polyhedron is closed and the edges are not self-intersecting.
+            @see Contains(), ContainsConvex(), ClosestPoint(), ClosestPointConvex(), Distance(), IntersectsConvex().
+            @todo Add Intersects(Circle/Disc). */
+        [[nodiscard]] bool Intersects(const Line&) const;
+        [[nodiscard]] bool Intersects(const LineSegment&) const;
+        [[nodiscard]] bool Intersects(const Ray&) const;
+        [[nodiscard]] bool Intersects(const Plane&) const;
+        [[nodiscard]] bool Intersects(const Polyhedron&) const;
+        [[nodiscard]] bool Intersects(const AABB& aabb) const;
+        [[nodiscard]] bool Intersects(const OBB&) const;
+        [[nodiscard]] bool Intersects(const Triangle&) const;
+        [[nodiscard]] bool Intersects(const Polygon&) const;
+        [[nodiscard]] bool Intersects(const Frustum&) const;
+        [[nodiscard]] bool Intersects(const Sphere&) const;
+        [[nodiscard]] bool Intersects(const Capsule& capsule) const;
+
+        [[nodiscard]] bool IsClosed() const;
+
+        [[nodiscard]] Plane FacePlane(int faceIndex) const;
+
+        [[nodiscard]] std::vector<Polygon> Faces() const;
+
+        [[nodiscard]] int NumEdges() const;
+
+        [[nodiscard]] LineSegment Edge(int edgeIndex) const;
+
+        [[nodiscard]] std::vector<std::pair<int, int>> EdgeIndices() const;
+
+        [[nodiscard]] std::vector<LineSegment> Edges() const;
+
+        [[nodiscard]] Polygon FacePolygon(int faceIndex) const;
+
+        [[nodiscard]] Vector3 FaceNormal(int faceIndex) const;
+
+        [[nodiscard]] bool IsConvex() const;
+
+
+        /// Returns true if the Euler formula (V + F - E == 2) holds for this Polyhedron.
+        /** The running time is O(E) ~ O(V).
+            @see NumVertices(), NumEdges(), NumFaces(). */
+        [[nodiscard]] bool EulerFormulaHolds() const;
+
+        [[nodiscard]] Vector3 ApproximateConvexCentroid() const;
+
+        [[nodiscard]] int ExtremeVertex(const Vector3 &direction) const;
+
+        [[nodiscard]] Vector3 ExtremePoint(const Vector3 &direction) const;
+
+        /// Tests if the given face of this Polyhedron contains the given point.
+        [[nodiscard]] bool FaceContains(int faceIndex, const Vector3 &worldSpacePoint, float polygonThickness = 1e-3f) const;
+
+        /// A helper for Contains() and FaceContains() tests: Returns a positive value if the given point is contained in the given face,
+        /// and a negative value if the given point is outside the face. The magnitude of the return value reports a pseudo-distance
+        /// from the point to the nearest edge of the face polygon. This is used as a robustness/stability criterion to estimate how
+        /// numerically believable the result is.
+        [[nodiscard]] float FaceContainmentDistance2D(int faceIndex, const Vector3 &worldSpacePoint, float polygonThickness = 1e-5f) const;
+        void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
+
+        Vector3 ClosestPoint(const LineSegment& lineSegment, Vector3 *lineSegmentPt) const;
+
+        [[nodiscard]] Vector3 ClosestPoint(const Vector3& point) const;
+
+        /// Returns true if this polyhedron has 0 vertices and 0 faces.
+        bool IsNull() const { return v.empty() && f.empty(); }
+
+    protected:
+    private:
+    };
+}

include/J3ML/Geometry/QuadTree.hpp

@@ -2,14 +2,13 @@
 
 #include <vector>
 #include <cstdint>
-#include <J3ML/LinearAlgebra/Vector2.h>
-#include "AABB2D.h"
+#include <J3ML/LinearAlgebra.hpp>
+#include <J3ML/Geometry/Forward.hpp>
+
+#include <J3ML/Geometry/AABB2D.hpp>
 
 namespace J3ML::Geometry {
 
-
-    using LinearAlgebra::Vector2;
-
     template<typename T>
     class QuadTree {
         /// A fixed split rule for all QuadTrees: A QuadTree leaf node is only ever split if the leaf contains at least this many objects.
@@ -53,7 +52,7 @@ namespace J3ML::Geometry {
                 }
             }
 
-            AABB2D ComputeAABB() {}
+            AABB2D ComputeAABB() { return AABB2D(); }
 
             float DistanceSq(const Vector2 &point) const {
                 Vector2 centered = point - center;
@@ -366,4 +365,4 @@ namespace J3ML::Geometry {
             leaf->objects.pop_back();
         }
     }
-}
+}

include/J3ML/Geometry/Ray.h

@@ -1,17 +0,0 @@
-//
-// Created by dawsh on 1/25/24.
-//
-
-#pragma once
-
-#include <J3ML/LinearAlgebra/Vector3.h>
-
-namespace J3ML::Geometry
-{
-    using LinearAlgebra::Vector3;
-    class Ray
-    {
-        Vector3 Origin;
-        Vector3 Direction;
-    };
-}

include/J3ML/Geometry/Ray.hpp

@@ -0,0 +1,164 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file Ray.hpp
+/// @desc The Ray geometry object. Used for Raycasting - intersection testing against geometric solids.
+/// @edit 2024-07-06
+#pragma once
+
+#include <format>
+#include <J3ML/LinearAlgebra.hpp>
+#include <vector>
+#include <ostream>
+#include <iosfwd>
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/Geometry/Forward.hpp>
+
+namespace J3ML::Geometry
+{
+    using J3ML::LinearAlgebra::Vector3;
+
+
+    // RaycastResult structure containing the first object the ray collides with,
+    // the surface intersection point,
+    // and the surface normal at the point of intersection.
+    struct RaycastResult
+    {
+        Vector3 Intersection;
+        Vector3 SurfaceNormal;
+        bool Hit;
+        //Shape* Target;
+        static RaycastResult NoHit() { return {Vector3::NaN, Vector3::NaN, false};}
+    };
+
+    /// A ray in 3D space is a line that starts from an origin point and extends to infinity in one direction
+    class Ray : public Shape
+    {
+    public: // Properties
+        // The position of this ray.
+        Vector3 Origin;
+        // The normalized direction vector of this ray.
+        // @note: For proper functionality, this direction vector needs to always be normalized
+        Vector3 Direction;
+    public: // Constructors
+
+        // The default constructor does not initialize any members of this class.
+        /** This means that the values of the members pos and dir are undefined after creating a new Ray using this
+            default constructor. Remember to assign to them before use.
+            @see pos, dir. */
+        Ray() = default;
+        /// Constructs a new ray by explicitly specifying the member variables.
+        /** @param pos The origin position of the ray.
+            @param dir The direction of the ray. This vector must be normalized, this function will not normalize
+                the vector for you (for performance reasons).
+            @see pos, dir. */
+        Ray(const Vector3& pos, const Vector3& dir);
+
+        /// Converts a Line to a Ray.
+        /** This conversion simply copies the members pos and dir over from the given Line to this Ray.
+            This meansthat the new ray starts at the same position, but only extends to one direction in space, instead of two.
+            @see class Line, ToLine() */
+        explicit Ray(const Line& line);
+
+        /// Converts a LineSegment to a Ray.
+        /** This constructor sets pos = lineSegment.a, and dir = (lineSegment.b - lineSegment.a).Normalized()
+            @see class LineSegment(), ToLineSegment() */
+        explicit Ray(const LineSegment& lineSegment);
+    public: // Methods
+        bool IsFinite() const;
+
+        /// Gets a point along the ray at the given distance.
+        /** Use this function to convert a 1D parametric point along the ray to a 3D point in the linear space.
+            @param distance The point to compute. GetPoint(0) will return pos. GetPoint(t) will return a point
+                at distance |t| from pos. Passing in negative values is allowed, but in that case, the
+                returned point does not actually lie on this Ray.
+            @return pos + distance * dir
+            @see pos, dir. */
+        Vector3 GetPoint(float distance) const;
+
+        void Translate(const Vector3& offset);
+
+        void Transform(const Matrix3x3& transform);
+        void Transform(const Matrix4x4& transform);
+        void Transform(const Quaternion& transform);
+
+        bool Contains(const Vector3& point, float distanceThreshold = 1e-3f) const;
+        bool Contains(const LineSegment& lineSegment, float distanceThreshhold = 1e-3f) const;
+
+        bool Equals(const Ray& otherRay, float epsilon = 1e-3f);
+        bool BitEquals(const Ray& other);
+
+        float Distance(const Ray& other) const;
+        float Distance(const Ray& other, float& d) const;
+        float Distance(const Ray& other, float&d, float& d2) const;
+        float Distance(const Line& other) const;
+        float Distance(const Line& other, float& d) const;
+        float Distance(const Line& other, float& d, float& d2) const;
+        float Distance(const LineSegment& other) const;
+        float Distance(const LineSegment& other, float& d) const;
+        float Distance(const LineSegment& other, float& d, float& d2) const;
+        float Distance(const Sphere& other) const;
+        float Distance(const Capsule& other) const;
+
+        Vector3 ClosestPoint(const Vector3& targetPoint) const;
+        Vector3 ClosestPoint(const Vector3 &targetPoint, float &d) const;
+        Vector3 ClosestPoint(const Ray& other) const;
+        Vector3 ClosestPoint(const Ray& other, float& d) const;
+        Vector3 ClosestPoint(const Ray& other, float& d, float& d2) const;
+        Vector3 ClosestPoint(const Line& other) const;
+        Vector3 ClosestPoint(const Line& other, float& d) const;
+        Vector3 ClosestPoint(const Line& other, float& d, float& d2) const;
+        Vector3 ClosestPoint(const LineSegment& other) const;
+        Vector3 ClosestPoint(const LineSegment& other, float& d) const;
+        Vector3 ClosestPoint(const LineSegment&, float &d, float &d2) const;
+
+        bool Intersects(const Triangle& triangle, float* d, Vector3* intersectionPoint) const;
+        bool Intersects(const Triangle& triangle) const;
+        bool Intersects(const Plane& plane, float* d);
+        bool Intersects(const Plane& plane) const;
+        bool Intersects(const Sphere& s, Vector3* intersectionPoint, Vector3* intersectionNormal, float* d) const;
+        bool Intersects(const Sphere& s) const;
+
+
+        bool Intersects(const AABB& aabb, float& dNear, float& dFar) const;
+        bool Intersects(const AABB& aabb) const;
+        bool Intersects(const OBB& aabb, float& dNear, float& dFar) const;
+        bool Intersects(const OBB& aabb) const;
+        bool Intersects(const Capsule& aabb) const;
+        bool Intersects(const Polygon& aabb) const;
+        bool Intersects(const Frustum& aabb) const;
+        bool Intersects(const Polyhedron& aabb) const;
+
+        Line ToLine() const;
+
+        LineSegment ToLineSegment(float d) const;
+        LineSegment ToLineSegment(float dStart, float dEnd) const;
+
+
+
+        RaycastResult Cast(const Triangle& target, float maxDistance = 99999999);
+        RaycastResult Cast(const Plane& target, float maxDistance = 99999999);
+        RaycastResult Cast(const AABB& target, float maxDistance = 99999999);
+        // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter3/raycast_sphere.html
+        RaycastResult Cast(const Sphere& target, float maxDistance = 99999999);
+        RaycastResult Cast(const OBB& target, float maxDistance = 99999999);
+        RaycastResult Cast(const Capsule& target, float maxDistance = 99999999);
+        RaycastResult Cast(const Frustum& target, float maxDistance = 99999999);
+        RaycastResult Cast(const TriangleMesh& target, float maxDistance = 9999999);
+
+        // Returns a RaycastResult structure containing the first object the ray collides with,
+        // the surface intersection point,
+        // and the surface normal at the point of intersection.
+       // RaycastResult Cast(std::vector<Shape> shapes, float maxDistance = 99999999);
+
+        void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
+
+        [[nodiscard]] std::string ToString() const;
+    };
+
+    std::ostream& operator << (std::ostream& o, const Ray& ray);
+}

include/J3ML/Geometry/Shape.hpp

@@ -0,0 +1,8 @@
+#pragma once
+
+class Shape {
+public:
+    virtual ~Shape() = default;
+};
+
+class Shape2D : public Shape{};

include/J3ML/Geometry/Sphere.h

@@ -1,70 +0,0 @@
-#pragma once
-
-#include <J3ML/Geometry.h>
-#include <J3ML/LinearAlgebra/Matrix3x3.h>
-#include <J3ML/LinearAlgebra/Matrix4x4.h>
-#include <J3ML/Geometry/LineSegment.h>
-#include <J3ML/Geometry/TriangleMesh.h>
-
-namespace J3ML::Geometry
-{
-    using J3ML::LinearAlgebra::Matrix3x3;
-    using J3ML::LinearAlgebra::Matrix4x4;
-
-    // A mathematical representation of a 3-dimensional sphere
-    class Sphere
-    {
-    public:
-        Vector3 Position;
-        float Radius;
-        Sphere() {}
-        Sphere(const Vector3& pos, float radius) : Position(pos), Radius(radius) {}
-        void Translate(const Vector3& offset)
-        {
-            Position = Position + offset;
-        }
-        void Transform(const Matrix3x3& transform)
-        {
-            Position = transform * Position;
-        }
-        void Transform(const Matrix4x4& transform)
-        {
-            Position = transform * Position;
-        }
-        inline float Cube(float inp) const
-        {
-            return inp*inp*inp;
-        }
-        float Volume() const
-        {
-            return 4.f * M_PI * Cube(Radius) / 3.f;
-        }
-        float SurfaceArea() const
-        {
-            return 4.f * M_PI * Cube(Radius) / 3.f;
-        }
-        bool IsFinite() const
-        {
-            return Position.IsFinite() && std::isfinite(Radius);
-        }
-        bool IsDegenerate()
-        {
-            return !(Radius > 0.f) || !Position.IsFinite();
-        }
-        bool Contains(const Vector3& point) const
-        {
-            return Position.DistanceSquared(point) <= Radius*Radius;
-        }
-        bool Contains(const Vector3& point, float epsilon) const
-        {
-            return Position.DistanceSquared(point) <= Radius*Radius + epsilon;
-        }
-        bool Contains(const LineSegment& lineseg)  const
-        {
-
-        }
-        TriangleMesh GenerateUVSphere() const {}
-        TriangleMesh GenerateIcososphere() const {}
-
-    };
-}

include/J3ML/Geometry/Sphere.hpp

@@ -0,0 +1,124 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file Sphere.hpp
+/// @desc The Sphere geometry object.
+/// @edit 2024-07-06
+
+#pragma once
+
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/Geometry/Forward.hpp>
+#include <J3ML/LinearAlgebra.hpp>
+
+
+namespace J3ML::Geometry
+{
+
+    // A mathematical representation of a 3-dimensional sphere
+    class Sphere : public Shape
+    {
+    public: // Properties
+        Vector3 Position; // The center point of this sphere.
+        float Radius; /// The radius of this sphere.
+    public: // Constructors
+
+        /// The default constructor does not initialize any members of this class.
+        /** This means that the values of the members pos and r are undefined after creating a new Sphere using this
+            default constructor. Remember to assign to them before use.
+            @see pos, r. */
+        Sphere() {}
+        /// Constructs a sphere with a given position and radius.
+        /** @param radius A value > 0 constructs a sphere with positive volume. A value of <= 0 is valid, and constructs a degenerate sphere.
+            @see pos, r, IsFinite(), IsDegenerate() */
+        Sphere(const Vector3& pos, float radius) : Position(pos), Radius(radius) {}
+
+        /// Constructs a sphere that passes through the given two points.
+        /** The constructed sphere will be the minimal sphere that encloses the given two points. The center point of this
+            sphere will lie midway between pointA and pointB, and the radius will be half the distance between pointA and
+            pointB. Both input points are assumed to be finite. */
+        Sphere(const Vector3 &pointA, const Vector3 &pointB);
+
+        /// Constructs a sphere that passes through the given three points.
+        /** @note The resulting sphere may not be the minimal enclosing sphere for the three points! */
+        Sphere(const Vector3 &pointA, const Vector3 &pointB, const Vector3 &pointC);
+
+        /// Constructs a sphere that passes through the given four points.
+        /** @note The resulting sphere may not be the minimal enclosing sphere for the four points! */
+        Sphere(const Vector3 &pointA, const Vector3 &pointB, const Vector3 &pointC, const Vector3 &pointD);
+
+    public:
+        /// Generates a random point on the surface of this sphere
+        /** The points are distributed uniformly.
+            This function uses the rejection method to generate a uniform distribution of points on the surface.
+            Therefore, it is assumed that this sphere is not degenerate, i.e. it has a positive radius.
+            A fixed number of 1000 tries is performed, after which a fixed point on the surface is returned as a fallback.
+            @param rng A pre-seeded random number generator object that is to be used by this function to generate random vlaues.
+            @see class RNG, RandomPointInside(), IsDegenerate()
+            @todo Add Sphere::PointOnSurface(polarYaw, polarPitch). */
+        Vector3 RandomPointOnSurface(RNG& rng) const;
+        static Vector3 RandomPointOnSurface(RNG& rng, const Vector3& center, float radius);
+
+        /// Generates a random point inside this sphere.
+        /** The points are distributed uniformly.
+            This function uses the rejection method to generate a uniform distribution of points inside a sphere.
+            Therefore, it is assumed that this sphere is not degenerate, i.e. it has a positive radius.
+            A fixed number of 1000 tries is performed, after which the sphere center position is returned as a fallback.
+            @param rng A pre-seeded random number generator object that is to be used by this function to generate random values.
+            @see class RNG, RandomPointOnSurface(), IsDegenerate().
+            @todo Add Sphere::Point(polarYaw, polarPitch, radius). */
+        Vector3 RandomPointInside(RNG& rng);
+        static Vector3 RandomPointInside(RNG& rng, const Vector3& center, float radius);
+    public:
+        /// Quickly returns an arbitrary point inside this Sphere. Used in GJK intersection test.
+        [[nodiscard]] Vector3 AnyPointFast() const { return Position; }
+        /// Computes the extreme point of this Sphere in the given direction.
+        /** An extreme point is a farthest point of this Sphere in the given direction. For
+            a Sphere, this point is 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 The extreme point of this Sphere in the given direction. */
+        [[nodiscard]] Vector3 ExtremePoint(const Vector3 &direction) const;
+        Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
+
+
+        Vector3 Centroid() const { return Position; }
+        Vector3 CenterPos() const { return Centroid(); }
+
+        /// Translates this Sphere in world space.
+        /** @param offset The amount of displacement to apply to this Sphere, in world space coordinates.
+            @see Transform(). */
+        void Translate(const Vector3& offset);
+
+
+        void Transform(const Matrix3x3& transform);
+
+        void Transform(const Matrix4x4& transform);
+
+        static inline float Cube(float inp);
+
+        [[nodiscard]] float Volume() const;
+
+        [[nodiscard]] float SurfaceArea() const;
+
+        [[nodiscard]] bool IsFinite() const;
+
+        [[nodiscard]] bool IsDegenerate() const;
+
+        [[nodiscard]] bool Contains(const Vector3& point) const;
+
+        [[nodiscard]] bool Contains(const Vector3& point, float epsilon) const;
+
+        [[nodiscard]] bool Contains(const LineSegment& lineseg)  const;
+        TriangleMesh GenerateUVSphere(int subdivisions = 10.f) const;
+        TriangleMesh GenerateIcososphere() const;
+        TriangleMesh GenerateCubesphere() const;
+
+
+        void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
+    };
+}

include/J3ML/Geometry/Triangle.h

@@ -1,9 +0,0 @@
-#pragma once
-
-namespace J3ML::Geometry
-{
-    class Triangle
-    {
-
-    };
-}

include/J3ML/Geometry/Triangle.hpp

@@ -0,0 +1,144 @@
+#pragma once
+
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/Geometry/Forward.hpp>
+#include <J3ML/LinearAlgebra.hpp>
+#include <cfloat>
+
+namespace J3ML::Geometry
+{
+    class Triangle : public Shape
+    {
+    public:
+        Vector3 V0;
+        Vector3 V1;
+        Vector3 V2;
+    public:
+
+    public:
+        static int NumFaces() { return 1; }
+        static int NumEdges() { return 3; }
+        static int NumVertices() { return 3; }
+    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;
+
+        Vector3 Centroid() const;
+
+        Vector3 CenterPoint() const;
+
+        /// Tests if the given object is fully contained inside this triangle.
+        /** @param triangleThickness An epsilon threshold value to use for this test. triangleThicknessSq is the squared version of this parameter.
+                This specifies the maximum distance the given object can lie from the plane defined by this triangle.
+            @see Distance(), Intersects(), ClosestPoint().
+            @todo Add Triangle::Contains(Circle) and Triangle::Contains(Disc). */
+        bool Contains(const Vector3& point, float triangleThicknessSq = 1e-5f) const;
+        bool Contains(const LineSegment& lineSeg, float triangleThickness = 1e-3f) const;
+        bool Contains(const Triangle& triangle, float triangleThickness = 1e-3f) const;
+
+
+        bool Intersects(const LineSegment& lineSegment, float* d = 0, Vector3* intersectionPoint = 0) 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.
+        inline Vector3 AnyPointFast() const { return V0; }
+
+        /// Computes an extreme point of this Triangle in the given direction.
+        /** An extreme point is a farthest point of this Triangle 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 Triangle in the given direction. The returned point is always a
+                vertex of this Triangle.
+            @see Vertex(). */
+        Vector3 ExtremePoint(const Vector3 &direction) const;
+        Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
+
+
+        static float IntersectLineTri(const Vector3 &linePos, const Vector3 &lineDir, const Vector3 &v0, const Vector3 &v1,
+                               const Vector3 &v2, float &u, float &v);
+
+        /// Computes the closest point on the edge of this triangle to the given object.
+        /** @param outU [out] If specified, receives the barycentric U coordinate of the returned point (in the UV convention).
+                This pointer may be null.
+            @param outV [out] If specified, receives the barycentric V coordinate of the returned point (in the UV convention).
+                This pointer may be null.
+            @param outD [out] If specified, receives the distance along the line of the closest point on the line to the edge of this triangle.
+            @return The closest point on the edge of this triangle to the given object.
+            @todo Add ClosestPointToTriangleEdge(Point/Ray/Triangle/Plane/Polygon/Circle/Disk/AABB/OBB/Sphere/Capsule/Frustum/Polyhedron).
+            @see Distance(), Contains(), Intersects(), ClosestPointToTriangleEdge(), Line::GetPoint. */
+        Vector3 ClosestPointToTriangleEdge(const Line &line, float *outU, float *outV, float *outD) const;
+        Vector3 ClosestPointToTriangleEdge(const LineSegment &lineSegment, float *outU, float *outV, float *outD) const;
+
+        Vector3 ClosestPoint(const LineSegment &lineSegment, Vector3 *otherPt = 0) const;
+        /// Returns the point at the given barycentric coordinates.
+        /** This function computes the vector space point at the given barycentric coordinates.
+            @param uvw The barycentric UVW coordinate triplet. The condition u+v+w == 1 should hold for the input coordinate.
+                If 0 <= u,v,w <= 1, the returned point lies inside this triangle.
+            @return u*a + v*b + w*c. */
+        Vector3 Point(const Vector3 &uvw) const;
+        Vector3 Point(float u, float v, float w) const;
+        /** These functions are an alternate form of Point(u,v,w) for the case when the barycentric coordinates are
+		represented as a (u,v) pair and not as a (u,v,w) triplet. This function is provided for convenience
+		and effectively just computes Point(1-u-v, u, v).
+		@param uv The barycentric UV coordinates. If 0 <= u,v <= 1 and u+v <= 1, then the returned point lies inside
+			this triangle.
+		@return a + (b-a)*u + (c-a)*v.
+		@see BarycentricUV(), BarycentricUVW(), BarycentricInsideTriangle(). */
+        Vector3 Point(const Vector2 &uv) const;
+        Vector3 Point(float u, float v) const;
+
+        /// Expresses the given point in barycentric (u,v,w) coordinates.
+        /** @note There are two different conventions for representing barycentric coordinates. One uses
+                a (u,v,w) triplet with the equation pt == u*a + v*b + w*c, and the other uses a (u,v) pair
+                with the equation pt == a + u*(b-a) + v*(c-a). These two are equivalent. Use the mappings
+                (u,v) -> (1-u-v, u, v) and (u,v,w)->(v,w) to convert between these two representations.
+            @param point The point of the vector space to express in barycentric coordinates. This point should
+                lie in the plane formed by this triangle.
+            @return The factors (u,v,w) that satisfy the weighted sum equation point == u*a + v*b + w*c.
+            @see BarycentricUV(), BarycentricInsideTriangle(), Point(), http://mathworld.wolfram.com/BarycentricCoordinates.html */
+        Vector3 BarycentricUVW(const Vector3 &point) const;
+
+        /// Expresses the given point in barycentric (u,v) coordinates.
+        /** @note There are two different conventions for representing barycentric coordinates. One uses
+                a (u,v,w) triplet with the equation pt == u*a + v*b + w*c, and the other uses a (u,v) pair
+                with the equation pt == a + u*(b-a) + v*(c-a). These two are equivalent. Use the mappings
+                (u,v) -> (1-u-v, u, v) and (u,v,w)->(v,w) to convert between these two representations.
+            @param point The point to express in barycentric coordinates. This point should lie in the plane
+                formed by this triangle.
+            @return The factors (u,v) that satisfy the weighted sum equation point == a + u*(b-a) + v*(c-a).
+            @see BarycentricUVW(), BarycentricInsideTriangle(), Point(). */
+        Vector2 BarycentricUV(const Vector3 &point) const;
+
+
+        Vector3 ClosestPoint(const Vector3 &p) const;
+
+        Plane PlaneCCW() const;
+
+        Plane PlaneCW() const;
+
+        Vector3 FaceNormal() const;
+
+        Vector3 Vertex(int i) const;
+
+        LineSegment Edge(int i) const;
+        Triangle(const Vector3& v0, const Vector3& v1, const Vector3& v2) : V0(v0), V1(v1), V2(v2) {}
+    };
+
+
+}

include/J3ML/Geometry/Triangle2D.h

@@ -1,10 +0,0 @@
-#pragma once
-
-
-namespace J3ML::Geometry
-{
-    class Shape2D {};
-    class Triangle2D {
-    public:
-    };
-}

include/J3ML/Geometry/Triangle2D.hpp

@@ -0,0 +1,242 @@
+#pragma once
+
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/LinearAlgebra/Forward.hpp>
+#include <J3ML/LinearAlgebra/Vector2.hpp>
+#include <J3ML/Geometry/Forward.hpp>
+#include <J3ML/Algorithm/RNG.hpp>
+
+namespace J3ML::Geometry
+{
+    using J3ML::LinearAlgebra::Vector2;
+    using J3ML::Algorithm::RNG;
+    /// Specifies a triangle through three points in 2D space.
+    /// This class stores three member vertices A, B, and C to specify the triangle.
+    /** @note The order in which the vertices are stored in this data structure is important.
+              The triangles (a,b,c) and (a,c,b) are otherwise equivalent, but their plane normals point to the opposite directions.
+        @see PlaneCCW(), PlaneCW() */
+    class Triangle2D : public Shape2D {
+
+    public:
+        Vector2 A; /// The first Triangle endpoint.
+        Vector2 B; /// The second Triangle endpoint.
+        Vector2 C; /// The third Triangle endpoint.
+    public:
+        /// The default constructor does not initialize any members of this class.
+        /** This means that the values of the members a, b, and c are undefined after creating a new Triangle2D using this
+            default constructor. Remember to assign to them before use. */
+        Triangle2D() {}
+        /// Constructs a triangle from three given endpoints.
+        /** The normal of the plane will be constructed to point towards the halfspace where
+            the vertices a, b, and c wind in counter-clockwise order. */
+        Triangle2D(const Vector2& a, const Vector2& b, const Vector2& c);
+    public:
+        static int NumFaces() { return 1; }
+        static int NumEdges() { return 3; }
+        static int NumVertices() { return 3; }
+
+        /// Translates this Triangle2D in world space.
+        /** @param offset The amount of displacement to apply to this Triangle2D, in world space coordinates.
+            @see Transform(). */
+        void Translate(const Vector2& offset);
+
+        /// Applies a transformation to this Triangle2D, in-place.
+        /** @see Translate(), classes Matrix3x3, Matrix4x4, Quaternion. */
+        void Transform(const Matrix3x3& transform);
+        void Transform(const Matrix4x4& transform);
+
+        /// Expresses the given point in barycentric (u,v,w) coordinates.
+        /** @note There are two different conventions for representing barycentric coordinates. One uses a
+                (u, v, w) triplet with the equation pt = u*a + v*b + w*c, and the other uses a (u,v) pair
+                with the equation pt = a + u*(b-a) + v*(c-a). These two are equivalent. Use the mappings
+                (u,v) -> (1-u-v, u, v) and (u,v,w)->(v,w) to convert between these two representations.
+            @param point The point of the vector space to express in barycentric coordinates. This point should
+                lie in the plane formed by this triangle.
+            @return The factors (u,v,w) that satisfy the weighted sum equation point == u*a + v*b + w*c.
+            @see BarycentricUV(), BarycentricInsideTriangle(), Point(), http://mathworld.wolfram.com/BarycentricCoordinates.html */
+        Vector3 BarycentricUVW(const Vector2& point) const;
+
+        /// Expresses the given point in barycentric (u, v) coordinates.
+        /** @note There are two different conventions for representing barycentric coordinates. One uses a
+                (u, v, w) triplet with the equation pt = u*a + v*b + w*c, and the other uses a (u,v) pair
+                with the equation pt = a + u*(b-a) + v*(c-a). These two are equivalent. Use the mappings
+                (u,v) -> (1-u-v, u, v) and (u,v,w)->(v,w) to convert between these two representations.
+            @param point The point to express in barycentric coordinates. This point should lie in the plane
+                formed by this triangle.
+            @return The factors (u, v) that satisfy the weighted sum equation point == a + u*(b-a) + v*(c-a)
+            @see BarycentricUVW(), BarycentricInsideTriangle(), Point() */
+        Vector2 BarycentricUV(const Vector2& point) const;
+
+        /// Tests if the given barycentric UVW coordinates lie inside a triangle.
+        /** A barycentric UVW coordinate represents a point inside a triangle if
+                a) 0 <= u,v,w <= 1 and
+                b) u+v+w == 1.
+            @param uvw The barycentric vector containing the barycentric (u,v,w) coordinates.
+            @return True if the given coordinates lie inside a triangle.
+            @see BarycentricUV(), BarycentricUVW(), Point(). */
+        static bool BarycentricInsideTriangle(const Vector3& uvw);
+
+        /// Returns the point at the given barycentric coordinates.
+        /** This function computes the vector space point at the given barycentric coordinates.
+            @param uvw The barycentric UVW coordiante triplet. The condition u+v+w == 1 should hold for the input coordinate.
+                If 0 <= u,v,w <= 1, the returned point lies inside this triangle.
+            @return u*a + v*b + w*c. */
+        Vector2 Point(const Vector3& uvw) const;
+        Vector2 Point(float u, float v, float w) const;
+
+        /** These functions are an alternate form of Point(u,v,w) for the case when the barycentric coordinates are
+            represented as a (u,v) pair and not as a (u,v,w) triplet. This function is provided for convenience
+            and effectively just computes Point(1-u-v, u, v).
+            @param uv The barycentric UV coordinates. If 0 <= u,v <= 1 and u+v <= 1, then the returned point lies inside
+                this triangle.
+            @return a + (b-a)*u + (c-a)*v.
+            @see BarycentricUV(), BarycentricUVW(), BarycentricInsideTriangle(). */
+        Vector2 Point(const Vector2& uv) const;
+        Vector2 Point(float u, float v) const;
+
+        /// Computes the center of mass of this triangle.
+        /** @return The point (a+b+c)/3. */
+        Vector2 Centroid() const;
+        /// Identical to Centroid(), we provide both to enable common signature with AABB2D, and OBB2D to allow them in template algorithms.
+        Vector2 CenterPoint() const { return Centroid();}
+        /// Computes the surface area of this triangle.
+        /** @return The surface area of this triangle.
+            @see Perimeter(), Area2D(), SignedArea(). */
+        float Area() const;
+
+        /// Computes the total edge length of this triangle.
+        /** @return |a-b| + |b-c| + |c-a|.
+            @see Area(), Edge(). */
+        float Perimeter() const;
+
+        /// Returns a pointer to the vertices of this triangle. The array contains three elements.
+        Vector2* VertexArrayPtr() { return &A;}
+        const Vector2* VertexArrayPtr() const { return &A;}
+
+        /// Returns a vertex of this triangle.
+        /** @param i The vertex of the triangle to get: 0, 1, or 2.
+            @return Vertex(0) returns the point A, Vertex(1) returns the point B, and Vertex(2) returns the point C.
+            @note If an index outside [0,2] is passed, an assume() failure occurs, and a NaN vector is returned.
+            @see Edge(). */
+        Vector2 Vertex(int i) const;
+        Vector2 CornerPoint(int i) const { return Vertex(i);} // An alias for Vertex() to be able to mesh Triangle2D into templatized algorithms.
+
+
+        //LineSegment2D Edge(int i) const; // TODO: Implement class LineSegment2D
+
+        /// Quickly returns an arbitrary point inside this Triangle2D. Used in GJK intersection test.
+        inline Vector2 AnyPointFast() const { return A;}
+
+        /// Computes an extreme point of this Triangle2D in the given direction.
+        /** An extreme point is the farthest point of this Triangle2D 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 Triangle2D in the given direction. The returned point is always a
+                vertex of this Triangle2D.
+            @see Vertex(). */
+        Vector2 ExtremePoint(const Vector2& direction) const;
+        Vector2 ExtremePoint(const Vector2& direction, float& projectionDistance) const;
+
+        /// Returns the tight AABB2D that encloses this Triangle2D.
+        AABB2D BoundingAABB() const;
+
+        /// Computes the surface area of the given 2D triangle.
+        /** This math library does not have a separate class for for 2D triangles. To compute the area of a 2D triangle,
+            use this Triangle2D class and set z=0 for each coordinate, or use this helper function.
+            @see Area(), SignedArea().*/
+        static float Area2D(const Vector2& p1, const Vector2& p2, const Vector2& p3);
+        /// Relates the barycentric coordinate of the given point to the surface area of triangle abc.
+        /** This function computes the ratio of the signed area of the triangle (point, b, c) to the signed area of
+            the triangle (a,b,c). This is the same as computng the barycentric u-coordinate of the given point
+            on the triangle (a,b,c).
+            @see Area(), Area2D(), BarycentricUVW(). */
+        static float SignedArea(const Vector2& point, const Vector2& a, const Vector2& b, const Vector2& c);
+
+        /// Returns true if this triangle is finite.
+        /** A triangle is <b><i>finite</i></b> if its vertices a,b, and c do not contain floating-point NaNs or +/-infs
+            in them.
+            @return True if each coordinate of each vertex of this triangle has a finite floating-point value.
+            @see A, B, C, IsDegenerate(), ::IsFinite(), IsInf(), IsNan(), IsFinite() */
+        bool IsFinite() const;
+        /// Returns true if this triangle is degenerate.
+        /** A triangle is <b><i>degenerate</i></b> if it is not finite, or if the surface area of the triangle is
+            close to zero.
+            @param epsilon The threshold to test against. If two vertices of this triangle are closer than this, the
+            triangle is considered degenerate.
+            @see a, b, c, IsFinite() */
+        bool IsDegenerate(float epsilon = 1e-3f) const;
+
+        /// Returns true if the triangle defined by the three given points is degenerate.
+        static bool IsDegenerate(const Vector2& p1, const Vector2& p2, const Vector2& p3, float epsilon = 1e-3f);
+
+        /// In some templated algorithms, the input can either be a Triangle2D or a Polygon2D. Provide trivial Polygon2D-specific API
+        /// for compatibility in those template functions.
+        bool IsConvex() const { return true;}
+        bool IsPlanar() const { return true;}
+        bool IsSimple() const { return true;}
+
+        /// Tests if the given object is fully contained inside this triangle.
+        /** @param triangleThickness An epsilon threshold value to use for this test. triangleThicknessSq is the squared version of this parameter.
+                This specifies the maximum distance the given object can lie from the plane defined by this triangle.
+            @see Distance(), Intersects(), ClosestPoint(). */
+        bool Contains(const Vector2& point, float triangleThicknessSq = 1e-5f) const;
+        /// Computes the distance between this triangle 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.
+            @see Contains(), Intersects(), ClosestPoint(). */
+        float Distance(const Vector2& point) const;
+        float DistanceSquared(const Vector2& point) const;
+        float DistanceSq(const Vector2& point) const { return DistanceSquared(point); }
+
+        /// A helper function used in line-triangle tests.
+        static float IntersectLineTri(const Vector2& linePos, const Vector2& lineDir,
+                                      const Vector2& v0, const Vector2& v1, const Vector2& v2,
+                                      float& u, float& v);
+
+        /// Projects this triangle onto the given axis.
+        /** This function is used in SAT tests (separate axis theorem) to check the interval this triangle
+            lies in on an 1D line.
+            @param axis THe axis to project onto. This vector can be unnormalized.
+            @param dMin [out] Returns the minimum extent of this triangle on the given axis.
+            @param dMax [out] Returns the maximum extent of this triangle on the given axis. */
+        void ProjectToAxis(const Vector2& axis, float& dMin, float& dMax) const;
+        int UniqueFaceNormals(Vector2* out) const;
+        int UniqueEdgeNormals(Vector2* out) const;
+
+        /// Computes the closest point on this triangle to the given object.
+        /** If the other object intersects this triangle, the function will return an arbitrary point inside
+            the region of intersection.
+            @see Contains(), Distance(), Intersects(), ClosestPointToTriangleEdge(). */
+        Vector2 ClosestPoint(const Vector2& point) const;
+        //Vector2 ClosestPoint(const LineSegment2D& lineSegment, Vector2* otherPt = 0) const;
+
+        /// Generates a random point inside this Triangle2D.
+        /** The points are distrubited uniformly.
+            The implementation of this function is based on Graphics Gems 1, p. 25:
+            "1.5 Generating random points on triangles. Method 2." The Method 1 presented in the book
+            uses a Sqrt() instead of the if().
+            @param rng A pre-seeded random number generator object that is to be used by this function to generate random values.
+            @see class RNG, RandomPointOnEdge(), RandomVertex(), Point() */
+        Vector2 RandomPointInside(RNG& rng) const;
+
+        /// Choose a corner vertex of this triangle2D at random.
+        /** This function returns one of the vertices {a, b, c} uniformly at random.
+            @param rng A pre-seeded random number generator object that is to be used by this function to generate random values.
+            @see class RNG(), RandomPointInside(), RandomPointOnEdge(), Vertex().*/
+        Vector2 RandomVertex(RNG& rng) const;
+        /// Generates a random point on the edge of this Triangle2D.
+        /** The points are distributed uniformly.
+            This function requires that this triangle is not degenerate. This it is, an assume() error will be printed,
+            and the return value will be undefined.
+            @param rng A pre-seeded random number generator object that is to be used by this function to generate random values.
+            @see class RNG, RandomPointInside, RandomVertex(), Edge(), class LineSegment2D, IsDegenerate(). */
+        Vector2 RandomPointOnEdge(RNG& rng) const;
+        std::string ToString() const;
+        bool Equals(const Triangle2D& rhs, float epsilon = 1e-3f) const {
+            return A.Equals(rhs.A, epsilon) && B.Equals(rhs.B, epsilon) && C.Equals(rhs.C, epsilon);
+        }
+    };
+
+    std::ostream& operator << (std::ostream& o, const Triangle2D& triangle);
+}

include/J3ML/Geometry/TriangleMesh.h

@@ -1,9 +0,0 @@
-#pragma once
-
-namespace J3ML::Geometry
-{
-    class TriangleMesh
-    {
-
-    };
-}

include/J3ML/Geometry/TriangleMesh.hpp

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <vector>
+#include <J3ML/LinearAlgebra.hpp>
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/Geometry/Forward.hpp>
+
+namespace J3ML::Geometry
+{
+
+    class TriangleMesh : public Shape
+    {
+    public:
+        /// Default constructor for a triangle mesh.
+        TriangleMesh(int expectedPolygonCount = 1000);
+
+    public:
+        std::vector<Vector3> Vertices;
+        std::vector<Vector3> Normals;
+        std::vector<Vector3> UVs;
+        std::vector<u64> Indices;
+
+        std::vector<float> GenerateVertexList();
+        //std::vector<Triangle> GenerateTriangleList();
+    public:
+
+    private:
+
+        //std::vector<Triangle> cachedTriangleList;
+    };
+
+}

include/J3ML/J3ML.h

@@ -1,241 +0,0 @@
-#pragma once
-
-
-
-//
-// Created by josh on 12/25/2023.
-//
-#include <cstdint>
-#include <cmath>
-#include <stdfloat>
-#include <string>
-#include <cassert>
-
-namespace J3ML::SizedIntegralTypes
-{
-    using u8 = uint8_t;
-    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;
-}
-
-
-
-namespace J3ML::SizedFloatTypes
-{
-    using f32 = float;
-    using f64 = double;
-    using f128 = long double;
-
-}
-
-using namespace J3ML::SizedIntegralTypes;
-using namespace J3ML::SizedFloatTypes;
-
-namespace J3ML::Math
-{
-
-
-    // Coming soon: Units Namespace
-    // For Dimensional Analysis
-    /*
-    namespace Units
-    {
-
-        struct Unit {};
-
-        struct Meters : public Unit { };
-        struct ImperialInches : public Unit {};
-        struct Time : public Unit {};
-        struct Grams : public Unit {};
-
-
-        struct MetersPerSecond : public Unit {};
-
-
-        template <typename TUnit>
-        struct Quantity
-        {
-        public:
-
-            float Value;
-        };
-
-        struct Mass : public Quantity<Grams> {};
-        struct Length : public Quantity<Meters> { };
-
-        struct Velocity : public Quantity<MetersPerSecond>{ };
-
-        class MetrixPrefix
-        {
-        public:
-            std::string Prefix;
-            std::string Symbol;
-            int Power;
-            float InverseMultiply(float input) const
-            {
-                return std::pow(input, -Power);
-            }
-            float Multiply(float input) const
-            {
-                return std::pow(input, Power);
-            }
-
-        };
-        namespace Prefixes
-        {
-            static constexpr MetrixPrefix Tera {"tera", "T", 12};
-            static constexpr MetrixPrefix Giga {"giga", "G", 9};
-            static constexpr MetrixPrefix Mega {"mega", "M", 6};
-            static constexpr MetrixPrefix Kilo {"kilo", "k", 3};
-            static constexpr MetrixPrefix Hecto {"hecto", "h", 2};
-            static constexpr MetrixPrefix Deca {"deca", "da", 1};
-            static constexpr MetrixPrefix None {"", "", 0};
-            static constexpr MetrixPrefix Deci {"", "", 0};
-            static constexpr MetrixPrefix Centi {"", "", 0};
-            static constexpr MetrixPrefix Milli {"", "", 0};
-            static constexpr MetrixPrefix Micro {"", "", 0};
-            static constexpr MetrixPrefix Nano {"", "", 0};
-            static constexpr MetrixPrefix Pico {"", "", 0};
-        }
-
-        Length operator ""_meters(long double value)
-        {
-            return {(float)value};
-        }
-        Length operator ""_m(long double value)
-        {
-            return {(float)value};
-        }
-        constexpr Length operator ""_kilometers(long double value)
-        {
-            return Length {(float)value};
-        }
-        Length operator ""_km(long double value)
-        {
-            return {(float)value};
-        }
-        Length operator ""_centimeters(long double value)
-        {
-            return {(float)value};
-        }
-        Length operator ""_cm(long double value)
-        {
-            return {(float)value};
-        }
-
-        Length operator ""_millimeters(long double value)
-        {
-            return {(float)value};
-        }
-        Length operator ""_mm(long double value)
-        {
-            return {(float)value};
-        }
-
-        Velocity operator ""_mps(long double value)
-        {
-            return {(float)value};
-        }
-
-        Velocity operator ""_meters_per_second(long double value)
-        {
-            return {(float)value};
-        }
-
-        Velocity operator ""_kmps(long double value)
-        {
-            return {(float)value};
-        }
-    }*/
-
-
-#pragma region Constants
-    static const float Pi           = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679f;
-    static const float RecipSqrt2Pi = 0.3989422804014326779399460599343818684758586311649346576659258296706579258993018385012523339073069364f;
-    static const float GoldenRatio  = 1.6180339887498948482045868343656381177203091798057628621354486227052604628189024497072072041893911375f;
-#pragma endregion
-
-#pragma region Math Functions
-
-    inline float Radians(float degrees);
-    inline float Degrees(float radians);
-
-
-    struct NumberRange
-    {
-        float LowerBound;
-        float UpperBound;
-    };
-
-
-    float NormalizeToRange(float input, float fromLower, float fromUpper, float toLower, float toUpper);
-    float NormalizeToRange(float input, const NumberRange& from, const NumberRange& to);
-    // auto rotation_normalized = NormalizeToRange(inp, {0, 360}, {-1, 1});
-
-    inline float Lerp(float a, float b, float t);
-    /// Linearly interpolates from a to b, under the modulus mod.
-    /// This function takes evaluates a and b in the range [0, mod] and takes the shorter path to reach from a to b.
-    inline float LerpMod(float a, float b, float mod, float t);
-    /// Computes the lerp factor a and b have to be Lerp()ed to get x.
-    inline float InverseLerp(float a, float b, float x);
-    /// See http://msdn.microsoft.com/en-us/library/bb509665(v=VS.85).aspx
-    inline float Step(float y, float x);
-    /// See http://msdn.microsoft.com/en-us/library/bb509658(v=vs.85).aspx
-    inline float Ramp(float min, float max, float x);
-
-    inline float PingPongMod(float x, float mod);
-
-    inline float Sqrt(float x);
-    inline float FastSqrt(float x);
-    /// Returns 1/Sqrt(x). (The reciprocal of the square root of x)
-    inline float RSqrt(float x);
-
-    inline float FastRSqrt(float x);
-
-
-#pragma endregion
-
-
-    namespace BitTwiddling
-    {
-        /// Parses a string of form "011101010" to a u32
-        u32 BinaryStringToValue(const char* s);
-
-        /// Returns the number of 1's set in the given value.
-        inline int CountBitsSet(u32 value);
-    }
-
-    namespace Interp
-    {
-        inline float SmoothStart(float t);
-    }
-
-    struct Rotation
-    {
-    public:
-        Rotation();
-        Rotation(float value);
-        float valueInRadians;
-        float ValueInRadians() const;
-        float ValueInDegrees() const;
-        Rotation operator+(const Rotation& rhs);
-    };
-
-
-    Rotation operator ""_rad(long double rads);
-
-    Rotation operator ""_radians(long double rads);
-
-    Rotation operator ""_deg(long double rads);
-
-    Rotation operator ""_degrees(long double rads);
-
-}

include/J3ML/J3ML.hpp

@@ -0,0 +1,503 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file J3ML.h
+/// @desc Core mathematical and utility functions, concrete types, and math constants.
+/// @edit 2024-07-05
+
+#pragma once
+
+#include <cstdint>
+#include <cmath>
+#include <string>
+#include <cassert>
+#include <vector>
+
+/// This set of functions may be set to use lookup tables or SIMD operations.
+/// If no options are set, they will default to using standard library implementation.
+#undef USE_LOOKUP_TABLES /// Pre-computed lookup tables.
+#undef USE_SSE           /// Streaming SIMD Extensions (x86)
+#undef USE_NEON          /// ARM Vector Processing
+#undef USE_AVX           /// Advanced Vector Extensions (x86)
+
+/// TODO: Implement lookup tables.
+/// TODO: Implement constexpr Trigonometric LUT generators that are parameterized (samples, samples-per-period, etc.)
+
+#ifdef USE_LOOKUP_TABLES
+
+#define LUT_SAMPLES 1024
+
+#pragma region Trigonometric Lookup Tables
+
+// Formula: sin(2*pi*t/T)
+/** Generated using Dr LUT - Free Lookup Table Generator
+  * https://github.com/ppelikan/drlut
+  **/
+// Formula: sin(2*pi*t/T)
+const uint8_t u8_sin_lut[1024] = {
+127,128,129,129,130,131,132,132,133,134,135,136,136,
+137,138,139,139,140,141,142,143,143,144,145,146,146,
+147,148,149,149,150,151,152,153,153,154,155,156,156,
+157,158,159,159,160,161,162,162,163,164,165,165,166,
+167,168,168,169,170,171,171,172,173,173,174,175,176,
+176,177,178,178,179,180,181,181,182,183,183,184,185,
+185,186,187,188,188,189,190,190,191,192,192,193,194,
+194,195,196,196,197,198,198,199,199,200,201,201,202,
+203,203,204,205,205,206,206,207,208,208,209,209,210,
+211,211,212,212,213,213,214,215,215,216,216,217,217,
+218,218,219,220,220,221,221,222,222,223,223,224,224,
+225,225,226,226,227,227,228,228,229,229,229,230,230,
+231,231,232,232,233,233,233,234,234,235,235,236,236,
+236,237,237,238,238,238,239,239,239,240,240,240,241,
+241,241,242,242,242,243,243,243,244,244,244,245,245,
+245,245,246,246,246,247,247,247,247,248,248,248,248,
+249,249,249,249,249,250,250,250,250,250,251,251,251,
+251,251,251,252,252,252,252,252,252,252,253,253,253,
+253,253,253,253,253,253,253,253,254,254,254,254,254,
+254,254,254,254,254,254,254,254,254,254,254,254,254,
+254,254,254,254,254,254,254,254,254,254,254,253,253,
+253,253,253,253,253,253,253,253,253,252,252,252,252,
+252,252,252,251,251,251,251,251,251,250,250,250,250,
+250,249,249,249,249,249,248,248,248,248,247,247,247,
+247,246,246,246,245,245,245,245,244,244,244,243,243,
+243,242,242,242,241,241,241,240,240,240,239,239,239,
+238,238,238,237,237,236,236,236,235,235,234,234,233,
+233,233,232,232,231,231,230,230,229,229,229,228,228,
+227,227,226,226,225,225,224,224,223,223,222,222,221,
+221,220,220,219,218,218,217,217,216,216,215,215,214,
+213,213,212,212,211,211,210,209,209,208,208,207,206,
+206,205,205,204,203,203,202,201,201,200,199,199,198,
+198,197,196,196,195,194,194,193,192,192,191,190,190,
+189,188,188,187,186,185,185,184,183,183,182,181,181,
+180,179,178,178,177,176,176,175,174,173,173,172,171,
+171,170,169,168,168,167,166,165,165,164,163,162,162,
+161,160,159,159,158,157,156,156,155,154,153,153,152,
+151,150,149,149,148,147,146,146,145,144,143,143,142,
+141,140,139,139,138,137,136,136,135,134,133,132,132,
+131,130,129,129,128,127,126,125,125,124,123,122,122,
+121,120,119,118,118,117,116,115,115,114,113,112,111,
+111,110,109,108,108,107,106,105,105,104,103,102,101,
+101,100, 99, 98, 98, 97, 96, 95, 95, 94, 93, 92, 92,
+ 91, 90, 89, 89, 88, 87, 86, 86, 85, 84, 83, 83, 82,
+ 81, 81, 80, 79, 78, 78, 77, 76, 76, 75, 74, 73, 73,
+ 72, 71, 71, 70, 69, 69, 68, 67, 66, 66, 65, 64, 64,
+ 63, 62, 62, 61, 60, 60, 59, 58, 58, 57, 56, 56, 55,
+ 55, 54, 53, 53, 52, 51, 51, 50, 49, 49, 48, 48, 47,
+ 46, 46, 45, 45, 44, 43, 43, 42, 42, 41, 41, 40, 39,
+ 39, 38, 38, 37, 37, 36, 36, 35, 34, 34, 33, 33, 32,
+ 32, 31, 31, 30, 30, 29, 29, 28, 28, 27, 27, 26, 26,
+ 25, 25, 25, 24, 24, 23, 23, 22, 22, 21, 21, 21, 20,
+ 20, 19, 19, 18, 18, 18, 17, 17, 16, 16, 16, 15, 15,
+ 15, 14, 14, 14, 13, 13, 13, 12, 12, 12, 11, 11, 11,
+ 10, 10, 10,  9,  9,  9,  9,  8,  8,  8,  7,  7,  7,
+  7,  6,  6,  6,  6,  5,  5,  5,  5,  5,  4,  4,  4,
+  4,  4,  3,  3,  3,  3,  3,  3,  2,  2,  2,  2,  2,
+  2,  2,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,
+  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+  0,  0,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,
+  1,  2,  2,  2,  2,  2,  2,  2,  3,  3,  3,  3,  3,
+  3,  4,  4,  4,  4,  4,  5,  5,  5,  5,  5,  6,  6,
+  6,  6,  7,  7,  7,  7,  8,  8,  8,  9,  9,  9,  9,
+ 10, 10, 10, 11, 11, 11, 12, 12, 12, 13, 13, 13, 14,
+ 14, 14, 15, 15, 15, 16, 16, 16, 17, 17, 18, 18, 18,
+ 19, 19, 20, 20, 21, 21, 21, 22, 22, 23, 23, 24, 24,
+ 25, 25, 25, 26, 26, 27, 27, 28, 28, 29, 29, 30, 30,
+ 31, 31, 32, 32, 33, 33, 34, 34, 35, 36, 36, 37, 37,
+ 38, 38, 39, 39, 40, 41, 41, 42, 42, 43, 43, 44, 45,
+ 45, 46, 46, 47, 48, 48, 49, 49, 50, 51, 51, 52, 53,
+ 53, 54, 55, 55, 56, 56, 57, 58, 58, 59, 60, 60, 61,
+ 62, 62, 63, 64, 64, 65, 66, 66, 67, 68, 69, 69, 70,
+ 71, 71, 72, 73, 73, 74, 75, 76, 76, 77, 78, 78, 79,
+ 80, 81, 81, 82, 83, 83, 84, 85, 86, 86, 87, 88, 89,
+ 89, 90, 91, 92, 92, 93, 94, 95, 95, 96, 97, 98, 98,
+ 99,100,101,101,102,103,104,105,105,106,107,108,108,
+109,110,111,111,112,113,114,115,115,116,117,118,118,
+119,120,121,122,122,123,124,125,125,126 };
+
+
+#pragma endregion
+#endif
+
+#include <J3ML/Algorithm/Reinterpret.hpp>
+
+/// Swaps two elements in-place without copying their data.
+template <typename T>
+void Swap(T &a, T &b)
+{
+    T temp = std::move(a);
+    a = std::move(b);
+    b = std::move(temp);
+}
+
+namespace J3ML {
+    /// Clean symbolic names for integers of specific size.
+    namespace SizedIntegralTypes {
+        using u8 = uint8_t;
+        using u16 = uint16_t;
+        using u32 = uint32_t;
+        using u64 = uint64_t;
+        using s8 = int8_t;
+        using s16 = int16_t;
+        using s32 = int32_t;
+        using s64 = int64_t;
+    }
+    //using namespace SizedIntegralTypes; // Bring into J3ML namespace.
+
+    namespace SizedFloatTypes { // TODO: Use C++23 <stdfloat>
+        using f16  = float;
+        using f32 = float;
+        using f64 = double;
+        using f128 = long double;
+    }
+    //using namespace SizedFloatTypes;    // Bring into J3ML namespace.
+}
+
+using namespace J3ML::SizedIntegralTypes;
+using namespace J3ML::SizedFloatTypes;
+
+namespace J3ML::BitTwiddling {
+    /// Parses a string of form "011101010" to a u32
+    u32 BinaryStringToValue(const char* s);
+
+    /// Returns the number of 1's set in the given value.
+    inline int CountBitsSet(u32 value);
+}
+
+namespace J3ML::Math {
+    enum class Quadrant { I, II, III, IV };
+
+    // Zero technically isn't a sign, but zero also isn't positive, or negative, so bite me.
+    enum class Sign { ZERO, POSITIVE, NEGATIVE};
+
+}
+
+namespace J3ML::Math::Constants { // TODO: Consider double precision for these.
+        /// sqrt(2pi) ^ -1
+        constexpr float RecipSqrt2Pi = 0.3989422804014326779399460599343818684758586311649346576659258296706579258993018385012523339073069364;
+        /// pi - https://www.mathsisfun.com/numbers/pi.html
+        constexpr float Pi = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679;
+        constexpr float TwoPi = Pi*2.0;
+        constexpr float PiOverTwo = Pi/2.0;
+        constexpr float ThreePiOverTwo = 3.0*Pi/2.0;
+        /// e - https://www.mathsisfun.com/numbers/e-eulers-number.html
+        constexpr float EulersNumber = 2.7182818284590452353602874713526624977572470936999595749669676277240766303535475945713821785251664274;
+        /// 2pi - The ratio of a circle's circumferecne to its radius, and the number of radians in one turn.
+        constexpr float Tau = 6.28318530717958647692;
+        /// sqrt(2)
+        constexpr float PythagorasConstant = 1.41421356237309504880;
+        /// sqrt(3)
+        constexpr float TheodorusConstant = 1.73205080756887729352;
+        /// Golden Ratio
+        constexpr float Phi = 1.61803398874989484820;
+        /// ln 2
+        constexpr float NaturalLog2 = 0.6931471805599453094172321214581765680755001343602552541206800094933936219696947156058633269964186875;
+        /// ln 10
+        constexpr float NaturalLog10 = 2.3025850929940456840179914546843642076011014886287729760333279009675726096773524802359972050895982983;
+        constexpr float Infinity = INFINITY;
+        constexpr float NegativeInfinity = -INFINITY;
+        constexpr float NotANumber = NAN;
+}
+
+namespace J3ML::Math {
+    using namespace Constants; // Bring into J3ML::Math namespace.
+}
+
+namespace J3ML::Math::Functions {
+         // TODO: Implement "Wrappers" for most standard math functions.
+         // We want to later-on implement lookup tables and SSE as conditional macros.
+
+    /// Clamps the given input value to the range [min, max].
+    /** @see Clamp01(), Min(), Max(). */
+    template<typename T>
+    T Clamp(const T &val, const T &floor, const T &ceil)
+    {
+        assert(floor <= ceil);
+        return val <= ceil ? (val >= floor ? val : floor) : ceil;
+    }
+    /// Clamps the given input value to the range [0, 1].
+    /** @see Clamp(), Min(), Max(). */
+    template<typename T>
+    T Clamp01(const T &val) { return Clamp(val, T(0), T(1)); }
+
+    /// Computes the smaller of the two values.
+    /** @see Clamp(), Clamp01(), Max() */
+    template <typename T>
+    T Min(const T& a, const T& b) {
+        return a <= b ? a : b;
+    }
+
+    /// Computes the larger of two values.
+    /** @see Clamp(), Clamp01(), Max() */
+    template <typename T>
+    T Max(const T& a, const T& b) {
+        return a >= b ? a : b;
+    }
+
+    /// Computes the smallest in an arbitrary list of values.
+    /** @see Clamp(), Clamp01(), Max() */
+    template <typename T>
+    T Min(const std::initializer_list<T>& list) {
+        T minimum = list[0];
+
+        for (T entry : list) {
+            if (entry <= minimum)
+                minimum = entry;
+        }
+        return minimum;
+    }
+
+    /// Computes the largest in an arbitrary list of values.
+    /** @see Clamp(), Clamp01(), Max() */
+    template <typename T>
+    T Max(const std::initializer_list<T>& list) {
+        T maximum = list[0];
+        for (T entry : list) {
+            if (entry >= maximum)
+                maximum = entry;
+        }
+        return maximum;
+    }
+
+    /** @return True if a > b. */
+    template <typename T>
+    bool GreaterThan(const T& a, const T& b) {
+        return a > b;
+    }
+
+    /** @return True if a < b. */
+    template <typename T>
+    bool LessThan(const T& a, const T& b) {
+        return a < b;
+    }
+
+    /** @return The absolute value of a. */
+    template <typename T>
+    T Abs(const T& a) {
+        return a >= 0 ? a : -a;
+    }
+
+    template<> inline float Abs(const float& a) {
+#ifdef USE_SSE
+#else
+        return a >= 0 ? a : -a;
+#endif
+    }
+
+    /// @return True if a and b are equal, using operator ==().
+    template <typename T>
+    bool EqualExact(const T& a, const T& b) {
+        return a == b;
+    }
+
+    /** Compares the two values for equality up to a small epsilon. */
+    bool Equal(float a, float b, float epsilon = 1e-3f);
+
+    /** Compares the two values for equality up to a small epsilon. */
+    bool Equal(double a, double b, float epsilon = 1e-3f);
+
+    /** Compares the two values for equality, allowing the given amount of absolute error. */
+    bool EqualAbs(float a, float b, float epsilon = 1e-3f);
+
+    /// Computes the relative error of the two variables.
+    float RelativeError(float a, float b);
+
+    template <typename T> bool IsFinite(T) { return true;}
+
+    template<> inline bool IsFinite(float f) { return (ReinterpretAs<u32>(f) << 1) < 0xFF000000u; }
+    template<> inline bool IsFinite(double d) { return (ReinterpretAs<u64>(d) << 1) < 0xFFE0000000000000ULL; }
+    template<> inline bool IsFinite(u32 i) { return (i << 1) < 0xFF000000u; }
+    template<> inline bool IsFinite(u64 i) { return (i << 1) < 0xFFE0000000000000ULL;}
+
+    inline bool IsNotANumber(float f) { return (ReinterpretAs<u32>(f) << 1) > 0xFF000000u; }
+    inline bool IsNotANumber(double d) { return (ReinterpretAs<u64>(d) << 1) > 0xFFE0000000000000ULL; }
+
+    inline bool IsInfinite(float f) { return (ReinterpretAs<u32>(f) << 1) == 0xFF000000u; }
+    inline bool IsInfinite(double d) { return (ReinterpretAs<u64>(d) << 1) == 0xFFE0000000000000ULL; }
+
+
+    namespace Trigonometric {
+        Sign SignOfSin(float radians);
+        Sign SignOfCos(float radians);
+        Sign SignOfTan(float radians);
+
+        Quadrant QuadrantOf(float radians);
+
+
+        float Radians(float deg); /// Converts the given amount of degrees into radians.
+        float Degrees(float rad); /// Converts the given amount of radians into degrees.
+
+        float Sin(float x); /// Computes the sine of x, in radians.
+        float Cos(float x); /// Computes the cosine of x, in radians.
+        float Tan(float x); /// Computes the tangent of x, in radians.
+
+        /// Simultaneously computes both sine and cosine of x, in radians.
+        /// This yields a small performance increase over computing them separately.
+        /// @see Sin(), Cos().
+        void SinCos(float x, float& outSin, float& outCos);
+
+        float Asin(float x); /// Computes the inverse sine of x, in radians.
+        float Acos(float x); /// Computes the inverse cosine of x, in radians.
+        float Atan(float x); /// Computes the inverse tangent of x, in radians.
+        float Atan2(float y, float x); /// Computes the signed (principal value) inverse tangent of y/x, in radians.
+        float Sinh(float x); /// Computes the hyperbolic sine of x, in radians.
+        float Cosh(float x); /// Computes the hyperbolic cosine of x, in radians.
+        float Tanh(float x); /// Computes the hyperbolic tangent of x, in radians.
+    }
+
+
+    using namespace Trigonometric;
+
+    bool IsPow2(u32 number); /// Returns true if the given number is a power of 2.
+    bool IsPow2(u64 number); /// Returns true if the given number is a power of 2.
+
+    float PowInt(float base, int exponent); /// Raises the given base to an integral exponent.
+    float Pow(float base, float exponent); /// Raises the given base to a float exponent.
+    float Exp(float exp); /// Returns e (the constant 2.71828...) raised to the given power.
+    float Log(float base, float value); /// Computes a logarithm of the given value in the specified base.
+    float Log2(float value); /// Computes a logarithm in base-2.
+    float Ln(float value); /// Computes a logarithm in the natural base (using e as the base).
+    float Log10(float value); /// Computes a logarithm in base-10;
+
+    float Ceil(float f);    /// Returns f rounded up to the next integer, as float.
+    int CeilInt(float f);   /// Returns f rounded up to the next integer, as integer.
+    float Floor(float f);   /// Returns f rounded down to the previous integer, as float.
+    int FloorInt(float f);  /// Returns f rounded down to the previous integer, as integer.
+    float Round(float f);   /// Returns f rounded to the nearest integer, as float.
+    int RoundInt(float f);  /// Returns f rounded to the nearest integer, as int.
+
+
+    float Round(float f, float decimalPlaces); /// Returns f rounded to the given decimal places.
+
+
+    float Sign(float f); /// Returns -1 or 1 depending on the sign of f.
+    ///
+    float SignOrZero(float f, float epsilon = 1e-8f);
+
+    /// Formats larger numbers into shortened 'Truncated' string representations.
+    /// 2241 -> 2.2k, 55421 -> 55.4k, 1000000 -> 1.0M
+    std::string Truncate(float input);
+
+
+
+
+
+    float RecipFast(float x);
+
+
+    struct NumberRange
+    {
+        float LowerBound;
+        float UpperBound;
+    };
+
+
+    float NormalizeToRange(float input, float fromLower, float fromUpper, float toLower, float toUpper);
+    float NormalizeToRange(float input, const NumberRange& from, const NumberRange& to);
+    // auto rotation_normalized = NormalizeToRange(inp, {0, 360}, {-1, 1});
+
+    /// Linearly interpolates between a and b.
+    /** @param t A value between [0,1].
+        @param a The first endpoint to lerp between.
+        @param b The second endpoint to lerp between.
+        @return This function computes a + t*(b-a). That is, if t==0, this function returns a. If t==1, this function returns b.
+            Otherwise, the returned value linearly moves from a to b as t ranges from 0 to 1.
+        @see LerpMod(), InvLerp(), Step(), SmoothStep(), PingPongMod(), Mod(), ModPos(), Frac(). */
+    float Lerp(float a, float b, float t);
+
+    /// Linearly interpolates from a to b, under the modulus mod.
+    /** This function takes evaluates a and b in the range [0, mod] and takes the shorter path to reach from a to b.
+        @see Lerp(), InvLerp(), Step(), SmoothStep(), PingPongMod(), Mod(), ModPos(), Frac(). */
+    float LerpMod(float a, float b, float mod, float t);
+
+    /// Computes the lerp factor a and b have to be Lerp()ed to get x.
+    /// /** @see Lerp(), LerpMod(), Step(), SmoothStep(), PingPongMod(), Mod(), ModPos(), Frac(). */
+    float InverseLerp(float a, float b, float x);
+    /// See http://msdn.microsoft.com/en-us/library/bb509665(v=VS.85).aspx
+    float Step(float y, float x);
+    /// See http://msdn.microsoft.com/en-us/library/bb509658(v=vs.85).aspx
+    float Ramp(float min, float max, float x);
+    /// Limits x to the range [0, mod], but instead of wrapping around from mod to 0, the result will move back
+    /// from mod to 0 as x goes from mod to 2*mod.
+    /** @see Lerp(), LerpMod(), InvLerp(), Step(), SmoothStep(), Mod(), ModPos(), Frac(). */
+    float PingPongMod(float x, float mod);
+
+    /// Computes a floating-point modulus.
+    /// This function returns a value in the range ]-mod, mod[.
+    /** @see Lerp(), LerpMod(), InvLerp(), Step(), SmoothStep(), PingPongMod(), ModPos(), Frac(). */
+    float Mod(float x, float mod);
+    /// Computes a floating-point modulus using an integer as the modulus.
+    float Mod(float x, int mod);
+
+    /// Computes a floating-point modulus, but restricts the output to the range [0, mod[.
+    /** @see Lerp(), LerpMod(), InvLerp(), Step(), SmoothStep(), PingPongMod(), Mod(), Frac(). */
+    float ModPos(float x, float mod);
+    /// Computes a floating-point modulus, but restricts the output to the range [0, mod[.
+    float ModPos(float x, int mod);
+    /// Returns the fractional part of x.
+    /** @see Lerp(), LerpMod(), InvLerp(), Step(), SmoothStep(), PingPongMod(), Mod(), ModPos(). */
+    float Frac(float x);
+    float Sqrt(float x); /// Returns the square root of x.
+    float FastSqrt(float x); /// Computes a fast approximation of the square root of x.
+    float RSqrt(float x); /// Returns 1/Sqrt(x). (The reciprocal of the square root of x)
+    float FastRSqrt(float x); /// SSE implementation of reciprocal square root.
+    float Recip(float x); /// Returns 1/x, the reciprocal of x.
+    float RecipFast(float x); /// Returns 1/x, the reciprocal of x, using a fast approximation (SSE rcp instruction).
+
+}
+
+namespace J3ML::Math::Functions::Interpolation
+{
+    inline float SmoothStart(float t);
+}
+
+
+namespace J3ML::Math {
+    using namespace Functions;
+}
+
+namespace J3ML::Math::Types {
+
+
+    struct Radians { // TODO: Fill in with relevant members.
+        float value;
+        float operator()() const { return value; }
+    };
+
+    struct Degrees { // TODO: Fill in with relevant members.
+        float value;
+        float operator()() const { return value; }
+    };
+
+
+}
+
+namespace J3ML::Math {
+    struct Rotation {
+        Rotation();
+        Rotation(float value);
+        Rotation(const Types::Radians& radians);
+
+        Rotation(const Types::Degrees& degrees);
+
+        float valueInRadians;
+        float ValueInRadians() const { return valueInRadians; }
+        Types::Radians Radians() const { return {valueInRadians}; }
+        float Degrees() const { return Functions::Degrees(valueInRadians); }
+
+        Rotation operator+(const Rotation& rhs);
+    };
+
+    Rotation operator ""_rad(long double rads);
+
+    Rotation operator ""_radians(long double rads);
+
+    Rotation operator ""_deg(long double rads);
+
+    Rotation operator ""_degrees(long double rads);
+}
+

include/J3ML/LinearAlgebra.hpp

@@ -15,16 +15,16 @@
 
 // Library Code //
 
-#include "J3ML/LinearAlgebra/Vector2.h"
-#include "J3ML/LinearAlgebra/Vector3.h"
-#include "J3ML/LinearAlgebra/Vector4.h"
-#include "J3ML/LinearAlgebra/Quaternion.h"
-#include "J3ML/LinearAlgebra/AxisAngle.h"
-#include "J3ML/LinearAlgebra/EulerAngle.h"
-#include "J3ML/LinearAlgebra/Matrix2x2.h"
-#include "J3ML/LinearAlgebra/Matrix3x3.h"
-#include "J3ML/LinearAlgebra/Matrix4x4.h"
-#include "J3ML/LinearAlgebra/Transform2D.h"
-#include "J3ML/LinearAlgebra/CoordinateFrame.h"
+#include "J3ML/LinearAlgebra/Vector2.hpp"
+#include "J3ML/LinearAlgebra/Vector3.hpp"
+#include "J3ML/LinearAlgebra/Vector4.hpp"
+#include "J3ML/LinearAlgebra/Quaternion.hpp"
+#include "J3ML/LinearAlgebra/AxisAngle.hpp"
+#include "J3ML/LinearAlgebra/EulerAngle.hpp"
+#include "J3ML/LinearAlgebra/Matrix2x2.hpp"
+#include "J3ML/LinearAlgebra/Matrix3x3.hpp"
+#include "J3ML/LinearAlgebra/Matrix4x4.hpp"
+#include "J3ML/LinearAlgebra/Transform2D.hpp"
+#include "J3ML/LinearAlgebra/CoordinateFrame.hpp"
 
 using namespace J3ML::LinearAlgebra;

include/J3ML/LinearAlgebra/Angle2D.hpp

@@ -1,5 +1,5 @@
 #pragma once
-
+#include <J3ML/J3ML.hpp>
 
 namespace J3ML::LinearAlgebra {
 
@@ -19,4 +19,4 @@ namespace J3ML::LinearAlgebra {
         }
     };
 
-}
+}

include/J3ML/LinearAlgebra/AxisAngle.h

@@ -1,26 +0,0 @@
-#pragma once
-
-#include <J3ML/LinearAlgebra/EulerAngle.h>
-#include <J3ML/LinearAlgebra/Quaternion.h>
-#include <J3ML/LinearAlgebra/AxisAngle.h>
-#include <J3ML/LinearAlgebra/Vector3.h>
-
-namespace J3ML::LinearAlgebra
-{
-
-    /// Transitional datatype, not useful for internal representation of rotation
-    /// But has uses for conversion and manipulation.
-    class AxisAngle {
-        Vector3 axis;
-        float angle;
-    public:
-        AxisAngle();
-
-        AxisAngle(const Vector3 &axis, float angle);
-
-        EulerAngle ToEulerAngleXYZ() const;
-
-        Quaternion ToQuaternion() const;
-        static AxisAngle FromEulerAngleXYZ(const EulerAngle&);
-    };
-}

include/J3ML/LinearAlgebra/AxisAngle.hpp

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <J3ML/LinearAlgebra/EulerAngle.hpp>
+#include <J3ML/LinearAlgebra/Quaternion.hpp>
+#include <J3ML/LinearAlgebra/Vector3.hpp>
+
+namespace J3ML::LinearAlgebra {
+    class AxisAngle;
+}
+
+/// Transitional datatype, not useful for internal representation of rotation
+/// But has uses for conversion and manipulation.
+class J3ML::LinearAlgebra::AxisAngle {
+public:
+    Vector3 axis;
+    // Radians.
+    float angle;
+public:
+    AxisAngle();
+    explicit AxisAngle(const Quaternion& q);
+    explicit AxisAngle(const EulerAngleXYZ& e);
+    AxisAngle(const Vector3& axis, float angle);
+
+};

include/J3ML/LinearAlgebra/CoordinateFrame.hpp

@@ -1,7 +1,7 @@
 #pragma once
 
 
-#include <J3ML/LinearAlgebra/Vector3.h>
+#include <J3ML/LinearAlgebra/Vector3.hpp>
 
 namespace J3ML::LinearAlgebra
 {

include/J3ML/LinearAlgebra/DirectionVector.hpp

@@ -0,0 +1,20 @@
+#pragma once
+#include <J3ML/LinearAlgebra/Vector3.hpp>
+
+namespace J3ML::LinearAlgebra {
+    class DirectionVectorRH;
+}
+
+/// Direction vector of a given Matrix3x3 RotationMatrix in a Right-handed coordinate space.
+class J3ML::LinearAlgebra::DirectionVectorRH : public Vector3 {
+private:
+    // This is purposefully not exposed because these types aren't usually convertable.
+    explicit DirectionVectorRH(const Vector3& rhs);
+public:
+    static DirectionVectorRH Forward(const Matrix3x3& rhs);
+    static DirectionVectorRH Backward(const Matrix3x3& rhs);
+    static DirectionVectorRH Left(const Matrix3x3& rhs);
+    static DirectionVectorRH Right(const Matrix3x3& rhs);
+    static DirectionVectorRH Up(const Matrix3x3& rhs);
+    static DirectionVectorRH Down(const Matrix3x3& rhs);
+};

include/J3ML/LinearAlgebra/EulerAngle.h

@@ -1,50 +0,0 @@
-#pragma once
-
-#include <J3ML/LinearAlgebra/Vector3.h>
-#include <J3ML/LinearAlgebra/Quaternion.h>
-#include <J3ML/LinearAlgebra/AxisAngle.h>
-
-namespace J3ML::LinearAlgebra {
-
-    class AxisAngle;
-
-// Essential Reading:
-// http://www.essentialmath.com/GDC2012/GDC2012_JMV_Rotations.pdf
-class EulerAngle {
-public:
-    EulerAngle();
-    EulerAngle(float pitch, float yaw, float roll);
-    EulerAngle(const Vector3& vec) : pitch(vec.x), yaw(vec.y), roll(vec.z) {}
-
-    AxisAngle ToAxisAngle() const;
-
-
-    explicit EulerAngle(const Quaternion& orientation);
-    explicit EulerAngle(const AxisAngle& orientation);
-
-    /// TODO: Implement separate upper and lower bounds
-    /// Preserves internal value of euler angles, normalizes and clamps the output.
-    /// This does not solve gimbal lock!!!
-    float GetPitch(float pitch_limit) const;
-    float GetYaw(float yaw_limit)     const;
-    float GetRoll(float roll_limit)   const;
-
-    bool operator==(const EulerAngle& a) const;
-    void clamp();
-
-    // TODO: Euler Angles do not represent a vector, length doesn't apply, nor is this information meaningful for this data type.
-    // If you need a meaningful representation of length in 3d space, use a vector!!
-    [[nodiscard]] float length() const {
-        return 0;
-    }
-    // TODO: Implement
-    Vector3 unitVector() const;
-
-    EulerAngle movementAngle() const;
-public:
-    float pitch;
-    float yaw;
-    float roll;
-};
-
-}

include/J3ML/LinearAlgebra/EulerAngle.hpp

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <J3ML/LinearAlgebra/Vector3.hpp>
+#include <J3ML/LinearAlgebra/Quaternion.hpp>
+#include <J3ML/LinearAlgebra/AxisAngle.hpp>
+
+namespace J3ML::LinearAlgebra {
+    class EulerAngleXYZ;
+}
+
+class J3ML::LinearAlgebra::EulerAngleXYZ {
+public:
+public:
+    float roll = 0; // X
+    float pitch = 0; // Y
+    float yaw = 0; // Z
+public:
+    EulerAngleXYZ(float roll, float pitch, float yaw);
+public:
+    explicit EulerAngleXYZ(const Quaternion& rhs);
+    explicit EulerAngleXYZ(const AxisAngle& rhs);
+    explicit EulerAngleXYZ(const Matrix3x3& rhs);
+};

include/J3ML/LinearAlgebra/Forward.hpp

@@ -4,10 +4,11 @@
 namespace J3ML::LinearAlgebra
 {
     class Vector2; // A type representing a position in a 2-dimensional coordinate space.
+    class Vector2i;
     class Vector3; // A type representing a position in a 3-dimensional coordinate space.
     class Vector4; // A type representing a position in a 4-dimensional coordinate space.
     class Angle2D; // Uses x,y components to represent a 2D rotation.
-    class EulerAngle; // Uses pitch,yaw,roll components to represent a 3D orientation.
+    class EulerAngleXYZ; // Uses pitch,yaw,roll components to represent a 3D orientation.
     class AxisAngle; //
     class CoordinateFrame; //
     class Matrix2x2;
@@ -15,14 +16,20 @@ namespace J3ML::LinearAlgebra
     class Matrix4x4;
     class Transform2D;
     class Transform3D;
+    class DirectionVectorRH; // A type representing a direction in 3D space.
     class Quaternion;
 
 
     using Position = Vector3;
+
+    template <int N> class PBVolume;
 }
 
 // Methods required by LinearAlgebra types
 namespace J3ML::LinearAlgebra
 {
 
-}
+}
+
+
+using namespace J3ML::LinearAlgebra;

include/J3ML/LinearAlgebra/Matrices.inl

@@ -0,0 +1,237 @@
+#pragma once
+
+/// Template Parameterized (Generic) Matrix Functions.
+
+#include <J3ML/LinearAlgebra/Quaternion.hpp>
+#include "J3ML/J3ML.hpp"
+
+
+namespace J3ML::LinearAlgebra {
+
+    template <typename Matrix>
+    bool InverseMatrix(Matrix &mat, float epsilon)
+    {
+        Matrix inversed = Matrix::Identity;
+
+        const int nc = std::min<int>(Matrix::Rows, Matrix::Cols);
+
+        for (int column = 0; column < nc; ++column)
+        {
+            // find the row i with i >= j such that M has the largest absolute value.
+            int greatest = column;
+            float greatestVal = std::abs(mat[greatest][column]);
+            for (int i = column+1; i < Matrix::Rows; i++)
+            {
+                float val = std::abs(mat[i][column]);
+                if (val > greatestVal) {
+                    greatest = i;
+                    greatestVal = val;
+                }
+            }
+
+            if (greatestVal < epsilon) {
+                mat = inversed;
+                return false;
+            }
+
+            // exchange rows
+            if (greatest != column) {
+                inversed.SwapRows(greatest, column);
+                mat.SwapRows(greatest, column);
+            }
+
+            // multiply rows
+            assert(!Math::EqualAbs(mat[column][column], 0.f, epsilon));
+            float scale = 1.f / mat[column][column];
+            inversed.ScaleRow(column, scale);
+            mat.ScaleRow(column, scale);
+
+            // add rows
+            for (int i = 0; i < column; i++) {
+                inversed.SetRow(i, inversed.Row(i) - inversed.Row(column) * mat[i][column]);
+                mat.SetRow(i, mat.Row(i) - mat.Row(column) * mat[i][column]);
+            }
+
+            for (int i = column + 1; i < Matrix::Rows; i++) {
+                inversed.SetRow(i, inversed.Row(i) - inversed.Row(column) * mat[i][column]);
+                mat.SetRow(i, mat.Row(i) - mat.Row(column) * mat[i][column]);
+            }
+        }
+        mat = inversed;
+        return true;
+    }
+
+
+    /// Computes the LU-decomposition on the given square matrix.
+    /// @return True if the composition was successful, false otherwise. If the return value is false, the contents of the output matrix are unspecified.
+    template <typename Matrix>
+    bool LUDecomposeMatrix(const Matrix &mat, Matrix &lower, Matrix &upper)
+    {
+        lower = Matrix::Identity;
+        upper = Matrix::Zero;
+
+        for (int i = 0; i < Matrix::Rows; ++i)
+        {
+            for (int col = i; col < Matrix::Cols; ++col)
+            {
+                upper[i][col] = mat[i][col];
+                for (int k = 0; k < i; ++k)
+                    upper[i][col] -= lower[i][k] * upper[k][col];
+            }
+            for (int row = i+1; row < Matrix::Rows; ++row)
+            {
+                lower[row][i] = mat[row][i];
+                for (int k = 0; k < i; ++k)
+                    lower[row][i] -= lower[row][k] * upper[k][i];
+                if (Math::EqualAbs(upper[i][i], 0.f))
+                    return false;
+                lower[row][i] /= upper[i][i];
+            }
+        }
+        return true;
+    }
+
+    /// Computes the Cholesky decomposition on the given square matrix *on the real domain*.
+    /// @return True if successful, false otherwise. If the return value is false, the contents of the output matrix are uspecified.
+    template <typename Matrix>
+    bool CholeskyDecomposeMatrix(const Matrix &mat, Matrix& lower)
+    {
+        lower = Matrix::Zero;
+        for (int i = 0; i < Matrix::Rows; ++i)
+        {
+            for (int j = 0; j < i; ++i)
+            {
+                lower[i][j] = mat[i][j];
+                for (int k = 0; k < j; ++k)
+                    lower[i][j] -= lower[i][j] * lower[j][k];
+                if (Math::EqualAbs(lower[j][j], 0.f))
+                    return false;
+                lower[i][j] /= lower[j][j];
+            }
+            lower[i][i] = mat[i][i];
+            if (lower[i][i])
+                return false;
+            for (int k = 0; k < i; ++k)
+                lower[i][i] -= lower[i][k] * lower[i][k];
+            lower[i][i] = std::sqrt(lower[i][i]);
+        }
+
+        return false;
+    }
+
+
+    template<typename Matrix>
+    void SetMatrixRotatePart(Matrix &m, const Quaternion &q) {
+        // See https://www.geometrictools.com/Documentation/LinearAlgebraicQuaternions.pdf .
+
+        assert(q.IsNormalized(1e-3f));
+        const float x = q.x;
+        const float y = q.y;
+        const float z = q.z;
+        const float w = q.w;
+        m[0][0] = 1 - 2 * (y * y + z * z);
+        m[0][1] = 2 * (x * y - z * w);
+        m[0][2] = 2 * (x * y + y * w);
+        m[1][0] = 2 * (x * y + z * w);
+        m[1][1] = 1 - 2 * (x * x + z * z);
+        m[1][2] = 2 * (y * z - x * w);
+        m[2][0] = 2 * (x * z - y * w);
+        m[2][1] = 2 * (y * z + x * w);
+        m[2][2] = 1 - 2 * (x * x + y * y);
+    }
+
+    /** 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 = Math::Sin(angle);
+        cosz = Math::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 = Math::Sin(angle);
+        cosz = Math::Cos(angle);
+
+        m[0][0] = cosz;
+        m[0][1] = 0.f;
+        m[0][2] = sinz;
+        m[1][0] = 0.f;
+        m[1][1] = 1.f;
+        m[1][2] = 0.f;
+        m[2][0] = -sinz;
+        m[2][1] = 0.f;
+        m[2][2] = cosz;
+    }
+
+    /** Sets the top-left 3x3 area of the matrix to the rotation matrix about the Z-axis. Elements
+        outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
+        in the order of M*v, and clockwise if rotated in the order v*M.
+        @param m The matrix to store the result.
+        @param angle The rotation angle in radians. */
+    template<typename Matrix>
+    void Set3x3PartRotateZ(Matrix &m, float angle) {
+        float sinz, cosz;
+        sinz = std::sin(angle);
+        cosz = std::cos(angle);
+
+        m[0][0] = cosz;
+        m[0][1] = -sinz;
+        m[0][2] = 0.f;
+        m[1][0] = sinz;
+        m[1][1] = cosz;
+        m[1][2] = 0.f;
+        m[2][0] = 0.f;
+        m[2][1] = 0.f;
+        m[2][2] = 1.f;
+    }
+
+
+    /** Computes the matrix M = R_x * R_y * R_z, where R_d is the cardinal rotation matrix
+        about the axis +d, rotating counterclockwise.
+        This function was adapted from https://www.geometrictools.com/Documentation/EulerAngles.pdf .
+        Parameters x y and z are the angles of rotation, in radians. */
+    template<typename Matrix>
+    void Set3x3PartRotateEulerXYZ(Matrix &m, float x, float y, float z) {
+        // TODO: vectorize to compute 4 sines + cosines at one time;
+        float cx = std::cos(x);
+        float sx = std::sin(x);
+        float cy = std::cos(y);
+        float sy = std::sin(y);
+        float cz = std::cos(z);
+        float sz = std::sin(z);
+
+        m[0][0] = cy * cz;
+        m[0][1] = -cy * sz;
+        m[0][2] = sy;
+        m[1][0] = cz * sx * sy + cx * sz;
+        m[1][1] = cx * cz - sx * sy * sz;
+        m[1][2] = -cy * sx;
+        m[2][0] = -cx * cz * sy + sx * sz;
+        m[2][1] = cz * sx + cx * sy * sz;
+        m[2][2] = cx * cy;
+    }
+
+
+
+}

include/J3ML/LinearAlgebra/Matrix.hpp

@@ -0,0 +1,78 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file Matrix.hpp
+/// @desc Templated implementation of arbitrary-sized N-by-M matrices.
+/// @edit 2024-08-01
+/// @note On backlog, low-priority.
+
+#pragma once
+
+#include <cstddef>
+#include <cstdlib>
+#include <algorithm>
+#include "Vector.hpp"
+
+namespace J3ML::LinearAlgebra
+{
+
+
+    template <uint ROWS, uint COLS, typename T>
+    class Matrix
+    {
+        static constexpr uint Diag = std::min(ROWS, COLS);
+
+        using RowVector = Vector<ROWS, T>;
+        using ColVector = Vector<COLS, T>;
+        using DiagVector = Vector<Diag, T>;
+
+        enum { Rows = ROWS };
+        enum { Cols = COLS };
+
+        void AssertRowSize(uint rows)
+        {
+            assert(rows < Rows && "");
+        }
+        void AssertColumnSize(uint cols)
+        {
+            assert(cols < Cols && "");
+        }
+
+        RowVector GetRow(uint index) const;
+        ColVector GetColumn(uint index) const;
+        void SetRow(uint index, RowVector);
+        void SetColumn(uint index, ColVector);
+
+        RowVector &Row(uint index) const;
+        ColVector &Column(uint index) const;
+
+        const T At(uint row, uint col) const
+        {
+            AssertRowSize(row);
+            AssertColumnSize(col);
+
+            return elems[row][col];
+        }
+
+        T &At(uint row, uint col)
+        {
+            AssertRowSize(row);
+            AssertColumnSize(col);
+
+            return elems[row][col];
+        }
+
+        float* ptr();
+        const float* ptr() const;
+
+        float operator[](uint index) const;
+        float& operator[](uint index);
+
+    private:
+        T elems[ROWS][COLS];
+    };
+}

include/J3ML/LinearAlgebra/Matrix2x2.h

@@ -1,36 +0,0 @@
-#pragma once
-
-
-#include <J3ML/LinearAlgebra/Vector2.h>
-
-namespace J3ML::LinearAlgebra {
-    class Matrix2x2 {
-    public:
-        enum { Rows = 3 };
-        enum { Cols = 3 };
-        static const Matrix2x2 Zero;
-        static const Matrix2x2 Identity;
-        static const Matrix2x2 NaN;
-
-        Matrix2x2() {}
-        Matrix2x2(float val);
-        Matrix2x2(float m00, float m01, float m10, float m11);
-        Matrix2x2(const Vector2& r1, const Vector2& r2);
-        Vector2 GetRow(int index) const;
-        Vector2 GetColumn(int index) const;
-        float At(int x, int y) const;
-
-        float Determinant() const;
-        Matrix2x2 Inverse() const;
-        Matrix2x2 Transpose() const;
-
-        Vector2 Transform(const Vector2& rhs) const;
-
-
-        Vector2 operator * (const Vector2& rhs) const;
-        Matrix2x2 operator * (const Matrix2x2 &rhs) const;
-
-    protected:
-        float elems[2][2];
-    };
-}

include/J3ML/LinearAlgebra/Matrix2x2.hpp

@@ -0,0 +1,60 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file Matrix2x2.hpp
+/// @desc A two-by-two Matrix object.
+/// @edit 2024-08-01
+/// @note On backlog, low-priority.
+
+#pragma once
+
+
+
+#include <J3ML/LinearAlgebra.hpp>
+
+namespace J3ML::LinearAlgebra {
+    class Matrix2x2 {
+    public:
+        enum { Rows = 2 };
+        enum { Cols = 2 };
+        static const Matrix2x2 Zero;
+        static const Matrix2x2 Identity;
+        static const Matrix2x2 NaN;
+
+        Matrix2x2() {}
+        Matrix2x2(float val);
+        Matrix2x2(float m00, float m01, float m10, float m11);
+        Matrix2x2(const Vector2& r1, const Vector2& r2);
+        explicit Matrix2x2(const float *data);
+
+        Vector2 GetRow(int index) const;
+        Vector2 GetColumn(int index) const;
+
+        void SetRow(int i, const Vector2& row);
+        void SetColumn(int i, const Vector2& col);
+        void SetAt(int x, int y, float value);
+
+        float At(int x, int y) const;
+        float &At(int x, int y);
+
+        float Determinant() const;
+        Matrix2x2 Inverse() const;
+        Matrix2x2 Transpose() const;
+
+        Vector2 Transform(const Vector2& rhs) const;
+
+
+        Vector2 operator * (const Vector2& rhs) const;
+        Matrix2x2 operator * (const Matrix2x2 &rhs) const;
+
+        inline float* ptr() { return &elems[0][0];}
+        [[nodiscard]] inline const float* ptr() const {return &elems[0][0];}
+
+    protected:
+        float elems[2][2];
+    };
+}

include/J3ML/LinearAlgebra/Matrix3x3.h

@@ -1,134 +0,0 @@
-#pragma once
-
-
-#include <J3ML/LinearAlgebra/Vector2.h>
-#include <J3ML/LinearAlgebra/Vector3.h>
-#include <J3ML/LinearAlgebra/Quaternion.h>
-
-namespace J3ML::LinearAlgebra {
-
-
-    class Quaternion;
-
-    /// A 3-by-3 matrix for linear transformations of 3D geometry.
-    /* This can represent any kind of linear transformations of 3D geometry, which include
-     * rotation, scale, shear, mirroring, and orthographic projection.
-     * A 3x3 matrix cannot represent translation, which requires a 3x4, or perspective projection (4x4).
-     * The elements of this matrix are
-     *  m_00, m_01, m_02
-     *  m_10, m_11, m_12
-     *  m_20, m_21, m_22
-     *
-     *  The element m_yx is the value on the row y and column x.
-     *  You can access m_yx using the double-bracket notation m[y][x], or using the member function At.
-     *
-     *  @note The member functions in this class use the convention that transforms are applied to
-     *  vectors in the form M * v. This means that "Matrix3x3 M, M1, M2; M = M1 * M2;" gives a transformation M
-     *  that applies M2 first, followed by M1 second
-     */
-    class Matrix3x3 {
-    public:
-        enum { Rows = 3 };
-        enum { Cols = 3 };
-
-        static const Matrix3x3 Zero;
-        static const Matrix3x3 Identity;
-        static const Matrix3x3 NaN;
-
-        Matrix3x3() {}
-        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);
-        explicit Matrix3x3(const Quaternion& orientation);
-
-        static Matrix3x3 RotateX(float radians);
-        static Matrix3x3 RotateY(float radians);
-        static Matrix3x3 RotateZ(float radians);
-
-        Vector3 GetRow(int index) const;
-        Vector3 GetColumn(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);
-
-        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);
-
-        static Matrix3x3 LookAt(const Vector3& forward, const Vector3& target, const Vector3& localUp, const Vector3& worldUp);
-
-        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.
-        // Transforming a vector v using this matrix computes the vector
-        // v' == M * v == R*S*v == (R * (S * v)) which means the scale operation
-        // 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);
-
-        /// Returns the main diagonal.
-        Vector3 Diagonal() const;
-        /// Returns the local +X/+Y/+Z axis in world space.
-        /// This is the same as transforming the vector{1,0,0} by this matrix.
-        Vector3 WorldX() const;
-        /// Returns the local +Y axis in world space.
-        /// This is the same as transforming the vector{0,1,0} by this matrix.
-        Vector3 WorldY() const;
-        /// Returns the local +Z axis in world space.
-        /// This is the same as transforming the vector{0,0,1} by this matrix.
-        Vector3 WorldZ() const;
-
-        /// Computes the determinant of this matrix.
-        // If the determinant is nonzero, this matrix is invertible.
-        // If the determinant is negative, this matrix performs reflection about some axis.
-        // From http://msdn.microsoft.com/en-us/library/bb204853(VS.85).aspx :
-        // "If the determinant is positive, the basis is said to be "positively" oriented (or right-handed).
-        // If the determinant is negative, the basis is said to be "negatively" oriented (or left-handed)."
-        // @note This function computes 9 LOADs, 9 MULs and 5 ADDs. */
-        float Determinant() const;
-
-        // Returns an inverted copy of this matrix. This
-        Matrix3x3 Inverse() const;
-
-        // Returns a transposed copy of this matrix.
-        Matrix3x3 Transpose() const;
-
-        // 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;
-
-
-        Matrix3x3 ScaleBy(const Vector3& rhs);
-        Vector3 GetScale() const;
-
-        Vector3 operator[](int row) const;
-
-        Vector3 operator * (const Vector3& rhs) const;
-        Matrix3x3 operator * (const Matrix3x3& rhs) const;
-
-    protected:
-        float elems[3][3];
-    };
-}

include/J3ML/LinearAlgebra/Matrix3x3.hpp

@@ -0,0 +1,472 @@
+#pragma once
+
+
+
+#include <J3ML/LinearAlgebra/Vector2.hpp>
+#include <J3ML/LinearAlgebra/Vector3.hpp>
+#include <J3ML/LinearAlgebra/Quaternion.hpp>
+#include <J3ML/LinearAlgebra/EulerAngle.hpp>
+#include <J3ML/Algorithm/RNG.hpp>
+
+using namespace J3ML::Algorithm;
+
+#include <cstring>
+
+namespace J3ML::LinearAlgebra {
+
+
+
+    /// A 3-by-3 matrix for linear transformations of 3D geometry.
+    /* This can represent any kind of linear transformations of 3D geometry, which include
+     * rotation, scale, shear, mirroring, and orthographic projection.
+     * A 3x3 matrix cannot represent translation, which requires a 3x4, or perspective projection (4x4).
+     * The elements of this matrix are
+     *  m_00, m_01, m_02
+     *  m_10, m_11, m_12
+     *  m_20, m_21, m_22
+     *
+     *  The element m_yx is the value on the row y and column x.
+     *  You can access m_yx using the double-bracket notation m[y][x], or using the member function At.
+     *
+     *  @note The member functions in this class use the convention that transforms are applied to
+     *  vectors in the form M * v. This means that "Matrix3x3 M, M1, M2; M = M1 * M2;" gives a transformation M
+     *  that applies M2 first, followed by M1 second. */
+    class Matrix3x3 {
+    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() = default;
+
+        Matrix3x3(const Matrix3x3& rhs) { Set(rhs); }
+        /// Creates a new Matrix3x3 by setting all matrix elements equal to val.
+        explicit Matrix3x3(float val);
+        /// Creates a new Matrix3x3 by explicitly specifying all the matrix elements.
+        /// The elements are specified in row-major format, i.e. the first row first followed by the second and third row.
+        /// E.g. The element m10 denotes the scalar at second (idx 1) row, first (idx 0) column.
+        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);
+
+        //explicit Matrix3x3(const EulerAngleXYZ& orientation);
+        explicit Matrix3x3(const EulerAngleXYZ& orientation) : Matrix3x3(Quaternion(orientation)) {};
+
+        //explicit Matrix3x3(const AxisAngle& orientation);
+        explicit Matrix3x3(const AxisAngle& orientation) : Matrix3x3(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);
+        /// Creates a new M3x3 that rotates about the given axis by the given angle
+        static Matrix3x3 RotateAxisAngle(const Vector3& axis, float angleRadians);
+        /// 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);
+        /// 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);
+
+        // Returns a uniformly random 3x3 matrix that performs only rotation.
+        /** This matrix produces a random orthonormal bassi for an orientation of an object. There is no mirroring
+            or scaling present in the generated matrix. Also, naturally since Matrix3x3 cannot represent translation or projection,
+            these properties are not present either. */
+        static Matrix3x3 RandomRotation(RNG& rng);
+
+        /// Returns a random 3x3 matrix with each entry randomized between the range [minElem, maxElem]
+        /** Warning: The matrices returned by this function do not represent well-formed 3D transformations.
+            This function is mostly used for testing and debugging purposes only. */
+        static Matrix3x3 RandomGeneral(RNG& rng, float minElem, float maxElem);
+
+
+        /// Creates a new Matrix3x3 that performs the rotation expressed by the given quaternion.
+        static Matrix3x3 FromQuat(const Quaternion& orientation);
+        /// Creates a new Matrix3x3 as a combination of rotation and scale.
+        // 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.
+        // Transforming a vector v using this matrix computes the vector
+        // v' == M * v == R*S*v == (R * (S * v)) which means the scale operation
+        // 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;
+        /// 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. */
+        [[nodiscard]] Vector3 GetColumn(int index) const;
+        [[nodiscard]] Vector3 Column(int index) const;
+        [[nodiscard]] Vector3 Col(int index) const;
+        /// This method also allows assignment to the retrieved column.
+        //Vector3& Col(int index);
+
+        /// Returns only the first three elements of the given column.
+        [[nodiscard]] Vector3 GetColumn3(int index) const;
+        [[nodiscard]] Vector3 Column3(int index) const;
+        [[nodiscard]] Vector3 Col3(int index) const;
+        /// This method also allows assignment to the retrieved column.
+        //Vector3& Col3(int index);
+
+        /// 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 to 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();
+
+        /// Swaps two columns.
+        void SwapColumns(int col1, int col2);
+
+        /// Swaps two rows.
+        void SwapRows(int row1, int row2);
+
+        float &At(int row, int col);
+        [[nodiscard]] 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 *p);
+
+        /// Orthonormalizes the basis formed by the column vectors of this matrix.
+        void Orthonormalize(int c0, int c1, int c2);
+
+        /// Accesses this structure as a float array.
+        /// @return A pointer to the upper-left element. The data is contiguous in memory.
+        /// ptr[0] gives the element [0][0], ptr[1] is [0][1]. ptr[2] is [0][2]
+        inline float* ptr() { return &elems[0][0];}
+        [[nodiscard]] inline const float* ptr() const {return &elems[0][0];}
+
+        /// 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]
+        [[nodiscard]] Vector3 Diagonal() const;
+        /// Returns the local +X/+Y/+Z axis in world space.
+        /// This is the same as transforming the vector{1,0,0} by this matrix.
+        [[nodiscard]] Vector3 WorldX() const;
+        /// Returns the local +Y axis in world space.
+        /// This is the same as transforming the vector{0,1,0} by this matrix.
+        [[nodiscard]] Vector3 WorldY() const;
+        /// Returns the local +Z axis in world space.
+        /// This is the same as transforming the vector{0,0,1} by this matrix.
+        [[nodiscard]] Vector3 WorldZ() const;
+
+        /// Computes the determinant of this matrix.
+        // If the determinant is nonzero, this matrix is invertible.
+        // If the determinant is negative, this matrix performs reflection about some axis.
+        // From http://msdn.microsoft.com/en-us/library/bb204853(VS.85).aspx :
+        // "If the determinant is positive, the basis is said to be "positively" oriented (or right-handed).
+        // If the determinant is negative, the basis is said to be "negatively" oriented (or left-handed)."
+        // @note This function computes 9 LOADs, 9 MULs and 5 ADDs. */
+        [[nodiscard]] float Determinant() const;
+
+        /// Computes the determinant of a symmetric matrix.
+        /** This function can be used to compute the determinant of a matrix in the case the matrix is known beforehand
+            to be symmetric. This function is slightly faster than Determinant().
+         * @return
+         */
+        [[nodiscard]] float DeterminantSymmetric() const;
+
+        // Returns an inverted copy of this matrix.
+        [[nodiscard]] Matrix3x3 Inverted() const;
+
+        // Returns a transposed copy of this matrix.
+        [[nodiscard]] Matrix3x3 Transposed() const;
+
+        /// Returns the inverse transpose of this matrix.
+        /// Use the matrix to transform covariant vectors (normal vectors).
+        [[nodiscard]] Matrix3x3 InverseTransposed() const;
+
+        /// Computes the inverse transpose of this matrix.
+        /// Use the matrix to transform covariant vectors (normal vectors).
+        bool InverseTranspose();
+
+        /// Inverts this matrix using numerically stable Gaussian elimination.
+        /// @return Returns true on success, false otherwise;
+        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 Inverted() function.\
+            Returns true on success. On failure, the matrix is not modified. This function fails if any of the
+            elements of this vector are not finite, or if the matrix contains a zero scaling factor on X, Y, or Z.
+            @note The returned matrix will be row-orthogonal, but not column-orthogonal in general.
+            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
+            Inverted().
+            This function may not be called if this matrix contains any scaling or shearing, but it may contain mirroring.*/
+        bool InverseOrthogonalUniformScale();
+
+        /// Inverts a rotation matrix.
+        /// If a matrix is of form M = R*S, where R is a rotation matrix and S is either identity or a mirroring matrix, then
+        /// the matrix M is orthonormal and this function can be used to compute the inverse.
+        /// This function is faster than calling InverseOrthogonalUniformScale(), InverseColOrthogonal(), or the generic Inverted()
+        /// This function may not be called if this matrix contains any scaling or shearing, but it may contain mirroring.
+        void InverseOrthonormal();
+
+        /// Inverts a symmetric matrix.
+        /** This function is faster than directly calling Inverted()
+            This function computes 6 LOADs, 9 STOREs, 21 MULs, 1 DIV, 1 CMP, and 8 ADDs.
+            @return True if computing the inverse succeeded, false otherwise (determinant was zero).
+            @note If this function fails, the original matrix is not modified.
+            @note This function operates in-place. */
+        bool InverseSymmetric();
+
+        /// Transposes this matrix.
+        /// This operation swaps all elements with respect to the diagonal.
+        void Transpose();
+
+
+        /// Removes the scaling performed by this matrix. That is, decomposes this matrix M into a form M = M' * S, where
+        /// M' has unitary column vectors and S is a diagonal matrix. Then replaces this matrix with M'
+        /// @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0 0 0 1]).
+        /// @note This function does not remove reflection (-1 scale along some axis).
+        void RemoveScale();
+
+
+        // Transforms the given vectors by this matrix M, i.e. returns M * (x,y,z)
+        [[nodiscard]] Vector2 Transform(const Vector2& rhs) const;
+        [[nodiscard]] Vector3 Transform(const Vector3& rhs) const;
+
+        /// Performs a batch transformation of the given array.
+        void BatchTransform(Vector3 *pointArray, int numPoints) const;
+        /// Performs a batch transformation of the given array.
+        void BatchTransform(Vector3 *pointArray, int numPoints, int stride) const;
+        /// Performs a batch transformation of the given array.
+        /// This function ignores the w component of the input vectors. These components are assumed to be either 0 or 1.
+        void BatchTransform(Vector4 *vectorArray, int numVectors) const;
+        /// Performs a batch transformation of the given array.
+        /// This function ignores the w component of the input vectors. These components are assumed to be either 0 or 1.
+        void BatchTransform(Vector4 *vectorArray, int numVectors, int stride) const;
+
+        /// Returns the sum of the diagonal elements of this matrix.
+        [[nodiscard]] float Trace() const;
+
+
+        Matrix3x3 ScaleBy(const Vector3& rhs);
+        [[nodiscard]] Vector3 GetScale() const;
+
+        /// Scales the given row by a scalar value.
+        void ScaleRow(int row, float scalar);
+        /// Scales the given column by a scalar value.
+        void ScaleCol(int col, float scalar);
+
+        Vector3 operator[](int row) const;
+
+        /// 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;
+
+        /// Unary operator + allows this structure to be used in an expression '+x'.
+        Matrix3x3 operator + () const { return *this;}
+
+
+        /// Multiplies the two matrices.
+        Matrix3x3 operator * (const Matrix3x3& rhs) const;
+        [[nodiscard]] Matrix3x3 Mul(const Matrix3x3& rhs) const;
+        /// Multiplies the two matrices.
+        Matrix4x4 operator * (const Matrix4x4& rhs) const;
+        [[nodiscard]] Matrix4x4 Mul(const Matrix4x4& rhs) const;
+
+        [[nodiscard]] Vector2 Mul(const Vector2& rhs) const;
+        [[nodiscard]] Vector3 Mul(const Vector3& rhs) const;
+        [[nodiscard]] Vector4 Mul(const Vector4& rhs) const;
+
+        /// Converts the quaternion to a M3x3 and multiplies the two matrices together.
+        Matrix3x3 operator *(const Quaternion& rhs) const;
+
+        [[nodiscard]] Matrix3x3 Mul(const Quaternion& rhs) const;
+
+        // Returns true if the column vectors of this matrix are all perpendicular to each other.
+        [[nodiscard]] bool IsColOrthogonal(float epsilon = 1e-3f) const;
+        [[nodiscard]] bool IsColOrthogonal3(float epsilon = 1e-3f) const { return IsColOrthogonal(epsilon);}
+        // Returns true if the row vectors of this matrix are all perpendicular to each other.
+        [[nodiscard]] bool IsRowOrthogonal(float epsilon = 1e-3f) const;
+
+
+        [[nodiscard]] bool HasUniformScale(float epsilon  = 1e-3f) const;
+
+        [[nodiscard]] Vector3 ExtractScale() const;
+
+
+        /// Tests if this matrix does not contain any NaNs or infs
+        /// @return Returns true if the entries of this M3x3 are all finite.
+        [[nodiscard]] bool IsFinite() const;
+
+        /// Tests if this is the identity matrix.
+        /// @return Returns true if this matrix is the identity matrix, up to the given epsilon.
+        [[nodiscard]] bool IsIdentity(float epsilon = 1e-3f) const;
+        /// Tests if this matrix is in lower triangular form.
+        /// @return Returns true if this matrix is in lower triangular form, up to the given epsilon.
+        [[nodiscard]] bool IsLowerTriangular(float epsilon = 1e-3f) const;
+        /// Tests if this matrix is in upper triangular form.
+        /// @return Returns true if this matrix is in upper triangular form, up to the given epsilon.
+        [[nodiscard]] bool IsUpperTriangular(float epsilon = 1e-3f) const;
+        /// Tests if this matrix has an inverse.
+        /// @return Returns true if this matrix can be inverted, up to the given epsilon.
+        [[nodiscard]] bool IsInvertible(float epsilon = 1e-3f) const;
+        /// Tests if this matrix is symmetric (M == M^T).
+        /// The test compares the elements for equality. Up to the given epsilon. A matrix is symmetric if it is its own transpose.
+        [[nodiscard]] bool IsSymmetric(float epsilon = 1e-3f) const;
+        /// Tests if this matrix is skew-symmetric (M == -M^T).
+        /// The test compares the elements of this matrix up to the given epsilon. A matrix M is skew-symmetric if the identity M=-M^T holds.
+        [[nodiscard]] bool IsSkewSymmetric(float epsilon = 1e-3f) const;
+        /// Returns true if this matrix does not perform any scaling,
+        /** A matrix does not do any scaling if the column vectors of this matrix are normalized in length,
+            compared to the given epsilon. Note that this matrix may still perform reflection,
+            i.e. it has a -1 scale along some axis.
+            @note This function only examines the upper 3-by-3 part of this matrix.
+            @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0,0,0,1] */
+        [[nodiscard]] bool HasUnitaryScale(float epsilon = 1e-3f) const;
+        /// Returns true if this matrix performs a reflection along some plane.
+        /** In 3D space, an even number of reflections corresponds to a rotation about some axis, so a matrix consisting of
+            an odd number of consecutive mirror operations can only reflect about one axis. A matrix that contains reflection reverses
+            the handedness of the coordinate system. This function tests if this matrix does perform mirroring.
+            This occurs if this matrix has a negative determinant.*/
+        [[nodiscard]] bool HasNegativeScale() const;
+
+        /// Returns true if the column and row vectors of this matrix form an orthonormal set.
+        /// @note In math terms, there does not exist such a thing as an 'orthonormal matrix'. In math terms, a matrix
+        /// is orthogonal if the column and row vectors are orthogonal *unit* vectors.
+        /// In terms of this library however, a matrix is orthogonal if its column and row vectors are orthogonal. (no need to be unitary),
+        /// and a matrix is orthonormal if the column and row vectors are orthonormal.
+        [[nodiscard]] bool IsOrthonormal(float epsilon = 1e-3f) const;
+
+        /// Returns true if this Matrix3x3 is equal to the given Matrix3x3, up to given per-element epsilon.
+        bool Equals(const Matrix3x3& other, float epsilon = 1e-3f) const;
+
+
+    protected: /// Member values
+        float elems[3][3]{};
+    };
+}

include/J3ML/LinearAlgebra/Matrix4x4.h

@@ -1,254 +0,0 @@
-#pragma once
-
-#include <J3ML/LinearAlgebra/Common.h>
-
-#include <J3ML/LinearAlgebra/Matrix3x3.h>
-#include <J3ML/LinearAlgebra/Quaternion.h>
-
-
-#include <J3ML/LinearAlgebra/Vector4.h>
-#include <algorithm>
-
-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
-     *  m_00, m_01, m_02, m_03
-     *  m_10, m_11, m_12, m_13
-     *  m_20, m_21, m_22, m_23,
-     *  m_30, m_31, m_32, m_33
-     *
-     *  The element m_yx is the value on the row y and column x.
-     *  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!
-        enum { Rows = 4 };
-        enum { Cols = 4 };
-
-        /// A constant matrix that has zeroes in all its entries
-        static const Matrix4x4 Zero;
-        /// A constant matrix that is the identity.
-        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!
-        static const Matrix4x4 NaN;
-
-        /// Creates a new float4x4 with uninitialized member values.
-        Matrix4x4() {}
-        Matrix4x4(float val);
-        /// Constructs this float4x4 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&);
-        /// Constructs a new float4x4 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 _10 denotes the scalar at second (index 1) row, first (index 0) column.
-        Matrix4x4(float m00, float m01, float m02, float m03,
-                  float m10, float m11, float m12, float m13,
-                  float m20, float m21, float m22, float m23,
-                  float m30, float m31, float m32, float m33);
-        /// Constructs the matrix by explicitly specifying the four 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.
-            @param col3 The fourth column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
-            position of the local space pivot. */
-        Matrix4x4(const Vector4& r1, const Vector4& r2, const Vector4& r3, const Vector4& r4);
-
-        explicit Matrix4x4(const Quaternion& orientation);
-
-        /// Constructs this float4x4 from the given quaternion and translation.
-        /// Logically, the translation occurs after the rotation has been performed.
-        Matrix4x4(const Quaternion& orientation, const Vector3 &translation);
-
-        /// Creates a LookAt matrix from a look-at direction vector.
-        /** A LookAt matrix is a rotation matrix that orients an object to face towards a specified target direction.
-            @param localForward Specifies the forward direction in the local space of the object. This is the direction
-                the model is facing at in its own local/object space, often +X (1,0,0), +Y (0,1,0) or +Z (0,0,1). The
-                vector to pass in here depends on the conventions you or your modeling software is using, and it is best
-                pick one convention for all your objects, and be consistent.
-                This input parameter must be a normalized vector.
-            @param targetDirection Specifies the desired world space direction the object should look at. This function
-                will compute a 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. The returned
-                matrix M is orthonormal 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.
-            @note The position of (the translation performed by) the resulting matrix will be set to (0,0,0), i.e. the object
-                will be placed to origin. Call SetTranslatePart() on the resulting matrix to set the position of the model.
-            @see RotateFromTo(). */
-        static Matrix4x4 LookAt(const Vector3& localFwd, const Vector3& targetDir, const Vector3& localUp, const Vector3& worldUp);
-
-        /// 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,
-            after applying rotation and scale. If this matrix represents a local->world space transformation for an object,
-            then this gives the world space position of the object.
-            @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0 0 0 1]). */
-        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;
-        void SetTranslatePart(float translateX, float translateY, float translateZ);
-        void SetTranslatePart(const Vector3& offset);
-        void SetRotatePart(const Quaternion& q);
-        void Set3x3Part(const Matrix3x3& r);
-
-        void SetRow(int row, const Vector3& rowVector, float m_r3);
-        void SetRow(int row, const Vector4& rowVector);
-        void SetRow(int row, float m_r0, float m_r1, float m_r2, float m_r3);
-
-        Vector4 GetRow(int index) const;
-        Vector4 GetColumn(int index) const;
-        Vector3 GetRow3(int index) const;
-        Vector3 GetColumn3(int index) const;
-
-        Vector3 GetScale() const
-        {
-
-        }
-        Matrix4x4 Scale(const Vector3&);
-
-        float &At(int row, int col);
-        float At(int x, int y) const;
-
-        template <typename T>
-        void Swap(T &a, T &b)
-        {
-            T temp = std::move(a);
-            a = std::move(b);
-            b = std::move(temp);
-        }
-
-
-        void SwapColumns(int col1, int col2);
-
-        /// Swaps two rows.
-        void SwapRows(int row1, int row2);
-        /// Swapsthe xyz-parts of two rows element-by-element
-        void SwapRows3(int row1, int row2);
-
-        void ScaleRow(int row, float scalar);
-        void ScaleRow3(int row, float scalar);
-        void ScaleColumn(int col, float scalar);
-        void ScaleColumn3(int col, float scalar);
-        /// Algorithm from Eric Lengyel's Mathematics for 3D Game Programming & Computer Graphics, 2nd Ed.
-        void Pivot();
-
-        /// Tests if this matrix does not contain any NaNs or infs.
-        /** @return Returns true if the entries of this float4x4 are all finite, and do not contain NaN or infs. */
-        bool IsFinite() 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;
-
-        Vector4 Diagonal() const;
-        Vector3 Diagonal3() const;
-        /// Returns the local +X axis in world space.
-        /// This is the same as transforming the vector (1,0,0) by this matrix.
-        Vector3 WorldX() const;
-        /// Returns the local +Y axis in world space.
-        /// This is the same as transforming the vector (0,1,0) by this matrix.
-        Vector3 WorldY() const;
-        /// Returns the local +Z axis in world space.
-        /// This is the same as transforming the vector (0,0,1) by this matrix.
-        Vector3 WorldZ() const;
-
-        /// Accesses this structure as a float array.
-        /// @return A pointer to the upper-left element. The data is contiguous in memory.
-        /// ptr[0] gives the element [0][0], ptr[1] is [0][1], ptr[2] is [0][2].
-        /// ptr[4] == [1][0], ptr[5] == [1][1], ..., and finally, ptr[15] == [3][3].
-        float *ptr() { return &elems[0][0]; }
-        const float *ptr() const { return &elems[0][0]; }
-
-        float Determinant3x3() const;
-        /// Computes the determinant of this matrix.
-        // If the determinant is nonzero, this matrix is invertible.
-        float Determinant() const;
-
-        #define SKIPNUM(val, skip) (val >= skip ? (val+1) : val)
-
-        float Minor(int i, int j) const;
-
-        Matrix4x4 Inverse() const;
-
-        Matrix4x4 Transpose() const;
-
-        Vector2 Transform(float tx, float ty) const;
-        Vector2 Transform(const Vector2& rhs) const;
-
-
-        Vector3 Transform(float tx, float ty, float tz) const;
-        Vector3 Transform(const Vector3& rhs) const;
-
-        Vector4 Transform(float tx, float ty, float tz, float tw) const;
-        Vector4 Transform(const Vector4& rhs) const;
-
-
-        Matrix4x4 Translate(const Vector3& rhs) const;
-        static Matrix4x4 FromTranslation(const Vector3& rhs);
-
-
-        static Matrix4x4 D3DOrthoProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
-        static Matrix4x4 D3DOrthoProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
-        static Matrix4x4 D3DPerspProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
-        static Matrix4x4 D3DPerspProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
-
-        static Matrix4x4 OpenGLOrthoProjLH(float n, float f, float h, float v);
-        static Matrix4x4 OpenGLOrthoProjRH(float n, float f, float h, float v);
-        static Matrix4x4 OpenGLPerspProjLH(float n, float f, float h, float v);
-        static Matrix4x4 OpenGLPerspProjRH(float n, float f, float h, float v);
-
-        Vector4 operator[](int row);
-
-        Matrix4x4 operator-() const;
-        Matrix4x4 operator +(const Matrix4x4& rhs) const;
-        Matrix4x4 operator - (const Matrix4x4& rhs) const;
-
-        Matrix4x4 operator *(float scalar) const;
-        Matrix4x4 operator /(float scalar) const;
-
-
-
-        Vector2 operator * (const Vector2& rhs) const;
-        Vector3 operator * (const Vector3& rhs) const;
-        Vector4 operator * (const Vector4& rhs) const;
-
-        Matrix4x4 operator * (const Matrix3x3 &rhs) const;
-
-        Matrix4x4 operator +() const;
-
-        Matrix4x4 operator * (const Matrix4x4& rhs) const;
-
-        Matrix4x4 &operator = (const Matrix3x3& rhs);
-        Matrix4x4 &operator = (const Quaternion& rhs);
-        Matrix4x4 &operator = (const Matrix4x4& rhs) = default;
-
-    protected:
-        float elems[4][4];
-
-
-    };
-}

include/J3ML/LinearAlgebra/Matrix4x4.hpp

@@ -0,0 +1,578 @@
+#pragma once
+
+
+#include <J3ML/LinearAlgebra/Forward.hpp>
+#include <J3ML/Algorithm/RNG.hpp>
+
+#include <algorithm>
+
+using namespace J3ML::Algorithm;
+
+namespace J3ML::LinearAlgebra {
+
+    /// @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,
+     *  m_30, m_31, m_32, m_33
+     *
+     *  The element m_yx is the value on the row y and column x.
+     *  You can access m_yx using the double-bracket notation m[y][x]
+     */
+    class Matrix4x4 {
+    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.
+        /// @note Never compare a matrix to this value. Due to how IEEE floats work, "nan == nan" returns false!
+        static const Matrix4x4 NaN;
+    public: /// Constructors
+        /// Creates a new Matrix4x4 with uninitialized member values.
+        Matrix4x4() {}
+        Matrix4x4(const Matrix4x4 &rhs) = default; // {Set(rhs);}
+        Matrix4x4(float val);
+        /// Constructs this Matrix4x4 to represent the same transformation as the given float3x3.
+        /** This function expands the last row and column of this matrix with the elements from the identity matrix. */
+        Matrix4x4(const Matrix3x3& m);
+        explicit Matrix4x4(const float* data);
+
+        /// Constructs a new float4x4 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 _10 denotes the scalar at second (index 1) row, first (index 0) column.
+        Matrix4x4(float m00, float m01, float m02, float m03,
+                  float m10, float m11, float m12, float m13,
+                  float m20, float m21, float m22, float m23,
+                  float m30, float m31, float m32, float m33);
+        /// Constructs the matrix by explicitly specifying the four 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.
+            @param col3 The fourth column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
+            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.
+        /// Logically, the translation occurs after the rotation has been performed.
+        Matrix4x4(const Quaternion& orientation, const Vector3 &translation);
+
+        /// Creates a LookAt matrix from a look-at direction vector.
+        /** A LookAt matrix is a rotation matrix that orients an object to face towards a specified target direction.
+            @param localForward Specifies the forward direction in the local space of the object. This is the direction
+                the model is facing at in its own local/object space, often +X (1,0,0), +Y (0,1,0) or +Z (0,0,1). The
+                vector to pass in here depends on the conventions you or your modeling software is using, and it is best
+                pick one convention for all your objects, and be consistent.
+                This input parameter must be a normalized vector.
+            @param targetDirection Specifies the desired world space direction the object should look at. This function
+                will compute a 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. The returned
+                matrix M is orthonormal 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.
+            @note The position of (the translation performed by) the resulting matrix will be set to (0,0,0), i.e. the object
+                will be placed to origin. Call SetTranslatePart() on the resulting matrix to set the position of the model.
+            @see RotateFromTo(). */
+        static Matrix4x4 LookAt(const Vector3& localFwd, const Vector3& targetDir, const Vector3& localUp, const Vector3& worldUp);
+
+        /// Returns a random 4x4 matrix with each entry randomized between the range [minElem, maxElem]
+        /** Warning: The matrices returned by this function do not represent well-formed 3D transformations.
+            This function is mostly used for testing and debugging purposes only. */
+        static Matrix4x4 RandomGeneral(RNG& rng, float minElem, float maxElem);
+
+        /// Creates a new Matrix4x4 that rotates about one of the principal axes.
+        /** Calling RotateX, RotateY, or RotateZ is slightly faster than calling the more generic RotateAxisAngle function.
+            @param radians The angle to rotate by, in radians. For example, Pi/4.f equals 45 degrees.
+            @param pointOnAxis If specified, the rotation is performed about an axis that passes through this point,
+            and not through the origin. The returned matrix will not be a pure rotation matrix, but will also contain translation. */
+        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);
+
+        /// Produces a matrix that shears along a principal axis.
+        /** The shear matrix offsets the two other axes according to the
+            position of the point along the shear axis. */
+        static Matrix4x4 ShearX(float yFactor, float zFactor);
+        static Matrix4x4 ShearY(float xFactor, float zFactor);
+        static Matrix4x4 ShearZ(float xFactor, float yFactor);
+
+        /// Identical to D3DXMatrixOrthoLH, except transposed to account for Matrix * vector convention used in J3ML.
+        /// See http://msdn.microsoft.com/en-us/library/windows/desktop/bb205346(v=vs.85).aspx
+        /// @note Use the M*v multiplication order to project points with this matrix.
+        static Matrix4x4 D3DOrthoProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+        /// Identical to D3DXMatrixOrthoRH, except transposed to account for Matrix * vector convention used in J3ML.
+        /// See http://msdn.microsoft.com/en-us/library/windows/desktop/bb205349(v=vs.85).aspx
+        /// @note Use the M*v multiplication order to project points with this matrix.
+        static Matrix4x4 D3DOrthoProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+
+        /// Identical to D3DXMatrixPerspectiveLH, except transposed to account for Matrix * vector convention used in J3ML.
+        /// See http://msdn.microsoft.com/en-us/library/windows/desktop/bb205352(v=vs.85).aspx
+        /// @note Use the M*v multiplication order to project points with this matrix.
+        static Matrix4x4 D3DPerspProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+        /// Identical to D3DXMatrixPerspectiveRH, except transposed to account for Matrix * vector convention used in J3ML.
+        /// See http://msdn.microsoft.com/en-us/library/windows/desktop/bb205355(v=vs.85).aspx
+        /// @note Use the M*v multiplication order to project points with this matrix.
+        static Matrix4x4 D3DPerspProjRH(float 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 nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+        /// Computes a right-handled orthographic projection matrix for OpenGL.
+        /// @note Use the M*v multiplication order to project points with this matrix.
+        static Matrix4x4 OpenGLOrthoProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+
+        /// Computes a left-handed perspective projection matrix for OpenGL.
+        /// @note Use the M*v multiplication order to project points with this matrix.
+        /// @param n Near-plane
+        /// @param f Far-plane
+        /// @param h Horizontal FOV
+        /// @param v Vertical FOV
+        static Matrix4x4 OpenGLPerspProjLH(float n, float f, float h, float v);
+        /// Identical to http://www.opengl.org/sdk/docs/man/xhtml/gluPerspective.xml , except uses viewport sizes instead of FOV to set up the
+        /// projection matrix.
+        /// @note Use the M*v multiplication order to project points with this matrix.
+        // @param n Near-plane
+        /// @param f Far-plane
+        /// @param h Horizontal FOV
+        /// @param v Vertical FOV
+        static Matrix4x4 OpenGLPerspProjRH(float n, float f, float h, float v);
+
+        /// Creates a new transformation matrix that translates by the given offset.
+        static Matrix4x4 Translate(const Vector3& translation);
+        /// Creates a new transformation matrix that scales by the given factors.
+        static Matrix4x4 Scale(const Vector3& scale);
+
+        /// Creates a new Matrix4x4 as a combination of translation, rotation, and scale.
+        /** This function creates a new Matrix4x4 M of the form M = T * R * S, where T is a translation matrix, R is a
+            rotation matrix, and S a scale matrix. Transforming a vector v using this matrix computes the vector
+            v' = M * v = T*R*S*v = (T * (R * (S * v))), which means that the scale operation is applied to the
+            vector first, followed by rotation, and finally translation. */
+        static Matrix4x4 FromTRS(const Vector3& translate, const Quaternion& rotate, const Vector3& scale);
+        static Matrix4x4 FromTRS(const Vector3& translate, const Matrix3x3& rotate, const Vector3& scale);
+        static Matrix4x4 FromTRS(const Vector3& translate, const Matrix4x4& rotate, const Vector3& scale);
+
+        void SetCol3(int col, const Vector3 &xyz);
+
+        bool HasNegativeScale() const;
+
+    public:
+        /// 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,
+            after applying rotation and scale. If this matrix represents a local->world space transformation for an object,
+            then this gives the world space position of the object.
+            @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0 0 0 1]). */
+        [[nodiscard]] 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).
+        [[nodiscard]] 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);
+
+        /// Sets the value of a given row.
+        /// @param row The index of the row to a set, in the range [0-3].
+        /// @param data A pointer to an array of 4 floats that contain the new x,y,z,w values for the row.
+        void SetRow(int row, const float* data);
+        void SetRow(int row, const Vector3& rowVector, float m_r3);
+        void SetRow(int row, const Vector4& rowVector);
+        void SetRow(int row, float m_r0, float m_r1, float m_r2, float m_r3);
+
+        /// Sets the value of a given column.
+        /// @param column The index of the column to set, in the range [0-3].
+        /// @param data A pointer to an array of 3 floats that contain the new x,y,z,w values for the column.
+        void SetCol(int col, const float *data);
+        void SetCol(int col, const Vector3& colVector, float m_c3);
+        void SetCol(int col, const Vector4& colVector);
+        void SetCol(int col, float m_c0, float m_c1, float m_c2, float m_c3);
+
+        void SetCol3(int col, float x, float y, float z);
+
+        /// Returns the given row.
+        /** @param The zero-based index [0, 3] of the row to get */
+        [[nodiscard]] Vector4 GetRow(int row) const;
+        [[nodiscard]] Vector4 Row(int row) const;
+        Vector4& Row(int row);
+
+        /// Returns only the first-three elements of the given row.
+        [[nodiscard]] Vector3 GetRow3(int index) const;
+        [[nodiscard]] Vector3 Row3(int i) const;
+        /// This method also allows assignment to the retrieved row.
+        Vector3& Row3(int row);
+
+        /// Returns the given column.
+        /** @param col The zero-based index [0, 3] of the column to get. */
+        [[nodiscard]] Vector4 GetColumn(int index) const;
+        [[nodiscard]] Vector4 Column(int index) const;
+        [[nodiscard]] Vector4 Col(int i) const;
+
+        /// Returns only the first three elements of the given column.
+        [[nodiscard]] Vector3 GetColumn3(int index) const;
+        [[nodiscard]] Vector3 Column3(int index) const;
+
+        [[nodiscard]] Vector3 Col3(int i) const;
+
+        /// Returns the scaling performed by this matrix. This function assumes taht the last row is [0 0 0 1].
+        /// GetScale().x specifies the amount of scaling applied to the local x direction vector when it is transformed by this matrix.
+        /// i.e. GetScale()[i] equals Col[i].Length();
+        [[nodiscard]] Vector3 GetScale() const;
+
+
+        float &At(int row, int col);
+        [[nodiscard]] float At(int x, int y) const;
+
+
+        void SwapColumns(int col1, int col2);
+
+        /// Swaps two rows.
+        void SwapRows(int row1, int row2);
+        /// Swapsthe xyz-parts of two rows element-by-element
+        void SwapRows3(int row1, int row2);
+
+        void ScaleRow(int row, float scalar);
+        void ScaleRow3(int row, float scalar);
+        void ScaleColumn(int col, float scalar);
+        void ScaleColumn3(int col, float scalar);
+        /// Reduces this matrix to its row-echelon form.
+        void Pivot();
+
+        /// Tests if this matrix does not contain any NaNs or infs.
+        /** @return Returns true if the entries of this float4x4 are all finite, and do not contain NaN or infs. */
+        bool IsFinite() const;
+
+
+        /// Tests if this matrix is the identity matrix.
+        bool IsIdentity(float epsilon = 1e-3f) const;
+
+        /// Tests if this matrix has an inverse.
+        /** @return Returns true if this matrix can be inverted, up to the given epsilon. */
+        bool IsInvertible(float epsilon = 1e-3f) const;
+
+        [[nodiscard]] Vector4 Diagonal() const;
+        [[nodiscard]] Vector3 Diagonal3() const;
+        /// Returns the local +X axis in world space.
+        /// This is the same as transforming the vector (1,0,0) by this matrix.
+        [[nodiscard]] Vector3 WorldX() const;
+        /// Returns the local +Y axis in world space.
+        /// This is the same as transforming the vector (0,1,0) by this matrix.
+        [[nodiscard]] Vector3 WorldY() const;
+        /// Returns the local +Z axis in world space.
+        /// This is the same as transforming the vector (0,0,1) by this matrix.
+        [[nodiscard]] Vector3 WorldZ() const;
+
+        /// Accesses this structure as a float array.
+        /// @return A pointer to the upper-left element. The data is contiguous in memory.
+        /// ptr[0] gives the element [0][0], ptr[1] is [0][1], ptr[2] is [0][2].
+        /// ptr[4] == [1][0], ptr[5] == [1][1], ..., and finally, ptr[15] == [3][3].
+        float *ptr() { return &elems[0][0]; }
+        [[nodiscard]] const float *ptr() const { return &elems[0][0]; }
+
+        /// Computes the determinant of the upper-left 3x3 submatrix of this matrix.
+        [[nodiscard]] float Determinant3x3() const;
+        /// Computes the determinant of this matrix.
+        // If the determinant is nonzero, this matrix is invertible.
+        [[nodiscard]] float Determinant() const;
+
+        /// Computes the determinant of this matrix.
+        /** If the determinant is nonzero, this matrix is invertible.
+            If the determinant is negative, this matrix performs reflection about some axis.
+            From http://msdn.microsoft.com/en-us/library/bb204853(VS.85).aspx :
+            "If the determinant is positive, the basis is said to be "positively" oriented (or right-handed).
+            If the determinant is negative, the basis is said to be "negatively" oriented (or left-handed)." */
+        float Determinant4() const;
+
+#define SKIPNUM(val, skip) (val >= skip ? (val+1) : val)
+
+        float Minor(int i, int j) const;
+
+        Matrix4x4 Inverse() const;
+
+        Matrix4x4 Transpose() const;
+
+        Vector2 Transform(float tx, float ty) const;
+        Vector2 Transform(const Vector2& rhs) const;
+
+
+        Vector3 Transform(float tx, float ty, float tz) const;
+        Vector3 Transform(const Vector3& rhs) const;
+
+        Vector4 Transform(float tx, float ty, float tz, float tw) const;
+        Vector4 Transform(const Vector4& rhs) const;
+
+        static Matrix4x4 FromTranslation(const Vector3& rhs);
+
+
+        Vector4 operator[](int row);
+
+        Matrix4x4 operator-() const;
+        Matrix4x4 operator +(const Matrix4x4& rhs) const;
+        Matrix4x4 operator - (const Matrix4x4& rhs) const;
+
+        Matrix4x4 operator *(float scalar) const;
+        Matrix4x4 operator /(float scalar) const;
+
+        Vector4 operator[](int row) const;
+
+        Vector2 operator * (const Vector2& rhs) const;
+        Vector3 operator * (const Vector3& rhs) const;
+        Vector4 operator * (const Vector4& rhs) const;
+
+        Vector2 Mul(const Vector2& rhs) const;
+        Vector3 Mul(const Vector3& rhs) const;
+        Vector4 Mul(const Vector4& rhs) const;
+
+        Matrix4x4 operator * (const Matrix3x3 &rhs) const;
+
+        Matrix4x4 operator +() const;
+
+        Matrix4x4 operator * (const Matrix4x4& rhs) const;
+
+        Matrix4x4 &operator = (const Matrix3x3& rhs);
+        Matrix4x4 &operator = (const Quaternion& rhs);
+        Matrix4x4 &operator = (const Matrix4x4& rhs) = default;
+
+        /// Returns the scale component of this matrix.
+        /** This function decomposes this matrix M into a form M = M' * S, where M' has the unitary column vectors and S is a diagonal matrix.
+            @return ExtractScale returns the diagonal entries of S, i.e. the scale of the columns of this matrix. If this matrix
+            represents a local->world space transformation for an object, then this scale represents a 'local scale', i.e.
+            scaling that is performed before translating and rotating the object from its local coordinate system to its world
+            position.
+            @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0 0 0 1]). */
+        Vector3 ExtractScale() const;
+
+        /// Removes the scaling performed by this matrix. That is, decomposes this matrix M into a form M = M' * S, where
+        /// M' has unitary column vectors and S is a diagonal matrix. Then replaces this matrix with M'.
+        /// @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0 0 0 1]).
+        /// @note This function assumes that this matrix has orthogonal basis vectors (row and column vector sets are orthogonal).
+        /// @note This function does not remove reflection (-1 scale along some axis).
+        void RemoveScale();
+
+
+        /// Decomposes this matrix to translate, rotate, and scale parts.
+        /** This function decomposes this matrix M to a form M = T * R * S, where T is a translation matrix, R is a rotation matrix, and S is a scale matrix
+            @note Remember that in the convention of this class, transforms are applied in the order M * v, so scale is
+            applied first, then rotation, and finally the translation last.
+            @note This function assumes that this matrix does not contain projection (The fourth row of this matrix is [0 0 0 1]).
+            @param translate [out] This vector receives the translation component this matrix performs. The translation is applied last
+            after rotation and scaling.
+            @param rotate [out] This object receives the rotation part of this transform.
+            @param scale [out] This vector receives the scaling along the local (before transformation by R) X, Y, and Z axes performed by this matrix. */
+        void Decompose(Vector3& translate, Quaternion& rotate, Vector3& scale) const;
+        void Decompose(Vector3& translate, Matrix3x3& rotate, Vector3& scale) const;
+        void Decompose(Vector3& translate, Matrix4x4& rotate, Vector3& scale) const;
+
+        /// Returns true if this matrix only contains uniform scaling, compared to the given epsilon.
+        /// @note If the matrix does not really do any scaling, this function returns true (scaling uniformly by a factor of 1).
+        /// @note This function only examines the upper 3-by-3 part of this matrix.
+        /// @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0 0 0 1]).
+        bool HasUniformScale(float epsilon = 1e-3f) const;
+
+        /// Returns true if the row vectors of 3x3-top-left submatrix are all perpendicular to each other.
+        bool IsColOrthogonal3(float epsilon = 1e-3f) const;
+        /// Returns true if the column vector of 3x3-top-left submatrix are all perpendicular to each other.
+        bool IsRowOrthogonal3(float epsilon = 1e-3f) const;
+
+        bool IsColOrthogonal(float epsilon = 1e-3f) const;
+        bool IsRowOrthogonal(float epsilon = 1e-3f) const;
+        /// Returns true if this matrix is seen to contain a "projective" part,
+        /// 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 _33);
+
+        /// 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 *p);
+
+        /// 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 CholeskyDecompose(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);
+
+        /// 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 Inverted() function.
+        /// Returns true on success. On failure, the matrix is not modified. This function fails if any of the
+        /// elements of this vector are not finite, or if the matrix contains a zero scaling factor on X, Y, or Z.
+        /// This function may not be called if this matrix contains any projection (last row differs from (0 0 0 1)).
+        /// @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 Inverted().
+        /// Returns true on success. On failure, the matrix is not modified. This function fails if any of the
+        /// elements of this vector are not finite, or if the matrix contains a zero scaling factor on X, Y, or Z.
+        /// This function may not be called if this matrix contains any shearing or nonuniform scaling.
+        /// 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 Inverted().
+        /// This function may not be called if this matrix contains any scaling or shearing, but it may contain mirroring.
+        /// This function may not be called if this matrix contains any projection (last row differs from (0 0 0 1)).
+        void InverseOrthonormal();
+
+        /// 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;
+        /// Returns the sum of the diagonal elements of this matrix.
+        float Trace() const;
+
+        [[nodiscard]] Quaternion ToQuat() const;
+
+        /// Returns true if this Matrix4x4 is equal to the given Matrix4x4, up to given per-element epsilon.
+        bool Equals(const Matrix4x4& other, float epsilon = 1e-3f) const;
+
+    protected:
+        float elems[4][4];
+
+
+        Vector3 TransformDir(float tx, float ty, float tz) const;
+    };
+}

include/J3ML/LinearAlgebra/Quaternion.h

@@ -1,110 +0,0 @@
-#pragma once
-
-
-
-
-#include <J3ML/LinearAlgebra/Matrix3x3.h>
-#include <J3ML/LinearAlgebra/Matrix4x4.h>
-#include <J3ML/LinearAlgebra/Vector4.h>
-#include <J3ML/LinearAlgebra/AxisAngle.h>
-//#include <J3ML/LinearAlgebra/AxisAngle.h>
-#include <cmath>
-
-
-
-namespace J3ML::LinearAlgebra
-{
-
-    class Matrix3x3;
-
-    class Quaternion : public Vector4 {
-    public:
-        Quaternion();
-
-        Quaternion(const Quaternion &rhs) = default;
-
-        explicit Quaternion(const Matrix3x3 &rotationMtrx);
-
-        explicit Quaternion(const Matrix4x4 &rotationMtrx);
-
-        // @note The input data is not normalized after construction, this has to be done manually.
-        Quaternion(float X, float Y, float Z, float W);
-
-        // Constructs this quaternion by specifying a rotation axis and the amount of rotation to be performed about that axis
-        // @param rotationAxis The normalized rotation axis to rotate about. If using Vector4 version of the constructor, the w component of this vector must be 0.
-        Quaternion(const Vector3 &rotationAxis, float rotationAngleBetween);
-
-        Quaternion(const Vector4 &rotationAxis, float rotationAngleBetween);
-        //void Inverse();
-
-        explicit Quaternion(Vector4 vector4);
-
-        void SetFromAxisAngle(const Vector3 &vector3, float between);
-
-        void SetFromAxisAngle(const Vector4 &vector4, float between);
-
-        Quaternion Inverse() const;
-
-        Quaternion Conjugate() const;
-
-        //void Normalize();
-        Vector3 GetWorldX() const;
-
-        Vector3 GetWorldY() const;
-
-        Vector3 GetWorldZ() const;
-
-        Vector3 GetAxis() const;
-
-        float GetAngle() const;
-
-
-        Matrix3x3 ToMatrix3x3() const;
-
-        Matrix4x4 ToMatrix4x4() const;
-
-        Matrix4x4 ToMatrix4x4(const Vector3 &translation) const;
-
-        Vector3 Transform(const Vector3& vec) const;
-        Vector3 Transform(float X, float Y, float Z) const;
-        // Note: We only transform the x,y,z components of 4D vectors, w is left untouched
-        Vector4 Transform(const Vector4& vec) const;
-        Vector4 Transform(float X, float Y, float Z, float W) const;
-
-        Quaternion GetInverse() const;
-        Quaternion Lerp(const Quaternion& b, float t) const;
-        Quaternion Slerp(const Quaternion& q2, float t) const;
-
-        Quaternion Normalize() const;
-
-
-        static Quaternion LookAt(const Vector3& position, const Vector3& direction, const Vector3& axisUp);
-
-        // TODO: Document (But do not override!) certain math functions that are the same for Vec4
-        // TODO: Double Check which operations need to be overriden for correct behavior!
-
-        // Multiplies two quaternions together.
-        // The product q1 * q2 returns a quaternion that concatenates the two orientation rotations.
-        // The rotation q2 is applied first before q1.
-        Quaternion operator * (const Quaternion& rhs) const;
-
-        Quaternion operator * (float scalar) const;
-
-        // Transforms the given vector by this Quaternion.
-        Vector3 operator * (const Vector3& rhs) const;
-        Vector4 operator * (const Vector4& rhs) const;
-
-        // Divides a quaternion by another. Divison "a / b" results in a quaternion that rotates the orientation b to coincide with orientation of
-        Quaternion operator / (const Quaternion& rhs) const;
-        Quaternion operator + (const Quaternion& rhs) const;
-        Quaternion operator + () const;
-        Quaternion operator - () const;
-        float Dot(const Quaternion &quaternion) const;
-
-        float Angle() const { return acos(w) * 2.f;}
-
-        float AngleBetween(const Quaternion& target) const;
-
-        AxisAngle ToAxisAngle() const;
-    };
-}

include/J3ML/LinearAlgebra/Quaternion.hpp

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

include/J3ML/LinearAlgebra/SparseMatrix.hpp

@@ -0,0 +1,22 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file SparseMatrix.hpp
+/// @desc Templated Matrix type in which most elements are expected to be zero, and thus are "compressed"
+/// @edit 2024-07-06
+
+#include <cstdlib>
+
+namespace J3ML::LinearAlgebra
+{
+    template <uint Rows, uint Cols>
+    class SparseMatrix
+    {
+
+    };
+
+}

include/J3ML/LinearAlgebra/Transform2D.hpp

@@ -1,7 +1,7 @@
 #pragma once
 
 
-#include <J3ML/LinearAlgebra/Matrix3x3.h>
+#include <J3ML/LinearAlgebra/Matrix3x3.hpp>
 
 namespace J3ML::LinearAlgebra {
     class Transform2D {

include/J3ML/LinearAlgebra/Vector.h

@@ -1,13 +0,0 @@
-#pragma once
-
-
-template <typename T, int Dims>
-class templated_vector
-{
-
-};
-
-
-using v2f = templated_vector<float, 2>;
-using v3f = templated_vector<float, 3>;
-using v4f = templated_vector<float, 4>;

include/J3ML/LinearAlgebra/Vector.hpp

@@ -0,0 +1,15 @@
+#pragma once
+
+
+#include <cstdlib>
+
+namespace J3ML::LinearAlgebra
+{
+    template <uint DIMS, typename T>
+    class Vector {
+    public:
+        enum { Dimensions = DIMS};
+        T elems[DIMS];
+    };
+
+}

include/J3ML/LinearAlgebra/Vector2.h

@@ -1,187 +0,0 @@
-#pragma clang diagnostic push
-#pragma ide diagnostic ignored "modernize-use-nodiscard"
-#pragma once
-#include <J3ML/J3ML.h>
-#include <cstddef>
-
-namespace J3ML::LinearAlgebra {
-    using namespace J3ML;
-
-    /// A 2D (x, y) ordered pair.
-    class Vector2 {
-    public:
-
-        enum {Dimensions = 2};
-
-        /// Default Constructor - Initializes values to zero
-        Vector2();
-        /// Constructs a new Vector2 with the value (X, Y)
-        Vector2(float X, float Y);
-        /// Constructs this float2 from a C array, to the value (data[0], data[1]).
-        explicit Vector2(const float* data);
-        // Constructs a new Vector2 with the value {scalar, scalar}
-        explicit Vector2(float scalar);
-        Vector2(const Vector2& rhs);   // Copy Constructor
-        //Vector2(Vector2&&) = default;   // Move Constructor
-
-        static const Vector2 Zero;
-        static const Vector2 Up;
-        static const Vector2 Left;
-        static const Vector2 Down;
-        static const Vector2 Right;
-        static const Vector2 NaN;
-
-        float GetX() const;
-        float GetY() const;
-        void SetX(float newX);
-        void SetY(float newY);
-
-        /// Casts this float2 to a C array.
-        /** This function does not allocate new memory or make a copy of this float2. This function simply
-		returns a C pointer view to this data structure. Use ptr()[0] to access the x component of this float2
-		and ptr()[1] to access the y component.
-		@note Since the returned pointer points to this class, do not dereference the pointer after this
-			float2 has been deleted. You should never store a copy of the returned pointer.
-		@note This function is provided for compatibility with other APIs which require raw C pointer access
-			to vectors. Avoid using this function in general, and instead always use the operator []
-			or the At() function to access the elements of this vector by index.
-		@return A pointer to the first float element of this class. The data is contiguous in memory.
-		@see operator [](), At(). */
-        float* ptr();
-        const float *ptr() const;
-
-        float operator[](std::size_t index) const;
-        float &operator[](std::size_t index);
-
-        const float At(std::size_t index) const;
-
-        float &At(std::size_t index);
-
-        Vector2 Abs() const;
-
-        bool IsWithinMarginOfError(const Vector2& rhs, float margin=0.001f) const;
-
-        bool IsNormalized(float epsilonSq = 1e-5f) const;
-        bool IsZero(float epsilonSq = 1e-6f) const;
-        bool IsPerpendicular(const Vector2& other, float epsilonSq=1e-5f) const;
-
-
-        bool operator == (const Vector2& rhs) const;
-        bool operator != (const Vector2& rhs) const;
-
-        Vector2 Min(const Vector2& min) const;
-        static Vector2 Min(const Vector2& value, const Vector2& minimum);
-
-        Vector2 Max(const Vector2& max) const;
-        static Vector2 Max(const Vector2& value, const Vector2& maximum);
-
-        Vector2 Clamp(const Vector2& min, const Vector2& max) const;
-        static Vector2 Clamp(const Vector2& min, const Vector2& middle, const Vector2& max);
-
-        /// Returns the magnitude between the two vectors.
-        float Distance(const Vector2& to) const;
-        static float Distance(const Vector2& from, const Vector2& to);
-
-        float DistanceSq(const Vector2& to) const;
-        static float DistanceSq(const Vector2& from, const Vector2& to);
-
-        float MinElement() const;
-
-        float MaxElement() const;
-
-        float Length() const;
-        static float Length(const Vector2& of);
-
-        float LengthSquared() const;
-        static float LengthSquared(const Vector2& of);
-
-        /// Returns the length of the vector, which is sqrt(x^2 + y^2)
-        float Magnitude() const;
-        static float Magnitude(const Vector2& of);
-
-
-        bool IsFinite() const;
-        static bool IsFinite(const Vector2& v);
-
-        /// Returns a float value equal to the magnitudes of the two vectors multiplied together and then multiplied by the cosine of the angle between them.
-        /// For normalized vectors, dot returns 1 if they point in exactly the same direction,
-        /// -1 if they point in completely opposite directions, and 0 if the vectors are perpendicular.
-        float Dot(const Vector2& rhs) const;
-        static float Dot(const Vector2& lhs, const Vector2& rhs);
-
-        /// Projects one vector onto another and returns the result. (IDK)
-        Vector2 Project(const Vector2& rhs) const;
-        /// @see Project
-        static Vector2 Project(const Vector2& lhs, const Vector2& rhs);
-
-        /// Returns a copy of this vector, resized to have a magnitude of 1, while preserving "direction"
-        Vector2 Normalize() const;
-        static Vector2 Normalize(const Vector2& of);
-
-        /// Linearly interpolates between two points.
-        /// Interpolates between the points and b by the interpolant t.
-        /// The parameter is (TODO: SHOULD BE!) clamped to the range[0, 1].
-        /// This is most commonly used to find a point some fraction of the wy along a line between two endpoints (eg. to move an object gradually between those points).
-        Vector2 Lerp(const Vector2& rhs, float alpha) const;
-        /// @see Lerp
-        static Vector2 Lerp(const Vector2& lhs, const Vector2& rhs, float alpha);
-        /// Note: Input vectors MUST be normalized first!
-        float AngleBetween(const Vector2& rhs) const;
-        static float AngleBetween(const Vector2& lhs, const Vector2& rhs);
-
-        /// Adds two vectors.
-        Vector2 operator +(const Vector2& rhs) const;
-        Vector2 Add(const Vector2& rhs) const;
-        static Vector2 Add(const Vector2& lhs, const Vector2& rhs);
-
-        /// Subtracts two vectors.
-        Vector2 operator -(const Vector2& rhs) const;
-        Vector2 Sub(const Vector2& rhs) const;
-        static Vector2 Sub(const Vector2& lhs, const Vector2& rhs);
-
-        /// Multiplies this vector by a scalar value.
-        Vector2 operator *(float rhs) const;
-        Vector2 Mul(float scalar) const;
-        static Vector2 Mul(const Vector2& lhs, float rhs);
-
-        /// Multiplies this vector by a vector, element-wise
-        /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
-        ///	 but this function is provided here for syntactical convenience.
-        Vector2 operator *(const Vector2& rhs) const
-        {
-
-        }
-        Vector2 Mul(const Vector2& v) const;
-
-        /// Divides this vector by a scalar.
-        Vector2 operator /(float rhs) const;
-        Vector2 Div(float scalar) const;
-        static Vector2 Div(const Vector2& lhs, float rhs);
-
-        /// Divides this vector by a vector, element-wise
-        /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
-        /// but this function is provided here for syntactical convenience
-        Vector2 Div(const Vector2& v) const;
-
-        /// Unary operator +
-        Vector2 operator +() const; // TODO: Implement
-        Vector2 operator -() const;
-        /// Assigns a vector to another
-        Vector2 &operator =(const Vector2 &rhs);
-        Vector2& operator+=(const Vector2& rhs);
-        Vector2& operator-=(const Vector2& rhs);
-        Vector2& operator*=(float scalar);
-        Vector2& operator/=(float scalar);
-
-    public:
-        float x = 0;
-        float y = 0;
-
-    };
-
-    static Vector2 operator*(float lhs, const Vector2 &rhs)
-    {
-        return rhs * lhs;
-    }
-}
-#pragma clang diagnostic pop

include/J3ML/LinearAlgebra/Vector2.hpp

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

include/J3ML/LinearAlgebra/Vector2i.hpp

@@ -0,0 +1,31 @@
+#pragma once
+#include <string>
+
+namespace J3ML::LinearAlgebra {
+    class Vector2i;
+}
+
+class J3ML::LinearAlgebra::Vector2i {
+public:
+    int x, y;
+public:
+    Vector2i();
+    Vector2i(int x, int y) : x(x), y(y) {}
+    explicit Vector2i(int rhs) : x(rhs), y(rhs) {}
+public:
+    bool operator == (const Vector2i& rhs) const;
+    bool operator != (const Vector2i& rhs) const;
+    Vector2i& operator =(const Vector2i& rhs);
+    Vector2i& operator +=(const Vector2i& rhs);
+    Vector2i& operator -=(const Vector2i& rhs);
+    Vector2i& operator *=(const Vector2i& rhs);
+    Vector2i& operator /=(const Vector2i& rhs);
+    Vector2i operator +(const Vector2i& rhs) const;
+    Vector2i operator -(const Vector2i& rhs) const;
+    Vector2i operator *(const Vector2i& rhs) const;
+    Vector2i operator *(int rhs) const;
+    Vector2i operator /(const Vector2i& rhs) const;
+    Vector2i operator /(int rhs) const;
+public:
+    [[nodiscard]] std::string ToString() const;
+};

include/J3ML/LinearAlgebra/Vector3.h

@@ -1,181 +0,0 @@
-#pragma once
-
-#include <J3ML/LinearAlgebra/Vector2.h>
-#include <J3ML/LinearAlgebra/Vector3.h>
-#include <cstddef>
-#include <cstdlib>
-#include <J3ML/LinearAlgebra/Angle2D.h>
-
-
-namespace J3ML::LinearAlgebra {
-
-// A 3D (x, y, z) ordered pair.
-class Vector3 {
-public:
-
-    enum {Dimensions = 3};
-
-    // Default Constructor - Initializes to zero
-    Vector3();
-    // Constructs a new Vector3 with the value (X, Y, Z)
-    Vector3(float X, float Y, float Z);
-    Vector3(const Vector3& rhs); // Copy Constructor
-    Vector3(Vector3&&) = default; // Move Constructor
-    Vector3& operator=(const Vector3& rhs);
-    explicit Vector3(const float* data);
-
-    static const Vector3 Zero;
-    static const Vector3 Up;
-    static const Vector3 Down;
-    static const Vector3 Left;
-    static const Vector3 Right;
-    static const Vector3 Forward;
-    static const Vector3 Backward;
-    static const Vector3 NaN;
-    static const Vector3 Infinity;
-    static const Vector3 NegativeInfinity;
-
-    float* ptr();
-
-    static void Orthonormalize(Vector3& a, Vector3& b);
-
-    Vector3 Abs() const;
-
-
-    /// Returns the DirectionVector for a given angle.
-    static Vector3 Direction(const Vector3 &rhs) ;
-
-
-    static void Orthonormalize(Vector3& a, Vector3& b, Vector3& c);
-
-    bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon)
-    {
-
-    }
-
-    Vector3 ProjectToNorm(const Vector3& direction) const;
-
-    float GetX() const;
-    float GetY() const;
-    float GetZ() const;
-    void SetX(float newX);
-    void SetY(float newY);
-    void SetZ(float newZ);
-
-    bool IsWithinMarginOfError(const Vector3& rhs, float margin=0.001f) const;
-    bool IsNormalized(float epsilonSq = 1e-5f) const;
-    bool IsZero(float epsilonSq = 1e-6f) const;
-    bool IsPerpendicular(const Vector3& other, float epsilonSq=1e-5f) const;
-
-    float operator[](std::size_t index) const;
-    float &operator[](std::size_t index);
-    bool operator == (const Vector3& rhs) const;
-    bool operator != (const Vector3& rhs) const;
-
-    bool IsFinite() const;
-
-    Vector3 Min(const Vector3& min) const;
-    static Vector3 Min(const Vector3& lhs, const Vector3& rhs);
-
-    Vector3 Max(const Vector3& max) const;
-    static Vector3 Max(const Vector3& lhs, const Vector3& rhs);
-
-    Vector3 Clamp(const Vector3& min, const Vector3& max) const;
-    static Vector3 Clamp(const Vector3& min, const Vector3& input, const Vector3& max);
-
-    /// Returns the magnitude between the two vectors.
-    float Distance(const Vector3& to) const;
-    static float Distance(const Vector3& from, const Vector3& to);
-
-    float DistanceSquared(const Vector3& to) const;
-    static float DistanceSquared(const Vector3& from, const Vector3& to);
-
-    float Length() const;
-    static float Length(const Vector3& of);
-
-    float LengthSquared() const;
-    static float LengthSquared(const Vector3& of);
-
-    /// Returns the length of the vector, which is sqrt(x^2 + y^2 + z^2)
-    float Magnitude() const;
-    static float Magnitude(const Vector3& of);
-
-    /// Returns a float value equal to the magnitudes of the two vectors multiplied together and then multiplied by the cosine of the angle between them.
-    /// For normalized vectors, dot returns 1 if they point in exactly the same direction,
-    /// -1 if they point in completely opposite directions, and 0 if the vectors are perpendicular.
-    float Dot(const Vector3& rhs) const;
-    static float Dot(const Vector3& lhs, const Vector3& rhs);
-
-    /// Projects one vector onto another and returns the result. (IDK)
-    Vector3 Project(const Vector3& rhs) const;
-    static Vector3 Project(const Vector3& lhs, const Vector3& rhs);
-
-    /// The cross product of two vectors results in a third vector which is perpendicular to the two input vectors.
-    /// The result's magnitude is equal to the magnitudes of the two inputs multiplied together and then multiplied by the sine of the angle between the inputs.
-    Vector3 Cross(const Vector3& rhs) const;
-    static Vector3 Cross(const Vector3& lhs, const Vector3& rhs);
-
-    /// Returns a copy of this vector, resized to have a magnitude of 1, while preserving "direction"
-    Vector3 Normalize() const;
-    static Vector3 Normalize(const Vector3& targ);
-
-    /// Linearly interpolates between two points.
-    /// Interpolates between the points and b by the interpolant t.
-    /// The parameter is (TODO: SHOULD BE!) clamped to the range[0, 1].
-    /// This is most commonly used to find a point some fraction of the wy along a line between two endpoints (eg. to move an object gradually between those points).
-    Vector3 Lerp(const Vector3& goal, float alpha) const;
-    static Vector3 Lerp(const Vector3& lhs, const Vector3& rhs, float alpha);
-
-
-    Angle2D AngleBetween(const Vector3& rhs) const;
-    static Angle2D AngleBetween(const Vector3& lhs, const Vector3& rhs);
-
-    // Adds two vectors
-    Vector3 operator+(const Vector3& rhs) const;
-    Vector3 Add(const Vector3& rhs) const;
-    static Vector3 Add(const Vector3& lhs, const Vector3& rhs);
-
-    /// Subtracts two vectors
-    Vector3 operator-(const Vector3& rhs) const;
-    Vector3 Sub(const Vector3& rhs) const;
-    static Vector3 Sub(const Vector3& lhs, const Vector3& rhs);
-
-    /// Multiplies this vector by a scalar value
-    Vector3 operator*(float rhs) const;
-    Vector3 Mul(float scalar) const;
-    static Vector3 Mul(const Vector3& lhs, float rhs);
-
-    /// Multiplies this vector by a vector, element-wise
-    /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
-    ///	 but this function is provided here for syntactical convenience.
-    Vector3 Mul(const Vector3& rhs) const
-    {
-
-    }
-
-    /// Divides this vector by a scalar
-    Vector3 operator/(float rhs) const;
-    Vector3 Div(float scalar) const;
-    static Vector3 Div(const Vector3& lhs, float rhs);
-
-
-    /// Divides this vector by a vector, element-wise
-    /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
-    /// but this function is provided here for syntactical convenience
-    Vector2 Div(const Vector2& v) const;
-
-    /// Unary + operator
-    Vector3 operator+() const; // TODO: Implement
-    /// Unary - operator (Negation)
-    Vector3 operator-() const;
-public:
-     float x = 0;
-     float y = 0;
-     float z = 0;
-};
-
-    static Vector3 operator*(float lhs, const Vector3& rhs)
-    {
-        return rhs * lhs;
-    }
-}

include/J3ML/LinearAlgebra/Vector3.hpp

@@ -0,0 +1,387 @@
+#pragma once
+
+#include <J3ML/LinearAlgebra/Vector2.hpp>
+#include <J3ML/LinearAlgebra/Vector3.hpp>
+#include <cstddef>
+#include <cstdlib>
+#include <J3ML/LinearAlgebra/Angle2D.hpp>
+#include <J3ML/Algorithm/RNG.hpp>
+
+
+using namespace J3ML::Algorithm;
+
+namespace J3ML::LinearAlgebra {
+
+/// 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, each element has the value of quiet NaN, or Not-A-Number.
+        @note Never compare a Vector3 to this value! Due to how IEEE floats work, "nan == nan" returns false!
+        That is, nothing is equal to NaN, not even NaN itself!
+        @note Due to static data initialization order being undefined in C++, do NOT use this
+        member data to initalize other static data in other compilation units! */
+    static const Vector3 NaN;
+    /// Specifies a compile-time constant Vector3 with value (+infinity, +infinity, +infinity).
+    /** @note Due to static data initialization order being undefined in C++, do NOT use this
+        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,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 UnitX;
+    /// 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 UnitY;
+    /// 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 UnitZ;
+
+public:
+
+    /// The default constructor does not initialize any members of this class.
+    /** This means that the values of the members x, y and z are all undefined after creating a new Vector3 using
+        this default constructor. Remember to assign to them before use.
+        @see x, y, z. */
+    Vector3();
+    // Constructs a new Vector3 with the value (X, Y, Z)
+    Vector3(float X, float Y, float Z);
+    Vector3(const Vector2& XY, float Z);
+    Vector3(const Vector3& rhs) = default; // Copy Constructor
+    Vector3(Vector3&&) = default; // Move Constructor
+    /// Constructs this float3 from a C array, to the value (data[0], data[1], data[2]).
+    /** @param data An array containing three elements for x, y and z. This pointer may not be null. */
+    explicit Vector3(const float* data);
+    /// Constructs a new Vector3 with the value (scalar, scalar, scalar).
+    explicit Vector3(float scalar);
+
+    /// Generates a new Vector3 by fillings its entries by the given scalar.
+    /// @see Vector3::Vector3(float scalar), SetFromScalar()
+    static Vector3 FromScalar(float scalar);
+
+    /// Generates a direction vector of the given length.
+    /** The returned vector points at a uniformly random direction.
+        @see RandomSphere(), RandomBox() */
+    static Vector3 RandomDir(RNG& rng, float length = 1.f);
+    static Vector3 RandomSphere(RNG& rng, const Vector3& center, float radius);
+    static Vector3 RandomBox(RNG& rng, float xmin, float xmax, float ymin, float ymax, float zmin, float zmax);
+    static Vector3 RandomBox(RNG& rng, const Vector3& min, const Vector3 &max);
+
+    static Vector3 RandomBox(RNG& rng, float min, float max);
+
+    static Vector3 RotateAroundAxis(const Vector3& vec, const Vector3& axis, const float radians);
+
+    static inline Vector3 RandomGeneral(RNG& rng, float minElem, float maxElem) { return RandomBox(rng, minElem, maxElem); }
+public:
+    /// Casts this float3 to a C array.
+    /** This function does not allocate new memory or make a copy of this Vector3. This function simply
+            returns a C pointer view to this data structure. Use ptr()[0] to access the x component of this float3,
+            ptr()[1] to access y, and ptr()[2] to access the z component of this Vector3.
+            @note Since the returned pointer points to this class, do not dereference the pointer after this
+                float3 has been deleted. You should never store a copy of the returned pointer.
+            @note This function is provided for compatibility with other APIs which require raw C pointer access
+                to vectors. Avoid using this function in general, and instead always use the operator [] or
+                the At() function to access the elements of this vector by index.
+            @return A pointer to the first float element of this class. The data is contiguous in memory.
+            @see operator [](), At(). */
+    float* ptr();
+    [[nodiscard]] const float *ptr() const { return &x;}
+
+    /// Accesses an element of this vector using array notation.
+    /** @param index The element to get. Pass in 0 for x, 1 for y and 2 for z.
+        @note If you have a non-const instance of this class, you can use this notation to set the elements of
+            this vector as well, e.g. vec[1] = 10.f; would set the y-component of this vector.
+        @see ptr(), At(). */
+    float operator[](std::size_t index) const;
+    float &operator[](std::size_t index);
+
+    /// Accesses an element of this vector.
+    /** @param index The element to get. Pass in 0 for x, 1 for y, and 2 for z.
+        @note If you have a non-const instance of this class, you can use this notation to set the elements of
+                this vector as well, e.g. vec.At(1) = 10.f; would set the y-component of this vector.
+        @see ptr(), operator [](). */
+    [[nodiscard]] float At(int index) const;
+    float &At(int index);
+
+    /// Makes the given vectors linearly independent and normalized in length.
+    /** This function directly follows the Gram-Schmidt procedure on the input vectors.
+        The vector a is first normalized, and vector b is modified to be perpendicular to a, and also normalized.
+        Finally, if specified, the vector c is adjusted to be perpendicular to a and b, and normalized.
+        @note If any of the input vectors is zero, then the resulting set of vectors cannot be made orthonormal.
+        @see Orthogonalize(), AreOrthogonal(), AreOrthonormal(). */
+    static void Orthonormalize(Vector3& a, Vector3& b);
+    static void Orthonormalize(Vector3& a, Vector3& b, Vector3& c);
+
+    /// Returns true if the given vectors are orthogonal to each other and all of length 1.
+    /** @see Orthogonalize(), AreOrthogonal(), Orthonormalize(), AreCollinear(). */
+    static bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon = 1e-3f);
+    static bool AreOrthonormal(const Vector3& a, const Vector3& b, const Vector3& c, float epsilon = 1e-3f);
+
+    [[nodiscard]] Vector3 Abs() const;
+    static Vector3 Abs(const Vector3& rhs);
+
+    /// Returns the DirectionVector for a given angle.
+    static Vector3 Direction(const Vector3 &rhs) ;
+
+    /// Scales this vector so that its new length is as given.
+    /** Calling this function is effectively the same as normalizing the vector first and then multiplying by newLength.
+        In the case of failure, this vector is set to (newLength, 0, 0), so calling this function will never result in an
+        unnormalized vector.
+        @note This function operates in-place.
+        @return The old length of this vector. If this function returns 0, the scaling failed, and this vector is arbitrarily
+            reset to (newLength, 0, 0). In case of failure, no error message is generated. You are expected to handle the failure
+            yourself.
+        @see ScaledToLength(). */
+    float ScaleToLength(float newLength);
+
+    /// Returns a scaled copy of this vector which has its new length as given.
+    /** This function assumes the length of this vector is not zero. In the case of failure, an error message is printed,
+        and the vector (newLength, 0, 0) is returned.
+        @see ScaleToLength(). */
+    [[nodiscard]] Vector3 ScaledToLength(float newLength) const;
+
+
+
+    [[nodiscard]] Vector3 ProjectToNorm(const Vector3& direction) const;
+
+    [[nodiscard]] float GetX() const;
+    [[nodiscard]] float GetY() const;
+    [[nodiscard]] float GetZ() const;
+    void SetX(float newX);
+    void SetY(float newY);
+    void SetZ(float newZ);
+
+    [[nodiscard]] Vector2 XY() const { return {x, y};}
+
+
+    [[nodiscard]] bool IsWithinMarginOfError(const Vector3& rhs, float margin=0.001f) const;
+    [[nodiscard]] bool IsNormalized(float epsilonSq = 1e-5f) const;
+    [[nodiscard]] bool IsZero(float epsilonSq = 1e-6f) const;
+    [[nodiscard]] bool IsPerpendicular(const Vector3& other, float epsilonSq=1e-5f) const;
+
+
+    bool operator == (const Vector3& rhs) const;
+    bool operator != (const Vector3& rhs) const;
+
+    [[nodiscard]] bool IsFinite() const;
+    [[nodiscard]] float MinElement() const;
+    static float MinElement(const Vector3& of);
+
+    /// Normalizes this Vector3.
+    /** In the case of failure, this vector is set to (1, 0, 0), so calling this function will never result in an
+		unnormalized vector.
+		@note If this function fails to normalize the vector, no error message is printed, the vector is set to (1,0,0) and
+			an error code 0 is returned. This is different than the behavior of the Normalized() function, which prints an
+			error if normalization fails.
+		@note This function operates in-place.
+		@return The old length of this vector, or 0 if normalization failed.
+		@see Normalized(). */
+    float TryNormalize();
+
+    /// Computes a new normalized direction vector that is perpendicular to this vector and the specified hint vector.
+    /** If this vector points toward the hint vector, the vector hint2 is returned instead.
+        @see AnotherPerpendicular(), Cross(). */
+    [[nodiscard]] Vector3 Perpendicular(const Vector3 &hint = Vector3(0,1,0), const Vector3 &hint2 = Vector3(0,0,1)) const;
+
+    /// Returns another vector that is perpendicular to this vector and the vector returned by Perpendicular().
+    /** The set (this, Perpendicular(), AnotherPerpendicular()) forms a right-handed normalized 3D basis.
+        @see Perpendicular(), Cross(). */
+    Vector3 AnotherPerpendicular(const Vector3& hint = Vector3(0,1,0), const Vector3& hint2 = Vector3(0,0,1)) const;
+
+    /// Completes this vector to generate a perpendicular basis.
+    /** This function computes two new vectors b and c which are both orthogonal to this vector and to each other.
+        That is, the set { this, b, c} is an orthogonal set. The vectors b and c that are outputted are also normalized.
+        @param outB [out] Receives vector b.
+        @param outC [out] Receives vector c.
+        @note When calling this function, this vector should not be zero! */
+    void PerpendicularBasis(Vector3& outB, Vector3& outC) const;
+
+
+
+    [[nodiscard]] Vector3 Min(const Vector3& min) const;
+    static Vector3 Min(const Vector3& a, const Vector3& b, const Vector3& c);
+    static Vector3 Min(const Vector3& lhs, const Vector3& rhs);
+
+    [[nodiscard]] Vector3 Max(const Vector3& max) const;
+    static Vector3 Max(const Vector3& a, const Vector3& b, const Vector3& c);
+    static Vector3 Max(const Vector3& lhs, const Vector3& rhs);
+
+    [[nodiscard]] Vector3 Clamp(const Vector3& min, const Vector3& max) const;
+    static Vector3 Clamp(const Vector3& min, const Vector3& input, const Vector3& max);
+
+    /// Returns the magnitude between the two vectors.
+    [[nodiscard]] float Distance(const Vector3& to) const;
+    static float Distance(const Vector3& from, const Vector3& to);
+    //float Distance(const Ray&) const;
+    //float Distance(const LineSegment&) const;
+    //float Distance(const Plane&) const;
+    //float DIstance(const Triangle&) const;
+
+    [[nodiscard]] float DistanceSquared(const Vector3& to) const;
+    // Function Alias for DistanceSquared
+    [[nodiscard]] float DistanceSq(const Vector3& to) const { return DistanceSquared(to); }
+    static float DistanceSquared(const Vector3& from, const Vector3& to);
+
+    [[nodiscard]] float Length() const;
+    static float Length(const Vector3& of);
+
+    [[nodiscard]] float LengthSquared() const;
+    static float LengthSquared(const Vector3& of);
+
+    /// Returns the length of the vector, which is sqrt(x^2 + y^2 + z^2)
+    [[nodiscard]] float Magnitude() const;
+    static float Magnitude(const Vector3& of);
+
+    /// Returns a float value equal to the magnitudes of the two vectors multiplied together and then multiplied by the cosine of the angle between them.
+    /// For normalized vectors, dot returns 1 if they point in exactly the same direction,
+    /// -1 if they point in completely opposite directions, and 0 if the vectors are perpendicular.
+    [[nodiscard]] float Dot(const Vector3& rhs) const;
+    static float Dot(const Vector3& lhs, const Vector3& rhs);
+
+    /// Projects one vector onto another and returns the result. (IDK)
+    [[nodiscard]] Vector3 Project(const Vector3& rhs) const;
+    static Vector3 Project(const Vector3& lhs, const Vector3& rhs);
+
+    /// The cross product of two vectors results in a third vector which is perpendicular to the two input vectors.
+    /// The result's magnitude is equal to the magnitudes of the two inputs multiplied together and then multiplied by the sine of the angle between the inputs.
+    [[nodiscard]] Vector3 Cross(const Vector3& rhs) const;
+    static Vector3 Cross(const Vector3& lhs, const Vector3& rhs);
+
+    /// Returns a copy of this vector, resized to have a magnitude of 1, while preserving "direction"
+    /// @note If the vector is zero and cannot be normalized, the vector (1, 0, 0) is returned, and an error message is printed.
+    /// If you do not want to generate an error message on failure, but want to handle the failure yourself, use the Normalized() function instead.
+    /// @see Normalized()
+    [[nodiscard]] Vector3 Normalized() const;
+    static Vector3 Normalized(const Vector3& targ);
+
+    /// Normalizes this Vector3.
+    /** In the case of failure, this vector is set to (1, 0, 0), so calling this function will never result in an
+        un-normalized vector.
+        @note If this function fails to normalize the vector, no error message is printed, the vector is set to (1,0,0) and
+        an error code 0 is returned. This is different than the behavior of the Normalized() function, which prints an
+        error if normalization fails.
+        @note This function operates in-place.
+        @return The old length of this vector, or 0 if normalization fails.
+        @see Normalized(). */
+    float Normalize();
+
+
+    /// Linearly interpolates between two points.
+    /// Interpolates between the points and b by the interpolant t.
+    /// The parameter is (TODO: SHOULD BE!) clamped to the range[0, 1].
+    /// This is most commonly used to find a point some fraction of the wy along a line between two endpoints (eg. to move an object gradually between those points).
+    [[nodiscard]] Vector3 Lerp(const Vector3& goal, float alpha) const;
+    static Vector3 Lerp(const Vector3& lhs, const Vector3& rhs, float alpha);
+
+    /// Returns the angle between this vector and the specified vector, in radians.
+    /** @note This function takes into account that this vector or the other vector can be unnormalized, and normalizes the computations.
+            If you are computing the angle between two normalized vectors, it is better to use AngleBetweenNorm().
+        @see AngleBetweenNorm(). */
+    [[nodiscard]] Angle2D AngleBetween(const Vector3& rhs) const;
+    static Angle2D AngleBetween(const Vector3& lhs, const Vector3& rhs);
+
+    /// Adds two vectors
+    Vector3 operator+(const Vector3& rhs) const;
+    [[nodiscard]] Vector3 Add(const Vector3& rhs) const;
+    static Vector3 Add(const Vector3& lhs, const Vector3& rhs);
+
+    /// Adds the vector(s, s, s) to this vector
+    [[nodiscard]] Vector3 Add(float s) const;
+
+    /// Subtracts two vectors
+    Vector3 operator-(const Vector3& rhs) const;
+    [[nodiscard]] Vector3 Sub(const Vector3& rhs) const;
+    static Vector3 Sub(const Vector3& lhs, const Vector3& rhs);
+
+    /// Multiplies this vector by a scalar value
+    Vector3 operator*(float rhs) const;
+    [[nodiscard]] Vector3 Mul(float scalar) const;
+    static Vector3 Mul(const Vector3& lhs, float rhs);
+
+    /// Multiplies this vector by a vector, element-wise
+    /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
+    ///	 but this function is provided here for syntactical convenience.
+    [[nodiscard]] Vector3 Mul(const Vector3& rhs) const;
+
+    /// Divides this vector by a scalar
+    Vector3 operator/(float rhs) const;
+    [[nodiscard]] Vector3 Div(float scalar) const;
+    static Vector3 Div(const Vector3& lhs, float rhs);
+
+    /// Divides this vector by a vector, element-wise
+    /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
+    /// but this function is provided here for syntactical convenience
+    [[nodiscard]] Vector3 Div(const Vector3& v) const;
+
+    /// Unary + operator
+    Vector3 operator+() const; // TODO: Implement
+    /// Unary - operator (Negation)
+    Vector3 operator-() const;
+
+    [[nodiscard]] bool Equals(const Vector3& rhs, float epsilon = 1e-3f) const;
+    [[nodiscard]] bool Equals(float _x, float _y, float _z, float epsilon = 1e-3f) const;
+
+    Vector3 &operator =(const Vector3& rhs);
+    Vector3& operator+=(const Vector3& rhs);
+    Vector3& operator-=(const Vector3& rhs);
+    Vector3& operator*=(float scalar);
+    Vector3& operator/=(float scalar);
+
+
+    void Set(float d, float d1, float d2);
+
+
+    [[nodiscard]] std::string ToString() const;
+};
+
+    std::ostream& operator << (std::ostream& o, const Vector3& vector);
+
+    static Vector3 operator*(float lhs, const Vector3& rhs)
+    {
+        return rhs * lhs;
+    }
+}

include/J3ML/LinearAlgebra/Vector4.h

@@ -1,105 +0,0 @@
-#pragma once
-
-
-#include <J3ML/LinearAlgebra/Vector3.h>
-
-namespace J3ML::LinearAlgebra {
-    class Vector4 {
-    public:
-        // Default Constructor
-        Vector4();
-        // Constructs a new Vector4 with x,y,z values from a Vector3
-        Vector4(const Vector3& xyz, float w = 0);
-        // Constructs a new Vector4 with the value (X, Y, Z, W)
-        Vector4(float X, float Y, float Z, float W);
-        Vector4(const Vector4& copy) = default;
-        Vector4(Vector4&& move) = default;
-        Vector4& operator=(const Vector4& rhs);
-
-        float* ptr()
-        {
-            return &x;
-        }
-
-        float GetX() const;
-        float GetY() const;
-        float GetZ() const;
-        float GetW() const;
-#if MUTABLE
-        void SetX(float newX) { x = newX;}
-        void SetY(float newY) { y = newY;}
-        void SetZ(float newZ) { z = newZ;}
-        void SetW(float newW) { w = newW;}
-#endif
-        static const Vector4 Zero;
-        static const Vector4 NaN;
-
-        float operator[](std::size_t index) const;
-
-        bool IsWithinMarginOfError(const Vector4& rhs, float margin=0.0001f) const;
-        bool IsNormalized(float epsilonSq = 1e-5f) const;
-        bool IsZero(float epsilonSq = 1e-6f) const;
-        bool IsFinite() const;
-        bool IsPerpendicular(const Vector4& other, float epsilonSq=1e-5f) const;
-
-        bool operator==(const Vector4& rhs) const;
-        bool operator!=(const Vector4& rhs) const;
-
-        Vector4 Min(const Vector4& min) const;
-        Vector4 Max(const Vector4& max) const;
-        Vector4 Clamp(const Vector4& min, const Vector4& max) const;
-        float Distance(const Vector4& to) const;
-        float Length() const;
-        float LengthSquared() const;
-        float Magnitude() const;
-        float Dot(const Vector4& rhs) const;
-        Vector4 Project(const Vector4& rhs) const;
-        // While it is feasable to compute a cross-product in four dimensions
-        // the cross product only has the orthogonality property in 3 and 7 dimensions
-        // You should consider instead looking at Gram-Schmidt Orthogonalization
-        // to find orthonormal vectors.
-        Vector4 Cross(const Vector4& rhs) const;
-        Vector4 Normalize() const;
-        Vector4 Lerp(const Vector4& goal, float alpha) const;
-
-        float AngleBetween(const Vector4& rhs) const;
-
-        // Adds two vectors
-        Vector4 operator+(const Vector4& rhs) const;
-        Vector4 Add(const Vector4& rhs) const;
-        static Vector4 Add(const Vector4& lhs, const Vector4& rhs);
-
-        // Subtracts two vectors
-        Vector4 operator-(const Vector4& rhs) const;
-        Vector4 Sub(const Vector4& rhs) const;
-        static Vector4 Sub(const Vector4& lhs, const Vector4& rhs);
-
-        // Multiplies this vector by a scalar value
-        Vector4 operator*(float rhs) const;
-        Vector4 Mul(float scalar) const;
-        static Vector4 Mul(const Vector4& lhs, float rhs);
-
-        // Divides this vector by a scalar
-        Vector4 operator/(float rhs) const;
-        Vector4 Div(float scalar) const;
-        static Vector4 Div(const Vector4& rhs, float scalar);
-
-        Vector4 operator+() const; // Unary + Operator
-        Vector4 operator-() const; // Unary - Operator (Negation)
-
-
-    public:
-#if MUTABLE
-        float x;
-        float y;
-        float z;
-        float w;
-#else
-         float x = 0;
-         float y = 0;
-         float z = 0;
-         float w = 0;
-#endif
-    };
-
-}

include/J3ML/LinearAlgebra/Vector4.hpp

@@ -0,0 +1,272 @@
+#pragma once
+
+#include <cstddef>
+#include <cmath>
+
+#include <J3ML/LinearAlgebra/Forward.hpp>
+
+namespace J3ML::LinearAlgebra {
+
+    /// A 3D vector of form (x,y,z,w) in a 4D homogeneous coordinate space.
+    /** This class has two sets of member functions. The functions ending in a suffix '3' operate only on the
+        (x, y, z) part, ignoring the w component (or assuming a value of 0 or 1, where expectable). The functions
+        without the '3' suffix operate on all four elements of the vector. */
+    class Vector4 {
+    public:
+        enum { Size = 4 };
+    public:
+        static const Vector4 Zero;
+        static const Vector4 NaN;
+    public:
+        /// The default constructor does not initialize any members of this class.
+        /** This means that the values of the members x, y, z, and w are all undefined after creating a new Vector4 using
+            this default constructor. Remember to assign to them before use.
+            @see x, y, z, w. */
+        Vector4();
+        /// Constructs a new Vector4 with x,y,z values from a Vector3
+        explicit Vector4(const Vector3& xyz, float w = 0);
+        /// Constructs a new Vector4 with the value (X, Y, Z, W)
+        /** @note If you are constructing a float4 from an array of consecutive values, always prefer calling "float4(ptr);" instead of "float4(ptr[0], ptr[1], ptr[2], ptr[3]);"
+            because there is a considerable SIMD performance benefit in the first form.
+        @see x, y, z, w. */
+        Vector4(float X, float Y, float Z, float W);
+        /// The Vector4 copy constructor.
+        Vector4(const Vector4& copy) { Set(copy); }
+        Vector4(Vector4&& move) = default;
+        Vector4& operator=(const Vector4& rhs);
+
+        /// Constructs this Vector4 from a C array, to the value (data[0], data[1], data[2], data[3]).
+        /** @param data An array containing four elements for x, y, z, and w. This pointer may not be null. */
+        explicit Vector4(const float* data);
+
+        /// Casts this Vector4 to a C array.
+        /** This function does not allocate new memory or make a copy of this Vector4. This function simply
+            returns a C pointer view to this data structure. Use ptr()[0] to access the x component of this Vector4,
+            ptr()[1] to access y, ptr()[2] to access z, and ptr()[3] to access the w component of this Vector4.
+            @note Since the returned pointer points to this class, do not dereference the pointer after this
+                Vector has been deleted. You should never store a copy of the returned pointer.
+            @note This function is provided for compatibility with other APIs which require raw C pointer access
+                to vectors. Avoid using this function in general, and instead always use the operator [] of this
+                class to access the elements of this vector by index. */
+        float* ptr();
+        [[nodiscard]] const float* ptr() const;
+
+        /// Accesses an element of this vector using array notation.
+        /** @param index The element to get. Pass in 0 for x, 1 for y, 2 for z and 3 for w.
+            @note If you have a non-const instance of this class, you can use this notation to set the elements of
+                this vector as well, e.g. vec[1] = 10.f; would set the y-component of this vector. */
+        float operator[](std::size_t index) const;
+        float &operator[](std::size_t index);
+
+        /// Accesses an element of this vector.
+        /** @param index The element to get. Pass in 0 for x, 1 for y, 2 for z and 3 for w.
+            @note If you have a non-const instance of this class, you can use this notation to set the elements of
+                this vector as well, e.g. vec.At(1) = 10.f; would set the y-component of this vector. */
+        float At(int index) const;
+        float& At(int index);
+
+        /// Returns the (x,y) part of this vector.
+        [[nodiscard]] Vector2 XY() const;
+
+        /// Returns the (x,y,z) part of this vector.
+        [[nodiscard]] Vector3 XYZ() const;
+
+        [[nodiscard]] float GetX() const { return x; }
+        [[nodiscard]] float GetY() const { return y; }
+        [[nodiscard]] float GetZ() const { return z; }
+        [[nodiscard]] float GetW() const { return w; }
+        void SetX(float newX) { x = newX;}
+        void SetY(float newY) { y = newY;}
+        void SetZ(float newZ) { z = newZ;}
+        void SetW(float newW) { w = newW;}
+
+        /// Sets all elements of this vector.
+        /** @see x, y, z, w, At(). */
+        void Set(float x, float y, float z, float w);
+        void Set(const Vector4& rhs);
+
+
+
+
+        [[nodiscard]] bool IsWithinMarginOfError(const Vector4& rhs, float margin=0.0001f) const;
+
+        /// Computes the squared length of the (x,y,z) part of this vector.
+        [[nodiscard]] float LengthSqXYZ() const;
+
+        /// Tests if the length of the (x, y, z) part of this vector is one, up to the given epsilon.
+        /** @see NormalizeW(), IsWZeroOrOne(), IsZero3(), IsZero4(), IsNormalized4(). */
+        [[nodiscard]] bool IsNormalized3(float epsilonSq = 1e-5f) const;
+
+        /// Returns true if the length of this vector is 1, up to the given epsilon.
+        /** This function takes into account all the four components of this vector when calculating the norm.
+            @see NormalizeW(), IsWZeroOrOne(), IsZero3(), IsZero4(), IsNormalized3(). */
+        [[nodiscard]] bool IsNormalized4(float epsilonSq = 1e-5f) const;
+        [[nodiscard]] bool IsNormalized(float epsilonSq = 1e-5f) const;
+
+        /// Tests if the (x, y, z) part of this vector is equal to (0,0,0), up to the given epsilon.
+        /** @see NormalizeW(), IsWZeroOrOne(), IsZero4(), IsNormalized3(), IsNormalized4(). */
+        [[nodiscard]] bool IsZero3(float epsilonSq = 1e-6f) const;
+
+
+        /// Returns true if this vector is equal to (0,0,0,0), up to the given epsilon.
+        /** @see NormalizeW(), IsWZeroOrOne(), IsZero3(), IsNormalized3(), IsNormalized4(). */
+        [[nodiscard]] bool IsZero(float epsilonSq = 1e-6f) const;
+        [[nodiscard]] bool IsZero4(float epsilonSq = 1e-6f) const;
+        /// Tests if this vector contains valid finite elements.
+        [[nodiscard]] bool IsFinite() const;
+
+        /// Tests if the (x, y, z) parts of two vectors are perpendicular to each other.
+        [[nodiscard]] bool IsPerpendicular(const Vector4& other, float epsilonSq=1e-5f) const;
+
+        [[nodiscard]] bool IsPerpendicular3(const Vector4& other, float epsilonSq = 1e-5f) const;
+
+        /// Divides each element by w to produce a Vector4 of form (x, y, z, 1).
+        /** This function performs the <b>perspective divide</b> or the <b>homogeneous divide</b> on this vector, which is the
+            process of dividing each element of this vector by w. If the w component of this vector is zero before division, the
+            result of this vector will be undefined.
+            @note This function operates in-place.
+            @see IsWZeroOrOne(). */
+        void NormalizeW();
+
+        bool operator==(const Vector4& rhs) const;
+        bool operator!=(const Vector4& rhs) const;
+
+        [[nodiscard]] bool Equals(const Vector4& rhs, float epsilon = 1e-3f) const;
+        [[nodiscard]] bool Equals(float _x, float _y, float _z, float _w, float epsilon = 1e-3f) const;
+
+        /// Returns an element-wise minimum of this and the vector (ceil, ceil, ceil, ceil).
+        /** Each element that is larger than ceil is replaced by ceil. */
+        [[nodiscard]] Vector4 Min(float ceil) const;
+
+        /// Returns an element-wise minimum of this and the given vector.
+        /** Each element that is larger than ceil is replaced by ceil.
+            @see Max(), Clamp(). */
+        [[nodiscard]] Vector4 Min(const Vector4& ceil) const;
+
+        /// Returns an element-wise maximum of this and the vector (floor, floor, floor, floor).
+        /** Each element that is smaller than floor is replaced by floor. */
+        [[nodiscard]] Vector4 Max(float floor) const;
+
+        /// Returns an element-wise maximum of this and the given vector.
+        /** Each element that is smaller than floor is replaced by floor.
+            @see Min(), Clamp(). */
+        [[nodiscard]] Vector4 Max(const Vector4& floor) const;
+
+        /// Returns a vector that has floor <= this[i] <= ceil for each element.
+        [[nodiscard]] Vector4 Clamp(float floor, float ceil) const;
+
+        /// Limits each element of this vector between the corresponding elements in floor and ceil.
+        /** @see Min(), Max(), Clamp01(). */
+        [[nodiscard]] Vector4 Clamp(const Vector4& floor, const Vector4& ceil) const;
+
+        /// Limits each element of this vector in the range [0, 1].
+        /** @see Min(), Max(), Clamp(). */
+        [[nodiscard]] Vector4 Clamp01() const;
+
+        [[nodiscard]] float Distance(const Vector4& to) const;
+
+        /// Computes the length of this vector.
+        /** @return Sqrt(x*x + y*y + z*z + w*w).
+            @see LengthSq3(), Length3(), LengthSq4(), Normalize3(), Normalize4(). */
+        [[nodiscard]] float Length4() const;
+        [[nodiscard]] float Length() const;
+
+        /// Computes the squared length of this vector.
+        /** Calling this function is faster than calling Length4(), since this function avoids computing a square root.
+            If you only need to compare lengths to each other, but are not interested in the actual length values,
+            you can compare by using LengthSq4(), instead of Length4(), since Sqrt() is an order-preserving
+            (monotonous and non-decreasing) function.
+            @return x*x + y*y + z*z + w*w.
+            @see Length3(), LengthSq3(), Length4(), Normalize3(), Normalize4(). */
+        [[nodiscard]] float LengthSquared4() const;
+        [[nodiscard]] float LengthSquared() const;
+        [[nodiscard]] float LengthSq4() const;
+        [[nodiscard]] float LengthSq() const;
+
+        /// Computes the length of the (x, y, z) part of this vector.
+        /** @note This function ignores the w component of this vector.
+            @return Sqrt(x*x + y*y + z*z).
+            @see LengthSq3(), LengthSq4(), Length4(), Normalize3(), Normalize4(). */
+        [[nodiscard]] float Length3() const { return std::sqrt(x*x + y*y + z*z); }
+
+        /// Computes the squared length of the (x, y, z) part of this vector.
+        /** Calling this function is faster than calling Length3(), since this function avoids computing a square root.
+            If you only need to compare lengths to each other, but are not interested in the actual length values,
+            you can compare by using LengthSq3(), instead of Length3(), since Sqrt() is an order-preserving
+            (monotonous and non-decreasing) function.
+            @note This function ignores the w component of this vector.
+            @return x*x + y*y + z*z.
+            @see Length3(), LengthSq4(), Length4(), Normalize3(), Normalize4(). */
+        [[nodiscard]] float LengthSq3() const;
+        [[nodiscard]] float LengthSquared3() const;
+
+        [[nodiscard]] float Magnitude() const;
+        [[nodiscard]] float Dot(const Vector4& rhs) const;
+        /// Computes the dot product of the (x, y, z) parts of this and the given float4.
+        /** @note This function ignores the w component of this vector (assumes w=0).
+            @see Dot4(), Cross3(). */
+        [[nodiscard]] float Dot3(const Vector3& rhs) const;
+        [[nodiscard]] float Dot3(const Vector4& rhs) const;
+
+        [[nodiscard]] Vector4 Project(const Vector4& rhs) const;
+        // While it is feasable to compute a cross-product in four dimensions
+        // the cross product only has the orthogonality property in 3 and 7 dimensions
+        // You should consider instead looking at Gram-Schmidt Orthogonalization
+        // to find orthonormal vectors.
+        [[nodiscard]] Vector4 Cross3(const Vector3& rhs) const;
+        [[nodiscard]] Vector4 Cross3(const Vector4& rhs) const;
+        [[nodiscard]] Vector4 Cross(const Vector4& rhs) const;
+
+        [[nodiscard]] Vector4 Normalized() const;
+        [[nodiscard]] Vector4 Lerp(const Vector4& goal, float alpha) const;
+
+        [[nodiscard]] float AngleBetween(const Vector4& rhs) const;
+
+        /// Adds two vectors. [indexTitle: operators +,-,*,/]
+        /** This function is identical to the member function Add().
+            @return float4(x + v.x, y + v.y, z + v.z, w + v.w); */
+        Vector4 operator +(const Vector4& rhs) const;
+
+        /// Adds a vector to this vector. [IndexTitle: Add/Sub/Mul/Div]
+        /// @return (x+v.x, y+v.y, z+v.z, w+v.w).
+        [[nodiscard]] Vector4 Add(const Vector4& rhs) const;
+        static Vector4 Add(const Vector4& lhs, const Vector4& rhs);
+
+        /// Subtracts the given vector from this vector. [similarOverload: operator+] [hideIndex]
+        /** This function is identical to the member function Sub().
+            @return float4(x - v.x, y - v.y, z - v.z, w - v.w); */
+        Vector4 operator -(const Vector4& rhs) const;
+        [[nodiscard]] Vector4 Sub(const Vector4& rhs) const;
+        static Vector4 Sub(const Vector4& lhs, const Vector4& rhs);
+
+        /// Multiplies this vector by a scalar. [similarOverload: operator+] [hideIndex]
+        /** This function is identical to the member function Mul().
+            @return float4(x * scalar, y * scalar, z * scalar, w * scalar); */
+        Vector4 operator *(float rhs) const;
+        [[nodiscard]] Vector4 Mul(float scalar) const;
+        static Vector4 Mul(const Vector4& lhs, float rhs);
+
+        /// Divides this vector by a scalar. [similarOverload: operator+] [hideIndex]
+        /** This function is identical to the member function Div().
+            @return float4(x / scalar, y / scalar, z / scalar, w * scalar); */
+        Vector4 operator /(float rhs) const;
+        [[nodiscard]] Vector4 Div(float scalar) const;
+        static Vector4 Div(const Vector4& rhs, float scalar);
+
+        Vector4 operator +() const; // Unary + Operator
+        /// Performs an unary negation of this vector. [similarOverload: operator+] [hideIndex]
+        /** This function is identical to the member function Neg().
+            @return float4(-x, -y, -z, -w). */
+        Vector4 operator -() const;
+
+
+    public:
+         float x = 0;
+         float y = 0;
+         float z = 0;
+         float w = 0;
+
+        void Normalize();
+    };
+
+}

include/J3ML/Math.hpp

@@ -0,0 +1,8 @@
+//
+// Created by dawsh on 7/6/24.
+//
+
+#ifndef MATH_HPP
+#define MATH_HPP
+
+#endif //MATH_HPP

include/J3ML/SSE.hpp

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

main.cpp

@@ -1,9 +1,48 @@
+/// Josh's 3D Math Library
+/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
+/// Developed and Maintained by Josh O'Leary @ Redacted Software.
+/// Special Thanks to William Tomasine II and Maxine Hayes.
+/// (c) 2024 Redacted Software
+/// This work is dedicated to the public domain.
+
+/// @file main.cpp
+/// @desc Demonstrates J3ML features.
+/// @edit 2024-07-06
+
+
 #include <iostream>
-#include <J3ML/Geometry.h>
-#include <J3ML/J3ML.h>
+#include <J3ML/Geometry.hpp>
+#include <J3ML/J3ML.hpp>
+#include <jlog/Logger.hpp>
+
 
 int main(int argc, char** argv)
 {
+
+    using namespace J3ML::Math;
+
+    // Test quadrant
+    for (float r = 0; r < TwoPi; r+=0.25f)
+    {
+        Quadrant q = QuadrantOf(r);
+        if (q == Quadrant::I)
+            std::cout << "I" << std::endl;
+        if (q == Quadrant::II)
+            std::cout << "II" << std::endl;
+        if (q == Quadrant::III)
+            std::cout << "III" << std::endl;
+        if (q == Quadrant::IV)
+            std::cout << "IV" << std::endl;
+    }
+
+    for (int i = 10; i < 9999999; i*=1.5f) {
+        std::cout << J3ML::Math::Functions::Truncate(i) << std::endl;
+    }
+
+    Ray a({420, 0, 0}, {1, 0, 0});
+
+
+    std::cout << a << std::endl;
     std::cout << "j3ml demo coming soon" << std::endl;
     return 0;
 }

src/J3ML/Algorithm/Bezier.cpp

@@ -0,0 +1,41 @@
+#include <J3ML/Algorithm/Bezier.hpp>
+#include <J3ML/LinearAlgebra/Vector2.hpp>
+#include <J3ML/LinearAlgebra/Vector3.hpp>
+
+namespace J3ML::Algorithm
+{
+    using namespace J3ML::LinearAlgebra;
+    Vector2 BezierNormal(float t, const Vector2 &p0, const Vector2 &p1,
+                                  const Vector2 &p2, const Vector2 &p3) {
+        Vector2 derived = BezierDerivative(t, p0, p1, p2, p3);
+        return derived.Normalized();
+    }
+
+    Vector2 BezierDerivative(float t, const Vector2 &p0, const Vector2 &p1, const Vector2 &p2, const Vector2 &p3) {
+        return 3 * Square(1 - t) * (p1 - p0) + 6 * (1 - t) * t * (p2 - p1) + 3 * Square(t) * (p3 - p2);
+    }
+
+    Vector2 Bezier(float t, const Vector2 &p0, const Vector2 &p1, const Vector2 &p2, const Vector2 &p3) {
+        return {Bezier(t, p0.x, p1.x, p2.x, p3.x), Bezier(t, p0.y, p1.y, p2.y, p3.y)};
+    }
+
+    Vector3 BezierDerivative(float t, const Vector3& p0, const Vector3& p1, const Vector3& p2, const Vector3& p3)
+    {
+        return 3 * Square(1 - t) * (p1 - p0) + 6 * (1 - t) * t * (p2 - p1) + 3 * Square(t) * (p3 - p2);
+    }
+
+    Vector3 BezierNormal(float t, const Vector3& p0, const Vector3& p1, const Vector3& p2, const Vector3& p3)
+    {
+        Vector3 derived = BezierDerivative(t, p0, p1, p2, p3);
+        return derived.Normalized();
+    }
+
+    Vector3 Bezier(float t, const Vector3& p0, const Vector3& p1, const Vector3& p2, const Vector3& p3)
+    {
+        return {
+            Bezier(t, p0.x, p1.x, p2.x, p3.x),
+            Bezier(t, p0.y, p1.y, p2.y, p3.y),
+            Bezier(t, p0.z, p1.z, p2.z, p3.z)};
+    }
+}
+

src/J3ML/Algorithm/GJK.cpp

@@ -0,0 +1,331 @@
+#include <J3ML/Algorithm/GJK.hpp>
+
+#include <J3ML/Geometry.hpp>
+
+namespace J3ML::Algorithms
+{
+
+    /// This function examines the simplex defined by the array of points in s, and calculates which voronoi region
+    /// of that simplex the origin is closest to. Based on that information, the function constructs a new simplex
+    /// that will be used to continue the search, and returns a new search direction for the GJK algorithm.
+    /// @param s [in, out] An array of points in the simplex. When this function returns, this point array is updates to contain the new search simplex.
+    /// @param n [in, out] The number of points in the array s. When this function returns, this reference is updated to specify how many points the new search simplex contains.
+    /// @return The new search direction vector.
+    Vector3 UpdateSimplex(Vector3 *s, int &n) {
+        if (n == 2)
+        {
+            // Four voronoi regions that the origin could be in
+            // 0) closest to vertex s[0]
+            // 1) closest to vertex s[1]
+            // 2) closest to line segment s[0]->s[1]. XX
+            // 3) contained in the line segment s[0]->s[1], and our search is over and the algorithm is now finished. XX
+
+            // By construction of the simplex, the cases 0) and 1) can never occur. Then only the cases marked with XX need to be checked.
+
+            // Sanity-check that the above reasoning is valid by testing each voronoi region and asserting that the ones we assume never to happen never will
+            float d0 = s[0].DistanceSq(Vector3::Zero);
+            float d1 = s[1].DistanceSq(Vector3::Zero);
+            float d2 = LineSegment(s[0], s[1]).DistanceSq(Vector3::Zero);
+            assert(d2 <= d0);
+            assert(d2 <= d1);
+            // Cannot be in case 0: the step 0 -> 1 must have been toward the zero direction
+            assert(Vector3::Dot(s[1]-s[0], -s[0]) >= 0.f);
+            // Cannot be in case 1: the zero direction cannot be in the voronoi region of vertex s[1].
+            assert(Vector3::Dot(s[1]-s[0], -s[1]) <= 0.f);
+
+            Vector3 d01 = s[1] - s[0];
+            Vector3 newSearchDir = Vector3::Cross(d01, Vector3::Cross(d01, s[1]));
+            if (newSearchDir.LengthSquared() > 1e-7f)
+                return newSearchDir;
+            else { // Case 3
+                n = 0;
+                return Vector3::Zero;
+            }
+        }
+        else if (n == 3)
+        {
+            // Nine voronoi regions:
+            // 0) closest to vertex s[0].
+            // 1) closest to vertex s[1].
+            // 2) closest to vertex s[2].
+            // 3) closest to edge s[0]->s[1].
+            // 4) closest to edge s[1]->s[2].  XX
+            // 5) closest to edge s[0]->s[2].  XX
+            // 6) closest to the triangle s[0]->s[1]->s[2], in the positive side.  XX
+            // 7) closest to the triangle s[0]->s[1]->s[2], in the negative side.  XX
+            // 8) contained in the triangle s[0]->s[1]->s[2], and our search is over and the algorithm is now finished.  XX
+
+            // By construction of the simplex, the origin must always be in a voronoi region that includes the point s[2], since that
+            // was the last added point. But it cannot be the case 2), since previous search took us deepest towards the direction s[1]->s[2],
+            // and case 2) implies we should have been able to go even deeper in that direction, or that the origin is not included in the convex shape,
+            // a case which has been checked for already before. Therefore the cases 0)-3) can never occur. Only the cases marked with XX need to be checked.
+
+            // Sanity-check that the above reasoning is valid by testing each voronoi region and assert()ing that the ones we assume never to
+		// happen never will.
+		float d[7];
+		d[0] = s[0].DistanceSq(Vector3::Zero);
+		d[1] = s[1].DistanceSq(Vector3::Zero);
+		d[2] = s[2].DistanceSq(Vector3::Zero);
+		d[3] = LineSegment(s[0], s[1]).DistanceSq(Vector3::Zero);
+		d[4] = LineSegment(s[1], s[2]).DistanceSq(Vector3::Zero);
+		d[5] = LineSegment(s[2], s[0]).DistanceSq(Vector3::Zero);
+		d[6] = Triangle(s[0], s[1], s[2]).DistanceSq(Vector3::Zero);
+
+		bool isContainedInTriangle = (d[6] <= 1e-3f); // Are we in case 8)?
+		float dist = INFINITY;
+		int minDistIndex = -1;
+		for(int i = 4; i < 7; ++i)
+			if (d[i] < dist)
+			{
+				dist = d[i];
+				minDistIndex = i;
+			}
+
+		    assert(isContainedInTriangle || dist <= d[0] + 1e-4f);
+            assert(isContainedInTriangle || dist <= d[1] + 1e-4f);
+            assert(isContainedInTriangle || dist <= d[2] + 1e-4f);
+            assert(isContainedInTriangle || dist <= d[3] + 1e-4f);
+
+            Vector3 d12 = s[2]-s[1];
+            Vector3 d02 = s[2]-s[0];
+            Vector3 triNormal = Vector3::Cross(d02, d12);
+
+            Vector3 e12 = Vector3::Cross(d12, triNormal);
+            float t12 = Vector3::Dot(s[1], e12);
+            if (t12 < 0.f)
+            {
+                // Case 4: Edge 1->2 is closest.
+                //assert(d[4] <= dist + 1e-3f * Max(1.f, d[4], dist));
+                Vector3 newDir = Vector3::Cross(d12, Vector3::Cross(d12, s[1]));
+                s[0] = s[1];
+                s[1] = s[2];
+                n = 2;
+                return newDir;
+            }
+            Vector3 e02 = Vector3::Cross(triNormal, d02);
+            float t02 = Vector3::Dot(s[0], e02);
+            if (t02 < 0.f)
+            {
+                // Case 5: Edge 0->2 is closest.
+                //assert(d[5] <= dist + 1e-3f * Max(1.f, d[5], dist));
+
+                Vector3 newDir = Vector3::Cross(d02, Vector3::Cross(d02, s[0]));
+                s[1] = s[2];
+                n = 2;
+                return newDir;
+            }
+            // Cases 6)-8):
+            //assert(d[6] <= dist + 1e-3f * Max(1.f, d[6], dist));
+            float scaledSignedDistToTriangle = triNormal.Dot(s[2]);
+            float distSq = scaledSignedDistToTriangle*scaledSignedDistToTriangle;
+            float scaledEpsilonSq = 1e-6f*triNormal.LengthSquared();
+
+            if (distSq > scaledEpsilonSq)
+            {
+                // The origin is sufficiently far away from the triangle.
+                if (scaledSignedDistToTriangle <= 0.f)
+                    return triNormal; // Case 6)
+                else
+                {
+                    // Case 7) Swap s[0] and s[1] so that the normal of Triangle(s[0],s[1],s[2]).PlaneCCW() will always point towards the new search direction.
+                    std::swap(s[0], s[1]);
+                    return -triNormal;
+                }
+            }
+            else
+            {
+                // Case 8) The origin lies directly inside the triangle. For robustness, terminate the search here immediately with success.
+                n = 0;
+                return Vector3::Zero;
+            }
+        }
+        else // n == 4
+        {
+            // A tetrahedron defines fifteen voronoi regions:
+            //  0) closest to vertex s[0].
+            //  1) closest to vertex s[1].
+            //  2) closest to vertex s[2].
+            //  3) closest to vertex s[3].
+            //  4) closest to edge s[0]->s[1].
+            //  5) closest to edge s[0]->s[2].
+            //  6) closest to edge s[0]->s[3].  XX
+            //  7) closest to edge s[1]->s[2].
+            //  8) closest to edge s[1]->s[3].  XX
+            //  9) closest to edge s[2]->s[3].  XX
+            // 10) closest to the triangle s[0]->s[1]->s[2], in the outfacing side.
+            // 11) closest to the triangle s[0]->s[1]->s[3], in the outfacing side. XX
+            // 12) closest to the triangle s[0]->s[2]->s[3], in the outfacing side. XX
+            // 13) closest to the triangle s[1]->s[2]->s[3], in the outfacing side. XX
+            // 14) contained inside the tetrahedron simplex, and our search is over and the algorithm is now finished. XX
+
+            // By construction of the simplex, the origin must always be in a voronoi region that includes the point s[3], since that
+            // was the last added point. But it cannot be the case 3), since previous search took us deepest towards the direction s[2]->s[3],
+            // and case 3) implies we should have been able to go even deeper in that direction, or that the origin is not included in the convex shape,
+            // a case which has been checked for already before. Therefore the cases 0)-5), 7) and 10) can never occur and
+            // we only need to check cases 6), 8), 9), 11), 12), 13) and 14), marked with XX.
+
+#ifdef MATH_ASSERT_CORRECTNESS
+            // Sanity-check that the above reasoning is valid by testing each voronoi region and assert()ing that the ones we assume never to
+		// happen never will.
+		double d[14];
+		d[0] = POINT_TO_FLOAT4(s[0]).Distance4Sq(POINT_TO_FLOAT4(vec::zero));
+		d[1] = POINT_TO_FLOAT4(s[1]).Distance4Sq(POINT_TO_FLOAT4(vec::zero));
+		d[2] = POINT_TO_FLOAT4(s[2]).Distance4Sq(POINT_TO_FLOAT4(vec::zero));
+		d[3] = POINT_TO_FLOAT4(s[3]).Distance4Sq(POINT_TO_FLOAT4(vec::zero));
+		d[4] = LineSegment(s[0], s[1]).DistanceSqD(vec::zero);
+		d[5] = LineSegment(s[0], s[2]).DistanceSqD(vec::zero);
+		d[6] = LineSegment(s[0], s[3]).DistanceSqD(vec::zero);
+		d[7] = LineSegment(s[1], s[2]).DistanceSqD(vec::zero);
+		d[8] = LineSegment(s[1], s[3]).DistanceSqD(vec::zero);
+		d[9] = LineSegment(s[2], s[3]).DistanceSqD(vec::zero);
+		d[10] = Triangle(s[0], s[1], s[2]).DistanceSqD(vec::zero);
+		d[11] = Triangle(s[0], s[1], s[3]).DistanceSqD(vec::zero);
+		d[12] = Triangle(s[0], s[2], s[3]).DistanceSqD(vec::zero);
+		d[13] = Triangle(s[1], s[2], s[3]).DistanceSqD(vec::zero);
+
+		vec Tri013Normal = Cross(s[1]-s[0], s[3]-s[0]);
+		vec Tri023Normal = Cross(s[3]-s[0], s[2]-s[0]);
+		vec Tri123Normal = Cross(s[2]-s[1], s[3]-s[1]);
+		vec Tri012Normal = Cross(s[2] - s[0], s[1] - s[0]);
+		assert(Dot(Tri012Normal, s[3] - s[0]) <= 0.f);
+		float InTri012 = Dot(-s[0], Tri012Normal);
+		float InTri013 = Dot(-s[3], Tri013Normal);
+		float InTri023 = Dot(-s[3], Tri023Normal);
+		float InTri123 = Dot(-s[3], Tri123Normal);
+		bool insideSimplex = InTri012 <= 0.f && InTri013 <= 0.f && InTri023 <= 0.f && InTri123 <= 0.f;
+
+		double dist = FLOAT_INF;
+		int minDistIndex = -1;
+		for(int i = 6; i < 14; ++i)
+			if (i == 6 || i == 8 || i == 9 || i == 11 || i == 12 || i == 13)
+				if (d[i] < dist)
+				{
+					dist = d[i];
+					minDistIndex = i;
+				}
+		assert4(insideSimplex || dist <= d[0] + 1e-4 * Max(1.0, d[0], dist), d[0], dist, insideSimplex, minDistIndex);
+		assert4(insideSimplex || dist <= d[1] + 1e-4 * Max(1.0, d[1], dist), d[1], dist, insideSimplex, minDistIndex);
+		assert4(insideSimplex || dist <= d[2] + 1e-4 * Max(1.0, d[2], dist), d[2], dist, insideSimplex, minDistIndex);
+		assert4(insideSimplex || dist <= d[4] + 1e-4 * Max(1.0, d[4], dist), d[4], dist, insideSimplex, minDistIndex);
+		assert4(insideSimplex || dist <= d[5] + 1e-4 * Max(1.0, d[5], dist), d[5], dist, insideSimplex, minDistIndex);
+		assert4(insideSimplex || dist <= d[7] + 1e-4 * Max(1.0, d[7], dist), d[7], dist, insideSimplex, minDistIndex);
+		assert4(insideSimplex || dist <= d[10] + 1e-4 * Max(1.0, d[10], dist), d[10], dist, insideSimplex, minDistIndex);
+#endif
+
+            Vector3 d01 = s[1] - s[0];
+            Vector3 d02 = s[2] - s[0];
+            Vector3 d03 = s[3] - s[0];
+            Vector3 tri013Normal = Vector3::Cross(d01, d03); // Normal of triangle 0->1->3 pointing outwards from the simplex.
+            Vector3 tri023Normal = Vector3::Cross(d03, d02); // Normal of triangle 0->2->3 pointing outwards from the simplex.
+
+#ifdef MATH_ASSERT_CORRECTNESS
+            float4d D01 = POINT_TO_FLOAT4(s[1]) - POINT_TO_FLOAT4(s[0]);
+		float4d D02 = POINT_TO_FLOAT4(s[2]) - POINT_TO_FLOAT4(s[0]);
+		float4d D03 = POINT_TO_FLOAT4(s[3]) - POINT_TO_FLOAT4(s[0]);
+		float4d tri013NormalD = D01.Cross(D03);
+		float4d tri023NormalD = D03.Cross(D02);
+		assert3(tri013NormalD.Dot(D02) <= 0.f, tri013NormalD, D02, tri013NormalD.Dot(D02));
+		assert3(tri023NormalD.Dot(D01) <= 0.f, tri023NormalD, D01, tri023NormalD.Dot(D01));
+#endif
+
+            Vector3 e03_1 = Vector3::Cross(tri013Normal, d03); // The normal of edge 0->3 on triangle 013.
+            Vector3 e03_2 = Vector3::Cross(d03, tri023Normal); // The normal of edge 0->3 on triangle 023.
+            float inE03_1 = Vector3::Dot(e03_1, s[3]);
+            float inE03_2 = Vector3::Dot(e03_2, s[3]);
+            if (inE03_1 <= 0.f && inE03_2 <= 0.f)
+            {
+                // Case 6) Edge 0->3 is closest. Simplex degenerates to a line segment.
+#ifdef MATH_ASSERT_CORRECTNESS
+                assert4(!insideSimplex && d[6] <= dist + 1e-3f * Max(1.0, d[6], dist), d[6], dist, insideSimplex, minDistIndex);
+#endif
+                Vector3 newDir = Vector3::Cross(d03, Vector3::Cross(d03, s[3]));
+                s[1] = s[3];
+                n = 2;
+                return newDir;
+            }
+
+            Vector3 d12 = s[2] - s[1];
+            Vector3 d13 = s[3] - s[1];
+            Vector3 tri123Normal = Vector3::Cross(d12, d13);
+            assert(Vector3::Dot(tri123Normal, -d02) <= 0.f);
+            Vector3 e13_0 = Vector3::Cross(d13, tri013Normal);
+            Vector3 e13_2 = Vector3::Cross(tri123Normal, d13);
+            float inE13_0 = Vector3::Dot(e13_0, s[3]);
+            float inE13_2 = Vector3::Dot(e13_2, s[3]);
+            if (inE13_0 <= 0.f && inE13_2 <= 0.f)
+            {
+                // Case 8) Edge 1->3 is closest. Simplex degenerates to a line segment.
+#ifdef MATH_ASSERT_CORRECTNESS
+                assert4(!insideSimplex && d[8] <= dist + 1e-3f * Max(1.0, d[8], dist), d[8], dist, insideSimplex, minDistIndex);
+#endif
+                Vector3 newDir = Vector3::Cross(d13, Vector3::Cross(d13, s[3]));
+                s[0] = s[1];
+                s[1] = s[3];
+                n = 2;
+                return newDir;
+            }
+
+            Vector3 d23 = s[3] - s[2];
+            Vector3 e23_0 = Vector3::Cross(tri023Normal, d23);
+            Vector3 e23_1 = Vector3::Cross(d23, tri123Normal);
+            float inE23_0 = Vector3::Dot(e23_0, s[3]);
+            float inE23_1 = Vector3::Dot(e23_1, s[3]);
+            if (inE23_0 <= 0.f && inE23_1 <= 0.f)
+            {
+                // Case 9) Edge 2->3 is closest. Simplex degenerates to a line segment.
+#ifdef MATH_ASSERT_CORRECTNESS
+                assert4(!insideSimplex && d[9] <= dist + 1e-3f * Max(1.0, d[9], dist), d[9], dist, insideSimplex, minDistIndex);
+#endif
+                Vector3 newDir = Vector3::Cross(d23, Vector3::Cross(d23, s[3]));
+                s[0] = s[2];
+                s[1] = s[3];
+                n = 2;
+                return newDir;
+            }
+
+            float inTri013 = Vector3::Dot(s[3], tri013Normal);
+            if (inTri013 < 0.f && inE13_0 >= 0.f && inE03_1 >= 0.f)
+            {
+                // Case 11) Triangle 0->1->3 is closest.
+#ifdef MATH_ASSERT_CORRECTNESS
+                assert4(!insideSimplex && d[11] <= dist + 1e-3f * Max(1.0, d[11], dist), d[11], dist, insideSimplex, minDistIndex);
+#endif
+                s[2] = s[3];
+                n = 3;
+                return tri013Normal;
+            }
+            float inTri023 = Vector3::Dot(s[3], tri023Normal);
+            if (inTri023 < 0.f && inE23_0 >= 0.f && inE03_2 >= 0.f)
+            {
+                // Case 12) Triangle 0->2->3 is closest.
+#ifdef MATH_ASSERT_CORRECTNESS
+                assert4(!insideSimplex && d[12] <= dist + 1e-3f * Max(1.0, d[12], dist), d[12], dist, insideSimplex, minDistIndex);
+#endif
+                s[1] = s[0];
+                s[0] = s[2];
+                s[2] = s[3];
+                n = 3;
+                return tri023Normal;
+            }
+            float inTri123 = Vector3::Dot(s[3], tri123Normal);
+            if (inTri123 < 0.f && inE13_2 >= 0.f && inE23_1 >= 0.f)
+            {
+                // Case 13) Triangle 1->2->3 is closest.
+#ifdef MATH_ASSERT_CORRECTNESS
+                assert4(!insideSimplex && d[13] <= dist + 1e-3f * Max(1.0, d[13], dist), d[13], dist, insideSimplex, minDistIndex);
+#endif
+                s[0] = s[1];
+                s[1] = s[2];
+                s[2] = s[3];
+                n = 3;
+                return tri123Normal;
+            }
+
+            // Case 14) Not in the voronoi region of any triangle or edge. The origin is contained in the simplex, the search is finished.
+            n = 0;
+            return Vector3::Zero;
+        }
+    }
+}
+
+

src/J3ML/Algorithm/RNG.cpp

@@ -1,4 +1,4 @@
-#include <J3ML/Algorithm/RNG.h>
+#include <J3ML/Algorithm/RNG.hpp>
 #include <stdexcept>
 #include <cassert>
 
@@ -36,21 +36,21 @@ namespace J3ML::Algorithm {
 
     u32 RNG::Int()
     {
-        assert(modulus != 0);
+        //assert(modulus != 0);
         /// TODO: Convert to using Shrage's method for approximate factorization (Numerical Recipes in C)
 
-        // Currently we cast everything to 65-bit to avoid overflow, which is quite dumb.
+        // Currently we cast everything to 64-bit to avoid overflow, which is quite dumb.
 
         // Creates the new random number
-        u64 newNum  = ((u64)lastNumber * (u64)multiplier + (u64)increment % (u64)modulus);
+        //u64 newNum  = ((u64)lastNumber * (u64)multiplier + (u64)increment % (u64)modulus);
 
         // TODO: use this on console platforms to rely on smaller sequences.
-        // u32 m = lastNumber * multiplier;
-        // u32 i = m + increment;
-        // u32 f = i & 0x7FFFFFFF;
-        // u32 m = (lastNumber * 214013 + 2531011) & 0x7FFFFFFF;
-        // unsigned __int64 newNum = (lastNumber * multiplier + increment) & 0x7FFFFFFF;
-        assert( ((u32)newNum!=0 || increment != 0) && "RNG degenerated to producing a stream of zeroes!");
+        u32 m = lastNumber * multiplier;
+        u32 i = m + increment;
+        u32 f = i & 0x7FFFFFFF;
+        //u32 m = (lastNumber * 214013 + 2531011) & 0x7FFFFFFF;
+        u64 newNum = (lastNumber * multiplier + increment) & 0x7FFFFFFF;
+        //assert( ((u32)newNum!=0 || increment != 0) && "RNG degenerated to producing a stream of zeroes!");
         lastNumber = (u32)newNum;
         return lastNumber;
     }
@@ -65,30 +65,14 @@ namespace J3ML::Algorithm {
         return num;
     }
 
-    /// Jesus-Fuck ~ Josh
-    /// As per C99, union-reinterpret should now be safe: http://stackoverflow.com/questions/8511676/portable-data-reinterpretation
-    union FloatIntReinterpret
-    {
-        float f;
-        u32 i;
-    };
 
-    template <typename To, typename From>
-    union ReinterpretOp {
-        To to;
-        From from;
-    };
 
-    template <typename To, typename From>
-    To ReinterpretAs(From input)
-    {
-        ReinterpretOp<To, From> fi {};
-        fi.to = input;
-        return fi.from;
-    }
+
+
 
     float RNG::Float() {
         u32 i = ((u32)Int() & 0x007FFFFF /* random mantissa */) | 0x3F800000 /* fixed exponent */;
+        // TODO: PROPERLY fix this.
         auto f = ReinterpretAs<float, u32>(i); // f is now in range [1, 2[
         f -= 1.f; // Map to range [0, 1[
         assert(f >= 0.f);
@@ -98,11 +82,11 @@ namespace J3ML::Algorithm {
 
 
     float RNG::Float01Incl() {
-        for (int i = 0; i < 100; ++i) {
+        for (int i = 0; i < 1000; ++i) {
             u32 val = (u32)Int() & 0x00FFFFFF;
             if (val > 0x800000)
                 continue;
-            else if (val = 0x800000)
+            else if (val == 0x800000)
                 return 1.f;
             else {
                 val |= 0x3F800000;
@@ -122,8 +106,8 @@ namespace J3ML::Algorithm {
         float f = ReinterpretAs<float, u32>(i); // f is now in range ]-2, -1[ union [1, 2].
         float fone = ReinterpretAs<float, u32>(one); // +/- 1, of same sign as f.
         f -= fone;
-        assert(f > -1.f);
-        assert(f < 1.f);
+        //assert(f > -1.f);
+        //assert(f < 1.f);
         return f;
     }
 
@@ -145,10 +129,10 @@ namespace J3ML::Algorithm {
     }
 
     float RNG::FloatIncl(float a, float b) {
-        assert(a <= b && "RNG::Float(a, b): Error in range: b < a!");
+        //assert(a <= b && "RNG::Float(a, b): Error in range: b < a!");
         float f = a + Float() * (b - a);
-        assert( a <= f);
-        assert(f <= b);
+        //assert( a <= f);
+        //assert(f <= b);
         return f;
     }
 }

src/J3ML/Geometry/AABB.cpp

@@ -1,11 +1,43 @@
-#include <J3ML/Geometry/AABB.h>
+#include <J3ML/Geometry/AABB.hpp>
 #include <cassert>
+#include <J3ML/Geometry/Plane.hpp>
+#include <J3ML/Geometry/Sphere.hpp>
+//#include <J3ML/Geometry/OBB.h>
+#include <J3ML/Geometry/LineSegment.hpp>
+#include <J3ML/Geometry/Triangle.hpp>
+#include <J3ML/Geometry/Polygon.hpp>
+#include <J3ML/Geometry/Frustum.hpp>
+#include <J3ML/Geometry/Capsule.hpp>
+#include <J3ML/Geometry/Ray.hpp>
+#include <J3ML/Geometry/TriangleMesh.hpp>
+#include <J3ML/Geometry/Polyhedron.hpp>
+#include <J3ML/Algorithm/RNG.hpp>
+#include <J3ML/Geometry/OBB.hpp>
 
 namespace J3ML::Geometry {
 
+    /// See Christer Ericson's Real-time Collision Detection, p. 87, or
+    /// James Arvo's "Transforming Axis-aligned Bounding Boxes" in Graphics Gems 1, pp. 548-550.
+    /// http://www.graphicsgems.org/
+    template<typename Matrix>
+    void AABBTransformAsAABB(AABB &aabb, Matrix &m)
+    {
+        const Vector3 centerPoint = (aabb.minPoint + aabb.maxPoint) * 0.5f;
+        const Vector3 halfSize = centerPoint - aabb.minPoint;
+        Vector3 newCenter = m.Mul(centerPoint);
+
+
+        // The following is equal to taking the absolute value of the whole matrix m.
+        Vector3 newDir = Vector3(std::abs(m[0][0] * halfSize.x) + std::abs(m[0][1] * halfSize.y) + std::abs(m[0][2] * halfSize.z),
+                                 std::abs(m[1][0] * halfSize.x) + std::abs(m[1][1] * halfSize.y) + std::abs(m[1][2] * halfSize.z),
+                                 std::abs(m[2][0] * halfSize.x) + std::abs(m[2][1] * halfSize.y) + std::abs(m[2][2] * halfSize.z));
+        aabb.minPoint = newCenter - newDir;
+        aabb.maxPoint = newCenter + newDir;
+    }
+
     AABB AABB::FromCenterAndSize(const J3ML::Geometry::Vector3 &center, const J3ML::Geometry::Vector3 &size) {
         Vector3 halfSize =  size * 0.5f;
-        return {center - halfSize, center + halfSize};
+        return AABB{center - halfSize, center + halfSize};
     }
 
     float AABB::MinX() const { return minPoint.x; }
@@ -41,6 +73,8 @@ namespace J3ML::Geometry {
         return (minPoint+maxPoint) * 0.5f;
     }
 
+    Vector3 AABB::CenterPoint() const { return Centroid(); }
+
     Vector3 AABB::Size() const {
         return this->maxPoint - this->minPoint;
     }
@@ -80,7 +114,7 @@ namespace J3ML::Geometry {
                 direction.z >= 0.f ? maxPoint.z : minPoint.z};
     }
 
-    Vector3 AABB::ExtremePoint(const Vector3 &direction, float &projectionDistance) {
+    Vector3 AABB::ExtremePoint(const Vector3 &direction, float &projectionDistance) const {
         auto extremePt = ExtremePoint(direction);
         projectionDistance = extremePt.Dot(direction);
         return extremePt;
@@ -166,30 +200,35 @@ 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);
     }
 
     void AABB::SetFromCenterAndSize(const Vector3& center, const Vector3& size)
     {
-
+        Vector3 halfSize = 0.5f * size;
+        minPoint = center - halfSize;
+        maxPoint = center + halfSize;
     }
 
 
     void AABB::SetFrom(const OBB& obb)
     {
-
+        Vector3 halfSize = Vector3::Abs(obb.axis[0] * obb.r[0]) + Vector3::Abs(obb.axis[1]*obb.r[1]) + Vector3::Abs(obb.axis[2]*obb.r[2]);
+        SetFromCenterAndSize(obb.pos, 2.f*halfSize);
     }
 
     void AABB::SetFrom(const Sphere& s)
     {
-
+        Vector3 d = Vector3(s.Radius, s.Radius, s.Radius);
+        minPoint = s.Position - d;
+        maxPoint = s.Position + d;
     }
 
     void AABB::SetFrom(const Vector3 *pointArray, int numPoints) {
@@ -201,8 +240,12 @@ namespace J3ML::Geometry {
             Enclose(pointArray[i]);
     }
 
-    Vector3 AABB::GetRandomPointInside() const {
+    Vector3 AABB::RandomPointInside(RNG &rng) const {
+        float f1 = rng.Float();
+        float f2 = rng.Float();
+        float f3 = rng.Float();
 
+        return PointInside(f1, f2, f3);
     }
 
     void AABB::SetNegativeInfinity() {
@@ -234,4 +277,593 @@ namespace J3ML::Geometry {
         Vector3 d = obb.r.x * absAxis0 + obb.r.y * absAxis1 + obb.r.z * absAxis2;
     }
 
+    void AABB::Enclose(const Sphere &sphere) {
+        Vector3 radiusCubed(sphere.Radius);
+
+        Enclose(sphere.Position - radiusCubed, sphere.Position + radiusCubed);
+    }
+
+    void AABB::Enclose(const Triangle &triangle) {
+        // TODO: Implement alternate min or comparison operators for Vector3
+        Enclose(Vector3::Min(triangle.V0, triangle.V1, triangle.V2), Vector3::Max(triangle.V0, triangle.V1, triangle.V2));
+    }
+
+    void AABB::Enclose(const Capsule &capsule) {
+        Vector3 radiusCubed(capsule.r);
+        minPoint = Vector3::Min(minPoint, Vector3::Min(capsule.l.A, capsule.l.B) - radiusCubed);
+        maxPoint = Vector3::Max(maxPoint, Vector3::Max(capsule.l.A, capsule.l.B) - radiusCubed);
+        Enclose(minPoint, maxPoint);
+    }
+
+    Vector3 AABB::GetClosestPoint(const Vector3 &point) const {
+        Vector3 result = point;
+        if (point.x > this->maxPoint.x)
+            result.x = this->maxPoint.x;
+        else if (point.x < this->minPoint.x)
+            result.x = this->minPoint.x;
+        else
+            result.x = point.x;
+
+        if (point.y > this->maxPoint.y)
+            result.y = this->maxPoint.y;
+        else if (point.y < this->minPoint.y)
+            result.y = this->minPoint.y;
+        else
+            result.y = point.y;
+
+        if (point.z > this->maxPoint.z)
+            result.z = this->maxPoint.z;
+        else if (point.z < this->minPoint.z)
+            result.z = this->minPoint.z;
+        else
+            result.z = point.z;
+
+        return result;
+    }
+
+    AABB::AABB(const Vector3 &min, const Vector3 &max) : minPoint(min), maxPoint(max)
+    {
+
+    }
+
+    AABB::AABB() {}
+
+    float Max(float a, float b)
+    {
+        return std::max(a, b);
+    }
+    float Max(float a, float b, float c)
+    {
+        return std::max(a, std::max(b, c));
+    }
+
+    float Min(float a, float b, float c)
+    {
+        return std::min(a, std::min(b, c));
+    }
+
+    // Compute the face normals of the AABB, because the AABB is at center
+    // and (of course) axis aligned, we know it's normals are the X,Y,Z axes.
+    Vector3 u0 = Vector3(1.f, 0.f, 0.f);
+    Vector3 u1 = Vector3(0.f, 1.f, 0.f);
+    Vector3 u2 = Vector3(0.f, 0.f, 1.f);
+
+
+
+
+
+    bool AABB::TestAxis(const Vector3& axis, const Vector3& v0, const Vector3& v1, const Vector3& v2) const
+    {
+
+        Vector3 e = this->Size();
+
+        // Testing axis: axis_u0_f0
+        // Project all 3 vertices of the triangle onto the Separating axis
+        float p0 = Vector3::Dot(v0, axis);
+        float p1 = Vector3::Dot(v1, axis);
+        float p2 = Vector3::Dot(v2, axis);
+
+        // Project the AABB onto the separating axis
+        // We don't care about the end points of the projection
+        // just the length of the half-size of the AABB
+        // that is, we're only casting the extents onto the
+        // separating axis, not the AABB center. We don't
+        // need to cast the center, because we know that the
+        // AABB is at origin compared to the triangle!
+        float r = e.x * std::abs(Vector3::Dot(u0, axis)) +
+                  e.y * std::abs(Vector3::Dot(u1, axis)) +
+                  e.z * std::abs(Vector3::Dot(u2, axis));
+
+        // Now do the actual test, basically see if either of
+        // the most extreme of the triangle points intersects r
+        // You might need to write Min & Max functions that take 3 arguments
+        if (Max(Max(p0, p1, p2), Min(p0, p1, p2)) > r)
+        {
+            // This means BOTH of the points of the projected triangle
+            // are outside the projected half-length of the AABB
+            // Therefore the axis is separating and we can exit
+            return false;
+        }
+
+        return true;
+    }
+
+
+    bool AABB::Intersects(const Triangle &triangle) const {
+        // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter4/aabb-triangle.html
+
+        Vector3 v0 = triangle.V0;
+        Vector3 v1 = triangle.V1;
+        Vector3 v2 = triangle.V2;
+
+        // Convert AABB to center-extentss form
+        Vector3 c = this->Centroid();
+        Vector3 e = this->Size();
+
+        // Translate the triangle as conceptually moving the AABB to origin
+        // This is the same as we did with the point in triangle test
+        v0 -= c;
+        v1 -= c;
+        v2 -= c;
+
+        // Compute the edge vectors of the triangle
+        // That is , get the lines between the points as vectors
+        Vector3 f0 = v1 - v0; // B - A
+        Vector3 f1 = v2 - v1; // C - B
+        Vector3 f2 = v0 - v2; // A - C
+
+        // There are a total of 13 axes to test!!!
+        // We first test against 9 axis, these axes are given by cross product combinations
+        // of the edges of the triangle and the edges of the AABB. You need to get an axis testing each of the 3 sides
+        // of the AABB against each of the 3 sides of the triangle. The result is 9 axes of separation.
+
+        // Compute the 9 axes
+        Vector3 axis_u0_f0 = Vector3::Cross(u0, f0);
+        Vector3 axis_u0_f1 = Vector3::Cross(u0, f1);
+        Vector3 axis_u0_f2 = Vector3::Cross(u0, f2);
+        Vector3 axis_u1_f0 = Vector3::Cross(u1, f0);
+        Vector3 axis_u1_f1 = Vector3::Cross(u1, f1);
+        Vector3 axis_u1_f2 = Vector3::Cross(u1, f2);
+        Vector3 axis_u2_f0 = Vector3::Cross(u1, f0);
+        Vector3 axis_u2_f1 = Vector3::Cross(u1, f1);
+        Vector3 axis_u2_f2 = Vector3::Cross(u1, f2);
+
+        if (TestAxis(axis_u0_f0, v0, v1, v2)) return true;
+        if (TestAxis(axis_u0_f1, v0, v1, v2)) return true;
+        if (TestAxis(axis_u0_f2, v0, v1, v2)) return true;
+        if (TestAxis(axis_u1_f0, v0, v1, v2)) return true;
+        if (TestAxis(axis_u1_f1, v0, v1, v2)) return true;
+        if (TestAxis(axis_u1_f2, v0, v1, v2)) return true;
+        if (TestAxis(axis_u2_f0, v0, v1, v2)) return true;
+        if (TestAxis(axis_u2_f1, v0, v1, v2)) return true;
+        if (TestAxis(axis_u2_f2, v0, v1, v2)) return true;
+
+        // Next we have 3 face normals from the AABB
+        // for these tests we are conceptually checking if the bounding box
+        // of the triangle intersects the bounding box of the AABB
+        // that is to say, the separating axis for all tests are axis aligned:
+        // axis1: (1, 0, 0), axis2: (0, 1, 0), axis3: (0, 0, 1)
+        // Do the SAT given the 3 primary axes of the AABB
+        // You already have two vectors for this: u0, u1, and u2
+
+        if (TestAxis(u0, v0, v1, v2)) return true;
+        if (TestAxis(u1, v0, v1, v2)) return true;
+        if (TestAxis(u2, v0, v1, v2)) return true;
+
+        // Finally we have one last axis to test, the face normal of the triangle
+        // We can get the normal of the triangle by crossing the first two line segments
+        Vector3 triangleNormal = Vector3::Cross(f0, f1);
+        if (TestAxis(triangleNormal, u0, u1, u2))
+            return true;
+
+        // Passed testing for all 13 separating axes that exist
+        return false;
+    }
+
+    bool AABB::Intersects(const LineSegment &lineSegment) const
+    {
+        Vector3 dir = lineSegment.B - lineSegment.A;
+        float len = dir.Length();
+        if (len <= 1e-4f) // Degenerate line segment? Fall back to point-in-AABB test.
+            return Contains(lineSegment.A);
+
+        float invLen = 1.f / len;
+        dir *= invLen;
+        float tNear = 0.f, tFar = len;
+#ifdef MATH_SIMD
+        return IntersectLineAABB_SSE(lineSegment.a, dir, tNear, tFar);
+#else
+        return IntersectLineAABB_CPP(lineSegment.A, dir, tNear, tFar);
+#endif
+    }
+
+    bool AABB::Intersects(const AABB& aabb) const {
+        // If any of the cardinal X,Y,Z axes is a separating axis, then
+        // there is no intersection.
+        return minPoint.x < aabb.maxPoint.x &&
+               minPoint.y < aabb.maxPoint.y &&
+               minPoint.z < aabb.maxPoint.z &&
+               aabb.minPoint.x < maxPoint.x &&
+               aabb.minPoint.y < maxPoint.y &&
+               aabb.minPoint.z < maxPoint.z;
+    }
+
+    TriangleMesh AABB::Triangulate(int numFacesX, int numFacesY, int numFacesZ, bool ccwIsFrontFacing) const {
+
+        TriangleMesh mesh;
+
+        assert(numFacesX >= 1);
+        assert(numFacesX >= 1);
+        assert(numFacesX >= 1);
+
+        // Generate both X-Y planes.
+        int i = 0;
+        for (int face = 0; face < 6; ++face) // Faces run in the order -X, +X, -Y, +Y, -Z, +Z.
+        {
+            int numFacesU;
+            int numFacesV;
+            bool flip = (face == 1 || face == 2 || face == 5);
+            if (ccwIsFrontFacing)
+                flip = !flip;
+            if (face == 0 || face == 1) {
+                numFacesU = numFacesY;
+                numFacesV = numFacesZ;
+            } else if (face == 2 || face == 3) {
+                numFacesU = numFacesX;
+                numFacesV = numFacesZ;
+            } else // if (face == 4 || face == 5)
+            {
+                numFacesU = numFacesX;
+                numFacesV = numFacesY;
+            }
+            for (int x = 0; x < numFacesU; ++x) {
+                for (int y = 0; numFacesV; ++y) {
+                    float u = (float)x / (numFacesU);
+                    float v = (float)y / (numFacesV);
+                    float u2 = (float)(x+1) / (numFacesU);
+                    float v2 = (float)(y+1) / (numFacesV);
+
+                    //mesh.Vertices[i]   = FacePoint(face, u, v);
+                    //mesh.Vertices[i+1] =
+                    // TODO: Come back to this once TriangleMesh is proper fleshed out
+                }
+            }
+        }
+        return mesh;
+    }
+
+    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());
+        assert(tNear <= tFar && "AABB::IntersectLineAABB: User gave a degenerate line as input for the intersection test!");
+        // The user should have inputted values for tNear and tFar to specify the desired subrange [tNear, tFar] of the line
+        // for this intersection test.
+        // For a Line-AABB test, pass in
+        //    tNear = -FLOAT_INF;
+        //    tFar = FLOAT_INF;
+        // For a Ray-AABB test, pass in
+        //    tNear = 0.f;
+        //    tFar = FLOAT_INF;
+        // For a LineSegment-AABB test, pass in
+        //    tNear = 0.f;
+        //    tFar = LineSegment.Length();
+
+        // Test each cardinal plane (X, Y and Z) in turn.
+        if (!Math::EqualAbs(lineDir.x, 0.f))
+        {
+            float recipDir = Math::RecipFast(lineDir.x);
+            float t1 = (minPoint.x - linePos.x) * recipDir;
+            float t2 = (maxPoint.x - linePos.x) * recipDir;
+
+            // tNear tracks distance to intersect (enter) the AABB.
+            // tFar tracks the distance to exit the AABB.
+            if (t1 < t2)
+                tNear = Max(t1, tNear), tFar = std::min(t2, tFar);
+            else // Swap t1 and t2.
+                tNear = Max(t2, tNear), tFar = std::min(t1, tFar);
+
+            if (tNear > tFar)
+                return false; // Box is missed since we "exit" before entering it.
+        }
+        else if (linePos.x < minPoint.x || linePos.x > maxPoint.x)
+            return false; // The ray can't possibly enter the box, abort.
+
+        if (!Math::EqualAbs(lineDir.y, 0.f))
+        {
+            float recipDir = Math::RecipFast(lineDir.y);
+            float t1 = (minPoint.y - linePos.y) * recipDir;
+            float t2 = (maxPoint.y - linePos.y) * recipDir;
+
+            if (t1 < t2)
+                tNear = Max(t1, tNear), tFar = std::min(t2, tFar);
+            else // Swap t1 and t2.
+                tNear = Max(t2, tNear), tFar = std::min(t1, tFar);
+
+            if (tNear > tFar)
+                return false; // Box is missed since we "exit" before entering it.
+        }
+        else if (linePos.y < minPoint.y || linePos.y > maxPoint.y)
+            return false; // The ray can't possibly enter the box, abort.
+
+        if (!Math::EqualAbs(lineDir.z, 0.f)) // ray is parallel to plane in question
+        {
+            float recipDir = Math::RecipFast(lineDir.z);
+            float t1 = (minPoint.z - linePos.z) * recipDir;
+            float t2 = (maxPoint.z - linePos.z) * recipDir;
+
+            if (t1 < t2)
+                tNear = Max(t1, tNear), tFar = std::min(t2, tFar);
+            else // Swap t1 and t2.
+                tNear = Max(t2, tNear), tFar = std::min(t1, tFar);
+        }
+        else if (linePos.z < minPoint.z || linePos.z > maxPoint.z)
+            return false; // The ray can't possibly enter the box, abort.
+
+        return tNear <= tFar;
+    }
+
+    Vector3 AABB::AnyPointFast() const { return minPoint;}
+
+    Vector3 AABB::RandomPointOnSurface(RNG &rng) const {
+        int i = rng.Int(0, 5);
+        float f1 = rng.Float();
+        float f2 = rng.Float();
+        return FacePoint(i, f1, f2);
+    }
+
+    Vector3 AABB::RandomPointOnEdge(RNG &rng) const {
+        int i = rng.Int(0, 11);
+        float f = rng.Float();
+        return PointOnEdge(i, f);
+    }
+
+    Vector3 AABB::RandomCornerPoint(RNG &rng) const {
+        return CornerPoint(rng.Int(0, 7));
+    }
+
+    void AABB::Translate(const Vector3 &offset) {
+        minPoint += offset;
+        maxPoint += offset;
+    }
+
+    AABB AABB::Translated(const Vector3 &offset) const {
+        return AABB(minPoint+offset, maxPoint+offset);
+    }
+
+    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);
+    }
+
+    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);
+
+    }
+
+    void AABB::TransformAABB(const Quaternion &transform) {
+        Vector3 newCenter = transform.Transform(Centroid());
+        Vector3 newDir = Vector3::Abs((transform.Transform(Size())*0.5f));
+        minPoint = newCenter - newDir;
+        maxPoint = newCenter + newDir;
+    }
+
+    OBB AABB::Transform(const Matrix3x3 &transform) const {
+        OBB obb;
+        obb.SetFrom(*this, transform);
+        return obb;
+    }
+
+    bool AABB::Contains(const Vector3 &aabbMinPoint, const Vector3 &aabbMaxPoint) const {
+        return minPoint.x <= aabbMinPoint.x && maxPoint.x >= aabbMaxPoint.x &&
+               minPoint.y <= aabbMinPoint.y && maxPoint.y >= aabbMaxPoint.y &&
+               minPoint.z <= aabbMinPoint.z && maxPoint.z >= aabbMaxPoint.z;
+    }
+
+    bool AABB::Contains(const LineSegment &lineSegment) const {
+        return Contains(Vector3::Min(lineSegment.A, lineSegment.B), Vector3::Max(lineSegment.A, lineSegment.B));
+    }
+
+
+
+    bool AABB::Contains(const Vector3 &point) const {
+        return minPoint.x <= point.x && point.x <= maxPoint.x &&
+               minPoint.y <= point.y && point.y <= maxPoint.y &&
+               minPoint.z <= point.z && point.z <= maxPoint.z;
+    }
+
+    OBB AABB::Transform(const Matrix4x4 &transform) const {
+        OBB obb;
+        obb.SetFrom(*this, transform);
+        return obb;
+    }
+
+    OBB AABB::Transform(const Quaternion &transform) const {
+        OBB obb;
+        obb.SetFrom(*this, transform);
+        return obb;
+    }
+
+    bool AABB::Contains(const AABB &aabb) const {
+        return Contains(aabb.minPoint, aabb.maxPoint);
+    }
+
+    bool AABB::Contains(const OBB &obb) const {
+        return Contains(obb.MinimalEnclosingAABB());
+    }
+
+    bool AABB::Contains(const Sphere &sphere) const {
+        auto radiusVec = Vector3(sphere.Radius,sphere.Radius, sphere.Radius);
+        return Contains(sphere.Position - radiusVec, sphere.Position + radiusVec);
+    }
+
+    bool AABB::Contains(const Capsule &capsule) const {
+        return Contains(capsule.MinimalEnclosingAABB());
+    }
+
+    bool AABB::Contains(const Triangle &triangle) const {
+        return Contains(triangle.BoundingAABB());
+    }
+
+    bool AABB::Contains(const Polygon &polygon) const {
+        return Contains(polygon.MinimalEnclosingAABB());
+    }
+
+    bool AABB::Contains(const Frustum &frustum) const {
+        return Contains(frustum.MinimalEnclosingAABB());
+    }
+
+    bool AABB::Contains(const Polyhedron &polyhedron) const {
+        return Contains(polyhedron.MinimalEnclosingAABB());
+    }
+
+    bool AABB::IntersectLineAABB(const Vector3 &linePos, const Vector3 &lineDir, float tNear, float tFar) const {
+        //assert(lineDir.IsNormalized() && lineDir && lineDir.LengthSquared());
+        assert(tNear <= tFar && "");
+        // The user should have inputted values for tNear and tFar to specify the desired subrange [tNear, tFar] of the line
+        // for this intersection test.
+        // For a Line-AABB test, pass in
+        //    tNear = -FLOAT_INF;
+        //    tFar = FLOAT_INF;
+        // For a Ray-AABB test, pass in
+        //    tNear = 0.f;
+        //    tFar = FLOAT_INF;
+        // For a LineSegment-AABB test, pass in
+        //    tNear = 0.f;
+        //    tFar = LineSegment.Length();
+
+        // Test each cardinal plane (X, Y, and Z) in turn.
+        if (!Math::EqualAbs(lineDir.x, 0.f)) {
+            float recipDir = 1.f / lineDir.x;
+            float t1 = (minPoint.x - linePos.x) * recipDir;
+            float t2 = (maxPoint.x - linePos.x) * recipDir;
+
+            // tNear tracks distance to intersect (enter) the AABB
+            // tFar tracks the distance to exit the AABB
+            if (t1 < t2)
+                tNear = std::max(t1, tNear), tFar = std::min(t2, tFar);
+            else // swap t1 and t2;
+                tNear = std::max(t2, tNear), tFar = std::min(t1, tFar);
+
+            if (tNear > tFar)
+                return false; // Box is missed since we "exit" before entering it
+        }
+        else if (linePos.x < minPoint.x || linePos.x > maxPoint.x)
+            return false; // the ray can't possibly enter the box
+
+        if (!Math::EqualAbs(lineDir.y, 0.f)) // ray is parallel to plane in question
+        {
+            float recipDir = 1.f / lineDir.y;
+            float t1 = (minPoint.y - linePos.y) * recipDir;
+            float t2 = (maxPoint.y - linePos.y) * recipDir;
+
+            if (t1 < t2)
+                tNear = std::max(t1, tNear), tFar = std::min(t2, tFar);
+            else
+                tNear = std::max(t2, tNear), tFar = std::min(t1, tFar);
+
+            if (tNear > tFar)
+                return false;
+        }
+        else if (linePos.y < minPoint.y || linePos.y > maxPoint.y)
+            return false; // The ray can't possibly enter the box, abort.
+
+        if (!Math::EqualAbs(lineDir.z, 0.f)) // ray is parallel to plane in question
+        {
+            float recipDir = 1.f / lineDir.z;
+            float t1 = (minPoint.z - linePos.z) * recipDir;
+            float t2 = (maxPoint.z - linePos.z) * recipDir;
+
+            if (t1 < t2)
+                tNear = std::max(t1, tNear), tFar = std::min(t2, tFar);
+            else // Swap t1 and t2.
+                tNear = std::max(t2, tNear), tFar = std::min(t1, tFar);
+        } else if (linePos.z < minPoint.z || linePos.z > maxPoint.z)
+            return false;
+
+        return tNear <= tFar;
+
+    }
+
+    void AABB::ProjectToAxis(const Vector3 &axis, float &dMin, float &dMax) const {
+        Vector3 c = (minPoint + maxPoint) * 0.5f;
+        Vector3 e = maxPoint - c;
+
+        // Compute the projection interval radius of the AABB onto L(t) = aabb.center + t * plane.normal;
+        float r = std::abs(e[0]*std::abs(axis[0]) + e[1]*std::abs(axis[1]) + e[2]*std::abs(axis[2]));
+
+        // Compute the distance of the box center from plane.
+        float s = axis.Dot(c);
+        dMin = s - r;
+        dMax = s + r;
+    }
+
+    Vector3 AABB::MinPoint() const {
+        return {MinX(), MinY(), MinZ()};
+    }
+
+    Vector3 AABB::MaxPoint() const {
+        return {MaxX(), MaxY(), MaxZ()};
+    }
+
 }

src/J3ML/Geometry/AABB2D.cpp

@@ -0,0 +1,96 @@
+#include <J3ML/Geometry/AABB2D.hpp>
+
+namespace J3ML::Geometry
+{
+
+    float AABB2D::Width() const { return maxPoint.x - minPoint.x;  }
+
+    float AABB2D::Height() const { return maxPoint.y - minPoint.y; }
+
+    float AABB2D::DistanceSq(const Vector2 &pt) const {
+        Vector2 cp = pt.Clamp(minPoint, maxPoint);
+        return cp.DistanceSq(pt);
+    }
+
+    void AABB2D::SetNegativeInfinity() {
+        minPoint = Vector2::Infinity;
+        maxPoint = -Vector2::Infinity;
+    }
+
+    void AABB2D::Enclose(const Vector2 &point) {
+        minPoint = Vector2::Min(minPoint, point);
+        maxPoint = Vector2::Max(maxPoint, point);
+    }
+
+    bool AABB2D::Intersects(const AABB2D &rhs) const {
+        return maxPoint.x >= rhs.minPoint.x &&
+               maxPoint.y >= rhs.minPoint.y &&
+               rhs.maxPoint.x >= minPoint.x &&
+               rhs.maxPoint.y >= minPoint.y;
+    }
+
+    bool AABB2D::Contains(const AABB2D &rhs) const {
+        return rhs.minPoint.x >= minPoint.x && rhs.minPoint.y >= minPoint.y
+               && rhs.maxPoint.x <= maxPoint.x && rhs.maxPoint.y <= maxPoint.y;
+    }
+
+    bool AABB2D::Contains(const Vector2 &pt) const {
+        return pt.x >= minPoint.x && pt.y >= minPoint.y
+               && pt.x <= maxPoint.x && pt.y <= maxPoint.y;
+    }
+
+    bool AABB2D::Contains(int x, int y) const {
+        return x >= minPoint.x && y >= minPoint.y
+               && x <= maxPoint.x && y <= maxPoint.y;
+    }
+
+    bool AABB2D::IsDegenerate() const {
+        return minPoint.x >= maxPoint.x || minPoint.y >= maxPoint.y;
+    }
+
+    bool AABB2D::HasNegativeVolume() const {
+        return maxPoint.x < minPoint.x || maxPoint.y < minPoint.y;
+    }
+
+    bool AABB2D::IsFinite() const {
+        return minPoint.IsFinite() && maxPoint.IsFinite() && minPoint.MinElement() > -1e5f && maxPoint.MaxElement() < 1e5f;
+    }
+
+    Vector2 AABB2D::PosInside(const Vector2 &normalizedPos) const {
+        return minPoint + normalizedPos.Mul(maxPoint - minPoint);
+    }
+
+    Vector2 AABB2D::ToNormalizedLocalSpace(const Vector2 &pt) const {
+        return (pt - minPoint).Div(maxPoint - minPoint);
+    }
+
+    AABB2D AABB2D::operator+(const Vector2 &pt) const {
+        AABB2D a;
+        a.minPoint = minPoint + pt;
+        a.maxPoint = maxPoint + pt;
+        return a;
+    }
+
+    AABB2D AABB2D::operator-(const Vector2 &pt) const {
+        AABB2D a;
+        a.minPoint = minPoint - pt;
+        a.maxPoint = maxPoint - pt;
+        return a;
+    }
+
+    Vector2 AABB2D::Centroid() {
+        return (minPoint + (maxPoint / 2.f));
+    }
+
+    Vector2 AABB2D::CornerPoint(int cornerIndex) {
+        assert(0 <= cornerIndex && cornerIndex <= 3);
+        switch(cornerIndex)
+        {
+            default:
+            case 0: return minPoint;
+            case 1: return {minPoint.x, maxPoint.y};
+            case 2: return {maxPoint.x, minPoint.y};
+            case 3: return maxPoint;
+        }
+    }
+}