Actually builds now (Sorry bout that)

reci bullshit

.gitea/workflows/buildtest.yml

@@ -1,10 +1,14 @@
-name: Run tests
-run-name: Run tests for ${{ gitea.repository }}.
+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!"
@@ -13,12 +17,8 @@ jobs:
         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 "Installing cmake"
-      - run: apt-get update && apt-get install -y cmake
-      - run: echo "Build ${{ gitea.repository }}"
-      - run: mkdir build
-      - run: cd build && cmake ../
-      - run: cd build && make
-      - run: echo "Build and run ${{ gitea.repository }} test suite"
-      - run: cd build && make test
+      - 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 }}."

CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.18)
+cmake_minimum_required(VERSION 3.18...3.28)
 PROJECT(J3ML
         VERSION 1.1
         LANGUAGES CXX
@@ -16,9 +16,7 @@ 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")
@@ -33,6 +31,16 @@ 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/Prerelease-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()
@@ -46,6 +54,8 @@ add_subdirectory(tests)
 add_executable(MathDemo main.cpp)
 target_link_libraries(MathDemo ${PROJECT_NAME})
 
+
+
 if(WIN32)
     #target_compile_options(MathDemo PRIVATE -mwindows)
 endif()

README.md

@@ -58,4 +58,6 @@ 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.
+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,45 @@
+/// @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; }
+
+    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;
+    }
+
+    Vector2 Bezier(float t, const Vector2& p0, const Vector2& p1, const Vector2& p2, const Vector2& p3);
+
+    // Tangent
+    Vector2 BezierDerivative(float t, const Vector2& p0, const Vector2& p1, const Vector2& p2, const Vector2& p3);
+
+    // Normal
+    Vector2 BezierNormal(float t, const Vector2& p0, const Vector2& p1, const Vector2& p2, const Vector2& p3);
+}

include/J3ML/Algorithm/Clock.hpp

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

include/J3ML/Algorithm/GJK.hpp

@@ -1,10 +1,11 @@
-// @file GJK.h
+// @file GJK.hpp
 /// Implementation of the Gilbert-Johnson-Keerthi (GJK) convex polyhedron intersection test
 
-#include <J3ML/LinearAlgebra.h>
-#include <J3ML/Geometry.h>
 #pragma once
 
+#include <J3ML/LinearAlgebra.hpp>
+#include <J3ML/Geometry.hpp>
+
 namespace J3ML::Algorithms
 {
 

include/J3ML/Algorithm/RNG.hpp

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "J3ML/J3ML.h"
+#include "J3ML/J3ML.hpp"
 
 namespace J3ML::Algorithm
 {

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

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

include/J3ML/Geometry.hpp

@@ -0,0 +1,26 @@
+#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/Shape.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.hpp

@@ -1,15 +1,38 @@
+/// 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/LinearAlgebra.h>
-#include <J3ML/Geometry/Common.h>
-#include <J3ML/Geometry/Shape.h>
-#include "J3ML/Algorithm/RNG.h"
+#include <J3ML/LinearAlgebra/Common.hpp>
+#include <J3ML/Geometry/Common.hpp>
+#include <J3ML/Geometry/Shape.hpp>
+
+#include "Polygon.hpp"
+#include "Sphere.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;
 
@@ -47,12 +70,25 @@ namespace J3ML::Geometry
             @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);
+        explicit AABB(const OBB &obb) {
+            SetFrom(obb);
+        }
 
         /// Constructs this AABB to enclose the given Sphere.
         /** @see class Sphere. */
-        explicit AABB(const Sphere &s);
+        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);
@@ -93,6 +129,7 @@ namespace J3ML::Geometry
 
         /// @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
@@ -184,6 +221,7 @@ namespace J3ML::Geometry
         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);
@@ -329,12 +367,27 @@ namespace J3ML::Geometry
         void Enclose(const LineSegment &lineSegment);
         void Enclose(const OBB &obb);
         void Enclose(const Sphere &sphere);
+
         void Enclose(const Triangle &triangle);
+
         void Enclose(const Capsule &capsule);
-        void Enclose(const Frustum &frustum);
-        void Enclose(const Polygon &polygon);
-        void Enclose(const Polyhedron &polyhedron);
-        void Enclose(const Vector3 *pointArray, int numPoints);
+
+        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;

include/J3ML/Geometry/AABB2D.hpp

@@ -1,7 +1,19 @@
+/// 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/Vector2.h>
-#include "Shape.h"
+#include <J3ML/LinearAlgebra/Vector2.hpp>
+#include "Shape.hpp"
 
 namespace J3ML::Geometry
 {
@@ -45,7 +57,6 @@ namespace J3ML::Geometry
 
         Vector2 ToNormalizedLocalSpace(const Vector2 &pt) const;
 
-
         AABB2D operator+(const Vector2& pt) const;
 
         AABB2D operator-(const Vector2& pt) const;

include/J3ML/Geometry/Capsule.hpp

@@ -1,21 +1,31 @@
+/// 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 "LineSegment.h"
-#include "Shape.h"
-#include <J3ML/LinearAlgebra.h>
-#include <J3ML/Geometry/Common.h>
+#include <J3ML/LinearAlgebra/Common.hpp>
+#include <J3ML/Geometry/Common.hpp>
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/Geometry/Circle.hpp>
 
 namespace J3ML::Geometry
 {
 
-
     /// A 3D cylinder with spherical ends.
     class Capsule : public Shape
     {
     public:
-        // Specifies the two inner points of this capsule
+        /// Specifies the two inner points of this capsule
         LineSegment l;
-        // Specifies the radius of this capsule
+        /// Specifies the radius of this capsule
         float r;
     public:
         /// The default constructor does not initialize any members of this class.
@@ -34,8 +44,17 @@ namespace J3ML::Geometry
             @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.
-        inline Vector3 AnyPointFast() const { return l.A; }
+        [[nodiscard]] inline 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.
@@ -46,10 +65,10 @@ namespace J3ML::Geometry
         Vector3 UniformPointPerhapsInside(float height, float x, float y) const;
 
         /// Returns the Sphere defining the 'bottom' section of this Capsule (corresponding to the endpoint l.a)
-        Sphere SphereA() const;
+        [[nodiscard]] Sphere SphereA() const;
 
         /// Returns the Sphere defining the 'top' section of this Capsule (corresponding to the endpoint l.b)
-        Sphere SphereB() const;
+        [[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,
@@ -57,18 +76,22 @@ namespace J3ML::Geometry
             @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. */
-        Vector3 ExtremePoint(const Vector3 &direction) const;
+        [[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. */
-        bool IsDegenerate() const;
+        [[nodiscard]] bool IsDegenerate() const;
+
         /// Computes the total height of this capsule, i.e. LineLength() + Diameter().
         /** <img src="CapsuleFunctions.png" />
             @see LineLength(). */
-        float Height() const;
+        [[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.
-        float Diameter() const;
+        [[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
@@ -76,41 +99,42 @@ namespace J3ML::Geometry
             @note The bottom-most point of the capsule is different than the point l.a. The returned point is the point at the very far
                 edge of this capsule, and does not lie on the internal line. See the attached diagram.
             @see Top(), l. */
-        Vector3 Bottom() const;
+        [[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(). */
-        Vector3 Center() const;
-        Vector3 Centroid() const; ///< [similarOverload: Center]
+
+        [[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. */
-        Vector3 UpDirection() const;
+        [[nodiscard]] Vector3 UpDirection() const;
 
         /// Computes the volume of this Capsule.
         /** @return pi * r^2 * |b-a| + 4 * pi * r^2 / 3.
             @see SurfaceArea(). */
-        float Volume() const;
+        [[nodiscard]] float Volume() const;
 
         /// Computes the surface area of this Capsule.
         /** @return 2 * pi * r * |b-a| + 4 * pi * r^2.
             @see Volume(). */
-        float SurfaceArea() const;
+        [[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;
+        Circle CrossSection(float l) const;
 
         Vector3 ExtremePoint(const Vector3& direction);
 
         /// Returns the smallest AABB that encloses this capsule.
         /** @see MinimalEnclosingOBB(). */
-        AABB MinimalEnclosingAABB() const;
+        [[nodiscard]] AABB MinimalEnclosingAABB() const;
 
         /// Returns the smallest OBB that encloses this capsule.
         /** @see MinimalEnclosingAABB(). */
@@ -132,7 +156,7 @@ namespace J3ML::Geometry
             @note The topmost point of the capsule is different than the point l.b. The returned point is the point at the very far
                 edge of this capsule, and does not lie on the internal line. See the attached diagram.
             @see Bottom(), l. */
-        Vector3 Top() const;
+        [[nodiscard]] Vector3 Top() const;
 
         /// Applies a transformation to this capsule.
         /** @param transform The transformation to apply to this capsule. This transformation must be

include/J3ML/Geometry/Circle.hpp

@@ -0,0 +1,163 @@
+/// 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/LinearAlgebra.hpp>
+#include <J3ML/Geometry/Common.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:
+        /// 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/Common.hpp

@@ -22,6 +22,9 @@ namespace J3ML::Geometry
     class Triangle;
     class Triangle2D;
     class TriangleMesh;
+
+    template <int N> class PBVolume;
+
 }
 
 // Methods required by Geometry types
@@ -36,4 +39,6 @@ namespace J3ML::Geometry
 
         bool operator==(const Interval& rhs) const = default;
     };
-}
+}
+
+using namespace J3ML::Geometry;

include/J3ML/Geometry/Frustum.hpp

@@ -3,10 +3,10 @@
 //
 #pragma once
 
-#include <J3ML/Geometry/Common.h>
-#include "Plane.h"
-#include "Shape.h"
-#include <J3ML/LinearAlgebra.h>
+#include <J3ML/Geometry/Common.hpp>
+#include "Plane.hpp"
+#include "Shape.hpp"
+#include <J3ML/LinearAlgebra.hpp>
 
 namespace J3ML::Geometry
 {
@@ -142,175 +142,250 @@ namespace J3ML::Geometry
         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.
-        */
+        /// The default constructor creates an uninitialized Frustum object.
+        /** This means that the values of the members type, projectiveSpace, handedness, pos, front, up, nearPlaneDistance, farPlaneDistance, horizontalFov/orthographicWidth and
+            verticalFov/orthographicHeight are all NaN after creating a new Frustum using this
+            default constructor. Remember to assign to them before use.
+            @note As an exception to other classes in MathGeoLib, this class initializes its members to NaNs, whereas the other classes leave the members uninitialized. This difference
+            is because the Frustum class implements a caching mechanism where world, projection and viewProj matrices are recomputed on demand, which does not work nicely together
+            if the defaults were uninitialized. */
         Frustum();
 
-        /// Quickly returns an arbitrary point inside this Frustum. Used in GJK intersection test.
-        inline Vector3 AnyPointFast() const { return CornerPoint(0); }
-
         static Frustum CreateFrustumFromCamera(const CoordinateFrame& cam, float aspect, float fovY, float zNear, float zFar);
+    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(). */
-        AABB MinimalEnclosingAABB() const;
+        [[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(). */
-        OBB MinimalEnclosingOBB() const;
+        [[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 nearPlaneDistance The z distance from the eye point to the position of the Frustum near clip plane. Always pass a positive value here.
-            @param farPlaneDistance The z distance from the eye point to the position of the Frustum far clip plane. Always pass a value that is larger than nearClipDistance.
+        /** @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 nearPlaneDistance, float 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 horizontalFov, float verticalFov);
+        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 orthographicWidth, float orthographicHeight);
+        void SetOrthographic(float w, float h);
+
         /// Returns the handedness of the projection formula used by this Frustum.
         /** @see SetKind(), FrustumHandedness. */
-        FrustumHandedness Handedness() const { return handedness; }
+        [[nodiscard]] FrustumHandedness Handedness() const { return handedness; }
         /// Returns the type of the projection formula used by this Frustum.
         /** @see SetPerspective(), SetOrthographic(), FrustumType. */
-        FrustumType Type() const { return type; }
+        [[nodiscard]] FrustumType Type() const { return type; }
         /// Returns the convention of the post-projective space used by this Frustum.
         /** @see SetKind(), FrustumProjectiveSpace. */
-        FrustumProjectiveSpace ProjectiveSpace() const { return projectiveSpace;}
+        [[nodiscard]] FrustumProjectiveSpace ProjectiveSpace() const { return projectiveSpace;}
         /// Returns the world-space position of this Frustum.
         /** @see SetPos(), Front(), Up(). */
-        const Vector3 &Pos() const {return pos;}
+        [[nodiscard]] const Vector3 &Pos() const {return pos;}
         /// Returns the world-space camera look-at direction of this Frustum.
         /** @see Pos(), SetFront(), Up(). */
-        const Vector3 &Front() const { return front; }
+        [[nodiscard]] const Vector3 &Front() const { return front; }
         /// Returns the world-space camera up direction of this Frustum.
         /** @see Pos(), Front(), SetUp(). */
-        const Vector3 &Up() const { return up; }
+        [[nodiscard]] const Vector3 &Up() const { return up; }
         /// Returns the distance from the Frustum eye to the near clip plane.
         /** @see SetViewPlaneDistances(), FarPlaneDistance(). */
-        float NearPlaneDistance() const { return nearPlaneDistance; }
+        [[nodiscard]] float NearPlaneDistance() const { return nearPlaneDistance; }
         /// Returns the distance from the Frustum eye to the far clip plane.
         /** @see SetViewPlaneDistances(), NearPlaneDistance(). */
-        float FarPlaneDistance() const { return farPlaneDistance;}
+        [[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(). */
-        float HorizontalFov() const { return horizontalFov;}
+        [[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(). */
-        float VerticalFov() const { return verticalFov;}
+        [[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(). */
-        float OrthographicWidth() const { return orthographicWidth; }
+        [[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(). */
-        float OrthograhpicHeight() const { return orthographicHeight; }
+        [[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. */
-        int NumEdges() const { return 12; }
+        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. */
-        float AspectRatio() const;
+        [[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 horizontalFov, float aspectRatio);
+        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 verticalFov, float aspectRatio);
+        void SetVerticalFovAndAspectRatio(float vFov, float aspectRatio);
 
-        Vector3 CornerPoint(int cornerIndex) const;
+        /// 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;
 
-        Vector3 NearPlanePos(float x, float y) const;
-        Vector3 FarPlanePos(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. */
-        Vector3 WorldRight() const;
+        [[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]
+
 
-        Plane TopPlane() const;     ///< [similarOverload: LeftPlane] [hideIndex]
-        Plane BottomPlane() const;  ///< [similarOverload: LeftPlane] [hideIndex]
-        Plane RightPlane() const;   ///< [similarOverload: LeftPlane] [hideIndex]
         /// Returns the plane equation of the specified side of this Frustum.
         /** The normal vector of the returned plane points outwards from the volume inside the frustum.
             This means the negative half-space of the Frustum is the space inside the Frustum.
             [indexTitle: Left/Right/Top/BottomPlane]
             @see NearPlane(), FarPlane(), GetPlane(), GetPlanes(). */
-        Plane LeftPlane() const;
+        [[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(). */
-        Plane FarPlane() const;
+        [[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(). */
-        Plane NearPlane() const;
+        [[nodiscard]] Plane NearPlane() const;
+
         /// Computes the width of the near plane quad in world space units.
         /** @see NearPlaneHeight(). */
-        float NearPlaneWidth() const;
+        [[nodiscard]] float NearPlaneWidth() const;
+
         /// Computes the height of the near plane quad in world space units.
         /** @see NearPlaneHeight(). */
-        float NearPlaneHeight() const;
+        [[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).
@@ -325,13 +400,56 @@ namespace J3ML::Geometry
         /** This function returns a Polyhedron representation of this Frustum. This conversion is exact, meaning that the returned
             Polyhedron represents exactly the same set of points that this Frustum does.
             @see MinimalEnclosingAABB(), MinimalEnclosingOBB(). */
-        Polyhedron ToPolyhedron() const;
+        [[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(). */
-        //PBVolume<6> ToPBVolume() const;
+        [[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.
@@ -339,21 +457,27 @@ namespace J3ML::Geometry
                 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;
+        [[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(). */
-        float Distance(const Vector3 &point) const;
+        [[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
@@ -362,18 +486,19 @@ namespace J3ML::Geometry
             The first parameter of this function specifies the other object to test against.
             @see Contains(), Distance(), ClosestPoint().
             @todo Add Intersects(Circle/Disc). */
-        bool Intersects(const Ray& ray) const;
-        //bool Intersects(const Line& line) const;
-        bool Intersects(const LineSegment& lineSegment) const;
-        bool Intersects(const AABB& aabb) const;
-        bool Intersects(const OBB& obb) const;
-        bool Intersects(const Plane& plane) const;
-        bool Intersects(const Triangle& triangle) const;
-        bool Intersects(const Polygon& lineSegment) const;
-        bool Intersects(const Sphere& aabb) const;
-        bool Intersects(const Capsule& obb) const;
-        bool Intersects(const Frustum& plane) const;
-        bool Intersects(const Polyhedron& triangle) const;
+        [[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.
@@ -387,9 +512,23 @@ namespace J3ML::Geometry
 
         Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
 
-        LineSegment Edge(int edgeIndex) const;
+        [[nodiscard]] LineSegment Edge(int edgeIndex) const;
 
-        bool Intersects(const Line &line) 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);

include/J3ML/Geometry/KDTree.h

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

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

@@ -1,7 +1,7 @@
 #pragma once
 
 
-#include "LineSegment.h"
+#include "LineSegment.hpp"
 
 namespace J3ML::Geometry
 {

include/J3ML/Geometry/LineSegment.h

@@ -1,92 +0,0 @@
-#pragma once
-
-#include <J3ML/LinearAlgebra/Vector3.h>
-#include "Plane.h"
-#include <J3ML/Geometry/Common.h>
-
-namespace J3ML::Geometry
-{
-
-    /// 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)); }
-
-    using LinearAlgebra::Vector3;
-    class LineSegment
-    {
-    public:
-        Vector3 A;
-        Vector3 B;
-    public:
-        LineSegment();
-        LineSegment(const Vector3& a, const Vector3& b);
-
-        /// 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) const;
-        bool Contains(const Vector3& point, float distanceThreshold = 1e-3f) const;
-
-
-        /// Quickly returns an arbitrary point inside this LineSegment. Used in GJK intersection test.
-        inline Vector3 AnyPointFast() const { return A; }
-
-        /// 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.*/
-        Vector3 ExtremePoint(const Vector3 &direction) const;
-        Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
-
-        void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
-        Vector3 GetPoint(float d) const;
-        Vector3 ClosestPoint(const Vector3 &point, float &d) const;
-        Vector3 ClosestPoint(const Ray &other, float &d, float &d2) const;
-        float Distance(const Vector3 &point) const;
-        float DistanceSq(const Vector3& point) const;
-        float Distance(const Vector3 &point, float &d) const;
-        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 Plane& other) const;
-
-        Vector3 Dir() const;
-
-        float Length() const;
-
-        float LengthSq() const;
-
-        float DistanceSq(const LineSegment &other) const;
-
-        Vector3 ClosestPoint(const LineSegment &other, float &d, float &d2) const;
-
-        Vector3 ClosestPoint(const Line &other, float &d, float &d2) const;
-
-        bool Intersects(const LineSegment &segment) const;
-    };
-
-    LineSegment operator *(const Matrix4x4 &transform, const LineSegment &l);
-}

include/J3ML/Geometry/LineSegment.hpp

@@ -0,0 +1,246 @@
+#pragma once
+
+#include <J3ML/LinearAlgebra/Common.hpp>
+#include <J3ML/Geometry/Common.hpp>
+#include <J3ML/LinearAlgebra/Vector3.hpp>
+
+namespace J3ML::Geometry
+{
+
+    using LinearAlgebra::Vector3;
+
+    /// A line segment in 3D space is a finite line with a start and end point.
+    class LineSegment
+    {
+    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/OBB.hpp

@@ -1,8 +1,8 @@
 #pragma once
 
-#include <J3ML/Geometry/Common.h>
-#include <J3ML/Geometry/AABB.h>
-#include <J3ML/Geometry/Polyhedron.h>
+#include <J3ML/Geometry/Common.hpp>
+#include <J3ML/Geometry/AABB.hpp>
+#include <J3ML/Geometry/Polyhedron.hpp>
 
 
 namespace J3ML::Geometry {
@@ -108,5 +108,30 @@ namespace J3ML::Geometry {
         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,245 @@
+/// 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 "Sphere.hpp"
+#include "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.hpp

@@ -1,21 +1,24 @@
 #pragma once
-#include <J3ML/LinearAlgebra/Vector3.h>
-#include "Shape.h"
-#include "Ray.h"
+#include <J3ML/LinearAlgebra/Vector3.hpp>
+#include "Shape.hpp"
+#include "Ray.hpp"
 
 
 namespace J3ML::Geometry
 {
     using J3ML::LinearAlgebra::Vector3;
 
-    class Plane : public Shape
+    class Plane
     {
     public:
         Vector3 Position;
         Vector3 Normal;
         float distance = 0.f;
     public:
-        Plane() : Shape() {}
+        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);
@@ -82,5 +85,22 @@ namespace J3ML::Geometry
         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.hpp

@@ -1,9 +1,20 @@
+/// 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/Common.h>
+#include <J3ML/Geometry/Common.hpp>
 #include <vector>
-#include "Shape.h"
-#include "J3ML/LinearAlgebra.h"
+#include "Shape.hpp"
+#include <J3ML/LinearAlgebra/Common.hpp>
 
 namespace J3ML::Geometry {
 
@@ -13,7 +24,7 @@ namespace J3ML::Geometry {
         std::vector<Vector3> vertices;
 
         /// Quickly returns an arbitrary point inside this AABB. Used in GJK intersection test.
-        Vector3 AnyPointFast() const { return !vertices.empty() ? vertices[0] : Vector3::NaN; }
+        Vector3 AnyPointFast() const;
 
         AABB MinimalEnclosingAABB() const;
         int NumVertices() const;
@@ -80,6 +91,14 @@ namespace J3ML::Geometry {
         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,111 +0,0 @@
-#pragma once
-
-#include <J3ML/Geometry/Common.h>
-#include <J3ML/Geometry/Shape.h>
-#include <vector>
-#include <J3ML/LinearAlgebra/Vector3.h>
-
-
-namespace J3ML::Geometry
-{
-    using namespace J3ML::LinearAlgebra;
-
-    // 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;
-
-        int NumVertices() const {return (int)v.size();}
-        int NumFaces() const { return (int)f.size();}
-        AABB MinimalEnclosingAABB() const;
-
-        Vector3 Vertex(int vertexIndex) const;
-
-
-
-        bool Contains(const Vector3&) const;
-        bool Contains(const LineSegment&) const;
-        bool Contains(const Triangle&) const;
-        bool Contains(const Polygon&) const;
-        bool Contains(const AABB&) const;
-        bool Contains(const OBB&) const;
-        bool Contains(const Frustum&) const;
-        bool Contains(const Polyhedron&) const;
-
-        bool ContainsConvex(const Vector3&, float epsilon = 1e-4f) const;
-        bool ContainsConvex(const LineSegment&) const;
-        bool ContainsConvex(const Triangle&) const;
-
-        bool Intersects(const Line&) const;
-        bool Intersects(const LineSegment&) const;
-        bool Intersects(const Ray&) const;
-        bool Intersects(const Plane&) const;
-        bool Intersects(const Polyhedron&) const;
-        bool Intersects(const AABB&) const;
-        bool Intersects(const OBB&) const;
-        bool Intersects(const Triangle&) const;
-        bool Intersects(const Polygon&) const;
-        bool Intersects(const Frustum&) const;
-        bool Intersects(const Sphere&) const;
-        bool Intersects(const Capsule& capsule) const;
-
-        bool IsClosed() const;
-
-        Plane FacePlane(int faceIndex) const;
-
-        std::vector<Polygon> Faces() const;
-
-        int NumEdges() const;
-
-        LineSegment Edge(int edgeIndex) const;
-
-        std::vector<std::pair<int, int>> EdgeIndices() const;
-
-        std::vector<LineSegment> Edges() const;
-
-        Polygon FacePolygon(int faceIndex) const;
-
-        Vector3 FaceNormal(int faceIndex) const;
-
-        bool IsConvex() const;
-
-        Vector3 ApproximateConvexCentroid() const;
-
-        int ExtremeVertex(const Vector3 &direction) const;
-
-        Vector3 ExtremePoint(const Vector3 &direction) const;
-
-        /// Tests if the given face of this Polyhedron contains the given point.
-        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.
-        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;
-    protected:
-    private:
-    };
-}

include/J3ML/Geometry/Polyhedron.hpp

@@ -0,0 +1,134 @@
+#pragma once
+
+#include <J3ML/Geometry/Common.hpp>
+#include <J3ML/Geometry/Shape.hpp>
+#include <vector>
+#include <J3ML/LinearAlgebra/Vector3.hpp>
+
+
+namespace J3ML::Geometry
+{
+    using namespace J3ML::LinearAlgebra;
+
+    // Represents a three-dimensional closed geometric solid defined by flat polygonal faces.
+    class Polyhedron
+    {
+    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,8 +2,8 @@
 
 #include <vector>
 #include <cstdint>
-#include <J3ML/LinearAlgebra/Vector2.h>
-#include "AABB2D.h"
+#include <J3ML/LinearAlgebra/Vector2.hpp>
+#include <J3ML/Geometry/AABB2D.hpp>
 
 namespace J3ML::Geometry {
 

include/J3ML/Geometry/Ray.h

@@ -1,67 +0,0 @@
-//
-// Created by dawsh on 1/25/24.
-//
-
-#pragma once
-
-#include <J3ML/LinearAlgebra/Vector3.h>
-#include <vector>
-#include "TriangleMesh.h"
-#include "Frustum.h"
-#include "OBB.h"
-
-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, nullptr};}
-    };
-
-    // A ray in 3D space is a line that starts from an origin point and extends to infinity in one direction
-    class Ray
-    {
-    public:
-        // 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;
-        Ray() {}
-        Ray(const Vector3& pos, const Vector3& dir);
-        //explicit Ray(const Line& line);
-        explicit Ray(const LineSegment& lineSegment);
-        bool IsFinite() const;
-        Vector3 GetPoint(float distance) 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;
-
-        Vector3 ClosestPoint(const LineSegment&, float &d, float &d2) const;
-
-        Vector3 ClosestPoint(const Vector3 &targetPoint, float &d) const;
-    };
-}

include/J3ML/Geometry/Ray.hpp

@@ -0,0 +1,167 @@
+/// 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 <J3ML/LinearAlgebra/Vector3.hpp>
+#include <vector>
+#include "TriangleMesh.hpp"
+#include "Frustum.hpp"
+#include "OBB.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, nullptr};}
+    };
+
+    /// A ray in 3D space is a line that starts from an origin point and extends to infinity in one direction
+    class Ray
+    {
+    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 {
+            return std::format("Ray(origin:[{}], direction:[{}])", Origin.ToString(), Direction.ToString());
+        }
+    };
+
+    inline std::ostream& operator << (std::ostream& o, const Ray& ray) {
+        o << ray.ToString();
+        return o;
+    }
+}

include/J3ML/Geometry/Shape.hpp

@@ -1,6 +1,5 @@
 #pragma once
 
-
 namespace J3ML::Geometry
 {
     class GeometricPrimitive

include/J3ML/Geometry/Sphere.h

@@ -1,108 +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>
-#include <J3ML/Geometry/Shape.h>
-
-namespace J3ML::Geometry
-{
-    using J3ML::LinearAlgebra::Matrix3x3;
-    using J3ML::LinearAlgebra::Matrix4x4;
-
-    // A mathematical representation of a 3-dimensional sphere
-    class Sphere : public Shape
-    {
-    public:
-        Vector3 Position;
-        float Radius;
-    public:
-        Sphere() {}
-        Sphere(const Vector3& pos, float radius) : Position(pos), Radius(radius) {}
-
-        /// Generates a random point on the surface of this sphere
-        /** The points are distributed uniformly.
-            This function uses the rejection method to generate a uniform distribution of points on the surface.
-            Therefore, it is assumed that this sphere is not degenerate, i.e. it has a positive radius.
-            A fixed number of 1000 tries is performed, after which a fixed point on the surface is returned as a fallback.
-            @param rng A pre-seeded random number generator object that is to be used by this function to generate random vlaues.
-            @see class RNG, RandomPointInside(), IsDegenerate()
-            @todo Add Sphere::PointOnSurface(polarYaw, polarPitch). */
-        Vector3 RandomPointOnSurface(RNG& rng) const;
-        static Vector3 RandomPointOnSurface(RNG& rng, const Vector3& center, float radius);
-
-        /// Generates a random point inside this sphere.
-        /** The points are distributed uniformly.
-            This function uses the rejection method to generate a uniform distribution of points inside a sphere.
-            Therefore, it is assumed that this sphere is not degenerate, i.e. it has a positive radius.
-            A fixed number of 1000 tries is performed, after which the sphere center position is returned as a fallback.
-            @param rng A pre-seeded random number generator object that is to be used by this function to generate random values.
-            @see class RNG, RandomPointOnSurface(), IsDegenerate().
-            @todo Add Sphere::Point(polarYaw, polarPitch, radius). */
-        Vector3 RandomPointInside(RNG& rng);
-        static Vector3 RandomPointInside(RNG& rng, const Vector3& center, float radius);
-    public:
-        /// Quickly returns an arbitrary point inside this Sphere. Used in GJK intersection test.
-        Vector3 AnyPointFast() const { return Position; }
-        /// Computes the extreme point of this Sphere in the given direction.
-        /** 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. */
-        Vector3 ExtremePoint(const Vector3 &direction) const;
-        Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
-
-
-        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 {}
-
-        void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
-    };
-}

include/J3ML/Geometry/Sphere.hpp

@@ -0,0 +1,127 @@
+/// 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.hpp>
+#include <J3ML/LinearAlgebra/Matrix3x3.hpp>
+#include <J3ML/LinearAlgebra/Matrix4x4.hpp>
+#include <J3ML/Geometry/LineSegment.hpp>
+#include <J3ML/Geometry/TriangleMesh.hpp>
+#include <J3ML/Geometry/Shape.hpp>
+
+namespace J3ML::Geometry
+{
+    using J3ML::LinearAlgebra::Matrix3x3;
+    using J3ML::LinearAlgebra::Matrix4x4;
+
+    // A mathematical representation of a 3-dimensional sphere
+    class Sphere
+    {
+    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() const {}
+        TriangleMesh GenerateIcososphere() const {}
+
+        void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
+    };
+}

include/J3ML/Geometry/Triangle.hpp

@@ -1,8 +1,8 @@
 #pragma once
 
-#include "J3ML/LinearAlgebra/Vector3.h"
-#include <J3ML/Geometry/Common.h>
-#include <J3ML/LinearAlgebra.h>
+#include "J3ML/LinearAlgebra/Vector3.hpp"
+#include <J3ML/Geometry/Common.hpp>
+#include <J3ML/LinearAlgebra.hpp>
 #include <cfloat>
 
 namespace J3ML::Geometry
@@ -14,6 +14,13 @@ namespace J3ML::Geometry
         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
@@ -28,6 +35,10 @@ namespace J3ML::Geometry
 
         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.
@@ -37,6 +48,9 @@ namespace J3ML::Geometry
         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;

include/J3ML/Geometry/Triangle2D.hpp

@@ -5,5 +5,8 @@ namespace J3ML::Geometry
 {
     class Triangle2D {
     public:
+        Vector2 A;
+        Vector2 B;
+        Vector2 C;
     };
 }

include/J3ML/Geometry/TriangleMesh.h

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

include/J3ML/Geometry/TriangleMesh.hpp

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <vector>
+#include <J3ML/LinearAlgebra/Common.hpp>
+#include <J3ML/Geometry/Common.hpp>
+#include "J3ML/J3ML.hpp"
+
+
+using namespace J3ML::LinearAlgebra;
+
+namespace J3ML::Geometry
+{
+
+    class TriangleMesh
+    {
+    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();
+    private:
+        std::vector<float> cachedVertexList;
+        //std::vector<Triangle> cachedTriangleList;
+    };
+
+}

include/J3ML/J3ML.h

@@ -1,253 +0,0 @@
-#pragma once
-
-//
-// Created by josh on 12/25/2023.
-//
-#include <cstdint>
-#include <cmath>
-#include <string>
-#include <cassert>
-
-// TODO: Move to separate math lib, find duplicates!
-template <typename T>
-void Swap(T &a, T &b)
-{
-    T temp = std::move(a);
-    a = std::move(b);
-    b = std::move(temp);
-}
-
-namespace J3ML::SizedIntegralTypes
-{
-    using u8 = uint8_t;
-    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;
-}
-
-
-
-namespace J3ML::SizedFloatTypes
-{
-    using f32 = float;
-    using f64 = double;
-    using f128 = long double;
-
-}
-
-using namespace J3ML::SizedIntegralTypes;
-using namespace J3ML::SizedFloatTypes;
-
-//On windows there is no shorthand for pi???? - Redacted.
-#ifdef _WIN32
-#define M_PI 3.14159265358979323846
-#endif
-
-namespace J3ML::Math
-{
-
-    bool EqualAbs(float a, float b, float epsilon = 1e-3f);
-    float RecipFast(float x);
-
-
-    // Coming soon: Units Namespace
-    // For Dimensional Analysis
-    /*
-    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,377 @@
+/// 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>
+
+/// TODO: Implement lookup tables.
+
+#ifdef USE_LOOKUP_TABLES
+
+static float fast_cossin_table[MAX_CIRCLE_ANGLE];
+
+#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);
+}
+
+/// Clean symbolic names for integers of specific size.
+namespace J3ML::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;
+}
+
+namespace J3ML::SizedFloatTypes
+{
+    // TODO: Use C++23 <stdfloat>
+    using f16  = float;
+    using f32 = float;
+    using f64 = double;
+    using f128 = long double;
+
+}
+
+using namespace J3ML::SizedIntegralTypes;
+using namespace J3ML::SizedFloatTypes;
+
+namespace J3ML::Math::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);
+}
+
+
+
+// TODO: Implement "Wrappers" for most standard math functions.
+// We want to later-on implement lookup tables and SSE as conditional macros.
+
+namespace J3ML::Math::Constants {
+    /// sqrt(2pi) ^ -1
+    constexpr float RecipSqrt2Pi = 0.3989422804014326779399460599343818684758586311649346576659258296706579258993018385012523339073069364;
+    /// pi - https://www.mathsisfun.com/numbers/pi.html
+    constexpr float Pi = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679;
+    /// 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;
+}
+
+/// 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)
+
+namespace J3ML::Math::Functions {
+
+    /// 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; }
+
+
+
+    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.
+
+    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 Interp
+    {
+        inline float SmoothStart(float t);
+    }
+
+
+
+}
+
+namespace J3ML::Math {
+    using namespace Math::Constants;
+    using namespace Math::Functions;
+
+
+    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/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,4 +1,5 @@
 #pragma once
+#include "J3ML/J3ML.hpp"
 
 
 namespace J3ML::LinearAlgebra {
@@ -19,4 +20,4 @@ namespace J3ML::LinearAlgebra {
         }
     };
 
-}
+}

include/J3ML/LinearAlgebra/AxisAngle.hpp

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

include/J3ML/LinearAlgebra/Common.hpp

@@ -19,10 +19,15 @@ namespace J3ML::LinearAlgebra
 
 
     using Position = Vector3;
+
+    template <int N> class PBVolume;
 }
 
 // Methods required by LinearAlgebra types
 namespace J3ML::LinearAlgebra
 {
 
-}
+}
+
+
+using namespace J3ML::LinearAlgebra;

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/EulerAngle.hpp

@@ -1,8 +1,8 @@
 #pragma once
 
-#include <J3ML/LinearAlgebra/Vector3.h>
-#include <J3ML/LinearAlgebra/Quaternion.h>
-#include <J3ML/LinearAlgebra/AxisAngle.h>
+#include <J3ML/LinearAlgebra/Vector3.hpp>
+#include <J3ML/LinearAlgebra/Quaternion.hpp>
+#include <J3ML/LinearAlgebra/AxisAngle.hpp>
 
 namespace J3ML::LinearAlgebra {
 
@@ -18,6 +18,8 @@ public:
 
     AxisAngle ToAxisAngle() const;
 
+    [[nodiscard]] Quaternion ToQuaternion() const;
+
 
     explicit EulerAngle(const Quaternion& rhs);
     explicit EulerAngle(const AxisAngle& rhs);

include/J3ML/LinearAlgebra/Matrices.inl

@@ -2,12 +2,144 @@
 
 /// Template Parameterized (Generic) Matrix Functions.
 
-#include <J3ML/LinearAlgebra/Quaternion.h>
-
+#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.
@@ -16,8 +148,8 @@ namespace J3ML::LinearAlgebra {
     template<typename Matrix>
     void Set3x3PartRotateX(Matrix &m, float angle) {
         float sinz, cosz;
-        sinz = std::sin(angle);
-        cosz = std::cos(angle);
+        sinz = Math::Sin(angle);
+        cosz = Math::Cos(angle);
 
         m[0][0] = 1.f;
         m[0][1] = 0.f;
@@ -38,8 +170,8 @@ namespace J3ML::LinearAlgebra {
     template<typename Matrix>
     void Set3x3PartRotateY(Matrix &m, float angle) {
         float sinz, cosz;
-        sinz = std::sin(angle);
-        cosz = std::cos(angle);
+        sinz = Math::Sin(angle);
+        cosz = Math::Cos(angle);
 
         m[0][0] = cosz;
         m[0][1] = 0.f;

include/J3ML/LinearAlgebra/Matrix.hpp

@@ -1,9 +1,21 @@
+/// 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.h"
+#include "Vector.hpp"
 
 namespace J3ML::LinearAlgebra
 {

include/J3ML/LinearAlgebra/Matrix2x2.hpp

@@ -1,9 +1,21 @@
+/// 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/Vector2.h>
-#include <J3ML/LinearAlgebra/Common.h>
-#include <J3ML/LinearAlgebra/Matrices.inl>
+
+#include <J3ML/LinearAlgebra/Common.hpp>
+
 
 namespace J3ML::LinearAlgebra {
     class Matrix2x2 {

include/J3ML/LinearAlgebra/Matrix3x3.hpp

@@ -1,11 +1,12 @@
 #pragma once
 
 
-#include <J3ML/LinearAlgebra/Common.h>
-#include <J3ML/LinearAlgebra/Vector2.h>
-#include <J3ML/LinearAlgebra/Vector3.h>
-#include <J3ML/LinearAlgebra/Quaternion.h>
-#include <J3ML/Algorithm/RNG.h>
+
+#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;
 
@@ -13,25 +14,7 @@ using namespace J3ML::Algorithm;
 
 namespace J3ML::LinearAlgebra {
 
-    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);
-    }
 
     /// A 3-by-3 matrix for linear transformations of 3D geometry.
     /* This can represent any kind of linear transformations of 3D geometry, which include
@@ -79,6 +62,9 @@ namespace J3ML::LinearAlgebra {
         Matrix3x3(const Vector3& col0, const Vector3& col1, const Vector3& col2);
         /// Constructs this matrix3x3 from the given quaternion.
         explicit Matrix3x3(const Quaternion& orientation);
+        /// Constructs this matrix3x3 from the given euler angle.
+        explicit Matrix3x3(const EulerAngle& 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.
@@ -261,6 +247,8 @@ namespace J3ML::LinearAlgebra {
         /// matrix, and there is scaling ,shearing, or mirroring in this matrix)
         bool TryConvertToQuat(Quaternion& q) const;
 
+        /// Converts this rotation matrix to an Euler Angle.
+        [[nodiscard]] EulerAngle ToEulerAngle() const;
 
         /// Returns the main diagonal.
         /// The main diagonal consists of the elements at m[0][0], m[1][1], m[2][2]
@@ -469,8 +457,6 @@ namespace J3ML::LinearAlgebra {
             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.

include/J3ML/LinearAlgebra/Matrix4x4.hpp

@@ -1,12 +1,8 @@
 #pragma once
 
-#include <J3ML/LinearAlgebra/Common.h>
 
-#include <J3ML/LinearAlgebra/Matrix3x3.h>
-#include <J3ML/LinearAlgebra/Quaternion.h>
-#include <J3ML/LinearAlgebra/Vector4.h>
-#include <J3ML/LinearAlgebra/Matrices.inl>
-#include <J3ML/Algorithm/RNG.h>
+#include <J3ML/LinearAlgebra/Common.hpp>
+#include <J3ML/Algorithm/RNG.hpp>
 
 #include <algorithm>
 
@@ -14,115 +10,6 @@ using namespace J3ML::Algorithm;
 
 namespace J3ML::LinearAlgebra {
 
-    template <typename Matrix>
-    bool InverseMatrix(Matrix &mat, float epsilon)
-    {
-        Matrix inversed = Matrix::Identity;
-
-        const int nc = std::min<int>(Matrix::Rows, Matrix::Cols);
-
-        for (int column = 0; column < nc; ++column)
-        {
-            // find the row i with i >= j such that M has the largest absolute value.
-            int greatest = column;
-            float greatestVal = std::abs(mat[greatest][column]);
-            for (int i = column+1; i < Matrix::Rows; i++)
-            {
-                float val = std::abs(mat[i][column]);
-                if (val > greatestVal) {
-                    greatest = i;
-                    greatestVal = val;
-                }
-            }
-
-            if (greatestVal < epsilon) {
-                mat = inversed;
-                return false;
-            }
-
-            // exchange rows
-            if (greatest != column) {
-                inversed.SwapRows(greatest, column);
-                mat.SwapRows(greatest, column);
-            }
-
-            // multiply rows
-            assert(!Math::EqualAbs(mat[column][column], 0.f, epsilon));
-            float scale = 1.f / mat[column][column];
-            inversed.ScaleRow(column, scale);
-            mat.ScaleRow(column, scale);
-
-            // add rows
-            for (int i = 0; i < column; i++) {
-                inversed.SetRow(i, inversed.Row(i) - inversed.Row(column) * mat[i][column]);
-                mat.SetRow(i, mat.Row(i) - mat.Row(column) * mat[i][column]);
-            }
-
-            for (int i = column + 1; i < Matrix::Rows; i++) {
-                inversed.SetRow(i, inversed.Row(i) - inversed.Row(column) * mat[i][column]);
-                mat.SetRow(i, mat.Row(i) - mat.Row(column) * mat[i][column]);
-            }
-        }
-        mat = inversed;
-        return true;
-    }
-
-
-    /// Computes the LU-decomposition on the given square matrix.
-    /// @return True if the composition was successful, false otherwise. If the return value is false, the contents of the output matrix are unspecified.
-    template <typename Matrix>
-    bool LUDecomposeMatrix(const Matrix &mat, Matrix &lower, Matrix &upper)
-    {
-        lower = Matrix::Identity;
-        upper = Matrix::Zero;
-
-        for (int i = 0; i < Matrix::Rows; ++i)
-        {
-            for (int col = i; col < Matrix::Cols; ++col)
-            {
-                upper[i][col] = mat[i][col];
-                for (int k = 0; k < i; ++k)
-                    upper[i][col] -= lower[i][k] * upper[k][col];
-            }
-            for (int row = i+1; row < Matrix::Rows; ++row)
-            {
-                lower[row][i] = mat[row][i];
-                for (int k = 0; k < i; ++k)
-                    lower[row][i] -= lower[row][k] * upper[k][i];
-                if (Math::EqualAbs(upper[i][i], 0.f))
-                    return false;
-                lower[row][i] /= upper[i][i];
-            }
-        }
-        return true;
-    }
-
-    /// Computes the Cholesky decomposition on the given square matrix *on the real domain*.
-    /// @return True if successful, false otherwise. If the return value is false, the contents of the output matrix are uspecified.
-    template <typename Matrix>
-    bool CholeskyDecomposeMatrix(const Matrix &mat, Matrix& lower)
-    {
-        lower = Matrix::Zero;
-        for (int i = 0; i < Matrix::Rows; ++i)
-        {
-            for (int j = 0; j < i; ++i)
-            {
-                lower[i][j] = mat[i][j];
-                for (int k = 0; k < j; ++k)
-                    lower[i][j] -= lower[i][j] * lower[j][k];
-                if (Math::EqualAbs(lower[j][j], 0.f))
-                    return false;
-                lower[i][j] /= lower[j][j];
-            }
-            lower[i][i] = mat[i][i];
-            if (lower[i][i])
-                return false;
-            for (int k = 0; k < i; ++k)
-                lower[i][i] -= lower[i][k] * lower[i][k];
-            lower[i][i] = std::sqrt(lower[i][i]);
-        }
-    }
-
     /// @brief A 4-by-4 matrix for affine transformations and perspective projections of 3D geometry.
     /// This matrix can represent the most generic form of transformations for 3D objects,
     /// including perspective projections, which a 4-by-3 cannot store, and translations, which a 3-by-3 cannot represent.
@@ -184,6 +71,8 @@ namespace J3ML::LinearAlgebra {
 
         /// Constructs this Matrix4x4 from the given quaternion.
         explicit Matrix4x4(const Quaternion& orientation);
+        /// Constructs this Matrix4x4 from the given Euler Angle.
+        explicit Matrix4x4(const EulerAngle& orientation);
 
         /// Constructs this float4x4 from the given quaternion and translation.
         /// Logically, the translation occurs after the rotation has been performed.
@@ -343,6 +232,10 @@ namespace J3ML::LinearAlgebra {
         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.
@@ -350,9 +243,9 @@ namespace J3ML::LinearAlgebra {
             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;
+        [[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).
-        Matrix3x3 GetRotatePart() const;
+        [[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. */
@@ -398,35 +291,36 @@ namespace J3ML::LinearAlgebra {
 
         /// Returns the given row.
         /** @param The zero-based index [0, 3] of the row to get */
-        Vector4 GetRow(int row) const;
-        Vector4 Row(int row) const;
+        [[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.
-        Vector3 GetRow3(int index) const;
-        Vector3 Row3(int i) const;
+        [[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. */
-        Vector4 GetColumn(int index) const;
-        Vector4 Column(int index) const;
-        Vector4 Col(int i) const;
+        [[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.
-        Vector3 GetColumn3(int index) const;
-        Vector3 Column3(int index) const { return GetColumn3(index);}
-        Vector3 Col3(int i) const;
+        [[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();
-        Vector3 GetScale() const;
+        [[nodiscard]] Vector3 GetScale() const;
 
 
         float &At(int row, int col);
-        float At(int x, int y) const;
+        [[nodiscard]] float At(int x, int y) const;
 
 
         void SwapColumns(int col1, int col2);
@@ -447,35 +341,48 @@ namespace J3ML::LinearAlgebra {
         /** @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;
 
-        Vector4 Diagonal() const;
-        Vector3 Diagonal3() 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.
-        Vector3 WorldX() const;
+        [[nodiscard]] Vector3 WorldX() const;
         /// Returns the local +Y axis in world space.
         /// This is the same as transforming the vector (0,1,0) by this matrix.
-        Vector3 WorldY() const;
+        [[nodiscard]] Vector3 WorldY() const;
         /// Returns the local +Z axis in world space.
         /// This is the same as transforming the vector (0,0,1) by this matrix.
-        Vector3 WorldZ() const;
+        [[nodiscard]] Vector3 WorldZ() const;
 
         /// 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]; }
+        [[nodiscard]] const float *ptr() const { return &elems[0][0]; }
 
-        float Determinant3x3() const;
+        /// 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.
-        float Determinant() const;
+        [[nodiscard]] float Determinant() const;
 
-        #define SKIPNUM(val, skip) (val >= skip ? (val+1) : val)
+        /// 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;
 
@@ -539,11 +446,7 @@ namespace J3ML::LinearAlgebra {
         /// @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0 0 0 1]).
         /// @note This function assumes that this matrix has orthogonal basis vectors (row and column vector sets are orthogonal).
         /// @note This function does not remove reflection (-1 scale along some axis).
-        void RemoveScale()
-        {
-            float tx = Row3(0).Normalize();
-            float ty = Row3(1).Normalize();
-        }
+        void RemoveScale();
 
 
         /// Decomposes this matrix to translate, rotate, and scale parts.
@@ -662,6 +565,9 @@ namespace J3ML::LinearAlgebra {
         /// Returns the sum of the diagonal elements of this matrix.
         float Trace() const;
 
+        [[nodiscard]] Quaternion ToQuat() const;
+
+        [[nodiscard]] EulerAngle ToEulerAngle() 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;

include/J3ML/LinearAlgebra/Quaternion.hpp

@@ -1,11 +1,7 @@
 #pragma once
 
-#include <J3ML/LinearAlgebra/Common.h>
-#include <J3ML/LinearAlgebra/Matrix3x3.h>
-#include <J3ML/LinearAlgebra/Matrix4x4.h>
-#include <J3ML/LinearAlgebra/Vector4.h>
-#include <J3ML/LinearAlgebra/AxisAngle.h>
-#include <J3ML/Algorithm/RNG.h>
+#include <J3ML/LinearAlgebra/Common.hpp>
+#include <J3ML/Algorithm/RNG.hpp>
 #include <cmath>
 
 namespace J3ML::LinearAlgebra
@@ -45,7 +41,7 @@ namespace J3ML::LinearAlgebra
         Quaternion(const Vector3 &rotationAxis, float rotationAngleRadians);
         Quaternion(const Vector4 &rotationAxis, float rotationAngleRadians);
 
-        explicit Quaternion(Vector4 vector4);
+        explicit Quaternion(const Vector4& vector4);
         explicit Quaternion(const EulerAngle& angle);
         explicit Quaternion(const AxisAngle& angle);
 
@@ -104,7 +100,7 @@ namespace J3ML::LinearAlgebra
 
 
         /// Returns a uniformly random unitary quaternion.
-        static Quaternion RandomRotation(J3ML::Algorithm::RNG &rng);
+        static Quaternion RandomRotation(RNG &rng);
     public:
         void SetFromAxisAngle(const Vector3 &vector3, float between);
 
@@ -216,19 +212,23 @@ namespace J3ML::LinearAlgebra
         // 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 / (float scalar) const
-        {
-            assert(!Math::EqualAbs(scalar, 0.f));
-        }
+
+
+
         Quaternion operator + () const;
         Quaternion operator - () const;
 

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/Vector2.hpp

@@ -1,6 +1,8 @@
 #pragma once
-#include <J3ML/J3ML.h>
+
+
 #include <cstddef>
+#include <J3ML/Algorithm/RNG.hpp>
 
 namespace J3ML::LinearAlgebra {
     using namespace J3ML;
@@ -105,6 +107,16 @@ namespace J3ML::LinearAlgebra {
         [[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;
@@ -227,8 +239,8 @@ namespace J3ML::LinearAlgebra {
         /// Returns a normalized copy of this vector.
         /** @note If the vector is zero and cannot be normalized, the vector (1, 0) is returned, and an error message is printed.
             If you do not want to generate an error message on failure, but want to handle the failure yourself, use the
-            Normalize() function instead.
-            @see Normalize() */
+            Normalized() function instead.
+            @see Normalized() */
         [[nodiscard]] Vector2 Normalized() const;
 
         /// Linearly interpolates between two points.
@@ -368,7 +380,9 @@ namespace J3ML::LinearAlgebra {
             @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);
     };
 
     Vector2 operator*(float lhs, const Vector2 &rhs);
-}
+}

include/J3ML/LinearAlgebra/Vector2i.hpp

@@ -0,0 +1,11 @@
+#pragma once
+
+namespace J3ML::LinearAlgebra
+{
+    class Vector2i
+    {
+    public:
+        int x;
+        int y;
+    };
+}

include/J3ML/LinearAlgebra/Vector3.hpp

@@ -1,11 +1,11 @@
 #pragma once
 
-#include <J3ML/LinearAlgebra/Vector2.h>
-#include <J3ML/LinearAlgebra/Vector3.h>
+#include <J3ML/LinearAlgebra/Vector2.hpp>
+#include <J3ML/LinearAlgebra/Vector3.hpp>
 #include <cstddef>
 #include <cstdlib>
-#include <J3ML/LinearAlgebra/Angle2D.h>
-#include <J3ML/Algorithm/RNG.h>
+#include <J3ML/LinearAlgebra/Angle2D.hpp>
+#include <J3ML/Algorithm/RNG.hpp>
 
 
 using namespace J3ML::Algorithm;
@@ -113,6 +113,7 @@ public:
 
     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:
@@ -143,7 +144,7 @@ public:
         @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 [](). */
-    float At(int index) const;
+    [[nodiscard]] float At(int index) const;
     float &At(int index);
 
     /// Makes the given vectors linearly independent and normalized in length.
@@ -160,36 +161,54 @@ public:
     static bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon = 1e-3f);
     static bool AreOrthonormal(const Vector3& a, const Vector3& b, const Vector3& c, float epsilon = 1e-3f);
 
-    Vector3 Abs() const;
+    [[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;
 
-    Vector3 ProjectToNorm(const Vector3& direction) const;
-
-    float GetX() const;
-    float GetY() const;
-    float GetZ() 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);
 
-    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;
+    [[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;
 
-    bool IsFinite() const;
-    float MinElement() const;
+    [[nodiscard]] bool IsFinite() const;
+    [[nodiscard]] float MinElement() const;
     static float MinElement(const Vector3& of);
 
     /// Normalizes this Vector3.
@@ -206,62 +225,77 @@ public:
     /// 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(). */
-    Vector3 Perpendicular(const Vector3 &hint = Vector3(0,1,0), const Vector3 &hint2 = Vector3(0,0,1)) const;
+    [[nodiscard]] Vector3 Perpendicular(const Vector3 &hint = Vector3(0,1,0), const Vector3 &hint2 = Vector3(0,0,1)) const;
 
-    Vector3 Min(const Vector3& min) 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);
 
-    Vector3 Max(const Vector3& max) const;
+    [[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);
 
-    Vector3 Clamp(const Vector3& min, const Vector3& max) const;
+    [[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.
-    float Distance(const Vector3& to) const;
+    [[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;
 
-    float DistanceSquared(const Vector3& to) const;
+    [[nodiscard]] float DistanceSquared(const Vector3& to) const;
     // Function Alias for DistanceSquared
-    float DistanceSq(const Vector3& to) const { return DistanceSquared(to); }
+    [[nodiscard]] float DistanceSq(const Vector3& to) const { return DistanceSquared(to); }
     static float DistanceSquared(const Vector3& from, const Vector3& to);
 
-    float Length() const;
+    [[nodiscard]] float Length() const;
     static float Length(const Vector3& of);
 
-    float LengthSquared() const;
+    [[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)
-    float Magnitude() const;
+    [[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.
-    float Dot(const Vector3& rhs) const;
+    [[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)
-    Vector3 Project(const Vector3& rhs) const;
+    [[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.
-    Vector3 Cross(const Vector3& rhs) const;
+    [[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 Normalize() function instead.
-    /// @see Normalize()
-    Vector3 Normalized() const;
+    /// 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.
@@ -280,56 +314,56 @@ public:
     /// 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;
+    [[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(). */
-    Angle2D AngleBetween(const Vector3& rhs) const;
+    [[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;
-    Vector3 Add(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
-    Vector3 Add(float s) const;
+    [[nodiscard]] Vector3 Add(float s) const;
 
     /// Subtracts two vectors
     Vector3 operator-(const Vector3& rhs) const;
-    Vector3 Sub(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;
-    Vector3 Mul(float scalar) 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.
-    Vector3 Mul(const Vector3& rhs) const;
+    [[nodiscard]] Vector3 Mul(const Vector3& rhs) const;
 
     /// Divides this vector by a scalar
     Vector3 operator/(float rhs) const;
-    Vector3 Div(float scalar) 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
-    Vector3 Div(const Vector3& v) const;
+    [[nodiscard]] Vector3 Div(const Vector3& v) const;
 
     /// Unary + operator
     Vector3 operator+() const; // TODO: Implement
     /// Unary - operator (Negation)
     Vector3 operator-() const;
 
-    bool Equals(const Vector3& rhs, float epsilon = 1e-3f) const;
-    bool Equals(float _x, float _y, float _z, float epsilon = 1e-3f) 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);
@@ -339,8 +373,13 @@ public:
 
 
     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,134 +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;
-        }
-        Vector3 XYZ() const
-        {
-            return {x, y, z};
-        }
-
-        float GetX() const { return x; }
-        float GetY() const { return y; }
-        float GetZ() const { return z; }
-        float GetW() const { return w; }
-#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;
-        float &operator[](std::size_t index);
-
-        bool IsWithinMarginOfError(const Vector4& rhs, float margin=0.0001f) const;
-
-        float LengthSqXYZ() const;
-
-        bool IsNormalized3(float epsilonSq = 1e-5f) const
-        {
-            return std::abs(LengthSqXYZ()-1.f) <= epsilonSq;
-        }
-        bool IsNormalized4(float epsilonSq = 1e-5f) const
-        {
-            return std::abs(LengthSquared()-1.f) <= epsilonSq;
-        }
-        bool IsNormalized(float epsilonSq = 1e-5f) const { return IsNormalized4(epsilonSq); }
-        bool IsZero(float epsilonSq = 1e-6f) const;
-        bool IsFinite() const;
-        bool IsPerpendicular(const Vector4& other, float epsilonSq=1e-5f) const
-        {
-            float dot = Dot(other);
-            return dot*dot <= epsilonSq * LengthSquared() * other.LengthSquared();
-        }
-        bool IsPerpendicular3(const Vector4& other, float epsilonSq = 1e-5f) const
-        {
-
-        }
-
-        bool operator==(const Vector4& rhs) const;
-        bool operator!=(const Vector4& rhs) const;
-
-        bool Equals(const Vector4& rhs, float epsilon = 1e-3f) const;
-        bool Equals(float _x, float _y, float _z, float _w, float epsilon = 1e-3f) 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 Cross3(const Vector3& rhs) const;
-        Vector4 Cross3(const Vector4& rhs) const;
-        Vector4 Cross(const Vector4& rhs) const { return Cross3(rhs); }
-        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/Common.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. */
+        inline 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,30 @@
+/// 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"
 
 int main(int argc, char** argv)
 {
+
+    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,20 @@
+#include <J3ML/Algorithm/Bezier.hpp>
+
+namespace J3ML::Algorithm
+{
+    using namespace J3ML::LinearAlgebra;
+    Vector2 BezierNormal(float t, const Vector2 &p0, const Vector2 &p1,
+                                  const Vector2 &p2, const Vector2 &p3) {
+        auto 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)};
+    }
+}
+

src/J3ML/Algorithm/GJK.cpp

@@ -1,6 +1,6 @@
-#include <J3ML/Algorithm/GJK.h>
+#include <J3ML/Algorithm/GJK.hpp>
 
-#include <J3ML/Geometry.h>
+#include <J3ML/Geometry.hpp>
 
 namespace J3ML::Algorithms
 {

src/J3ML/Algorithm/RNG.cpp

@@ -1,4 +1,4 @@
-#include <J3ML/Algorithm/RNG.h>
+#include <J3ML/Algorithm/RNG.hpp>
 #include <stdexcept>
 #include <cassert>
 
@@ -50,7 +50,7 @@ namespace J3ML::Algorithm {
         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!");
+        //assert( ((u32)newNum!=0 || increment != 0) && "RNG degenerated to producing a stream of zeroes!");
         lastNumber = (u32)newNum;
         return lastNumber;
     }
@@ -65,27 +65,10 @@ 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 */;
@@ -123,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;
     }
 
@@ -146,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,17 +1,17 @@
-#include <J3ML/Geometry/AABB.h>
+#include <J3ML/Geometry/AABB.hpp>
 #include <cassert>
-#include <J3ML/Geometry/Plane.h>
-#include <J3ML/Geometry/Sphere.h>
+#include <J3ML/Geometry/Plane.hpp>
+#include <J3ML/Geometry/Sphere.hpp>
 //#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>
-#include <J3ML/Algorithm/RNG.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>
 
 namespace J3ML::Geometry {
 
@@ -72,6 +72,8 @@ namespace J3ML::Geometry {
         return (minPoint+maxPoint) * 0.5f;
     }
 
+    Vector3 AABB::CenterPoint() const { return Centroid(); }
+
     Vector3 AABB::Size() const {
         return this->maxPoint - this->minPoint;
     }
@@ -274,6 +276,24 @@ 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)
@@ -457,7 +477,57 @@ namespace J3ML::Geometry {
     }
 
     bool AABB::Intersects(const AABB& aabb) const {
-        return Intersection(aabb).has_value();
+        // 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
+                }
+            }
+        }
     }
 
     std::optional<AABB> AABB::Intersection(const AABB& rhs) const {

src/J3ML/Geometry/AABB2D.cpp

@@ -1,4 +1,4 @@
-#include <J3ML/Geometry/AABB2D.h>
+#include <J3ML/Geometry/AABB2D.hpp>
 
 namespace J3ML::Geometry
 {

src/J3ML/Geometry/Capsule.cpp

@@ -1,8 +1,8 @@
-#include <J3ML/Algorithm/GJK.h>
-#include <J3ML/Geometry/Capsule.h>
-#include <J3ML/Geometry/AABB.h>
-#include <J3ML/Geometry/Sphere.h>
-#include <J3ML/Geometry/Polygon.h>
+#include <J3ML/Algorithm/GJK.hpp>
+#include <J3ML/Geometry/Capsule.hpp>
+#include <J3ML/Geometry/AABB.hpp>
+#include <J3ML/Geometry/Sphere.hpp>
+#include <J3ML/Geometry/Polygon.hpp>
 
 namespace J3ML::Geometry
 {
@@ -106,6 +106,14 @@ namespace J3ML::Geometry
         return polyhedron.Intersects(*this);
     }
 
+    Sphere Capsule::SphereA() const {
+        return Sphere(l.A, r);
+    }
+
+    Sphere Capsule::SphereB() const {
+        return Sphere(l.B, r);
+    }
+
     Vector3 Capsule::ExtremePoint(const Vector3 &direction) const {
         float len = direction.Length();
         assert(len > 0.f);
@@ -118,8 +126,79 @@ namespace J3ML::Geometry
         return extremePoint;
     }
 
+    bool Capsule::IsDegenerate() const {
+        return r <= 0.f; // Ask Bill :p
+    }
+
+    float Capsule::Height() const {
+        return LineLength() + Diameter();
+    }
+
     Capsule Capsule::Translated(const Vector3 &offset) const
     {
         return Capsule(l.A + offset, l.B + offset, r);
     }
+
+    Vector3 Capsule::Bottom() const { return l.A - UpDirection() * r;}
+
+    Vector3 Capsule::Center() const { return l.CenterPoint();}
+
+    Vector3 Capsule::Centroid() const { return Center(); }
+
+    float Capsule::Diameter() const { return 2.f * r;}
+
+    float Capsule::LineLength() const { return l.Length(); }
+
+    void Capsule::SetFrom(const Sphere &s) {
+        l = LineSegment(s.Position, s.Position);
+        r = s.Radius;
+    }
+
+
+    void Capsule::SetDegenerate() {
+        r = -1.f;
+    }
+
+    Vector3 Capsule::AnyPointFast() const { return l.A; }
+
+
+    Vector3 Capsule::UniformPointPerhapsInside(float height, float x, float y) const {
+        return MinimalEnclosingOBB().PointInside(height, x, y);
+    }
+
+
+    Vector3 Capsule::UpDirection() const
+    {
+        Vector3 d = l.B - l.A;
+        d.Normalize(); // Will always result in a normalized vector, even if l.a == l.b.
+        return d;
+    }
+
+    Vector3 Capsule::Top() const
+    {
+        return l.B + UpDirection() * r;
+    }
+
+    float Capsule::Volume() const
+    {
+        return Math::Pi * r * r * LineLength() + 4.f * Math::Pi * r * r * r / 3.f;
+    }
+
+    float Capsule::SurfaceArea() const
+    {
+        return 2.f * Math::Pi * r * LineLength() + 4.f * Math::Pi * r * r;
+    }
+
+    OBB Capsule::MinimalEnclosingOBB() const {
+        OBB obb;
+        obb.axis[0] = UpDirection();
+        obb.axis[0].PerpendicularBasis(obb.axis[1], obb.axis[2]);
+        obb.pos = Center();
+        obb.r[0] = Height() * 0.5f;
+        obb.r[1] = r;
+        obb.r[2] = r;
+        return obb;
+    }
+
+
 }

src/J3ML/Geometry/Circle.cpp

@@ -0,0 +1,38 @@
+#include <J3ML/Geometry/Circle.hpp>
+
+
+
+namespace J3ML
+{
+    Circle::Circle(const Vector3& center, const Vector3& norm, float radius)
+    : Position(center), Normal(norm), Radius(radius)
+    {}
+
+    Vector3 Circle::BasisU() const { return Normal.Perpendicular(); }
+    Vector3 Circle::BasisV() const { return Normal.AnotherPerpendicular();}
+
+
+    Vector3 Circle::GetPoint(float angleRadians) const
+    {
+        float sin, cos;
+        Math::SinCos(angleRadians, sin, cos);
+        return Position + Radius * (sin * BasisU() + cos * BasisV());
+    }
+
+    Vector3 Circle::GetPoint(float angleRadians, float d) const
+    {
+        float sin, cos;
+        Math::SinCos(angleRadians, sin, cos);
+        return Position + Radius * d * (sin * BasisU() + cos * BasisV());
+    }
+
+    Vector3 Circle::ExtremePoint(const Vector3& direction) const
+    {
+        Vector3 d = direction - direction.ProjectToNorm(Normal);
+
+        if (d.IsZero())
+            return Position;
+        else
+            return Position + d.ScaledToLength(Radius);
+    }
+}

src/J3ML/Geometry/Common.cpp

@@ -1,4 +1,4 @@
-#include <J3ML/Geometry/Common.h>
+#include <J3ML/Geometry/Common.hpp>
 
 namespace J3ML::Geometry {
 

src/J3ML/Geometry/Frustum.cpp

@@ -1,11 +1,11 @@
-#include <J3ML/Geometry/Common.h>
-#include <J3ML/Geometry/Frustum.h>
+#include <J3ML/Geometry/Common.hpp>
+#include <J3ML/Geometry/Frustum.hpp>
 #include <cmath>
-#include "J3ML/Geometry/AABB.h"
-#include <J3ML/Geometry/Polyhedron.h>
-#include <J3ML/Geometry/LineSegment.h>
-#include <J3ML/Geometry/Polygon.h>
-#include <J3ML/Algorithm/GJK.h>
+#include <J3ML/Geometry/AABB.hpp>
+#include <J3ML/Geometry/Polyhedron.hpp>
+#include <J3ML/Geometry/LineSegment.hpp>
+#include <J3ML/Geometry/Polygon.hpp>
+#include <J3ML/Algorithm/GJK.hpp>
 
 
 namespace J3ML::Geometry
@@ -96,6 +96,22 @@ namespace J3ML::Geometry
         }
     }
 
+    Vector2 Frustum::ViewportToScreenSpace(float x, float y, int screenWidth, int screenHeight) {
+        return {(x + 1.f) * 0.5f * (screenWidth - 1.f), (1.f - y) * 0.5f * (screenHeight - 1.f)};
+    }
+
+    Vector2 Frustum::ViewportToScreenSpace(const Vector2 &point, int screenWidth, int screenHeight) {
+        return ViewportToScreenSpace(point.x, point.y, screenWidth, screenHeight);
+    }
+
+    Vector2 Frustum::ScreenToViewportSpace(float x, float y, int screenWidth, int screenHeight) {
+        return {x * 2.f / (screenWidth -1.f) - 1.f, 1.f - y * 2.f / (screenHeight - 1.f)};
+    }
+
+    Vector2 Frustum::ScreenToViewportSpace(const Vector2 &point, int screenWidth, int screenHeight) {
+        return ScreenToViewportSpace(point.x, point.y, screenWidth, screenHeight);
+    }
+
     Vector3 Frustum::ExtremePoint(const Vector3 &direction, float &projectionDistance) const
     {
         Vector3 corners[8];
@@ -130,6 +146,19 @@ namespace J3ML::Geometry
         return frustum;
     }
 
+
+    PBVolume<6> Frustum::ToPBVolume() const {
+        PBVolume<6> frustumVolume;
+        frustumVolume.p[0] = NearPlane();
+        frustumVolume.p[1] = LeftPlane();
+        frustumVolume.p[2] = RightPlane();
+        frustumVolume.p[3] = TopPlane();
+        frustumVolume.p[4] = BottomPlane();
+        frustumVolume.p[5] = FarPlane();
+
+        return frustumVolume;
+    }
+
     AABB Frustum::MinimalEnclosingAABB() const {
         AABB aabb;
         aabb.SetNegativeInfinity();
@@ -138,6 +167,141 @@ namespace J3ML::Geometry
         return aabb;
     }
 
+    bool Frustum::IsFinite() const {
+        return pos.IsFinite() && front.IsFinite() && up.IsFinite() && Math::IsFinite(nearPlaneDistance)
+        && Math::IsFinite(farPlaneDistance) && Math::IsFinite(horizontalFov) && Math::IsFinite(verticalFov);
+    }
+
+    OBB Frustum::MinimalEnclosingOBB(float expandGuardband) const {
+        assert(IsFinite());
+        assert(front.IsNormalized());
+        assert(up.IsNormalized());
+
+        OBB obb;
+        obb.pos = pos + (nearPlaneDistance + farPlaneDistance) * 0.5f * front;
+        obb.axis[1] = up;
+        obb.axis[2] = -front;
+        obb.axis[0] = Vector3::Cross(obb.axis[1], obb.axis[2]);
+        obb.r = Vector3::Zero;
+
+        for (int i = 0; i < 8; ++i)
+            obb.Enclose(CornerPoint(i));
+
+        // Expand the generated OBB very slightly to avoid numerical issues when
+        // testing whether this Frustum actually is contained inside the generated OBB.
+        obb.r.x += expandGuardband;
+        obb.r.y += expandGuardband;
+        obb.r.z += expandGuardband;
+
+        return obb;
+    }
+
+    void Frustum::SetKind(FrustumProjectiveSpace projectiveSpace, FrustumHandedness handedness) {
+
+    }
+
+    void Frustum::SetViewPlaneDistances(float n, float f) {
+        nearPlaneDistance = n;
+        farPlaneDistance = f;
+        ProjectionMatrixChanged();
+
+    }
+
+    void Frustum::SetFrame(const Vector3 &pos, const Vector3 &front, const Vector3 &up) {
+        this->pos = pos;
+        this->front = front;
+        this->up = up;
+        WorldMatrixChanged();
+    }
+
+    void Frustum::SetPos(const Vector3 &pos) {
+        this->pos = pos;
+        WorldMatrixChanged();
+    }
+
+    void Frustum::SetFront(const Vector3 &front) {
+        this->front = front;
+        WorldMatrixChanged();
+    }
+
+    void Frustum::SetUp(const Vector3 &up) {
+        this->up = up;
+        WorldMatrixChanged();
+    }
+
+    void Frustum::SetPerspective(float h, float v) {
+        type = FrustumType::Perspective;
+        this->horizontalFov = h;
+        this->verticalFov = v;
+        ProjectionMatrixChanged();
+    }
+
+    void Frustum::SetOrthographic(float w, float h) {
+        type = FrustumType::Orthographic;
+        orthographicWidth = w;
+        orthographicHeight = h;
+        ProjectionMatrixChanged();
+    }
+
+    float Frustum::AspectRatio() const {
+        return Math::Tan(horizontalFov* 0.5f) / Math::Tan(verticalFov*0.5f);
+    }
+
+    void Frustum::SetHorizontalFovAndAspectRatio(float hFov, float aspectRatio) {
+        type = FrustumType::Perspective;
+        horizontalFov = hFov;
+        verticalFov = 2.f * Math::Atan(Math::Tan(hFov * 0.5f) / aspectRatio);
+        ProjectionMatrixChanged();
+    }
+
+    void Frustum::SetVerticalFovAndAspectRatio(float vFov, float aspectRatio) {
+        type = FrustumType::Perspective;
+        verticalFov = vFov;
+        horizontalFov = 2.f * Math::Atan(Math::Tan(vFov * 0.5f) * aspectRatio);
+        return ProjectionMatrixChanged();
+    }
+
+    Ray Frustum::UnProject(float x, float y) const {
+        assert(x >= -1.f);
+        assert(x <= 1.f);
+        assert(y >= -1.f);
+        assert(y <= 1.f);
+
+        if (type == FrustumType::Perspective) {
+            Vector3 nearPlanePos = NearPlanePos(x, y);
+            return Ray(pos, (nearPlanePos - pos).Normalized());
+        } else
+            return UnProjectFromNearPlane(x, y);
+    }
+
+    Ray Frustum::UnProject(const Vector2 &xy) const {
+        return UnProject(xy.x, xy.y);
+    }
+
+    Ray Frustum::UnProjectFromNearPlane(float x, float y) const {
+        return UnProjectLineSegment(x, y).ToRay();
+    }
+
+    LineSegment Frustum::UnProjectLineSegment(float x, float y) const {
+        Vector3 nearPlanePos = NearPlanePos(x, y);
+        Vector3 farPlanePos = FarPlanePos(x, y);
+        return {nearPlanePos, farPlanePos};
+    }
+
+    Vector3 Frustum::PointInside(float x, float y, float z) const {
+        assert(z >= 0.f);
+        assert(z <= 1.f);
+        return UnProjectLineSegment(x, y).GetPoint(z);
+    }
+
+    Vector3 Frustum::PointInside(const Vector3 &xyz) const {
+        return PointInside(xyz.x, xyz.y, xyz.z);
+    }
+
+    Vector3 Frustum::CenterPoint() const {
+        return pos + (nearPlaneDistance + farPlaneDistance) * 0.5f * front;
+    }
+
     Vector3 Frustum::CornerPoint(int cornerIndex) const {
         assert(0 <= cornerIndex && cornerIndex <= 7);
         switch(cornerIndex)
@@ -175,6 +339,8 @@ namespace J3ML::Geometry
         }
     }
 
+    Vector3 Frustum::NearPlanePos(const Vector2 &point) const { return NearPlanePos(point.x, point.y); }
+
     Vector3 Frustum::FarPlanePos(float x, float y) const {
         assert(type == FrustumType::Perspective || type == FrustumType::Orthographic);
 
@@ -195,6 +361,134 @@ namespace J3ML::Geometry
         }
     }
 
+    Vector3 Frustum::FarPlanePos(const Vector2 &point) const {
+        return FarPlanePos(point.x, point.y);
+    }
+
+    Plane Frustum::TopPlane() const {
+        if (type == FrustumType::Perspective) {
+            Vector3 topSide = front + Math::Tan(verticalFov * 0.5f) * up;
+            Vector3 right = WorldRight();
+            Vector3 topSideNormal = ((handedness == FrustumHandedness::Right) ? Vector3::Cross(right, topSide) : Vector3::Cross(topSide, right)).Normalized();
+        }
+    }
+
+    Plane Frustum::BottomPlane() const {
+        if (type == FrustumType::Perspective) {
+            Vector3 bottomSide = front - Math::Tan(verticalFov * 0.5f) * up;
+            Vector3 left = -WorldRight();
+            Vector3 bottomSideNormal = ((handedness == FrustumHandedness::Right) ? Vector3::Cross(left, bottomSide) : Vector3::Cross(bottomSide, left)).Normalized();
+        } else {
+            return Plane(NearPlanePos(0.f, -1.f), -up);
+        }
+    }
+
+
+
+    Plane Frustum::RightPlane() const {
+        if (type == FrustumType::Perspective) {
+            Vector3 right = WorldRight();
+            right.ScaleToLength(Math::Tan(horizontalFov * 0.5f));
+            Vector3 rightSide = front + right;
+            Vector3 rightSideNormal = ((handedness == FrustumHandedness::Right) ? Vector3::Cross(rightSide, up) : Vector3::Cross(up, rightSide)).Normalized();
+        } else {
+            Vector3 right = WorldRight();
+            return Plane(NearPlanePos(1.f,0.f), right.Normalized());
+        }
+    }
+
+    Plane Frustum::LeftPlane() const {
+        if (type == FrustumType::Perspective) {
+            Vector3 left = -WorldRight();
+            left.ScaleToLength(Math::Tan(horizontalFov*0.5f));
+            Vector3 leftSide = front + left;
+            Vector3 leftSideNormal = ((handedness == FrustumHandedness::Right) ? Vector3::Cross(up, leftSide) : Vector3::Cross(leftSide, up)).Normalized();
+            return Plane(pos, leftSideNormal);
+        } else {
+            Vector3 left = -WorldRight();
+            return Plane(NearPlanePos(-1.f, 0.f), left.Normalized());
+        }
+    }
+
+    Plane Frustum::FarPlane() const {
+       return Plane(pos + front * farPlaneDistance, front);
+    }
+
+    Plane Frustum::NearPlane() const {
+        return Plane(pos + front * nearPlaneDistance, -front);
+    }
+
+    float Frustum::NearPlaneWidth() const {
+        if (type == FrustumType::Perspective)
+            return Math::Tan(horizontalFov*0.5f)*2.f * nearPlaneDistance;
+        else
+            return orthographicWidth;
+    }
+
+    float Frustum::NearPlaneHeight() const {
+        if (type == FrustumType::Perspective)
+            return Math::Tan(verticalFov*0.5f) * 2.f * nearPlaneDistance;
+        else
+            return orthographicWidth;
+    }
+
+    Plane Frustum::GetPlane(int faceIndex) const {
+        assert(0 <= faceIndex && faceIndex <= 5);
+        switch(faceIndex)
+        {
+            default: // For release builds where assume() is disabled, always return the first option if out-of-bounds.
+            case 0: return NearPlane();
+            case 1: return FarPlane();
+            case 2: return LeftPlane();
+            case 3: return RightPlane();
+            case 4: return TopPlane();
+            case 5: return BottomPlane();
+        }
+    }
+
+    void Frustum::GetPlanes(Plane *outArray) const {
+        assert(outArray);
+        for (int i = 0; i < 6; ++i)
+            outArray[i] = GetPlane(i);
+    }
+
+    void Frustum::Translate(const Vector3 &offset) {
+        pos += offset;
+    }
+
+
+    void Frustum::Transform(const Matrix3x3& transform) {
+        assert(transform.HasUniformScale());
+        pos = transform * pos;
+        front = transform * front;
+        float scaleFactor = front.Normalize();
+        up = (transform * up).Normalized();
+        nearPlaneDistance *= scaleFactor;
+        farPlaneDistance *= scaleFactor;
+
+        if (type == FrustumType::Orthographic) {
+            orthographicWidth *= scaleFactor;
+            orthographicHeight *= scaleFactor;
+        }
+    }
+
+    void Frustum::Transform(const Matrix4x4& transform) {
+        assert(transform.Row(3).Equals(0,0,0,1));
+
+        assert(transform.HasUniformScale());
+
+        // TODO: This is incorrect. Technically we need only a mat3x4 to implement this operation.
+        // But we choose to not implement this in our code, instead opting for only 3x3 and 4x4.
+        // Care should be taken when using this, it may need to be edited for mathematical correctness
+        Transform(transform.GetRotatePart());
+    }
+
+    void Frustum::Transform(const Quaternion& transform) {
+        Transform(transform.ToMatrix3x3());
+    }
+
+
+
     Polyhedron Frustum::ToPolyhedron() const {
         // Note to maintainer: this function is an exact copy of AABB::ToPolyhedron() and OBB::ToPolyhedron().
 
@@ -234,6 +528,182 @@ namespace J3ML::Geometry
         return p;
     }
 
+    Matrix4x4 Frustum::ViewProjMatrix() const { return viewProjectionMatrix; }
+
+    Matrix4x4 Frustum::ComputeViewProjMatrix() const { return ComputeProjectionMatrix() * ComputeViewMatrix();}
+
+    Vector3 Frustum::Project(const Vector3 &point) const {
+        Vector4 projectedPoint = ViewProjMatrix().Mul(Vector4(point, 1.f));
+        projectedPoint.NormalizeW();
+        return projectedPoint.XYZ();
+    }
+
+    Matrix4x4 Frustum::WorldMatrix() const { return worldMatrix;}
+
+    Matrix4x4 Frustum::ComputeWorldMatrix() const {
+        assert(pos.IsFinite());
+        assert(up.IsNormalized(1e-3f));
+        assert(front.IsNormalized(1e-3f));
+        assert(up.IsPerpendicular(front));
+
+        Matrix4x4 m;
+        m.SetCol3(0, WorldRight().Normalized());
+        m.SetCol3(1, up);
+        if (handedness == FrustumHandedness::Right)
+            m.SetCol3(2, -front);
+        else
+            m.SetCol3(2, front);
+        m.SetCol3(3, pos);
+        assert(!m.HasNegativeScale());
+        return m;
+    }
+
+    Matrix4x4 Frustum::ViewMatrix() const { auto m = worldMatrix; m.InverseOrthonormal(); return m; }
+
+    Matrix4x4 Frustum::ComputeViewMatrix() const {
+        Matrix4x4 world = ComputeWorldMatrix();
+        world.InverseOrthonormal();
+        return world;
+    }
+
+    Matrix4x4 Frustum::ProjectionMatrix() const { return projectionMatrix;}
+
+    Matrix4x4 Frustum::ComputeProjectionMatrix() const {
+        if (type == FrustumType::Invalid || projectiveSpace == FrustumProjectiveSpace::Invalid)
+            return Matrix4x4::NaN;
+
+        assert(type == FrustumType::Perspective || type == FrustumType::Orthographic);
+        assert(projectiveSpace == FrustumProjectiveSpace::GL || projectiveSpace == FrustumProjectiveSpace::D3D);
+        assert(handedness == FrustumHandedness::Left || handedness == FrustumHandedness::Right);
+
+        if (type == FrustumType::Perspective) {
+            if (projectiveSpace == FrustumProjectiveSpace::GL) {
+                if (handedness == FrustumHandedness::Right)
+                    return Matrix4x4::OpenGLPerspProjRH(nearPlaneDistance, farPlaneDistance, NearPlaneWidth(), NearPlaneHeight());
+                else
+                    return Matrix4x4::OpenGLPerspProjLH(nearPlaneDistance, farPlaneDistance, NearPlaneWidth(), NearPlaneHeight());
+
+            } else if (projectiveSpace == FrustumProjectiveSpace::D3D) {
+                if (handedness == FrustumHandedness::Right)
+                    return Matrix4x4::D3DPerspProjRH(nearPlaneDistance, farPlaneDistance, NearPlaneWidth(), NearPlaneHeight());
+                else
+                    return Matrix4x4::D3DPerspProjLH(nearPlaneDistance, farPlaneDistance, NearPlaneHeight(), NearPlaneHeight());
+            }
+        } else if (type == FrustumType::Orthographic) {
+            if (projectiveSpace == FrustumProjectiveSpace::GL) {
+                if (handedness == FrustumHandedness::Right)
+                    return Matrix4x4::OpenGLOrthoProjRH(nearPlaneDistance, farPlaneDistance, orthographicWidth, orthographicHeight);
+                else if (handedness == FrustumHandedness::Left)
+                    return Matrix4x4::OpenGLOrthoProjLH(nearPlaneDistance, farPlaneDistance, orthographicWidth, orthographicHeight);
+
+            } else if (projectiveSpace == FrustumProjectiveSpace::D3D) {
+                if (handedness == FrustumHandedness::Right)
+                    return Matrix4x4::D3DOrthoProjRH(nearPlaneDistance, farPlaneDistance, orthographicWidth, orthographicHeight);
+                else
+                    return Matrix4x4::D3DOrthoProjLH(nearPlaneDistance, farPlaneDistance, orthographicWidth, orthographicHeight);
+            }
+        }
+        assert(false && "Not all values of Frustum were initialized properly! Please initialize correctly before calling Frustum::ProjectionMatrix()!");
+        return Matrix4x4::NaN;
+    }
+
+    bool Frustum::Contains(const Vector3 &point) const {
+        const float eps = 1e-3f;
+        const float position = 1.f + eps;
+        const float neg = -position;
+        Vector3 projected = Project(point);
+
+        if (projectiveSpace == FrustumProjectiveSpace::D3D) {
+            return neg <= projected.x && projected.x <= position &&
+                   neg <= projected.y && projected.y <= position &&
+                   -eps <= projected.z && projected.z <= position;
+        } else if (projectiveSpace == FrustumProjectiveSpace::GL) {
+            return neg <= projected.x && projected.x <= position &&
+                   neg <= projected.y && projected.y <= position &&
+                   neg <= projected.z && projected.z <= position;
+        } else {
+            // TODO: Make Frustum::Contains agnostic of the projection settings.
+            assert(false && "Not all values of Frustum were initialized properly! Please initialize correctly before calling Frustum::Contains()!");
+            return false;
+        }
+    }
+
+    bool Frustum::Contains(const LineSegment &lineSegment) const {
+        return Contains(lineSegment.A) && Contains(lineSegment.B);
+    }
+
+    bool Frustum::Contains(const Triangle &triangle) const {
+        return Contains(triangle.V0) && Contains(triangle.V1) && Contains(triangle.V2);
+    }
+
+    bool Frustum::Contains(const Polygon &polygon) const {
+        for (int i = 0; i < polygon.NumVertices(); ++i)
+            if (!Contains(polygon.Vertex(i)))
+                return false;
+        return true;
+    }
+
+    bool Frustum::Contains(const AABB &aabb) const {
+        for (int i = 0; i < 8; ++i)
+            if (!Contains(aabb.CornerPoint(i)))
+                return false;
+
+        return true;
+    }
+
+    bool Frustum::Contains(const OBB &obb) const {
+        for (int i = 0; i < 8; ++i)
+            if (!Contains(obb.CornerPoint(i)))
+                return false;
+
+        return true;
+    }
+
+    bool Frustum::Contains(const Frustum &frustum) const {
+        for (int i = 0; i < 8; ++i)
+            if (!Contains(frustum.CornerPoint(i)))
+                return false;
+
+        return true;
+    }
+
+    bool Frustum::Contains(const Polyhedron &polyhedron) const {
+        assert(polyhedron.IsClosed());
+
+        for (int i = 0; i < polyhedron.NumVertices(); ++i)
+            if (!Contains(polyhedron.Vertex(i)))
+                return false;
+
+        return true;
+    }
+
+    float Frustum::Distance(const Vector3 &point) const {
+        Vector3 pt = ClosestPoint(point);
+        return pt.Distance(point);
+    }
+
+    Vector3 Frustum::ClosestPoint(const Vector3 &point) const {
+        if (type == FrustumType::Orthographic) {
+            float frontHalfSize = (farPlaneDistance - nearPlaneDistance) * 0.5f;
+            float halfWidth = orthographicWidth * 0.5f;
+            float halfHeight = orthographicHeight * 0.5f;
+
+            Vector3 frustumCenter = pos + (frontHalfSize + nearPlaneDistance) * front;
+            Vector3 right = Vector3::Cross(front, up);
+            assert(right.IsNormalized());
+            Vector3 d = point - frustumCenter;
+            Vector3 closestPoint = frustumCenter;
+
+            closestPoint += Math::Clamp(Vector3::Dot(d, front), -frontHalfSize, frontHalfSize) * front;
+            closestPoint += Math::Clamp(Vector3::Dot(d, right), -halfWidth, halfWidth) * right;
+            closestPoint += Math::Clamp(Vector3::Dot(d, up), -halfHeight, halfHeight) * up;
+
+            return closestPoint;
+        } else {
+            return ToPolyhedron().ClosestPoint(point);
+        }
+    }
+
     bool Frustum::Intersects(const Ray &ray) const {
         return this->ToPolyhedron().Intersects(ray);
     }
@@ -294,6 +764,22 @@ namespace J3ML::Geometry
         return this->ToPolyhedron().Intersects(polyhedron);
     }
 
+    void Frustum::WorldMatrixChanged() {
+        worldMatrix = ComputeWorldMatrix();
+        Matrix4x4 viewMatrix = worldMatrix;
+        viewMatrix.InverseOrthonormal();
+        viewProjectionMatrix = projectionMatrix * viewMatrix;
+    }
+
+    void Frustum::ProjectionMatrixChanged() {
+        projectionMatrix = ComputeProjectionMatrix();
+        if (!Math::IsNotANumber(worldMatrix[0][0])) {
+            Matrix4x4 viewMatrix = worldMatrix;
+            viewMatrix.InverseOrthonormal();
+            viewProjectionMatrix = projectionMatrix * viewMatrix;
+        }
+    }
+
     Frustum::Frustum()
             : type(FrustumType::Invalid),
               pos(Vector3::NaN),
@@ -314,4 +800,6 @@ namespace J3ML::Geometry
         else
             return Vector3::Cross(up, front);
     }
-}
+
+
+}

src/J3ML/Geometry/Line.cpp

@@ -1,6 +1,6 @@
-#include <J3ML/LinearAlgebra.h>
-#include <J3ML/Geometry/Line.h>
-#include <J3ML/Geometry/LineSegment.h>
+#include <J3ML/LinearAlgebra.hpp>
+#include <J3ML/Geometry/Line.hpp>
+#include <J3ML/Geometry/LineSegment.hpp>
 namespace J3ML::Geometry
 {
 

src/J3ML/Geometry/LineSegment.cpp

@@ -1,6 +1,9 @@
-#include <J3ML/Geometry/LineSegment.h>
-#include "J3ML/Geometry/Capsule.h"
-#include <J3ML/Geometry/Line.h>
+#include <J3ML/Geometry/LineSegment.hpp>
+#include "J3ML/Geometry/Capsule.hpp"
+#include <J3ML/Geometry/Line.hpp>
+#include <J3ML/LinearAlgebra/Vector3.hpp>
+#include <J3ML/LinearAlgebra/Matrix3x3.hpp>
+#include <J3ML/Geometry/Plane.hpp>
 
 namespace J3ML::Geometry {
 
@@ -9,6 +12,10 @@ namespace J3ML::Geometry {
 
     }
 
+    LineSegment::LineSegment(const Ray &ray, float d): A(ray.Origin), B(ray.GetPoint(d)) {}
+
+    LineSegment::LineSegment(const Line &line, float d): A(line.Position), B(line.GetPoint(d)) {}
+
     LineSegment::LineSegment() {}
 
     void LineSegment::ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const
@@ -24,6 +31,10 @@ namespace J3ML::Geometry {
         return (1.f - d) * A + d * B;
     }
 
+    Vector3 LineSegment::CenterPoint() const { return (A+B) * 0.5f;}
+
+    void LineSegment::Reverse() { Swap(A, B);}
+
     float LineSegment::Distance(const Vector3 &point, float &d) const
     {
         /// See Christer Ericson's Real-Time Collision Detection, p.130.
@@ -73,10 +84,23 @@ namespace J3ML::Geometry {
 
     Vector3 LineSegment::ClosestPoint(const Vector3 &point, float &d) const {
         Vector3 dir = B - A;
-        d = Clamp01(Vector3::Dot(point - A, dir) / dir.LengthSquared());
+        d = Math::Clamp01(Vector3::Dot(point - A, dir) / dir.LengthSquared());
         return A + d * dir;
     }
 
+    Vector3 LineSegment::ClosestPoint(const Ray &other) const { float d, d2; return ClosestPoint(other, d, d2);}
+
+    Vector3 LineSegment::ClosestPoint(const Ray &other, float &d) const { float d2; return ClosestPoint(other, d, d2);}
+
+    Vector3 LineSegment::ClosestPoint(const Ray &other, float &d, float &d2) const {
+        other.ClosestPoint(*this, d2, d);
+        return GetPoint(d);
+    }
+
+    Vector3 LineSegment::ClosestPoint(const Line &other) const { float d, d2; return ClosestPoint(other, d, d2); }
+
+    Vector3 LineSegment::ClosestPoint(const Line &other, float &d) const { float d2; return ClosestPoint(other, d, d2); }
+
     Vector3 LineSegment::ClosestPoint(const Line &other, float &d, float &d2) const
     {
         Line::ClosestPointLineLine(other.Position, other.Direction, A,B - A, d2, d);
@@ -96,10 +120,9 @@ namespace J3ML::Geometry {
             return GetPoint(d);
     }
 
-    Vector3 LineSegment::ClosestPoint(const Ray &other, float &d, float &d2) const {
-        other.ClosestPoint(*this, d2, d);
-        return GetPoint(d);
-    }
+    Vector3 LineSegment::ClosestPoint(const LineSegment &other) const { float d, d2; return ClosestPoint(other, d, d2); }
+
+    Vector3 LineSegment::ClosestPoint(const LineSegment &other, float &d) const { float d2; return ClosestPoint(other, d, d2); }
 
     Vector3 LineSegment::ClosestPoint(const LineSegment &other, float &d, float &d2) const
     {
@@ -209,10 +232,14 @@ namespace J3ML::Geometry {
         return A.DistanceSq(B);
     }
 
-    bool LineSegment::Intersects(const LineSegment &segment) const {
+    bool LineSegment::IsFinite() const { return A.IsFinite() && B.IsFinite(); }
+
+    bool LineSegment::Intersects(const LineSegment &segment, float epsilon) const {
         return false;
     }
 
+    Vector3 LineSegment::AnyPointFast() const { return A; }
+
     Vector3 LineSegment::ExtremePoint(const Vector3 &direction) const {
         return Vector3::Dot(direction, B-A) >= 0.f ? B : A;
     }
@@ -234,6 +261,14 @@ namespace J3ML::Geometry {
         return GetPoint(d).Distance(other.GetPoint(d2));
     }
 
+    Ray LineSegment::ToRay() const {
+        return Ray(A, Dir());
+    }
+
+    Line LineSegment::ToLine() const {
+        return Line(A, Dir());
+    }
+
     LineSegment operator*(const Matrix4x4 &transform, const LineSegment &l) {
         return LineSegment(transform.Mul(l.A), transform.Mul(l.B));
     }

src/J3ML/Geometry/OBB.cpp

@@ -1,12 +1,13 @@
-#include <J3ML/Geometry/Shape.h>
-#include <J3ML/Geometry/AABB.h>
-#include <J3ML/Geometry/LineSegment.h>
-#include <J3ML/Geometry/Polyhedron.h>
-#include <J3ML/Geometry/OBB.h>
-#include <J3ML/Geometry/Sphere.h>
+#include <J3ML/Geometry/Shape.hpp>
+#include <J3ML/Geometry/AABB.hpp>
+#include <J3ML/Geometry/LineSegment.hpp>
+#include <J3ML/Geometry/Polyhedron.hpp>
+#include <J3ML/Geometry/OBB.hpp>
+#include <J3ML/Geometry/Sphere.hpp>
 
-namespace J3ML::Geometry
-{
+namespace J3ML::Geometry {
+
+    using namespace J3ML::Math;
 
     Polyhedron OBB::ToPolyhedron() const {
         // Note to maintainer: This function is an exact copy of AABB::ToPolyhedron() and Frustum::ToPolyhedron()
@@ -22,12 +23,12 @@ namespace J3ML::Geometry
         // generate the 6 faces of this OBB.
         const int faces[6][4] =
                 {
-                        {0, 1, 3, 2}, // X-
-                        {4, 6, 7, 5}, // X+
-                        {0, 4, 5, 1}, // Y-
-                        {7, 6, 2, 3}, // Y+
-                        {0, 2, 6, 4}, // Z-
-                        {1, 5, 7, 3}  // Z+
+            {0, 1, 3, 2}, // X-
+            {4, 6, 7, 5}, // X+
+            {0, 4, 5, 1}, // Y-
+            {7, 6, 2, 3}, // Y+
+            {0, 2, 6, 4}, // Z-
+            {1, 5, 7, 3}  // Z+
                 };
 
         for (int f = 0; f < 6; ++f)
@@ -405,6 +406,50 @@ namespace J3ML::Geometry
         return m;
     }
 
+    Vector3 OBB::ClosestPoint(const Vector3& targetPoint) const {
+        Vector3 d = targetPoint - pos;
+        Vector3 closestPoint = pos; // Start at the center point of the OBB.
+        for (int i = 0; i < 3; ++i) // Project the target onto the OBB axes and walk towards that point.
+            closestPoint += Clamp(Vector3::Dot(d, axis[i]), -r[i], r[i]) * axis[i];
+
+
+        return closestPoint;
+    }
+
+
+    void OBB::Enclose(const Vector3& point) {
+        Vector3 p = point - pos;
+
+        for (int i = 0; i < 3; ++i) {
+            assert(Math::EqualAbs(axis[i].Length(), 1.f));
+            float dist = p.Dot(axis[i]);
+            float distanceFromOBB = Math::Abs(dist) - r[i];
+            if (distanceFromOBB > 0.f) {
+                r[i] += distanceFromOBB * 0.5f;
+                if (dist > 0.f) // TODO: Optimize out this comparison!
+                    pos += axis[i] * distanceFromOBB * 0.5f;
+                else
+                    pos -= axis[i] * distanceFromOBB * 0.5f;
+
+                p = point - pos;
+                assert(Math::EqualAbs(Math::Abs(p.Dot(axis[i])), r[i], 1e-1f));
+            }
+        }
+        assert(Distance(point) <= 1e-3f);
+    }
+
+    float OBB::Distance(const Vector3& point) const {
+        Vector3 closestPoint = ClosestPoint(point);
+        return point.Distance(closestPoint);
+    }
+
+    bool OBB::Contains(const Vector3 &point) const {
+        Vector3 pt = point - pos;
+        return Abs(Vector3::Dot(pt, axis[0])) <= r[0] &&
+               Abs(Vector3::Dot(pt, axis[1])) <= r[1] &&
+               Abs(Vector3::Dot(pt, axis[2])) <= r[2];
+    }
+
     Matrix4x4 OBB::LocalToWorld() const
     {
         // To produce a normalized local->world matrix, do the following.

src/J3ML/Geometry/Plane.cpp

@@ -1,9 +1,9 @@
-#include <J3ML/Geometry/Plane.h>
-#include <J3ML/Geometry/AABB.h>
-#include <J3ML/Geometry/OBB.h>
-#include <J3ML/Geometry/Capsule.h>
-#include <J3ML/Geometry/Polygon.h>
-#include <J3ML/Geometry/Sphere.h>
+#include <J3ML/Geometry/Plane.hpp>
+#include <J3ML/Geometry/AABB.hpp>
+#include <J3ML/Geometry/OBB.hpp>
+#include <J3ML/Geometry/Capsule.hpp>
+#include <J3ML/Geometry/Polygon.hpp>
+#include <J3ML/Geometry/Sphere.hpp>
 
 namespace J3ML::Geometry
 {
@@ -130,12 +130,13 @@ namespace J3ML::Geometry
 #endif
     }
 
+    Plane::Plane(const Vector3 &normal, float d): Normal(normal), distance(d) {}
+
     Plane::Plane(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3) {
         Set(v1, v2, v3);
     }
 
-    Plane::Plane(const Vector3 &pos, const Vector3 &norm)
-            : Shape(), Position(pos), Normal(norm) {}
+    Plane::Plane(const Vector3 &pos, const Vector3 &norm) : Position(pos), Normal(norm) {}
 
     bool Plane::Intersects(J3ML::Geometry::Ray ray, float *dist) const {
         float t;
@@ -231,6 +232,14 @@ namespace J3ML::Geometry
         return projected;
     }
 
+    Vector3 Plane::ProjectToNegativeHalf(const Vector3 &point) const {
+        return point - Math::Max(0.f, (Normal.Dot(point) - distance)) * Normal;
+    }
+
+    Vector3 Plane::ProjectToPositiveHalf(const Vector3 &point) const {
+        return point - Math::Min(0.f, (Vector3::Dot(Normal, point) - distance)) * Normal;
+    }
+
     LineSegment Plane::Project(const LineSegment &lineSegment) {
         return LineSegment(Project(lineSegment.A), Project(lineSegment.B));
     }

src/J3ML/Geometry/Polygon.cpp

@@ -1,11 +1,13 @@
-#include <J3ML/Geometry/Polygon.h>
-#include <J3ML/Geometry/AABB.h>
-#include <J3ML/Geometry/Triangle.h>
-#include "J3ML/Geometry/Plane.h"
-#include "J3ML/Geometry/Line.h"
-#include <J3ML/Algorithm/GJK.h>
+#include <J3ML/Geometry/Polygon.hpp>
+#include <J3ML/Geometry/AABB.hpp>
+#include <J3ML/Geometry/Triangle.hpp>
+#include "J3ML/Geometry/Plane.hpp"
+#include "J3ML/Geometry/Line.hpp"
+#include <J3ML/Algorithm/GJK.hpp>
 
 namespace J3ML::Geometry {
+    Vector3 Polygon::AnyPointFast() const { return !vertices.empty() ? vertices[0] : Vector3::NaN; }
+
     AABB Polygon::MinimalEnclosingAABB() const {
         AABB aabb;
         aabb.SetNegativeInfinity();
@@ -570,6 +572,19 @@ namespace J3ML::Geometry {
         return closestPt;
     }
 
+    Polyhedron Polygon::ToPolyhedron() const {
+        Polyhedron poly;
+        poly.v = vertices;
+        poly.f.push_back(Polyhedron::Face());
+        poly.f.push_back(Polyhedron::Face());
+        for(int i = 0; i < NumVertices(); ++i)
+        {
+            poly.f[0].v.push_back(i);
+            poly.f[1].v.push_back(NumVertices()-1-i);
+        }
+        return poly;
+    }
+
     Vector3 Polygon::ClosestPoint(const LineSegment &lineSegment) const
     {
         return ClosestPoint(lineSegment, 0);

src/J3ML/Geometry/Polyhedron.cpp

@@ -1,11 +1,11 @@
-#include <J3ML/Geometry/Polyhedron.h>
-#include <J3ML/Geometry/AABB.h>
-#include <J3ML/Geometry/Triangle.h>
-#include <J3ML/Geometry/LineSegment.h>
-#include "J3ML/Geometry/Ray.h"
-#include "J3ML/Geometry/Line.h"
-#include <J3ML/Geometry/Polygon.h>
-#include <J3ML/Geometry/Capsule.h>
+#include <J3ML/Geometry/Polyhedron.hpp>
+#include <J3ML/Geometry/AABB.hpp>
+#include <J3ML/Geometry/Triangle.hpp>
+#include <J3ML/Geometry/LineSegment.hpp>
+#include "J3ML/Geometry/Ray.hpp"
+#include "J3ML/Geometry/Line.hpp"
+#include <J3ML/Geometry/Polygon.hpp>
+#include <J3ML/Geometry/Capsule.hpp>
 #include <set>
 #include <cfloat>
 
@@ -159,7 +159,7 @@ namespace J3ML::Geometry
 
             Vector3 Dir = ((Vector3)v[f[j].v[0]] + (Vector3)v[f[j].v[1]] + (Vector3)v[f[j].v[2]]) * 0.333333333333f - point;
 
-            //if (Dir.Normalize() <= 0.f)
+            //if (Dir.Normalized() <= 0.f)
                 //continue;
             Ray r(Vector3(), Dir);
 
@@ -368,7 +368,7 @@ namespace J3ML::Geometry
             Vector4 b = Vector4(poly.v[face.v[1]], 1.f);
             Vector4 c = Vector4(poly.v[face.v[2]], 1.f);
             Vector4 normal = (b-a).Cross(c-a);
-            normal.Normalize();
+            normal.Normalized();
             return normal;
 //		return ((vec)v[face.v[1]]-(vec)v[face.v[0]]).Cross((vec)v[face.v[2]]-(vec)v[face.v[0]]).Normalized();
         }
@@ -386,7 +386,7 @@ namespace J3ML::Geometry
                 normal.z += (double(poly.v[v0].x) - poly.v[v1].x) * (double(poly.v[v0].y) + poly.v[v1].y); // Project on xy
                 v0 = v1;
             }
-            normal.Normalize();
+            normal.Normalized();
             return normal;
 #if 0
             cv bestNormal;
@@ -397,7 +397,7 @@ namespace J3ML::Geometry
 		{
 			cv c = poly.v[face.v[i]];
 			cv normal = (c-b).Cross(a-b);
-			float len = normal.Normalize();
+			float len = normal.Normalized();
 			if (len > bestLen)
 			{
 				bestLen = len;
@@ -441,7 +441,7 @@ namespace J3ML::Geometry
 			cv edge = cv(poly.v[i]) - cv(poly.v[bestV0]);
 			edge.Normalize();
 			cv normal = bestEdge.Cross(edge);
-			cs len = normal.Normalize();
+			cs len = normal.Normalized();
 			if (len > bestLen)
 			{
 				bestLen = len;
@@ -508,6 +508,8 @@ namespace J3ML::Geometry
         return true;
     }
 
+    bool Polyhedron::EulerFormulaHolds() const { return NumVertices() + NumFaces() - NumEdges() == 2;}
+
     bool Polyhedron::ContainsConvex(const Vector3 &point, float epsilon) const
     {
         assert(IsConvex());
@@ -638,6 +640,64 @@ namespace J3ML::Geometry
         return false;
     }
 
+    template <typename T>
+    bool PolyhedronIntersectsAABB_OBB(const Polyhedron& p, const T& obj) {
+        if (p.Contains(obj.CenterPoint()))
+            return true;
+        if (obj.Contains(p.ApproximateConvexCentroid())) // @bug: This is not correct for concave polyhedrons!
+            return true;
+
+        // Test for each edge of the AABB / OBB whether this polyhedron intersects it.
+        for(int i = 0; i < obj.NumEdges(); ++i)
+            if (p.Intersects(obj.Edge(i)))
+                return true;
+
+        // Test for each edge of this polyhedron whether the AABB / OBB intersects it.
+        for(size_t i = 0; i < p.f.size(); ++i)
+        {
+            assert(!p.f[i].v.empty()); // Cannot have degenerate faces here, and for performance reasons, don't start checking for this condition in release mode!
+            int v0 = p.f[i].v.back();
+            Vector3 l0 = p.v[v0];
+            for(size_t j = 0; j < p.f[i].v.size(); ++j)
+            {
+                int v1 = p.f[i].v[j];
+                Vector3 l1 = p.v[v1];
+                if (v0 < v1 && obj.Intersects(LineSegment(l0, l1))) // If v0 < v1, then this line segment is the canonical one.
+                    return true;
+                l0 = l1;
+                v0 = v1;
+            }
+        }
+
+        return false;
+
+    }
+
+    bool Polyhedron::Intersects(const AABB &aabb) const {
+        return PolyhedronIntersectsAABB_OBB(*this, aabb);
+    }
+
+    bool Polyhedron::Intersects(const OBB& obb) const {
+        return PolyhedronIntersectsAABB_OBB(*this, obb);
+    }
+
+    bool Polyhedron::Intersects(const Frustum& obb) const {
+        return PolyhedronIntersectsAABB_OBB(*this, obb);
+    }
+
+    bool Polyhedron::Intersects(const Triangle& obb) const {
+        return PolyhedronIntersectsAABB_OBB(*this, obb);
+    }
+
+    bool Polyhedron::Intersects(const Sphere& sphere) const {
+        Vector3 closestPt = ClosestPoint(sphere.Position);
+        return closestPt.DistanceSq(sphere.Position) <= sphere.Radius * sphere.Radius;
+    }
+
+    bool Polyhedron::Intersects(const Polygon& polygon) const {
+        return Intersects(polygon.ToPolyhedron());
+    }
+
     bool Polyhedron::Intersects(const Capsule &capsule) const {
         Vector3 pt, ptOnLineSegment;
         pt = ClosestPoint(capsule.l, &ptOnLineSegment);
@@ -677,6 +737,22 @@ namespace J3ML::Geometry
         return closestPt;
     }
 
+    Vector3 Polyhedron::ClosestPoint(const Vector3 &point) const {
+        if (Contains(point))
+            return point;
 
+        Vector3 closestPoint = Vector3::NaN;
+        float closestDistance = Math::Infinity;
+
+        for (int i = 0; i < NumFaces(); ++i) {
+            Vector3 closestOnPoly = FacePolygon(i).ClosestPoint(point);
+            float d = closestOnPoly.DistanceSq(point);
+            if (d < closestDistance) {
+                closestPoint = closestOnPoly;
+                closestDistance = d;
+            }
+        }
+        return closestPoint;
+    }
 }
 

src/J3ML/Geometry/QuadTree.cpp

@@ -1,4 +1,4 @@
-#include <J3ML/Geometry/QuadTree.h>
+#include <J3ML/Geometry/QuadTree.hpp>
 
 namespace Geometry
 {

src/J3ML/Geometry/Ray.cpp

@@ -1,5 +1,5 @@
-#include <J3ML/Geometry/Ray.h>
-#include <J3ML/Geometry/Sphere.h>
+#include <J3ML/Geometry/Ray.hpp>
+#include <J3ML/Geometry/Sphere.hpp>
 
 
 namespace J3ML::Geometry

src/J3ML/Geometry/Sphere.cpp

@@ -1,4 +1,4 @@
-#include <J3ML/Geometry/Sphere.h>
+#include <J3ML/Geometry/Sphere.hpp>
 
 namespace J3ML::Geometry
 {
@@ -26,6 +26,46 @@ namespace J3ML::Geometry
         return extremePoint;
     }
 
+    void Sphere::Translate(const Vector3 &offset) {
+        Position = Position + offset;
+    }
+
+    void Sphere::Transform(const Matrix3x3 &transform) {
+        Position = transform * Position;
+    }
+
+    void Sphere::Transform(const Matrix4x4 &transform) {
+        Position = transform * Position;
+    }
+
+    float Sphere::Cube(float inp) {
+        return inp*inp*inp;
+    }
+
+    float Sphere::Volume() const {
+        return 4.f * M_PI * Cube(Radius) / 3.f;
+    }
+
+    float Sphere::SurfaceArea() const {
+        return 4.f * M_PI * Cube(Radius) / 3.f;
+    }
+
+    bool Sphere::IsFinite() const {
+        return Position.IsFinite() && std::isfinite(Radius);
+    }
+
+    bool Sphere::IsDegenerate() const {
+        return !(Radius > 0.f) || !Position.IsFinite();
+    }
+
+    bool Sphere::Contains(const Vector3 &point) const {
+        return Position.DistanceSquared(point) <= Radius*Radius;
+    }
+
+    bool Sphere::Contains(const Vector3 &point, float epsilon) const {
+        return Position.DistanceSquared(point) <= Radius*Radius + epsilon;
+    }
+
     Vector3 Sphere::RandomPointOnSurface(RNG &rng) const {
         Vector3 v = Vector3::Zero;
         // Rejection sampling analysis: The unit sphere fills ~52.4% of the volume of its enclosing box,

src/J3ML/Geometry/Triangle.cpp

@@ -1,12 +1,29 @@
-#include <J3ML/Geometry/Triangle.h>
-#include <J3ML/LinearAlgebra.h>
-#include <J3ML/Geometry/AABB.h>
-#include <J3ML/Geometry/LineSegment.h>
-#include <J3ML/Geometry/Line.h>
-#include <J3ML/Geometry/Capsule.h>
+#include <J3ML/Geometry/Triangle.hpp>
+#include <J3ML/LinearAlgebra.hpp>
+#include <J3ML/Geometry/AABB.hpp>
+#include <J3ML/Geometry/LineSegment.hpp>
+#include <J3ML/Geometry/Line.hpp>
+#include <J3ML/Geometry/Capsule.hpp>
 
 namespace J3ML::Geometry
 {
+    bool Triangle::Intersects(const LineSegment &l, float *d, Vector3 *intersectionPoint) const {
+        /** The Triangle-Line/LineSegment/Ray intersection tests are based on M&ouml;ller-Trumbore method:
+                "T. M&ouml;ller, B. Trumbore. Fast, Minimum Storage Ray/Triangle Intersection. 2005."
+                http://jgt.akpeters.com/papers/MollerTrumbore97/. */
+        float u,v;
+        float t = IntersectLineTri(l.A, l.B - l.A, V0, V1, V2, u, v);
+
+        bool success = (t >= 0.f && t <= 1.f);
+        if (!success)
+            return false;
+        if (d)
+            *d = t;
+        if (intersectionPoint)
+            *intersectionPoint = l.GetPoint(t);
+        return true;
+    }
+
     Interval Triangle::ProjectionInterval(const Vector3& axis) const {
         // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter4/generic_sat.html
         float min = axis.Dot(V0);
@@ -105,6 +122,12 @@ namespace J3ML::Geometry
         return aabb;
     }
 
+    Vector3 Triangle::Centroid() const {
+        return (V0 + V1 + V2) * (1.f/3.f);
+    }
+
+    Vector3 Triangle::CenterPoint() const { return Centroid();}
+
     bool Triangle::Intersects(const AABB &aabb) const {
         return aabb.Intersects(*this);
     }

src/J3ML/Geometry/TriangleMesh.cpp

@@ -1 +1,10 @@
-#include <J3ML/Geometry/TriangleMesh.h>
+#include <J3ML/Geometry/TriangleMesh.hpp>
+
+#include <J3ML/LinearAlgebra/Vector3.hpp>
+
+TriangleMesh::TriangleMesh(int expectedPolygonCount) {
+    //Vertices.reserve(expectedPolygonCount);
+    //Normals.reserve(expectedPolygonCount);
+    //UVs.reserve(expectedPolygonCount);
+    //Indices.reserve(expectedPolygonCount);
+}

src/J3ML/J3ML.cpp

@@ -1,38 +1,210 @@
-#include <J3ML/J3ML.h>
+/// 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.cpp
+/// @edit 2024-07-05
+
+#include <format>
+#include <iomanip>
+#include <strstream>
+#include "J3ML/J3ML.hpp"
+
+
+float PowUInt(float base, u32 exponent)
+{
+    // 'Fast Exponentiation': We interpret exponent in base two and calculate the power by
+    // squaring and multiplying by base.
+
+    // Find the highest bit that is set.
+    u32 e = 0x80000000;
+    while((exponent & e) == 0 && e > 0)
+        e >>= 1;
+
+    float val = 1.f;
+    do
+    {
+        val *= val; // Shifts the exponent one place left
+        val *= (exponent & e) != 0 ? base : 1.f; // Adds a 1 as the LSB of the exponent
+        e >>= 1;
+    } while(e > 0);
+
+    return val;
+}
 
 
 namespace J3ML
 {
 
-    Math::Rotation Math::operator ""_degrees(long double rads) { return {Radians((float)rads)}; }
+    float Math::Functions::Radians(float degrees) { return degrees * (Pi/180.f); }
 
-    Math::Rotation Math::operator ""_deg(long double rads) { return {Radians((float)rads)}; }
+    float Math::Functions::Degrees(float radians) { return radians * (180.f/Pi); }
+
+
+
+    Math::Rotation Math::operator ""_degrees(long double rads) { return {Functions::Radians((float)rads)}; }
+
+    Math::Rotation Math::operator ""_deg(long double rads) { return {Functions::Radians((float)rads)}; }
 
     Math::Rotation Math::operator ""_radians(long double rads) { return {(float)rads}; }
 
     Math::Rotation Math::operator ""_rad(long double rads) { return {(float)rads}; }
 
-    float Math::FastRSqrt(float x) {
+    float Math::Functions::FastRSqrt(float x) {
         return 1.f / FastSqrt(x);
     }
 
-    float Math::RSqrt(float x) {
+    float Math::Functions::PingPongMod(float x, float mod) {
+        x = Mod(x, mod * 2.f);
+        return x >= mod ? (2.f * mod - x) : x;
+    }
+
+    float Math::Functions::Sqrt(float x) {
+        return std::sqrt(x);
+    }
+
+    float Math::Functions::FastSqrt(float x) {
+        // TODO: implement FastSqrt from MGL
+        return std::sqrt(x);
+    }
+
+    float Math::Functions::RSqrt(float x) {
         return 1.f / Sqrt(x);
     }
 
-    float Math::Radians(float degrees) { return degrees * (Pi/180.f); }
 
-    float Math::Degrees(float radians) { return radians * (180.f/Pi); }
 
-    bool Math::EqualAbs(float a, float b, float epsilon) {
-        return std::abs(a - b) < epsilon;
+
+    int SigFigsTable[] = {0,0,0,1,0,0,1,0,0,1};
+
+    int DivBy[] = {1,1,1, 1000,1000,1000, 1000000, 1000000, 1000000, 1000000000, 1000000000,1000000000};
+
+    std::vector<std::string> Suffixes = {
+        "", "", "",
+        "K", "K", "K",
+        "M", "M", "M",
+        "B", "B", "B",
+        "T", "T", "T",
+        "Q", "Q", "Q"
+    };
+
+    float Math::Functions::Round(float f, float decimalPlaces) {
+        float mult = Pow(10, decimalPlaces);
+        return Floor(f * mult + 0.5f) / mult;
     }
 
-    float Math::RecipFast(float x) {
+    float Math::Functions::Sign(float f) { return f >= 0.f ? 1.f : -1.f;}
+
+    std::string Math::Functions::Truncate(float input) {
+
+        std::stringstream ss;
+        std::string str = "";
+
+        if (input < 1000)
+            ss << std::fixed << std::setprecision(0) << input;
+        else {
+            int figs = CeilInt(Log10(input)) - 1;
+            auto suffix = Suffixes[figs];
+            auto roundTo = SigFigsTable[figs];
+            auto divBy = DivBy[figs];
+            auto fractional = input / (float)divBy;
+
+            // Increment roundTo for extra precision!!
+
+            ss << std::fixed << std::setprecision(roundTo) << fractional << suffix;
+        }
+
+
+
+        str = ss.str();
+        return str;
+
+    }
+
+    bool Math::Functions::Equal(float a, float b, float epsilon) {
+        return Abs(a-b) <= epsilon;
+    }
+
+    bool Math::Functions::Equal(double a, double b, float epsilon) {
+        return Abs(a - b) <= epsilon;
+    }
+
+    float Math::Functions::RelativeError(float a, float b) {
+        if (a == b) return 0.f; // Handles the special case where approximation and real are both zero.
+        return Abs((a-b) / Max(Abs(a), Abs(b)));
+    }
+
+    bool Math::Functions::EqualAbs(float a, float b, float epsilon) {
+        // TODO: No different from Equal?
+        return Abs(a - b) < epsilon;
+    }
+
+    float Math::Functions::RecipFast(float x) {
         // TODO: Implement SSE rcp instruction.
         return 1.f / x;
     }
 
+    float Math::Functions::Lerp(float a, float b, float t) { return a + t * (b-a);}
+
+    float Math::Functions::LerpMod(float a, float b, float mod, float t) {
+        a = ModPos(a, mod);
+        b = ModPos(b, mod);
+        if (Abs(b - a) * 2.f <= mod)
+            return Lerp(a, b, t);
+        else {
+            if (a < b)
+                return ModPos(Lerp(a + mod, b, t), mod);
+            else
+                return ModPos(Lerp(a, b + mod, t), mod);
+        }
+    }
+
+    float Math::Functions::InverseLerp(float a, float b, float x) {
+        assert(Abs(b - a) > 1e-5f);
+        return (x - a) / (b - a);
+    }
+
+
+    float Math::Functions::Step(float y, float x) {
+        return (x >= y) ? 1.f : 0.f;
+    }
+
+    float Math::Functions::Ramp(float min, float max, float x) {
+
+        return x <= min ? 0.f : (x >= max ? 1.f : (x - min) / (max - min));
+    }
+
+    float Math::Functions::Mod(float x, float mod) {
+        return std::fmod(x, mod);
+    }
+
+    float Math::Functions::Mod(float x, int mod) {
+        // TODO: Optimize?
+        return std::fmod(x,(float)mod);
+    }
+
+    float Math::Functions::ModPos(float x, float mod) {
+        float m = fmod(x, mod);
+        return m >= 0.f ? m : (m + mod);
+    }
+
+    float Math::Functions::ModPos(float x, int mod) {
+        return ModPos(x, (float)mod);
+    }
+
+    float Math::Functions::Frac(float x) {
+        return x - Floor(x);
+    }
+
+    float Math::Functions::Recip(float x) {
+        return 1.f / x;
+    }
+
+
+
     Math::Rotation::Rotation() : valueInRadians(0) {}
 
     Math::Rotation::Rotation(float value) : valueInRadians(value) {}
@@ -49,4 +221,91 @@ namespace J3ML
     int Math::BitTwiddling::CountBitsSet(u32 value) {
 
     }
-}
+
+    namespace Math::Functions {
+        float Sin(float x) {
+#ifdef USE_LOOKUP_TABLES
+#elif USE_SSE
+#else
+            return std::sin(x);
+#endif
+        }
+
+        float Cos(float x) {
+#ifdef USE_LOOKUP_TABLES
+#elif USE_SSE
+#else
+            return std::cos(x);
+#endif
+        }
+
+        float Tan(float x) { return std::tan(x); }
+
+        void SinCos(float x, float &outSin, float &outCos) {
+            outSin = Sin(x);
+            outCos = Cos(x);
+        }
+
+        float Asin(float x) { return std::asin(x); }
+
+        float Acos(float x) { return std::acos(x); }
+
+        float Atan(float x) { return std::atan(x); }
+
+        float Atan2(float y, float x) { return std::atan2(y, x); }
+
+        float Sinh(float x) { return std::sinh(x); }
+
+        float Cosh(float x) { return std::cosh(x); }
+
+        float Tanh(float x) { return std::tanh(x); }
+
+        bool IsPow2(u32 number) {
+            return (number & (number - 1)) == 0;
+        }
+
+        bool IsPow2(u64 number) {
+            return (number & (number - 1)) == 0;
+        }
+
+        float PowInt(float base, int exponent) {
+            if (exponent == 0)
+                return 1.f;
+            else if (exponent < 0)
+                return 1.f / PowUInt(base, (u32) -exponent);
+            else return PowUInt(base, (u32) exponent);
+        }
+
+        float Pow(float base, float exponent) { return std::pow(base, exponent); }
+
+        float Exp(float exp) { return std::exp(exp); }
+
+        float Log(float base, float value) {
+            return std::log(value) / std::log(base);
+        }
+
+        float Log2(float value) {
+            return Log(2.f, value);
+        }
+
+        float Ln(float value) {
+            return std::log(value);
+        }
+
+        float Log10(float value) {
+            return Log(10, value);
+        }
+
+        float Ceil(float f) { return std::ceil(f); }
+        int CeilInt(float f) { return (int)std::ceil(f);}
+
+        float Floor(float x) { return std::floor(x); }
+
+        int FloorInt(float x) { return (int)std::floor(x); }
+
+        float Round(float x) { return Floor(x+0.5f); }
+
+        int RoundInt(float x) { return (int)Round(x); }
+
+    }
+}

src/J3ML/LinearAlgebra.cpp

@@ -1,4 +1,4 @@
-#include "J3ML/LinearAlgebra.h"
+#include "J3ML/LinearAlgebra.hpp"
 #include <cassert>
 
 namespace LinearAlgebra {

src/J3ML/LinearAlgebra/AxisAngle.cpp

@@ -1,5 +1,5 @@
-#include <J3ML/LinearAlgebra/AxisAngle.h>
-#include <J3ML/LinearAlgebra/Quaternion.h>
+#include <J3ML/LinearAlgebra/AxisAngle.hpp>
+#include <J3ML/LinearAlgebra/Quaternion.hpp>
 namespace J3ML::LinearAlgebra {
 
     AxisAngle::AxisAngle() : axis(Vector3::Zero) {}

src/J3ML/LinearAlgebra/CoordinateFrame.cpp

@@ -1 +1 @@
-#include <J3ML/LinearAlgebra/CoordinateFrame.h>
+#include <J3ML/LinearAlgebra/CoordinateFrame.hpp>

src/J3ML/LinearAlgebra/EulerAngle.cpp

@@ -1,4 +1,4 @@
-#include <J3ML/LinearAlgebra/EulerAngle.h>
+#include <J3ML/LinearAlgebra/EulerAngle.hpp>
 #include <cmath>
 #include <algorithm>
 
@@ -89,6 +89,42 @@ namespace J3ML::LinearAlgebra {
         roll = std::atan2(x * s - y * z * t, 1 - (x*x + z*z) * t);
     }
 
+    AxisAngle EulerAngle::ToAxisAngle() const {
+        auto c1 = std::cos(yaw / 2);
+        auto c2 = std::cos(pitch / 2);
+        auto c3 = std::cos(roll / 2);
+        auto s1 = std::sin(yaw / 2);
+        auto s2 = std::sin(pitch / 2);
+        auto s3 = std::sin(roll / 2);
+
+        auto angle = 2 * std::acos(c1*c2*c3 - s1*s2*s3);
+
+        auto x = s1*s2*c3 + c1*c2*s3;
+        auto y = s1*c2*c3 + c1*s2*s3;
+        auto z = c1*s2*c3 - s1*c2*s3;
+
+        // todo: normalize?
+        // sqrt(x^2 + y^2 + z^2) = sqrt((s1 s2 c3 +c1 c2 s3)^2+(s1 c2 c3 + c1 s2 s3)^2+(c1 s2 c3 - s1 c2 s3)^2)
+
+        return {{x,y,z}, angle};
+    }
+
+    Quaternion EulerAngle::ToQuaternion() const {
+        auto c1 = std::cos(yaw / 2);
+        auto c2 = std::cos(pitch / 2);
+        auto c3 = std::cos(roll / 2);
+        auto s1 = std::sin(yaw / 2);
+        auto s2 = std::sin(pitch / 2);
+        auto s3 = std::sin(roll / 2);
+
+        auto w = c1*c2*c3 - s1*s2*s3;
+        auto x = s1*s2*c3 + c1*c2*s3;
+        auto y = s1*c2*c3 + c1*s2*s3;
+        auto z = c1*s2*c3 - s1*c2*s3;
+
+        return {w,x,y,z};
+    }
+
     EulerAngle::EulerAngle(const Quaternion &rhs) {
         double test = rhs.x * rhs.y + rhs.z * rhs.w;
         if (test > 0.499) { // Singularity at north pole

src/J3ML/LinearAlgebra/Matrix2x2.cpp

@@ -1,4 +1,5 @@
-#include <J3ML/LinearAlgebra/Matrix2x2.h>
+#include <J3ML/LinearAlgebra/Matrix2x2.hpp>
+#include <J3ML/LinearAlgebra/Vector2.hpp>
 
 namespace J3ML::LinearAlgebra {
 

src/J3ML/LinearAlgebra/Matrix3x3.cpp

@@ -1,5 +1,8 @@
-#include <J3ML/LinearAlgebra/Matrix3x3.h>
+#include <J3ML/LinearAlgebra/Matrix3x3.hpp>
+#include <J3ML/LinearAlgebra/Matrix4x4.hpp>
 #include <J3ML/LinearAlgebra/Matrices.inl>
+#include <J3ML/LinearAlgebra/Vector3.hpp>
+#include <J3ML/LinearAlgebra/Vector4.hpp>
 #include <cmath>
 
 namespace J3ML::LinearAlgebra {
@@ -110,6 +113,25 @@ namespace J3ML::LinearAlgebra {
         SetRotatePart(orientation);
     }
 
+    Matrix3x3::Matrix3x3(const EulerAngle &orientation) {
+        auto sa = std::sin(orientation.pitch);
+        auto ca = std::cos(orientation.pitch);
+        auto sb = std::sin(orientation.roll);
+        auto cb = std::cos(orientation.roll);
+        auto sh = std::sin(orientation.yaw);
+        auto ch = std::cos(orientation.yaw);
+
+        At(0, 0) = ch*ca;
+        At(0, 1) = -ch*sa*cb + sh*sh;
+        At(0, 2) = ch*sa*sb + sh*cb;
+        At(1, 0) = sa;
+        At(1, 1) = ca*cb;
+        At(1, 2) = -ca*cb;
+        At(2, 0) = -sh*ca;
+        At(2, 1) = sh*sa*cb + ch*sb;
+        At(2, 2) = -sh*sa*sb + ch*cb;
+    }
+
     float Matrix3x3::Determinant() const {
         const float a = elems[0][0];
         const float b = elems[0][1];
@@ -383,7 +405,7 @@ namespace J3ML::LinearAlgebra {
     }
 
     Vector4 Matrix3x3::Mul(const Vector4 &rhs) const {
-        return {Mul(rhs.XYZ()), rhs.GetW()};
+        return Vector4(Mul(rhs.XYZ()), rhs.GetW());
     }
 
     Vector3 Matrix3x3::Mul(const Vector3 &rhs) const {
@@ -771,6 +793,10 @@ namespace J3ML::LinearAlgebra {
         x[2] = b[2] / v22;
         x[1] = b[1] - x[2] * v12;
         x[0] = b[0] - x[2] * v02 - x[1] * v01;
+
+
+
+        return true;
     }
 
     void Matrix3x3::ScaleCol(int col, float scalar) {
@@ -972,6 +998,19 @@ namespace J3ML::LinearAlgebra {
         return false;
     }
 
+    EulerAngle Matrix3x3::ToEulerAngle() const {
+        auto heading = std::atan2(-At(2, 0), At(0, 0));
+        auto attitude = std::asin(At(1, 0));
+        auto bank = std::atan2(-At(1,2), At(1,1));
+        if (At(1, 0) == 1 || At(1, 0) == -1) // North Pole || South Pole
+        {
+            heading = std::atan2(At(0, 2), At(2,2));
+            bank = 0;
+        }
+
+        return {attitude, heading, bank};
+    }
+
     void Matrix3x3::BatchTransform(Vector3 *pointArray, int numPoints, int stride) const {
         assert(pointArray || numPoints == 0);
         assert(stride >= (int)sizeof(Vector3));

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -1,5 +1,9 @@
-#include <J3ML/LinearAlgebra/Matrix4x4.h>
-#include <J3ML/LinearAlgebra/Vector4.h>
+#include <J3ML/LinearAlgebra/Matrix4x4.hpp>
+#include <J3ML/LinearAlgebra/Vector4.hpp>
+#include <J3ML/LinearAlgebra/Matrix3x3.hpp>
+#include <J3ML/LinearAlgebra/Quaternion.hpp>
+#include <J3ML/LinearAlgebra/Matrices.inl>
+
 
 namespace J3ML::LinearAlgebra {
 
@@ -78,7 +82,12 @@ namespace J3ML::LinearAlgebra {
     }
 
     Matrix4x4::Matrix4x4(const Quaternion &orientation) {
+        // https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
+        Set3x3Part(Matrix3x3(orientation));
+    }
 
+    Matrix4x4::Matrix4x4(const EulerAngle &orientation) {
+        Set3x3Part(Matrix3x3(orientation));
     }
 
     void Matrix4x4::SetTranslatePart(float translateX, float translateY, float translateZ) {
@@ -336,6 +345,11 @@ namespace J3ML::LinearAlgebra {
         return At(0, 0) * Minor(0,0) - At(0, 1) * Minor(0,1) + At(0, 2) * Minor(0,2) - At(0, 3) * Minor(0,3);
     }
 
+    float Matrix4x4::Determinant4() const {
+        assert(IsFinite());
+        return At(0,0) * Minor(0, 0) - At(0, 1) * Minor(0,1) + At(0,2) * Minor(0,2) - At(0, 3) * Minor(0, 3);
+    }
+
     float Matrix4x4::Determinant3x3() const {
 
         const float a = elems[0][0];
@@ -396,10 +410,20 @@ namespace J3ML::LinearAlgebra {
         return true;
     }
 
+    bool Matrix4x4::IsIdentity(float epsilon) const {
+        for (int iy = 0; iy < Rows; ++iy)
+            for (int ix = 0; ix < Cols; ++ix)
+                if (!Math::EqualAbs(elems[iy][ix], (ix == iy) ? 1.f : 0.f, epsilon))
+                    return false;
+        return true;
+    }
+
     Vector3 Matrix4x4::GetColumn3(int index) const {
         return Vector3{At(0, index), At(1, index), At(2, index)};
     }
 
+    Vector3 Matrix4x4::Column3(int index) const { return GetColumn3(index);}
+
     Vector2 Matrix4x4::operator*(const Vector2 &rhs) const { return this->Transform(rhs);}
 
     Vector3 Matrix4x4::operator*(const Vector3 &rhs) const { return this->Transform(rhs);}
@@ -617,6 +641,11 @@ namespace J3ML::LinearAlgebra {
         return {GetColumn3(0).Length(), GetColumn3(1).Length(), GetColumn3(2).Length()};
     }
 
+    void Matrix4x4::RemoveScale() {
+        float tx = Row3(0).Normalize();
+        float ty = Row3(1).Normalize();
+    }
+
     bool Matrix4x4::IsColOrthogonal(float epsilon) const {
         return IsColOrthogonal3(epsilon);
     }
@@ -727,6 +756,40 @@ namespace J3ML::LinearAlgebra {
         return elems[0][0] + elems[1][1] + elems[2][2] + elems[3][3];
     }
 
+    Quaternion Matrix4x4::ToQuat() const {
+        auto m00 = At(0,0);
+        auto m01 = At(0, 1);
+        auto m02 = At(0, 2);
+        auto m10 = At(1,0);
+        auto m11 = At(1, 1);
+        auto m12 = At(1, 2);
+        auto m20 = At(2,0);
+        auto m21 = At(2, 1);
+        auto m22 = At(2, 2);
+
+        auto w = std::sqrt(1.f + m00 + m11 + m22) / 2.f;
+        float w4 = (4.f * w);
+        return {
+            (m21 - m12) / w4,
+            (m02 - m20) / w4,
+            (m10 - m01) / w4,
+            w
+        };
+    }
+
+    EulerAngle Matrix4x4::ToEulerAngle() const {
+        auto heading = std::atan2(-At(2, 0), At(0, 0));
+        auto attitude = std::asin(At(1, 0));
+        auto bank = std::atan2(-At(1,2), At(1,1));
+        if (At(1, 0) == 1 || At(1, 0) == -1) // North Pole || South Pole
+        {
+            heading = std::atan2(At(0, 2), At(2,2));
+            bank = 0;
+        }
+
+        return {attitude, heading, bank};
+    }
+
     bool Matrix4x4::InverseOrthogonalUniformScale() {
         assert(!ContainsProjection());
         assert(IsColOrthogonal(1e-3f));
@@ -804,6 +867,8 @@ namespace J3ML::LinearAlgebra {
         p[1][0] = 0;           p[1][1] = 2.f * n / v; p[1][2] = 0;         p[1][3] = 0.f;
         p[2][0] = 0;           p[2][1] = 0;           p[2][2] = f / (f-n); p[2][3] = n * f / (n-f);
         p[3][0] = 0;           p[3][1] = 0;           p[3][2] = 1.f;       p[3][3] = 0.f;
+
+        return p;
     }
 
     Matrix4x4 Matrix4x4::D3DPerspProjRH(float n, float f, float h, float v) {
@@ -879,6 +944,13 @@ namespace J3ML::LinearAlgebra {
         return Matrix4x4::Translate(translate) * Matrix4x4(rotate) * Matrix4x4::Scale(scale);
     }
 
+
+    void Matrix4x4::SetCol3(int col, const Vector3& xyz) {
+        SetCol3(col, xyz.x, xyz.y, xyz.z);
+    }
+
+    bool Matrix4x4::HasNegativeScale() const { return Determinant() < 0.f; }
+
     void Matrix4x4::SetCol3(int col, float x, float y, float z) {
         // TODO: implement assertations
         At(0, col) = x;

src/J3ML/LinearAlgebra/Quaternion.cpp

@@ -1,8 +1,9 @@
-#include <J3ML/LinearAlgebra/Vector3.h>
-#include <J3ML/LinearAlgebra/Vector4.h>
-#include <J3ML/LinearAlgebra/Matrix3x3.h>
-#include <J3ML/LinearAlgebra/Matrix4x4.h>
-#include <J3ML/LinearAlgebra/Quaternion.h>
+#include <J3ML/LinearAlgebra/Vector3.hpp>
+#include <J3ML/LinearAlgebra/Vector4.hpp>
+#include <J3ML/LinearAlgebra/Matrix3x3.hpp>
+#include <J3ML/LinearAlgebra/Matrix4x4.hpp>
+#include <J3ML/LinearAlgebra/Quaternion.hpp>
+#include <J3ML/LinearAlgebra/AxisAngle.hpp>
 
 namespace J3ML::LinearAlgebra {
 
@@ -69,6 +70,16 @@ namespace J3ML::LinearAlgebra {
         return Quaternion(x * scalar, y * scalar, z * scalar, w * scalar);
     }
 
+    Quaternion Quaternion::operator/(float scalar) const {
+        assert(!Math::EqualAbs(scalar, 0.f));
+
+        return *this * (1.f / scalar);
+    }
+
+    Vector3 Quaternion::operator*(const Vector3 &rhs) const {
+        return Transform(rhs);
+    }
+
     Quaternion Quaternion::operator+() const { return *this; }
 
     Quaternion::Quaternion() {}
@@ -80,8 +91,11 @@ namespace J3ML::LinearAlgebra {
         return x * rhs.x + y * rhs.y + z * rhs.z + w * rhs.w;
     }
 
-    Quaternion::Quaternion(Vector4 vector4) {
-
+    Quaternion::Quaternion(const Vector4& rhs) {
+        x = rhs.x;
+        y = rhs.y;
+        z = rhs.z;
+        w = rhs.w;
     }
 
     Quaternion Quaternion::Normalized() const {
@@ -153,6 +167,10 @@ namespace J3ML::LinearAlgebra {
         return delta.Normalized().Angle();
     }
 
+    Vector4 Quaternion::operator*(const Vector4 &rhs) const {
+        return Transform(rhs);
+    }
+
     Quaternion Quaternion::operator/(const Quaternion &rhs) const {
         return {
                 x*rhs.w - y*rhs.z + z*rhs.y - w*rhs.x,
@@ -249,7 +267,7 @@ namespace J3ML::LinearAlgebra {
             if (lenSq >= 1e-6f && lenSq <= 1.f)
                 return Quaternion(x,y,z,w) / std::sqrt(lenSq);
         }
-        assert(false && "Quaternion::RandomRotation failed!");
+        //assert(false && "Quaternion::RandomRotation failed!");
         return Quaternion::Identity;
     }
 

src/J3ML/LinearAlgebra/Transform2D.cpp

@@ -1,4 +1,4 @@
-#include <J3ML/LinearAlgebra/Transform2D.h>
+#include <J3ML/LinearAlgebra/Transform2D.hpp>
 
 namespace J3ML::LinearAlgebra {
 

src/J3ML/LinearAlgebra/Vector2.cpp

@@ -1,4 +1,4 @@
-#include <J3ML/LinearAlgebra/Vector2.h>
+#include <J3ML/LinearAlgebra/Vector2.hpp>
 #include <cassert>
 #include <algorithm>
 #include <valarray>
@@ -461,6 +461,13 @@ namespace J3ML::LinearAlgebra {
         b -= a.Dot(b) * a;
     }
 
+    Vector2 Vector2::RandomBox(Algorithm::RNG &rng, float minElem, float maxElem) {
+        float x = rng.Float(minElem, maxElem);
+        float y = rng.Float(minElem, maxElem);
+
+        return {x, y};
+    }
+
     bool Vector2::AreOrthogonal(const Vector2 &a, const Vector2 &b, float epsilon) {
         return a.IsPerpendicular(b, epsilon);
     }

src/J3ML/LinearAlgebra/Vector3.cpp

@@ -1,8 +1,8 @@
-#include <J3ML/LinearAlgebra/Vector3.h>
+#include <J3ML/LinearAlgebra/Vector3.hpp>
 #include <algorithm>
 #include <cassert>
 #include <cmath>
-#include <J3ML/Geometry/Sphere.h>
+#include <J3ML/Geometry/Sphere.hpp>
 
 namespace J3ML::LinearAlgebra {
 
@@ -105,7 +105,7 @@ namespace J3ML::LinearAlgebra {
 
     bool Vector3::operator!=(const Vector3& rhs) const
     {
-        return this->IsWithinMarginOfError(rhs) == false;
+        return !this->IsWithinMarginOfError(rhs);
     }
 
 
@@ -318,9 +318,9 @@ namespace J3ML::LinearAlgebra {
     Angle2D Vector3::AngleBetween(const Vector3 &rhs) const {
         const auto Pi_x_180 = 180.f / M_PI;
         auto dist = this->Distance(rhs);
-        float x = -(asinf((rhs.y - this->y) / dist));
-        float y = (atan2f(rhs.x - this->x,rhs.z - this->z));
-        return {x, y};
+        float dx = -(asinf((rhs.y - this->y) / dist));
+        float dy = (atan2f(rhs.x - this->x,rhs.z - this->z));
+        return {dx, dy};
     }
 
     Angle2D Vector3::AngleBetween(const Vector3 &lhs, const Vector3 &rhs) // TODO: 3D Angle representation?
@@ -329,12 +329,28 @@ namespace J3ML::LinearAlgebra {
     }
 
     Vector3 Vector3::Direction(const Vector3 &rhs) {
-        float x = (cos(Math::Radians(rhs.y)) * cos(Math::Radians(rhs.x)));
-        float y = -sin(Math::Radians(rhs.x));
-        float z = (sin(Math::Radians(rhs.y)) * cos(Math::Radians(rhs.x)));
+        float x = (Math::Cos(Math::Radians(rhs.y)) * Math::Cos(Math::Radians(rhs.x)));
+        float y = -Math::Sin(Math::Radians(rhs.x));
+        float z = (Math::Sin(Math::Radians(rhs.y)) * Math::Cos(Math::Radians(rhs.x)));
         return {x, y, z};
     }
 
+    float Vector3::ScaleToLength(float newLength) {
+        float length = LengthSquared();
+        if (length < 1e-6f) {
+            Set(newLength, 0, 0); // Will always produce a vector of the requested length.
+            return 0.f;
+        }
+
+        length = Math::Sqrt(length);
+        float scalar = newLength / length;
+        x *= scalar;
+        y *= scalar;
+        z *= scalar;
+
+        return length;
+    }
+
     Vector3 Vector3::Abs() const {
         return {std::abs(x), std::abs(y), std::abs(z)};
     }
@@ -468,6 +484,17 @@ namespace J3ML::LinearAlgebra {
         return Vector3::Zero;
     }
 
+
+    Vector3 Vector3::AnotherPerpendicular(const J3ML::LinearAlgebra::Vector3 &hint,
+                                          const J3ML::LinearAlgebra::Vector3 &hint2) const {
+        assert(!this->IsZero());
+        assert(hint.IsNormalized());
+        assert(hint2.IsNormalized());
+        Vector3 firstPerpendicular = Perpendicular(hint, hint2);
+        Vector3 v = this->Cross(firstPerpendicular);
+        return v.Normalized();
+    }
+
     float Vector3::TryNormalize() {
         assert(IsFinite());
         float length = Length();
@@ -501,6 +528,15 @@ namespace J3ML::LinearAlgebra {
         z = d2;
     }
 
+    std::string Vector3::ToString() const {
+        return std::format("{},{},{}", x, y, z);
+    }
+
+    std::ostream & operator<<(std::ostream &o, const Vector3 &vector) {
+        o << vector.ToString();
+        return o;
+    }
+
     Vector3::Vector3(const Vector2& XY, float Z) {
         x = XY.x;
         y = XY.y;
@@ -550,9 +586,36 @@ namespace J3ML::LinearAlgebra {
         return RandomBox(rng, {min,min,min}, {max,max,max});
     }
 
+    Vector3 Vector3::RotateAroundAxis(const Vector3 &vec, const Vector3 &axis, const float radians) {
+        float cosTheta = cos(radians);
+        float sinTheta = sin(radians);
+        return vec * cosTheta + Vector3::Cross(axis, vec) * sinTheta + axis * Vector3::Dot(axis, vec) * (1 - cosTheta);
+    }
+
     Vector3 Vector3::RandomBox(RNG &rng, float xmin, float xmax, float ymin, float ymax, float zmin, float zmax) {
         return RandomBox(rng, {xmin,ymin,zmin}, {xmax,ymax,zmax});
     }
 
+    Vector3 Vector3::Add(float s) const {
+        return *this + Vector3(s, s, s);
+    }
+
+    Vector3 Vector3::ScaledToLength(float newLength) const {
+        assert(!IsZero());
+
+        Vector3 v = *this;
+        v.ScaleToLength(newLength);
+        return v;
+    }
+
+    void Vector3::PerpendicularBasis(Vector3 &outB, Vector3 &outC) const {
+        // Pixar orthonormal basis code: https://graphics.pixar.com/library/OrthonormalB/paper.pdf
+        float sign = copysignf(1.0f, z);
+        const float a = -1.0f / (sign + z);
+        const float b = x * y * a;
+        outB = Vector3(1.0f + sign * x * x * a, sign * b,             -sign * x);
+        outC = Vector3(                      b, sign + y * y * a,            -y);
+    }
+
 
 }

src/J3ML/LinearAlgebra/Vector4.cpp

@@ -2,13 +2,16 @@
 #pragma region vector4
 
 
-#include <J3ML/LinearAlgebra/Vector4.h>
-#include <J3ML/LinearAlgebra/Vector3.h>
+#include <J3ML/LinearAlgebra/Vector4.hpp>
+#include <J3ML/LinearAlgebra/Vector3.hpp>
 #include <cmath>
 #include <algorithm>
 
 namespace J3ML::LinearAlgebra {
 
+    using namespace J3ML::Math;
+
+
     const Vector4 Vector4::Zero = {0,0,0,0};
     const Vector4 Vector4::NaN = {NAN, NAN, NAN, NAN};
 
@@ -18,6 +21,30 @@ namespace J3ML::LinearAlgebra {
     Vector4::Vector4(float X, float Y, float Z, float W): x(X),y(Y),z(Z),w(W)
     { }
 
+    bool Vector4::IsFinite() const {
+        return std::isfinite(x) && std::isfinite(y) && std::isfinite(z) && std::isfinite(w);
+    }
+
+    bool Vector4::IsPerpendicular(const Vector4 &other, float epsilonSq) const {
+        float dot = Dot(other);
+        return dot*dot <= epsilonSq * LengthSquared() * other.LengthSquared();
+    }
+
+    bool Vector4::IsPerpendicular3(const Vector4 &other, float epsilonSq) const {
+        float dot = Dot3(other);
+        return dot*dot <= epsilonSq * LengthSq() * other.LengthSq();
+    }
+
+    void Vector4::NormalizeW() {
+        if (std::abs(w) > 1e-6f) {
+            float invW = 1.f / w;
+            x *= invW;
+            y *= invW;
+            z *= invW;
+            w = 1.f;
+        }
+    }
+
 bool Vector4::operator==(const Vector4& rhs) const
 {
     return this->IsWithinMarginOfError(rhs);
@@ -38,6 +65,10 @@ Vector4 Vector4::Min(const Vector4& min) const
     };
 }
 
+Vector4 Vector4::Max(float floor) const {
+    return {std::max(x, floor), std::max(y, floor), std::max(z, floor), std::max(w, floor)};
+}
+
 Vector4 Vector4::Max(const Vector4& max) const
 {
     return {
@@ -48,6 +79,14 @@ Vector4 Vector4::Max(const Vector4& max) const
     };
 }
 
+Vector4 Vector4::Clamp(float floor, float ceil) const {
+    return {Math::Clamp(x, floor, ceil),
+            Math::Clamp(y, floor, ceil),
+            Math::Clamp(z, floor, ceil),
+            Math::Clamp(w, floor, ceil)
+    };
+}
+
 Vector4 Vector4::Clamp(const Vector4& min, const Vector4& max) const
 {
     return {
@@ -63,9 +102,14 @@ float Vector4::Distance(const Vector4& to) const
     return ( (*this) - to ).Magnitude();
 }
 
-float Vector4::Length() const
-{
-    return std::sqrt(LengthSquared());
+float Vector4::Length4() const {
+    return std::sqrt(x*x + y*y + z*z + w*w);
+}
+
+float Vector4::Length() const { return Length4(); }
+
+float Vector4::LengthSquared4() const {
+    return x*x + y*y + z*z + w*w;
 }
 
 float Vector4::LengthSquared() const
@@ -73,6 +117,14 @@ float Vector4::LengthSquared() const
     return (x*x + y*y + z*z + w*w);
 }
 
+float Vector4::LengthSq4() const { return LengthSquared4();}
+
+float Vector4::LengthSq() const { return LengthSquared();}
+
+float Vector4::LengthSquared3() const { return LengthSq3(); }
+
+float Vector4::LengthSq3() const { return x*x + y*y + z*z; }
+
 float Vector4::Magnitude() const
 {
     return std::sqrt(x*x + y*y + z*z + w*w);
@@ -80,8 +132,8 @@ float Vector4::Magnitude() const
 
 float Vector4::Dot(const Vector4& rhs) const
 {
-    auto a = this->Normalize();
-    auto b = rhs.Normalize();
+    auto a = this->Normalized();
+    auto b = rhs.Normalized();
 
     return  a.x * b.x +
             a.y * b.y +
@@ -89,13 +141,31 @@ float Vector4::Dot(const Vector4& rhs) const
             a.w * b.w;
 }
 
+float Vector4::Dot3(const Vector3 &rhs) const {
+    return x * rhs.x + y * rhs.y + z * rhs.z;
+}
+
+float Vector4::Dot3(const Vector4 &rhs) const {
+    return x * rhs.x + y * rhs.y + z * rhs.z;
+}
+
 Vector4 Vector4::Project(const Vector4& rhs) const
 {
     float scalar = this->Dot(rhs) / (rhs.Magnitude()* rhs.Magnitude());
     return rhs * scalar;
 }
 
-Vector4 Vector4::Normalize() const
+void Vector4::Normalize()
+{
+    if (Length() > 0) {
+        x = x / Length();
+        y = y / Length();
+        z = z / Length();
+        w = w / Length();
+    }
+}
+
+Vector4 Vector4::Normalized() const
 {
     if (Length() > 0)
         return {
@@ -150,6 +220,36 @@ Vector4 Vector4::operator-(const Vector4& rhs) const
         return *this;
     }
 
+    Vector4::Vector4(const float *data) {
+        assert(data);
+        x = data[0];
+        y = data[1];
+        z = data[2];
+        w = data[3];
+    }
+
+    float * Vector4::ptr() { return &x; }
+
+    const float * Vector4::ptr() const { return &x; }
+
+    Vector2 Vector4::XY() const { return {x,y}; }
+
+    Vector3 Vector4::XYZ() const { return {x, y, z}; }
+
+    void Vector4::Set(float x_, float y_, float z_, float w_) {
+        this->x = x_;
+        this->y = y_;
+        this->z = z_;
+        this->w = w_;
+    }
+
+    void Vector4::Set(const Vector4 &rhs) {
+        x = rhs.x;
+        y = rhs.y;
+        z = rhs.z;
+        w = rhs.w;
+    }
+
     bool Vector4::Equals(const Vector4 &rhs, float epsilon) const {
         return std::abs(x - rhs.x) < epsilon &&
                std::abs(y - rhs.y) < epsilon &&
@@ -164,6 +264,10 @@ Vector4 Vector4::operator-(const Vector4& rhs) const
                std::abs(w - _w) < epsilon;
     }
 
+    Vector4 Vector4::Min(float ceil) const {
+        return {std::min(x, ceil), std::min(y, ceil), std::min(z, ceil), std::min(w, ceil)};
+    }
+
     float Vector4::operator[](std::size_t index) const {
         assert(index < 4);
         if (index==0) return x;
@@ -180,12 +284,39 @@ Vector4 Vector4::operator-(const Vector4& rhs) const
         if (index == 2) return z;
         if (index == 3) return w;
 
+        return x; // This point should never be reached.
+    }
+
+    float Vector4::At(int index) const {
+        assert(index >= 0);
+        assert(index < Size);
+        return ptr()[index];
+    }
+
+    float & Vector4::At(int index) {
+        assert(index >= 0);
+        assert(index < Size);
+        return ptr()[index];
     }
 
     float Vector4::LengthSqXYZ() const {
         return x*x * y*y * z*z;
     }
 
+    bool Vector4::IsNormalized3(float epsilonSq) const {
+        return std::abs(LengthSqXYZ()-1.f) <= epsilonSq;
+    }
+
+    bool Vector4::IsNormalized4(float epsilonSq) const {
+        return std::abs(LengthSquared()-1.f) <= epsilonSq;
+    }
+
+    bool Vector4::IsNormalized(float epsilonSq) const { return IsNormalized4(epsilonSq); }
+
+    bool Vector4::IsZero3(float epsilonSq) const {
+        return LengthSq3() <= epsilonSq;
+    }
+
     Vector4 Vector4::Cross3(const Vector3 &rhs) const {
         Vector4 dst;
         dst.x = y * rhs.z - z * rhs.y;
@@ -199,6 +330,6 @@ Vector4 Vector4::operator-(const Vector4& rhs) const
         return Cross3(rhs.XYZ());
     }
 
-
+    Vector4 Vector4::Cross(const Vector4 &rhs) const { return Cross3(rhs); }
 }
 #pragma endregion