j3ml autodoc test

.gitea/workflows/demo.yaml

@@ -23,3 +23,6 @@ jobs:
       - run: echo "${{ secrets.SSHKEY }}" > /id_rsa
       - run: chmod 600 /id_rsa
       - run: ssh -o "IdentitiesOnly=yes" -o "StrictHostKeyChecking=no" -i /id_rsa ${{ secrets.SSHUSER }}@${{ secrets.SSHIP }} -p ${{ secrets.SSHPORT }} 'touch cock'
+      - run: apt-get install doxygen
+      - run: doxygen
+      - run: scp -r -o "IdentitiesOnly=yes" -o "StrictHostKeyChecking=no" -i /id_rsa -P ${{ secrets.SSHPORT }} ${{ secrets.SSHUSER }}@${{ secrets.SSHIP }}:/home/${{ secrets.SSHUSER }}/j3mldoc

CMakeLists.txt

@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 3.18)
+PROJECT(J3ML
+        VERSION 1.1
+        LANGUAGES CXX
+)
+
+if (PROJECT_SOURCE_DIR STREQUAL PROJECT_BINARY_DIR)
+    message(FATAL_ERROR "In-source builds are not allowed")
+endif()
+
+set(CMAKE_CXX_STANDARD 20)
+#set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+if (WIN32)
+    set(CMAKE_CXX_FLAGS "-municode")
+endif(WIN32)
+
+
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
+
+# Enable Package Managers
+include(cmake/CPM.cmake)
+#include(cmake/gtest.cmake)
+
+include(CTest)
+
+file(GLOB_RECURSE J3ML_HEADERS "include/J3ML/*.h" "include/J3ML/*.hpp")
+file(GLOB_RECURSE J3ML_SRC     "src/J3ML/*.c" "src/J3ML/*.cpp")
+
+include_directories("include")
+
+add_library(J3ML SHARED ${J3ML_SRC}
+        include/J3ML/Geometry/Common.h
+        src/J3ML/Geometry/Triangle.cpp)
+set_target_properties(J3ML PROPERTIES LINKER_LANGUAGE CXX)
+
+install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME})
+
+install(FILES ${J3ML_HEADERS} DESTINATION include/${PROJECT_NAME})
+
+add_subdirectory(tests)
+
+add_executable(MathDemo main.cpp)
+target_link_libraries(MathDemo ${PROJECT_NAME})

README.md

@@ -1 +1,17 @@
-lol
+# Josh's 3D Math Library - J3ML
+
+Yet Another C++ Math Standard
+
+## Motivation
+This project was sparked by a desire to gain a deeper understanding into the math underlying computer graphics. While packages such as glm, eigen, etc. are amazing libraries, it removes the fun of having to learn it the hard way.
+
+## Use Cases
+## Features
+### LinearAlgebra
+#### Vectors
+#### Matrices
+#### Conversion Types
+### Geometry
+## Samples
+## Bugs / Issues
+## Compilation

cmake/CPM.cmake

@@ -0,0 +1,24 @@
+# SPDX-License-Identifier: MIT
+#
+# SPDX-FileCopyrightText: Copyright (c) 2019-2023 Lars Melchior and contributors
+
+set(CPM_DOWNLOAD_VERSION 0.38.7)
+set(CPM_HASH_SUM "83e5eb71b2bbb8b1f2ad38f1950287a057624e385c238f6087f94cdfc44af9c5")
+
+if(CPM_SOURCE_CACHE)
+  set(CPM_DOWNLOAD_LOCATION "${CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake")
+elseif(DEFINED ENV{CPM_SOURCE_CACHE})
+  set(CPM_DOWNLOAD_LOCATION "$ENV{CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake")
+else()
+  set(CPM_DOWNLOAD_LOCATION "${CMAKE_BINARY_DIR}/cmake/CPM_${CPM_DOWNLOAD_VERSION}.cmake")
+endif()
+
+# Expand relative path. This is important if the provided path contains a tilde (~)
+get_filename_component(CPM_DOWNLOAD_LOCATION ${CPM_DOWNLOAD_LOCATION} ABSOLUTE)
+
+file(DOWNLOAD
+     https://github.com/cpm-cmake/CPM.cmake/releases/download/v${CPM_DOWNLOAD_VERSION}/CPM.cmake
+     ${CPM_DOWNLOAD_LOCATION} EXPECTED_HASH SHA256=${CPM_HASH_SUM}
+)
+
+include(${CPM_DOWNLOAD_LOCATION})

cmake/gtest.cmake

@@ -0,0 +1,68 @@
+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/DifferentialSolvers.h

@@ -0,0 +1,42 @@
+//
+// Created by dawsh on 2/8/24.
+//
+
+namespace J3ML::Algorithm
+{
+    /// Implementations for a variety of Differential Equation Solving algorithms
+    namespace Solvers
+    {
+
+        // Consider a differential equation
+        // dy/dx = (x + y + xy)
+        float eq(float x, float y)
+        {
+            return (x + y + x*y);
+        }
+
+        // Accelleration = Velocity / Time
+        // Velocity = Position / Time
+        // Position = Vector3
+
+        //
+
+        float euler(float x0, float y, float h, float x)
+        {
+            float temp = -0.f;
+            // Iterating till the point at which we need approximation
+            while (x0 < x) {
+                temp = y;
+                y = y + h * eq(x0, y);
+                x0 = x0 + h;
+            }
+            return y;
+        }
+
+        class EulerMethodSolver {};
+        class SemiImplicitEulerMethodSolver {};
+        class GaussSeidelMethodSolver {};
+        class GradientDescentSolver {};
+        class VerletIntegrationSolver {};
+    }
+}

include/J3ML/Algorithm/GJK.h

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

include/J3ML/Algorithm/RNG.h

@@ -0,0 +1,99 @@
+#pragma once
+
+
+#include "J3ML/J3ML.h"
+
+namespace J3ML::Algorithm
+{
+/** @brief A linear congruential random number generator.
+
+	Uses D.H. Lehmer's Linear Congruential Method (1949) for generating random numbers.
+	Supports both Multiplicative Congruential Method (increment==0) and
+	Mixed Congruential Method (increment!=0)
+	It is perhaps the simplest and fastest method to generate pseudo-random numbers on
+	a computer. Per default uses the values for Minimal Standard LCG.
+	http://en.wikipedia.org/wiki/Linear_congruential_generator
+	http://www.math.rutgers.edu/~greenfie/currentcourses/sem090/pdfstuff/jp.pdf
+
+	Pros:
+	<ul>
+	    <li> Easy to implement.
+	    <li> Fast.
+	</ul>
+
+	Cons:
+	<ul>
+	    <li> NOT safe for cryptography because of the easily calculatable sequential
+	        correlation between successive calls. A case study:
+	        http://www.cigital.com/papers/download/developer_gambling.php
+
+	    <li> Tends to have less random low-order bits (compared to the high-order bits)
+	         Thus, NEVER do something like this:
+
+	           u32 numBetween1And10 = 1 + LCGRand.Int() % 10;
+
+	         Instead, take into account EVERY bit of the generated number, like this:
+
+	           u32 numBetween1And10 = 1 + (int)(10.0 * (double)LCGRand.Int()
+	                                                      /(LCGRand.Max()+1.0));
+	         or simply
+
+	           u32 numBetween1And10 = LCGRand.Float(1.f, 10.f);
+	</ul> */
+
+
+    class RNG {
+    public:
+        /// Initializes the generator from the current system clock.
+        RNG();
+        RNG(u32 seed, u32 multiplier = 69621,
+            u32 increment = 0, u32 modulus = 0x7FFFFFFF) // 2^31 - 1
+        {
+            Seed(seed, multiplier, increment, modulus);
+        }
+        /// Reinitializes the generator to the new settings.
+        void Seed(u32 seed, u32 multiplier, u32 increment, u32 modulus = 0x7FFFFFFF);
+        /// Returns a random integer picked uniformly in the range [0, MaxInt()]
+        u32 Int();
+        /// Returns the biggest number the generator can yield. [modulus - 1]
+        u32 MaxInt() const { return modulus - 1;}
+        /// Returns a random integer picked uniformly in the range [0, 2^32-1].
+        /// @note The configurable modulus and increment are not used by this function, but are always increment == 0, modulus=2^32
+        u32 IntFast();
+        /// Returns a random integer picked uniformly in the range [a, b]
+        /// @param a Lower bound, inclusive.
+        /// @param b Upper bound, inclusive.
+        /// @return A random integer picked uniformly in the range [a, b]
+        int Int(int a, int b);
+
+        /// Returns a random float picked uniformly in the range [0, 1].
+        float Float();
+
+        /// Returns a random float picked uniformly in the range [0, 1].
+        /// @note this is much slower than Float()! Prefer that function if possible.
+        float Float01Incl();
+
+        /// Returns a random float picked uniformly in the range ]-1, 1[.
+        /// @note This function has one more bit of randomness compared to Float(), but has a theoretical bias
+        /// towards 0.0, since floating point has two representations for 0 (+0, and -0).
+        float FloatNeg1_1();
+
+        /// Returns a random float picked uniformly in the range [a, b[.
+        /// @param a Lower bound, inclusive.
+        /// @param b Upper bound, exclusive.
+        /// @return A random float picked uniformly in the range [a, b[
+        /// @note This function is slower than RNG::FloatIncl(). If you don't care about the open/closed interval, prefer that function.
+        float Float(float a, float b);
+
+        /// Returns a random float picked uniformly in the range [a, b].
+        /// @param a Lower bound, inclusive.
+        /// @param b Upper bound, inclusive.
+        /// @return A random float picked uniformly in the range [a, b]
+        float FloatIncl(float a, float b);
+
+        u32 multiplier;
+        u32 increment;
+        u32 modulus;
+        u32 lastNumber;
+    };
+}

include/J3ML/Algorithm/Spring.h

@@ -0,0 +1,21 @@
+//
+// Created by dawsh on 2/8/24.
+//
+
+namespace J3ML::Algorithm
+{
+    // Numerical model of a "Spring" object
+    // Simulates any oscillating system i.e. a mass suspended from a spring.
+    class Spring
+    {
+        float Dampening;
+        float Stiffness;
+        float Goal;
+        float RestLength = 1.f;
+
+        bool Overdamped() const;
+        bool Undamped() const;
+        bool Underdamped() const;
+        bool CriticallyDamped() const;
+    };
+}

include/J3ML/Geometry.h

@@ -0,0 +1,23 @@
+#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/AABB.h

@@ -0,0 +1,175 @@
+#pragma once
+
+#include <J3ML/LinearAlgebra.h>
+#include <J3ML/Geometry/Common.h>
+#include <J3ML/Geometry/Shape.h>
+#include "J3ML/Algorithm/RNG.h"
+
+
+namespace J3ML::Geometry
+{
+
+
+    using namespace J3ML::LinearAlgebra;
+    using J3ML::Algorithm::RNG;
+    // A 3D axis-aligned bounding box
+    // This data structure can be used to represent coarse bounds of objects, in situations where detailed triangle-level
+    // computations can be avoided. In physics systems, bounding boxes are used as an efficient early-out test for geometry
+    // intersection queries.
+    // the 'Axis-aligned' part in the name means that the local axes of this bounding box are restricted to align with the
+    // axes of the world space coordinate system. This makes computation involving AABB's very fast, since AABB's cannot
+    // be arbitrarily oriented in the space with respect to each other.
+    // If you need to represent a box in 3D space with arbitrary orientation, see the class OBB. */
+
+    class AABB : public Shape {
+    public:
+        Vector3 minPoint;
+        Vector3 maxPoint;
+    public:
+        static int NumFaces() { return 6; }
+        static int NumEdges() { return 12; }
+        static int NumVertices() { return 8; }
+    public:
+        AABB();
+        AABB(const Vector3& min, const Vector3& max);
+
+        Vector3 HalfDiagonal() const { return HalfSize(); }
+
+        static AABB FromCenterAndSize(const Vector3 &center, const Vector3 &size);
+
+        float MinX() const;
+        float MinY() const;
+        float MinZ() const;
+        float MaxX() const;
+        float MaxY() const;
+        float MaxZ() const;
+
+        /// Returns the smallest sphere that contains this AABB.
+        /// This function computes the minimal volume sphere that contains all the points inside this AABB
+        Sphere MinimalEnclosingSphere() const;
+
+        Vector3 HalfSize() const;
+
+        /// Returns the largest sphere that can fit inside this AABB
+        /// This function computes the largest sphere that can fit inside this AABB.
+        Sphere MaximalContainedSphere() const;
+
+        bool IsFinite() const;
+
+        Vector3 Centroid() const;
+
+        Vector3 Size() const;
+
+        // Quickly returns an arbitrary point inside this AABB
+        Vector3 AnyPointFast() const;
+
+        Vector3 PointInside(float x, float y, float z) const;
+
+        // Returns an edge of this AABB
+        LineSegment Edge(int edgeIndex) const;
+
+        Vector3 CornerPoint(int cornerIndex) const;
+
+        Vector3 ExtremePoint(const Vector3 &direction) const;
+        Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
+
+        Vector3 PointOnEdge(int edgeIndex, float u) const;
+
+        Vector3 FaceCenterPoint(int faceIndex) const;
+
+        Vector3 FacePoint(int faceIndex, float u, float v) const;
+
+        Vector3 FaceNormal(int faceIndex) const;
+
+        Plane FacePlane(int faceIndex) const;
+
+        void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
+
+        static AABB MinimalEnclosingAABB(const Vector3 *pointArray, int numPoints);
+        float GetVolume() const;
+        float GetSurfaceArea() const;
+        Vector3 GetClosestPoint(const Vector3& point) const;
+
+        void Translate(const Vector3& offset);
+        AABB Translated(const Vector3& offset) const;
+        AABB TransformAABB(const Matrix3x3& transform);
+        AABB TransformAABB(const Matrix4x4& transform);
+        AABB TransformAABB(const Quaternion& transform);
+        OBB Transform(const Matrix3x3& transform) const;
+        OBB Transform(const Matrix4x4& transform) const;
+        OBB Transform(const Quaternion& transform) const;
+        bool Contains(const Vector3& point) const;
+        bool Contains(const Vector3& aabbMinPoint, const Vector3& aabbMaxPoint) const;
+        bool Contains(const LineSegment& lineSegment) const;
+        bool Contains(const AABB& aabb) const;
+        bool Contains(const OBB& obb) const;
+        bool Contains(const Sphere& sphere) const;
+        bool Contains(const Triangle& triangle) const;
+        bool Contains(const Polygon& polygon) const;
+        bool Contains(const Frustum& frustum) const;
+        bool Contains(const Polyhedron& polyhedron) const;
+        bool Contains(const Capsule& capsule) const;
+        // Tests whether this AABB and the given object intersect.
+        bool Intersects(const Ray& ray, float dNear, float dFar) const;
+        bool Intersects(const Capsule& capsule) const;
+        bool Intersects(const Triangle& triangle) const;
+        bool Intersects(const Polygon& polygon) const;
+        bool Intersects(const Frustum& frustum) const;
+        bool Intersects(const Polyhedron& polyhedron) const;
+        TriangleMesh Triangulate(int numFacesX, int numFacesY, int numFacesZ, bool ccwIsFrontFacing) const;
+
+        /// Finds the set intersection of this and the given AABB.
+        /** @return This function returns the AABB that is contained in both this and the given AABB.
+            @todo Add Intersection(OBB/Polyhedron). */
+        AABB Intersection(const AABB& rhs) const;
+
+
+        void SetFrom(const Vector3 *pVector3, int i);
+
+        void SetFromCenterAndSize(const Vector3 &center, const Vector3 &size);
+
+        void SetFrom(const OBB &obb);
+
+        void SetFrom(const Sphere &s);
+
+        Vector3 GetRandomPointInside(RNG& rng) const;
+        Vector3 GetRandomPointOnSurface(RNG& rng) const;
+        Vector3 GetRandomPointOnEdge(RNG& rng) const;
+        Vector3 GetRandomCornerPoint(RNG& rng) const;
+
+        void SetNegativeInfinity();
+
+        void Enclose(const Vector3 &point);
+
+        void Enclose(const Vector3 &aabbMinPt, const Vector3 &aabbMaxPt);
+
+        void Enclose(const LineSegment &lineSegment);
+
+        void Enclose(const OBB &obb);
+
+        bool TestAxis(const Vector3& axis, const Vector3& v0, const Vector3& v1, const Vector3& v2) const;
+
+        bool Intersects(const LineSegment &lineSegment) const;
+        /// Computes the intersection of a line, ray or line segment and an AABB.
+        /** Based on "T. Kay, J. Kajiya. Ray Tracing Complex Scenes. SIGGRAPH 1986 vol 20, number 4. pp. 269-"
+            http://www.siggraph.org/education/materials/HyperGraph/raytrace/rtinter3.htm
+            @param linePos The starting position of the line.
+            @param lineDir The direction of the line. This direction vector must be normalized!
+            @param tNear [in, out] For the test, the input line is treated as a line segment. Pass in the signed distance
+                from the line origin to the start of the line. For a Line-AABB test, -FLOAT_INF is typically passed here.
+                For a Ray-AABB test, 0.0f should be inputted. If intersection occurs, the signed distance from line origin
+                to the line entry point in the AABB is returned here.
+            @param tFar [in, out] Pass in the signed distance from the line origin to the end of the line. For Line-AABB and
+                Ray-AABB tests, pass in FLOAT_INF. For a LineSegment-AABB test, pass in the length of the line segment here.
+                If intersection occurs, the signed distance from line origin to the line exit point in the AABB
+                is returned here.
+            @return True if an intersection occurs, false otherwise.
+            @note This is a low level utility function. It may be more convenient to use one of the AABB::Intersects()
+                functions instead.
+            @see Intersects(). */
+        bool IntersectLineAABB(const Vector3& linePos, const Vector3& lineDir, float tNear, float tFar) const;
+        bool IntersectLineAABB_CPP(const Vector3 &linePos, const Vector3 &lineDir, float &tNear, float &tFar) const;
+    };
+
+
+}

include/J3ML/Geometry/AABB2D.h

@@ -0,0 +1,54 @@
+#pragma once
+
+#include <J3ML/LinearAlgebra/Vector2.h>
+#include "Shape.h"
+
+namespace J3ML::Geometry
+{
+    using LinearAlgebra::Vector2;
+    // CaveGame AABB
+    class AABB2D : public Shape2D
+    {
+    public:
+
+        Vector2 minPoint;
+        Vector2 maxPoint;
+        AABB2D() {}
+        AABB2D(const Vector2& min, const Vector2& max):
+        minPoint(min), maxPoint(max)
+        {}
+
+        float Width() const;
+        float Height() const;
+
+        float DistanceSq(const Vector2& pt) const;
+
+        void SetNegativeInfinity();
+
+        void Enclose(const Vector2& point);
+
+        bool Intersects(const AABB2D& rhs) const;
+
+        bool Contains(const AABB2D& rhs) const;
+
+        bool Contains(const Vector2& pt) const;
+
+        bool Contains(int x, int y) const;
+
+        bool IsDegenerate() const;
+
+        bool HasNegativeVolume() const;
+
+        bool IsFinite() const;
+
+        Vector2 PosInside(const Vector2 &normalizedPos) const;
+
+        Vector2 ToNormalizedLocalSpace(const Vector2 &pt) const;
+
+
+        AABB2D operator+(const Vector2& pt) const;
+
+        AABB2D operator-(const Vector2& pt) const;
+
+    };
+}

include/J3ML/Geometry/Capsule.h

@@ -0,0 +1,79 @@
+#pragma once
+
+#include "LineSegment.h"
+#include "Shape.h"
+#include <J3ML/LinearAlgebra/Vector3.h>
+#include <J3ML/Geometry/Common.h>
+
+namespace J3ML::Geometry
+{
+    // TODO: Move to separate math lib, find duplicates!
+    template <typename T>
+    void Swap(T &a, T &b)
+    {
+        T temp = std::move(a);
+        a = std::move(b);
+        b = std::move(temp);
+    }
+
+    using namespace LinearAlgebra;
+    class Capsule : public Shape
+    {
+    public:
+        // Specifies the two inner points of this capsule
+        LineSegment l;
+        // Specifies the radius of this capsule
+        float r;
+    public:
+        Capsule();
+        Capsule(const LineSegment& endPoints, float radius);
+        Capsule(const Vector3& bottomPt, const Vector3& topPt, float radius);
+
+        /// Quickly returns an arbitrary point inside this Capsule. Used in GJK intersection test.
+        inline Vector3 AnyPointFast() const { return l.A; }
+
+        /// Computes the extreme point of this Capsule in the given direction.
+        /** An extreme point is a farthest point of this Capsule in the given direction. Given a direction,
+            this point is not necessarily unique.
+            @param direction The direction vector of the direction to find the extreme point. This vector may
+                be unnormalized, but may not be null.
+            @return The extreme point of this Capsule in the given direction. */
+        Vector3 ExtremePoint(const Vector3 &direction) const;
+        Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
+
+        bool IsDegenerate() const;
+        float Height() const;
+        float Diameter() const;
+        Vector3 Bottom() const;
+        Vector3 Center() const;
+        Vector3 Centroid() const;
+        Vector3 ExtremePoint(const Vector3& direction);
+        AABB MinimalEnclosingAABB() const;
+
+        void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
+
+        bool Intersects(const Plane &plane) const;
+
+        bool Intersects(const Ray &ray) const;
+
+        bool Intersects(const Line &line) const;
+
+        bool Intersects(const LineSegment &lineSegment) const;
+
+        bool Intersects(const AABB &aabb) const;
+
+        bool Intersects(const OBB &obb) const;
+
+        bool Intersects(const Sphere &sphere) const;
+
+        bool Intersects(const Capsule &capsule) const;
+
+        bool Intersects(const Triangle &triangle) const;
+
+        bool Intersects(const Polygon &polygon) const;
+
+        bool Intersects(const Frustum &frustum) const;
+
+        bool Intersects(const Polyhedron &polyhedron) const;
+    };
+}

include/J3ML/Geometry/Common.h

@@ -0,0 +1,31 @@
+#pragma once
+
+
+// Forward declarations for classes that include each other
+namespace J3ML::Geometry
+{
+    class Shape;
+    class AABB2D;
+    class AABB;
+    class Capsule;
+    class Frustum;
+    class LineSegment;
+    class Line;
+    class OBB;
+    class Plane;
+    class Polygon;
+    class Polyhedron;
+    template<typename T> class QuadTree;
+    class Ray;
+    class Shape;
+    class Sphere;
+    class Triangle;
+    class Triangle2D;
+    class TriangleMesh;
+}
+
+// Methods required by Geometry types
+namespace J3ML::Geometry
+{
+
+}

include/J3ML/Geometry/Frustum.h

@@ -0,0 +1,243 @@
+//
+// Created by dawsh on 1/25/24.
+//
+#pragma once
+
+#include <J3ML/Geometry/Common.h>
+#include "Plane.h"
+#include "Shape.h"
+#include <J3ML/LinearAlgebra.h>
+
+namespace J3ML::Geometry
+{
+    /// A frustum can be set to one of the two common different forms.
+    enum class FrustumType
+    {
+        Invalid = 0,
+
+        /// Set the Frustum type to this value to define the orthographic projection formula. In orthographic projection,
+        /// 3D images are projected onto a 2D plane essentially by flattening the object along one direction (the plane normal).
+        /// The size of the projected images appear the same independent of their distance to the camera, and distant objects will
+        /// not appear smaller. The shape of the Frustum is identical to an oriented bounding box (OBB).
+        Orthographic,
+
+        /// Set the Frustum type to this value to use the perspective projection formula. With perspective projection, the 2D
+        /// image is formed by projecting 3D points towards a single point (the eye point/tip) of the Frustum, and computing the
+        /// point of intersection of the line of the projection and the near plane of the Frustum.
+        /// This corresponds to the optics in the real-world, and objects become smaller as they move to the distance.
+        /// The shape of the Frustum is a rectangular pyramid capped from the tip.
+        Perspective
+    };
+
+    /// The Frustum class offers choosing between the two common conventions for the value ranges in
+    /// post-projective space. If you are using either the OpenGL or Direct3D API, you must feed the API data that matches
+    /// the correct convention.
+    enum class FrustumProjectiveSpace
+    {
+        Invalid = 0,
+        /// If this option is chosen, the post-projective unit cube of the Frustum
+        /// is modelled after the OpenGL API convention, meaning that in projected space,
+        /// points inside the Frustum have the X and Y range in [-1, 1] and Z ranges in [-1, 1],
+        /// where the near plane maps to Z=-1 and the far plane maps to Z=1.
+        /// @note If you are submitting projection matrices to GPU hardware using the OpenGL API,
+        ///       you **must** use this convention. (or otherwise more than half of the precision of the GL depth buffer is wasted)
+        GL,
+
+        /// If this option is chosen, the post-projective unit cube is modelled after the
+        /// Direct3D API convention, which differs from the GL convention that Z ranges in [0, 1] instead.
+        /// Near plane maps to Z=0, and far plane maps to Z=1. The X and Y ranges in [-1, 1] as is with GL.
+        /// @note If you are submitting projection matrices to GPU hardware using the Direct3D API,
+        ///       you **must** use this convention.
+        D3D,
+    };
+
+    /// The handedness rule in J3ML bundles together two different conventions related to the camera:
+    /// * the chirality of the world and view spaces,
+    /// * the fixed local front direction of the Frustum.
+    /// @note The world and view spaces are always assumed to the same chirality, meaning that Frustum::ViewMatrix()
+    ///       (and hence Frustum::WorldMatrix()) always returns a matrix with a positive determinant, i.e. it does not mirror.
+    ///       If FrustumRightHanded is chosen, then Frustum::ProjectionMatrix() is a mirroring matrix, since the post-projective space
+    ///       is always left-handed.
+    /// @note Even though in the local space of the camera +Y is always up, in the world space one can use any 'world up' direction
+    ///       as one pleases, by orienting the camera via the Frustum::up vector.
+    enum class FrustumHandedness
+    {
+        Invalid = 0,
+
+        /// If a Frustum is left-handed, then in the local space of the Frustum (the view space), the camera looks towards +Z,
+        /// while +Y goes towards up, and +X goes towards right.
+        /// @note The fixed-pipeline D3D9 API traditionally used the FrustumLeftHanded convention.
+        Left,
+
+        /// If a Frustum is right-handed, then the camera looks towards -Z, +Y is up, and +X is right.
+        /// @note The fixed-pipeline OpenGL API traditionally used the FrustumRightHanded convention.
+        Right
+    };
+
+    /// Represents either an orthographic or a perspective viewing frustum.
+    class Frustum : public Shape {
+    public: /// Members
+
+
+        /// Specifies whether this frustum is a perspective or an orthographic frustum.
+        FrustumType type;
+        /// Specifies whether the [-1, 1] or [0, 1] range is used for the post-projective depth range.
+        FrustumProjectiveSpace projectiveSpace;
+        /// Specifies the chirality of world and view spaces
+        FrustumHandedness handedness;
+        /// The eye point of this frustum
+        Vector3 pos;
+        /// The normalized direction this frustum is watching towards.
+        Vector3 front;
+        /// The normalized up direction for this frustum.
+        /// This vector is specified in world (global) space. This vector is always normalized.
+        /// @note The vectors front and up must always be perpendicular to each other. This means that this up vector is not
+        /// a static/constant up vector, e.g. (0, 1, 0), but changes according to when the camera pitches up and down to
+        /// preserve the condition that front and up are always perpendicular
+        /// @note In the _local_ space of the Frustum, the direction +y is _always_ the up direction and cannot be changed. This
+        /// coincides to how Direct3D and OpenGL view and projection matrices are constructed
+        Vector3 up;
+        /// Distance from the eye point to the front plane
+        /// This parameter must be positive. If perspective projection is used, this parameter must be strictly positive
+        /// (0 is not allowed). If orthographic projection is used, 0 is possible (but uncommon, and not recommended).
+        /// When using the Frustum class to derive perspective projection matrices for a GPU, it should be noted that too
+        /// small values cause poor resolution of Z values near the back plane in post-perspective space, if non-linear
+        /// depth is used (which is common). The larger this value is, the more resolution there is for the Z values across the
+        /// depth range. Too large values cause clipping of geometry when they come very near the camera. */
+        float nearPlaneDistance;
+        /// Distance from the eye point to the back plane of the projection matrix.
+        /// This parameter must be strictly greater than nearPlaneDistance. The range [nearPlaneDistance, farPlaneDistance]
+        //	specifies the visible range of objects inside the Frustum. When using the Frustum class for deriving perspective
+        //	projection matrix for GPU rendering, it should be remembered that any geometry farther from the camera (in Z value)
+        //	than this distance will be clipped from the view, and not rendered.
+        float farPlaneDistance;
+
+        union {
+            /// Horizontal field-of-view, in radians. This field is only valid if type == PerspectiveFrustum.
+            /** @see type. */
+            float horizontalFov;
+            /// The width of the orthographic frustum. This field is only valid if type == OrthographicFrustum.
+            /** @see type. */
+            float orthographicWidth;
+        };
+
+        union {
+            /// Vertical field-of-view, in radians. This field is only valid if type == PerspectiveFrustum.
+            /** @see type. */
+            float verticalFov;
+            /// The height of the orthographic frustum. This field is only valid if type == OrthographicFrustum.
+            /** @see type. */
+            float orthographicHeight;
+        };
+
+        void WorldMatrixChanged();
+        void ProjectionMatrixChanged();
+
+        /// Frustums are typically used in batch culling operations.
+        /// Therefore the matrices associated with a frustum are cached for immediate access.
+        Matrix4x4 worldMatrix;
+        Matrix4x4 projectionMatrix;
+        Matrix4x4 viewProjectionMatrix;
+    public: /// Methods
+        Frustum()
+        : type(FrustumType::Invalid),
+        pos(Vector3::NaN),
+        front(Vector3::NaN),
+        up(Vector3::NaN),
+        nearPlaneDistance(NAN),
+        farPlaneDistance(NAN),
+        worldMatrix(Matrix4x4::NaN),
+        viewProjectionMatrix(Matrix4x4::NaN)
+        {
+            // For conveniency, allow automatic initialization of the graphics API and handedness in use.
+            // If neither of the #defines are set, user must specify per-instance.
+        }
+
+
+        /// 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);
+        AABB MinimalEnclosingAABB() const;
+        OBB MinimalEnclosingOBB() const;
+        void SetKind(FrustumProjectiveSpace projectiveSpace, FrustumHandedness handedness);
+        void SetViewPlaneDistances(float nearPlaneDistance, float farPlaneDistance);
+        void SetFrame(const Vector3& pos, const Vector3& front, const Vector3& up);
+        void SetPos(const Vector3& pos);
+        void SetFront(const Vector3& front);
+        void SetUp(const Vector3& up);
+        void SetPerspective(float horizontalFov, float verticalFov);
+        void SetOrthographic(float orthographicWidth, float orthographicHeight);
+        FrustumHandedness Handedness() const { return handedness; }
+        FrustumType Type() const { return type; }
+        FrustumProjectiveSpace ProjectiveSpace() const { return projectiveSpace;}
+        const Vector3 &Pos() const {return pos;}
+        const Vector3 &Front() const { return front; }
+        const Vector3 &Up() const { return up; }
+        float NearPlaneDistance() const { return nearPlaneDistance; }
+        float FarPlaneDistance() const { return farPlaneDistance;}
+        float HorizontalFov() const { return horizontalFov;}
+        float VerticalFov() const { return verticalFov;}
+        float OrthographicWidth() const { return orthographicWidth; }
+        float OrthograhpicHeight() const { return orthographicHeight; }
+        int NumEdges() const { return 12; }
+        float AspectRatio() const;
+
+        void SetHorizontalFovAndAspectRatio(float horizontalFov, float aspectRatio);
+
+
+        Vector3 CornerPoint(int cornerIndex) const;
+
+        Vector3 NearPlanePos(float x, float y) const;
+        Vector3 FarPlanePos(float x, float y) const;
+
+        Vector3 WorldRight() const
+        {
+            if (handedness == FrustumHandedness::Right)
+                return Vector3::Cross(front, up);
+            else
+                return Vector3::Cross(up, front);
+        }
+
+        Plane TopPlane() const;
+        Plane BottomPlane() const;
+        Plane RightPlane() const;
+        Plane LeftPlane() const;
+        Plane FarPlane() const;
+        Plane NearPlane() const;
+        float NearPlaneWidth() const;
+        float NearPlaneHeight() const;
+
+
+        void Translate(const Vector3& offset);
+        void Transform(const Matrix3x3& transform);
+        void Transform(const Matrix4x4& transform);
+        void Transform(const Quaternion& transform);
+
+
+        Polyhedron ToPolyhedron() const;
+
+        bool Intersects(const Ray& ray) const;
+        //bool Intersects(const Line& line) const;
+        bool Intersects(const LineSegment& lineSegment) const;
+        bool Intersects(const AABB& aabb) const;
+        bool Intersects(const OBB& obb) const;
+        bool Intersects(const 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;
+
+        void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
+
+        void GetCornerPoints(Vector3 *outPointArray) const;
+
+        Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
+
+        LineSegment Edge(int edgeIndex) const;
+
+        bool Intersects(const Line &line) const;
+    };
+}

include/J3ML/Geometry/Line.h

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

include/J3ML/Geometry/LineSegment.h

@@ -0,0 +1,92 @@
+#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/OBB.h

@@ -0,0 +1,106 @@
+#pragma once
+
+#include <J3ML/Geometry/Common.h>
+#include <J3ML/Geometry/AABB.h>
+#include <J3ML/Geometry/Polyhedron.h>
+
+
+namespace J3ML::Geometry {
+    /// A 3D arbitrarily oriented bounding box
+    // This data structure represents a box in 3D space. The local axes of this box can be arbitrarily oriented/rotated
+    // with respect to the global world coordinate system. This allows OBBs to more tightly bound objects than AABBs do,
+    // which always align with the world space axes. This flexibility has the drawback that the geometry tests and operations
+    // involving OBBs are more costly, and representing an OBB in memory takes more space (15 floats vs 6 floats)
+    class OBB : public Shape
+    {
+    public:
+        // The center position of this OBB
+        Vector3 pos;
+        // Stores half-sizes to x, y, and z directions in the local space of this OBB.
+        Vector3 r;
+        // Specifies normalized direction vectors for the local axes
+        Vector3 axis[3];
+
+        /// Default constructor that does not initialize any member values.
+        OBB() {}
+        // Constructs an OBB by explicitly initializing all member values
+        OBB(const Vector3& pos, const Vector3& radii, const Vector3& axis0, const Vector3& axis1, const Vector3& axis2);
+        OBB(const AABB& aabb);
+        inline static int NumFaces() { return 6;  }
+        inline static int NumEdges() { return 12; }
+        inline static int NumVertices() { return 8; }
+
+        Polyhedron ToPolyhedron() const;
+        //PBVolume<6> ToPBVolume() const;
+        AABB MinimalEnclosingAABB() const
+        {
+            AABB aabb;
+            aabb.SetFrom(*this);
+            return aabb;
+        }
+
+        bool Intersects(const LineSegment &lineSegment) const;
+
+        Sphere MinimalEnclosingSphere() const;
+        Sphere MaximalContainedSphere() const;
+        Vector3 Size() const;
+        Vector3 HalfSize() const;
+        Vector3 Diagonal() const;
+        Vector3 HalfDiagonal() const;
+        void Transform(const Matrix3x3& transform);
+        void Transform(const Matrix4x4& transform);
+        void Transform(const Quaternion& transform);
+        bool IsFinite() const;
+        bool IsDegenerate() const;
+        Vector3 CenterPoint() const;
+        Vector3 Centroid() const;
+
+        Vector3 AnyPointFast() const { return pos; }
+
+        float Volume() const;
+        float SurfaceArea() const;
+        Geometry::LineSegment Edge(int edgeIndex) const;
+        Vector3 CornerPoint(int cornerIndex) const;
+
+        Vector3 PointInside(float x, float y, float z) const;
+        Vector3 PointInside(const Vector3& pt) const;
+
+        void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
+
+        int UniqueFaceNormals(Vector3 *out) const;
+
+        int UniqueEdgeDirections(Vector3 *out) const;
+
+        Vector3 PointOnEdge(int edgeIndex, float u) const;
+
+        Vector3 FaceCenterPoint(int faceIndex) const;
+
+        void GetCornerPoints(Vector3 *outPointArray) const;
+
+        void GetFacePlanes(Plane *outPlaneArray) const;
+
+        Plane FacePlane(int faceIndex) const;
+
+        void ExtremePointsAlongDirection(const Vector3 &dir, const Vector3 *pointArray, int numPoints, int &idxSmallest,
+                                         int &idxLargest, float &smallestD, float &largestD);
+
+        Vector3 FacePoint(int faceIndex, float u, float v) const;
+
+        Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
+
+        Vector3 ExtremePoint(const Vector3 &direction) const;
+
+
+        void SetFrom(const AABB &aabb, const Matrix3x3 &transform);
+
+        void SetFrom(const AABB &aabb, const Matrix4x4 &transform);
+
+        void SetFrom(const AABB &aabb, const Quaternion &transform);
+
+        bool Intersects(const Plane& plane) const;
+
+        Matrix4x4 LocalToWorld() const;
+
+        Matrix4x4 WorldToLocal() const;
+    };
+}

include/J3ML/Geometry/Plane.h

@@ -0,0 +1,86 @@
+#pragma once
+#include <J3ML/LinearAlgebra/Vector3.h>
+#include "Shape.h"
+#include "Ray.h"
+
+
+namespace J3ML::Geometry
+{
+    using J3ML::LinearAlgebra::Vector3;
+
+    class Plane : public Shape
+    {
+    public:
+        Vector3 Position;
+        Vector3 Normal;
+        float distance = 0.f;
+    public:
+        Plane() : Shape() {}
+        Plane(const Vector3& v1, const Vector3 &v2, const Vector3& v3);
+        Plane(const Vector3& pos, const Vector3& norm);
+        Plane(const Line &line, const Vector3 &normal);
+
+        void Set(const Vector3& v1, const Vector3& v2, const Vector3& v3);
+
+        void Set(const Vector3 &point, const Vector3 &normal_);
+
+        bool Intersects(J3ML::Geometry::Ray ray, float *dist) const;
+
+        /// Tests if the given point lies on the positive side of this plane.
+        /** A plane divides the space in three sets: the negative halfspace, the plane itself, and the positive halfspace.
+            The normal vector of the plane points towards the positive halfspace.
+            @return This function returns true if the given point lies either on this plane itself, or in the positive
+                halfspace of this plane.
+            @see IsInPositiveDirection, AreOnSameSide(), Distance(), SignedDistance(). */
+        bool IsOnPositiveSide(const Vector3 &point) const;
+
+        float SignedDistance(const Vector3 &point) const;
+        float SignedDistance(const AABB &aabb) const;
+        float SignedDistance(const OBB &obb) const;
+        float SignedDistance(const Capsule &capsule) const;
+        //float Plane::SignedDistance(const Circle &circle) const { return Plane_SignedDistance(*this, circle); }
+        float SignedDistance(const Frustum &frustum) const;
+        //float SignedDistance(const Line &line) const { return Plane_SignedDistance(*this, line); }
+        float SignedDistance(const LineSegment &lineSegment) const;
+        float SignedDistance(const Ray &ray) const;
+        //float Plane::SignedDistance(const Plane &plane) const { return Plane_SignedDistance(*this, plane); }
+        float SignedDistance(const Polygon &polygon) const;
+        float SignedDistance(const Polyhedron &polyhedron) const;
+        float SignedDistance(const Sphere &sphere) const;
+        float SignedDistance(const Triangle &triangle) const;
+
+        static bool
+        IntersectLinePlane(const Vector3 &planeNormal, float planeD, const Vector3 &linePos, const Vector3 &lineDir,
+                           float &t);
+
+        float Distance(const Vector3 &) const;
+
+        float Distance(const LineSegment &lineSegment) const;
+
+        float Distance(const Sphere &sphere) const;
+
+        float Distance(const Capsule &capsule) const;
+
+        bool Intersects(const Line &line, float *dist) const;
+
+        bool Intersects(const LineSegment &lineSegment, float *dist) const;
+
+        bool Intersects(const Sphere &sphere) const;
+
+        bool Intersects(const Capsule &capsule) const;
+
+        bool Intersects(const AABB &aabb) const;
+
+        bool Intersects(const OBB &obb) const;
+
+        bool Intersects(const Triangle &triangle) const;
+
+        bool Intersects(const Frustum &frustum) const;
+
+        bool Intersects(const Polyhedron &polyhedron) const;
+
+        LineSegment Project(const LineSegment &segment);
+
+        Vector3 Project(const Vector3 &point) const;
+    };
+}

include/J3ML/Geometry/Polygon.h

@@ -0,0 +1,87 @@
+#pragma once
+
+#include <J3ML/Geometry/Common.h>
+#include <vector>
+#include "Shape.h"
+#include "J3ML/LinearAlgebra.h"
+
+namespace J3ML::Geometry {
+
+    class Polygon : public Shape
+    {
+    public:
+        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; }
+
+        AABB MinimalEnclosingAABB() const;
+        int NumVertices() const;
+        Vector3 Vertex(int vertexIndex) const;
+        Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
+
+        Vector3 ExtremePoint(const Vector3 &direction) const;
+
+        void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
+
+        bool Intersects(const Capsule &capsule) const;
+        bool Intersects(const Line &line) const;
+        bool Intersects(const Ray &ray) const;
+        bool Intersects(const LineSegment &lineSegment) const;
+        bool Intersects(const Plane &plane) const;
+        bool Intersects(const AABB &aabb) const;
+        bool Intersects(const OBB &obb) const;
+        bool Intersects(const Triangle &triangle, float polygonThickness = 1e-3f) const;
+        bool Intersects(const Polygon &polygon, float polygonThickness = 1e-3f) const;
+        bool Intersects(const Frustum &frustum) const;
+        bool Intersects(const Polyhedron &polyhedron) const;
+        bool Intersects(const Sphere &sphere) const;
+        std::vector<Triangle> Triangulate() const;
+        /// Tests if this polygon is planar.
+        /** A polygon is planar if all its vertices lie on the same plane.
+            @note Almost all functions in this class require that the polygon is planar. While you can store vertices of
+                non-planar polygons in this class, they are better avoided. Read the member function documentation carefully
+                to avoid calling for non-planar polygons any functions which assume planarity.
+            @see IsConvex(), IsSimple(), IsNull(), IsFinite(), IsDegenerate(). */
+        bool IsPlanar(float epsilonSq = 1e-4f) const;
+
+        Vector2 MapTo2D(int i) const;
+
+        Vector2 MapTo2D(const Vector3 &point) const;
+
+        Vector3 BasisU() const;
+
+        Vector3 BasisV() const;
+
+        Plane PlaneCCW() const;
+
+        bool Contains(const Polygon &worldSpacePolygon, float polygonThickness) const;
+
+        bool Contains(const Vector3 &worldSpacePoint, float polygonThicknessSq = 1e-2f) const;
+
+        bool Contains(const LineSegment &worldSpaceLineSegment, float polygonThickness) const;
+
+        bool Contains(const Triangle &worldSpaceTriangle, float polygonThickness) const;
+
+        LineSegment Edge(int i) const;
+
+        bool ConvexIntersects(const AABB &aabb) const;
+
+        bool ConvexIntersects(const OBB &obb) const;
+
+        bool ConvexIntersects(const Frustum &frustum) const;
+
+        Vector3 MapFrom2D(const Vector2 &point) const;
+
+        bool Intersects2D(const LineSegment &segment) const;
+
+        Vector3 ClosestPoint(const LineSegment &lineSegment) const;
+
+        Vector3 ClosestPoint(const LineSegment &lineSegment, Vector3 *lineSegmentPt) const;
+
+        Vector3 ClosestPoint(const Vector3 &point) const;
+    protected:
+
+
+    };
+}

include/J3ML/Geometry/Polyhedron.h

@@ -0,0 +1,111 @@
+#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/QuadTree.h

@@ -0,0 +1,369 @@
+#pragma once
+
+#include <vector>
+#include <cstdint>
+#include <J3ML/LinearAlgebra/Vector2.h>
+#include "AABB2D.h"
+
+namespace J3ML::Geometry {
+
+
+    using LinearAlgebra::Vector2;
+
+    template<typename T>
+    class QuadTree {
+        /// A fixed split rule for all QuadTrees: A QuadTree leaf node is only ever split if the leaf contains at least this many objects.
+        /// Leaves containing fewer than this many objects are always kept as leaves until the object count is exceeded.
+        constexpr static const int minQuadTreeNodeObjectCount = 16;
+
+        /// A fixed split limit rule for all QuadTrees: If the QuadTree node side length is smaller than this, the node will
+        /// never be split again into smaller subnodes. This provides a hard limit safety net for infinite/extra long recursion
+        /// in case multiple identical overlapping objects are placed into the tree.
+        constexpr static const float minQuadTreeQuadrantSize = 0.05f;
+
+    public:
+        struct Node {
+            Node *parent;
+            uint32_t childIndex;
+            std::vector<T> objects;
+
+            Vector2 center;
+            Vector2 radius;
+
+            bool IsLeaf() const { return childIndex == 0xFFFFFFFF; }
+
+            uint32_t TopLeftChildIndex() const { return childIndex; }
+
+            uint32_t TopRightChildIndex() const { return childIndex + 1; }
+
+            uint32_t BottomLeftChildIndex() const { return childIndex + 2; }
+
+            uint32_t BottomRightChildIndex() const { return childIndex + 3; }
+
+            /// This assumes that the QuadTree contains unique objects and never duplicates
+            void Remove(const T &object) {
+                for (size_t i = 0; i < objects.size(); ++i) {
+                    if (objects[i] == object) {
+                        AssociateQuadTreeNode(object,
+                                              0); // Mark in the object that it has been removed from the QuadTree.
+                        std::swap(objects[i], objects.back());
+                        objects.pop_back();
+                        return;
+                    }
+                }
+            }
+
+            AABB2D ComputeAABB() {}
+
+            float DistanceSq(const Vector2 &point) const {
+                Vector2 centered = point - center;
+                Vector2 closestPoint = centered.Clamp(-radius, radius);
+                return closestPoint.DistanceSq(centered);
+            }
+
+        };
+
+        // Helper struct used when traversing through the tree
+        struct TraversalStackItem {
+            Node *node;
+        };
+
+        QuadTree() :
+                rootNodeIndex(-1),
+                boundingAABB(Vector2(0, 0), Vector2(0, 0)) {
+            // TODO: currently storing persistent raw pointers to this array outside the array
+            nodes.reserve(200000);
+        }
+
+        /// Removes all nodes and objects in this tree and reintializes the tree to a single root node.
+        void Clear(const Vector2 &minXY = Vector2(-1, -1), const Vector2 &maxXY = Vector2(1, 1));
+
+        /// Places the given object onto the proper (leaf) node of the tree. After placing, if the leaf split rule is
+        /// satisfied, subdivides the leaf node into 4 subquadrants and reassings the objects to the new leaves.
+        void Add(const T &object);
+
+        /// Removes the given object from this tree.
+        /// To call this function, you must define a function QuadTree<T>::Node *GetQuadTreeNode(const T& object)
+        /// which returns the node of this quadtree where the object resides in.
+        void Remove(const T &object);
+
+        /// @return The bounding rectangle for the whole tree.
+        /// @note This bounding rectangle does not tightly bound the objects themselves, only the root node of the tree.
+        AABB2D BoundingAABB() const { return boundingAABB; }
+
+        /// @return The topmost node in the tree.
+        Node *Root();
+
+        const Node *Root() const;
+
+        /// Returns the total number of nodes (all nodes, i.e. inner nodes + leaves) in the tree.
+        /// Runs in constant time.
+        int NumNodes() const;
+
+        /// Returns the total number of leaf nodes in the tree.
+        /// @warning Runs in time linear 'O(n)' to the number of nodes in the tree.
+        int NumLeaves() const;
+
+        /// Returns the total number of inner nodes in the tree.
+        /// @warning Runs in time linear 'O(n)' to the number of nodes in the tree.
+        int NumInnerNodes() const;
+
+        /// Returns the total number of objects stored in the tree.
+        /// @warning Runs in time linear 'O(n)' to the number of nodes in the tree.
+        int NumObjects() const;
+
+        /// Returns the maximum height of the whole tree (the path from the root to the farthest leaf node).
+        int TreeHeight() const;
+
+        /// Returns the height of the subtree rooted at 'node'.
+        int TreeHeight(const Node *node) const;
+
+        /// Performs an AABB intersection query in this Quadtreee, and calls the given callback function for each non-empty
+        /// node of the tree which intersects the given AABB.
+        /** @param aabb The axis-aligned bounding box to intersect this QuadTree with.
+            @param callback A function or a function object of prototype
+                bool callbackFunction(QuadTree<T> &tree, const AABB2D &queryAABB, QuadTree<T>::Node &node);
+            If the callback function returns true, the execution of the query is stopped and this function immediately
+            returns afterwards. If the callback function returns false, the execution of the query continues. */
+        template<typename Func>
+        inline void AABBQuery(const AABB2D &aabb, Func &callback);
+
+        /// Finds all object pairs inside the given AABB which have colliding AABBs. For each such pair, calls the
+        /// specified callback function.
+        template<typename Func>
+        inline void CollidingPairsQuery(const AABB2D &aabb, Func &callback);
+
+        /// Performs various consistency checks on the given node. Use only for debugging purposes.
+        void DebugSanityCheckNode(Node *n);
+
+    private:
+        void Add(const T &object, Node *n);
+
+        /// Allocates a sequential 4-tuple of QuadtreeNodes, contiguous in memory.
+        int AllocateNodeGroup(Node *parent);
+
+        void SplitLeaf(Node *leaf);
+
+        std::vector<Node> nodes;
+        int rootNodeIndex;
+        AABB2D boundingAABB;
+
+        void GrowRootTopLeft();
+
+        void GrowRootTopRight();
+
+        void GrowRootBottomLeft();
+
+        void GrowRootBottomRight();
+
+        void GrowImpl(int quadrantForRoot);
+    };
+
+    // NOTE: Keep members defined here. Template-parameterized classes
+    // can't be split across header and implementation files
+    // but the presence of the implementation file is a requirement
+
+
+    template<typename T>
+    void QuadTree<T>::Clear(const Vector2 &minXY, const Vector2 &maxXY) {
+        nodes.clear();
+
+        boundingAABB.minPoint = minXY;
+        boundingAABB.maxPoint = maxXY;
+
+        rootNodeIndex = AllocateNodeGroup(0);
+
+        Node *root = Root();
+        root->center = (minXY + maxXY) * 0.5f;
+        root->radius = maxXY - root->center;
+    }
+
+    template<typename T>
+    void QuadTree<T>::Add(const T &object) {
+        Node *n = Root();
+        AABB2D objectAABB = GetAABB2D(object);
+
+        // Ramen Noodle Bowls of nested if statements are generally bad practice
+        // Unfortunately, sometimes the problem domain makes this unavoidable
+
+        if (objectAABB.minPoint.x >= boundingAABB.minPoint.x) {
+            // object fits left.
+            if (objectAABB.maxPoint.x <= boundingAABB.maxPoint.x) {
+                // object fits left and right.
+                if (objectAABB.minPoint.y >= boundingAABB.minPoint.y) {
+                    // Object fits left, right, and top.
+                    if (objectAABB.maxPoint.y <= boundingAABB.maxPoint.y) {
+                        // Object fits the whole root AABB. Can safely add into the existing tree size.
+                        AddObject(object, n);
+                        return;
+                    } else {
+                        // Object fits left, right, and top, but not bottom.
+                        GrowRootBottomRight(); // Could grow bottom-left as well, wouldn't matter here.
+                    }
+                } else {
+                    // Object fits left and right, but not to top.
+                    GrowRootTopRight(); // Could grow top-left as well, wouldn't matter here.
+                }
+            } else {
+                // Object fits left, but not to right. We must grow right. Check whether to grow top or bottom;
+                if (objectAABB.minPoint.y < boundingAABB.minPoint.y)
+                    GrowRootTopRight();
+                else
+                    GrowRootBottomRight();
+            }
+        } else {
+            // We must grow left. Check whether to grow top or bottom.
+            if (objectAABB.minPoint.y < boundingAABB.minPoint.y)
+                GrowRootTopLeft();
+            else
+                GrowRootBottomLeft();
+        }
+        // Now that we have grown the tree root node, try adding again.
+        Add(object);
+    }
+
+    template<typename T>
+    void QuadTree<T>::Remove(const T &object) {
+        Node *n = GetQuadTreeNode(object);
+        if (n) {
+            n->Remove(object);
+        }
+    }
+
+    template<typename T>
+    void QuadTree<T>::Add(const T &object, Node *n) {
+        for (;;) {
+            // Traverse the QuadTree to decide which quad to place this object on.
+            float left = n->center.x - MinX(object); // If left > 0.f, then the object overlaps with the left quadrant.
+            float right = MaxX(object) - n->center.x; // If right > 0.f, then the object overlaps with the right quadrant.
+
+            float top = n->center.y - MinY(object); // If top > 0.f, then the object overlaps with the top quadrant
+            float bottom =
+                    MaxY(object) - n->center.y; // If bottom > 0.f, then the object overlaps with the bottom quadrant
+            float leftAndRight = std::min(left, right); // If > 0.f, then the object straddles left-right halves.
+            float topAndBottom = std::min(top, bottom); // If > 0.f, then the object straddles top-bottom halves.
+            float straddledEitherOne = std::max(leftAndRight,
+                                                topAndBottom); // If > 0.f, thne the object is in two or more quadrants.
+
+            // Note: It can happen that !left && !right, or !top && !bottom.
+            // but the if()s are setup below so that right/bottom is taken if no left/top, so that is ok.
+
+            // We must put the object onto this node if
+            // a) the object straddled the parent->child split lines.
+            // b) this object is a leaf
+            if (straddledEitherOne > 0.f) {
+                n->objects.push_back(object);
+                AssociateQuadTreeNode(object, n);
+                return;
+            }
+            if (n->IsLeaf()) {
+                n->objects.push_back(object);
+                AssociateQuadTreeNode(object, n);
+
+                if ((int) n->objects.size() > minQuadTreeNodeObjectCount &&
+                    std::min(n->radius.x, n->radius.y) >= minQuadTreeQuadrantSize) {
+                    SplitLeaf(n);
+                }
+
+                return;
+            }
+            if (left > 0.f) {
+                if (top > 0.f) {
+                    n = &nodes[n->TopLeftChildIndex()];
+                } else {
+                    n = &nodes[n->BottomLeftChildIndex()];
+                }
+            } else {
+                if (top > 0.f) {
+                    n = &nodes[n->TopRightChildIndex()];
+                } else {
+                    n = &nodes[n->BottomRightChildIndex()];
+                }
+            }
+        }
+    }
+
+    template<typename T>
+    typename QuadTree<T>::Node *QuadTree<T>::Root() {
+        return nodes.empty() ? 0 : &nodes[rootNodeIndex];
+    }
+
+    template<typename T>
+    const typename QuadTree<T>::Node *QuadTree<T>::Root() const {
+        return nodes.empty() ? 0 : &nodes[rootNodeIndex];
+    }
+
+    template <typename T>
+    int QuadTree<T>::AllocateNodeGroup(Node* parent) {
+        size_t oldCap = nodes.capacity();
+
+        int index = (int)nodes.size();
+        Node n;
+        n.parent = parent;
+        n.childIndex = 0xFFFFFFFF;
+        if (parent) {
+            n.radius = parent->radius * 0.5f;
+            n.center = parent->center - n.radius;
+        }
+
+        nodes.push_back(n);
+        if (parent)
+            n.center.x = parent->center.x + n.radius.x;
+        nodes.push_back(n);
+
+        if (parent) {
+            n.center.x = parent->center.x - n.radius.x;
+            n.center.y = parent->center.y + n.radius.y;
+        }
+
+        nodes.push_back(n);
+        if (parent)
+            n.center.x = parent->center.x + n.radius.x;
+        nodes.push_back(n);
+
+        return index;
+    }
+
+    template <typename T>
+    void QuadTree<T>::SplitLeaf(Node *leaf) {
+        leaf->childIndex = AllocateNodeGroup(leaf);
+
+        size_t i = 0;
+        while (i < leaf->objects.size()) {
+            const T& object = leaf->objects[i];
+
+            // Traverse the QuadTree to decide which quad to place this object into
+            float left = leaf->center.x - MinX(object);
+            float right = MaxX(object) - leaf->center;
+            float top = leaf->center.y - MinY(object);
+            float bottom = MaxY(object) - leaf->center.y;
+            float leftAndRight = std::min(left, right);
+            float topAndBottom = std::min(top, bottom);
+
+            float straddledEitherOne = std::max(leftAndRight, topAndBottom);
+
+            if (straddledEitherOne > 0.f) {
+                ++i;
+                continue;
+            }
+
+            if (left > 0.f) {
+                if (top > 0.f) {
+                    Add(object, &nodes[leaf->TopLeftChildIndex()]);
+                } else {
+                    Add(object, &nodes[leaf->BottomLeftChildIndex()]);
+                }
+            } else {
+                if (top > 0.f) {
+                    Add(object, &nodes[leaf->TopRightChildIndex()]);
+                } else {
+                    Add(object, &nodes[leaf->BottomRightChildIndex()]);
+                }
+            }
+
+            // Remove the object we added to a child from this node.
+            leaf->objects[i] = leaf->objects.back();
+            leaf->objects.pop_back();
+        }
+    }
+}

include/J3ML/Geometry/Ray.h

@@ -0,0 +1,67 @@
+//
+// 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/Shape.h

@@ -0,0 +1,26 @@
+#pragma once
+
+
+namespace J3ML::Geometry
+{
+    class GeometricPrimitive
+    {
+    public:
+    protected:
+    private:
+    };
+
+    class Shape
+    {
+    public:
+    protected:
+    private:
+    };
+
+    class Shape2D
+    {
+    public:
+    protected:
+    private:
+    };
+}

include/J3ML/Geometry/Sphere.h

@@ -0,0 +1,84 @@
+#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) {}
+
+        /// 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/Triangle.h

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

include/J3ML/Geometry/Triangle2D.h

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

include/J3ML/Geometry/TriangleMesh.h

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

include/J3ML/J3ML.h

@@ -0,0 +1,242 @@
+#pragma once
+
+
+
+//
+// Created by josh on 12/25/2023.
+//
+#include <cstdint>
+#include <cmath>
+#include <stdfloat>
+#include <string>
+#include <cassert>
+
+namespace J3ML::SizedIntegralTypes
+{
+    using u8 = uint8_t;
+    using u16 = uint16_t;
+    using u32 = uint32_t;
+    using u64 = uint64_t;
+    using u128 = __uint128_t;
+
+    using s8 = int8_t;
+    using s16 = int16_t;
+    using s32 = int32_t;
+    using s64 = int64_t;
+    using s128 = __int128_t;
+}
+
+
+
+namespace J3ML::SizedFloatTypes
+{
+    using f32 = float;
+    using f64 = double;
+    using f128 = long double;
+
+}
+
+using namespace J3ML::SizedIntegralTypes;
+using namespace J3ML::SizedFloatTypes;
+
+namespace J3ML::Math
+{
+
+    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/LinearAlgebra.h

@@ -0,0 +1,30 @@
+//// Dawsh Linear Algebra Library - Everything you need for 3D math
+/// @file LinearAlgebra.h
+/// @description Includes all LinearAlgebra classes and functions
+/// @author Josh O'Leary, William Tomasine II
+/// @copyright 2024 Redacted Software
+/// @license Unlicense - Public Domain
+/// @revision 1.3
+/// @edited 2024-02-26
+
+#pragma once
+
+// TODO: Enforce Style Consistency (Function Names use MicroSoft Case)
+// TODO: Implement Templated Linear Algebra
+
+
+// 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"
+
+using namespace J3ML::LinearAlgebra;

include/J3ML/LinearAlgebra/Angle2D.h

@@ -0,0 +1,22 @@
+#pragma once
+
+
+namespace J3ML::LinearAlgebra {
+
+    class Angle2D {
+    public:
+        float x;
+        float y;
+        Angle2D(Math::Rotation rads);
+        Angle2D(float X, float Y)
+        {
+            x = X;
+            y = Y;
+        }
+
+        bool operator==(const Angle2D& rhs) const {
+            return (this->x==rhs.x && this->y==rhs.y);
+        }
+    };
+
+}

include/J3ML/LinearAlgebra/AxisAngle.h

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

include/J3ML/LinearAlgebra/Common.h

@@ -0,0 +1,28 @@
+#pragma once
+
+// Forward Declarations for classes that include each other
+namespace J3ML::LinearAlgebra
+{
+    class Vector2; // A type representing a position in a 2-dimensional coordinate space.
+    class Vector3; // A type representing a position in a 3-dimensional coordinate space.
+    class Vector4; // A type representing a position in a 4-dimensional coordinate space.
+    class Angle2D; // Uses x,y components to represent a 2D rotation.
+    class EulerAngle; // Uses pitch,yaw,roll components to represent a 3D orientation.
+    class AxisAngle; //
+    class CoordinateFrame; //
+    class Matrix2x2;
+    class Matrix3x3;
+    class Matrix4x4;
+    class Transform2D;
+    class Transform3D;
+    class Quaternion;
+
+
+    using Position = Vector3;
+}
+
+// Methods required by LinearAlgebra types
+namespace J3ML::LinearAlgebra
+{
+
+}

include/J3ML/LinearAlgebra/CoordinateFrame.h

@@ -0,0 +1,17 @@
+#pragma once
+
+
+#include <J3ML/LinearAlgebra/Vector3.h>
+
+namespace J3ML::LinearAlgebra
+{
+    /// The CFrame is fundamentally 4 vectors (position, forward, right, up vector)
+    class CoordinateFrame
+    {
+    public:
+        Vector3 Position;
+        Vector3 Front;
+        Vector3 Right;
+        Vector3 Up;
+    };
+}

include/J3ML/LinearAlgebra/EulerAngle.h

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

include/J3ML/LinearAlgebra/Matrix.h

@@ -0,0 +1,66 @@
+#pragma once
+
+#include <cstddef>
+#include <cstdlib>
+#include <algorithm>
+#include "Vector.h"
+
+namespace J3ML::LinearAlgebra
+{
+
+
+    template <uint ROWS, uint COLS, typename T>
+    class Matrix
+    {
+        static constexpr uint Diag = std::min(ROWS, COLS);
+
+        using RowVector = Vector<ROWS, T>;
+        using ColVector = Vector<COLS, T>;
+        using DiagVector = Vector<Diag, T>;
+
+        enum { Rows = ROWS };
+        enum { Cols = COLS };
+
+        void AssertRowSize(uint rows)
+        {
+            assert(rows < Rows && "");
+        }
+        void AssertColumnSize(uint cols)
+        {
+            assert(cols < Cols && "");
+        }
+
+        RowVector GetRow(uint index) const;
+        ColVector GetColumn(uint index) const;
+        void SetRow(uint index, RowVector);
+        void SetColumn(uint index, ColVector);
+
+        RowVector &Row(uint index) const;
+        ColVector &Column(uint index) const;
+
+        const T At(uint row, uint col) const
+        {
+            AssertRowSize(row);
+            AssertColumnSize(col);
+
+            return elems[row][col];
+        }
+
+        T &At(uint row, uint col)
+        {
+            AssertRowSize(row);
+            AssertColumnSize(col);
+
+            return elems[row][col];
+        }
+
+        float* ptr();
+        const float* ptr() const;
+
+        float operator[](uint index) const;
+        float& operator[](uint index);
+
+    private:
+        T elems[ROWS][COLS];
+    };
+}

include/J3ML/LinearAlgebra/Matrix2x2.h

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

include/J3ML/LinearAlgebra/Matrix3x3.h

@@ -0,0 +1,160 @@
+#pragma once
+
+#include <J3ML/LinearAlgebra/Common.h>
+#include <J3ML/LinearAlgebra/Vector2.h>
+#include <J3ML/LinearAlgebra/Vector3.h>
+#include <J3ML/LinearAlgebra/Quaternion.h>
+
+namespace J3ML::LinearAlgebra {
+
+
+    class Quaternion;
+
+    /// A 3-by-3 matrix for linear transformations of 3D geometry.
+    /* This can represent any kind of linear transformations of 3D geometry, which include
+     * rotation, scale, shear, mirroring, and orthographic projection.
+     * A 3x3 matrix cannot represent translation, which requires a 3x4, or perspective projection (4x4).
+     * The elements of this matrix are
+     *  m_00, m_01, m_02
+     *  m_10, m_11, m_12
+     *  m_20, m_21, m_22
+     *
+     *  The element m_yx is the value on the row y and column x.
+     *  You can access m_yx using the double-bracket notation m[y][x], or using the member function At.
+     *
+     *  @note The member functions in this class use the convention that transforms are applied to
+     *  vectors in the form M * v. This means that "Matrix3x3 M, M1, M2; M = M1 * M2;" gives a transformation M
+     *  that applies M2 first, followed by M1 second
+     */
+
+    class Matrix3x3 {
+    public:
+        enum { Rows = 3 };
+        enum { Cols = 3 };
+
+        static const Matrix3x3 Zero;
+        static const Matrix3x3 Identity;
+        static const Matrix3x3 NaN;
+
+        Matrix3x3() {}
+        Matrix3x3(float val);
+        Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22);
+        Matrix3x3(const Vector3& r1, const Vector3& r2, const Vector3& r3);
+        explicit Matrix3x3(const Quaternion& orientation);
+        explicit Matrix3x3(const float *data);
+
+
+        static Matrix3x3 RotateX(float radians);
+        static Matrix3x3 RotateY(float radians);
+        static Matrix3x3 RotateZ(float radians);
+
+        Vector3 GetRow(int index) const;
+        Vector3 GetColumn(int index) const;
+
+        Vector3 GetRow3(int index) const;
+
+        Vector3 GetColumn3(int index) const;
+
+        float &At(int row, int col);
+        float At(int x, int y) const;
+
+        void SetRotatePart(const Vector3& a, float angle);
+
+        /// Creates a new M3x3 that rotates about the given axis by the given angle
+        static Matrix3x3 RotateAxisAngle(const Vector3& axis, float angleRadians);
+
+        // TODO: Implement
+        static Matrix3x3 RotateFromTo(const Vector3& source, const Vector3& direction);
+
+        void SetRow(int i, const Vector3 &vector3);
+        void SetColumn(int i, const Vector3& vector);
+        void SetAt(int x, int y, float value);
+
+        void Orthonormalize(int c0, int c1, int c2);
+
+        static Matrix3x3 LookAt(const Vector3& forward, const Vector3& target, const Vector3& localUp, const Vector3& worldUp);
+
+        static Matrix3x3 FromQuat(const Quaternion& orientation);
+
+        Quaternion ToQuat() const;
+
+        /// Creates a new Matrix3x3 as a combination of rotation and scale.
+        // This function creates a new matrix M in the form M = R * S
+        // where R is a rotation matrix and S is a scale matrix.
+        // Transforming a vector v using this matrix computes the vector
+        // v' == M * v == R*S*v == (R * (S * v)) which means the scale operation
+        // is applied to the vector first, followed by rotation, and finally translation
+        static Matrix3x3 FromRS(const Quaternion& rotate, const Vector3& scale);
+        static Matrix3x3 FromRS(const Matrix3x3 &rotate, const Vector3& scale);
+
+
+        /// Creates a new transformation matrix that scales by the given factors.
+        // This matrix scales with respect to origin.
+        static Matrix3x3 FromScale(float sx, float sy, float sz);
+        static Matrix3x3 FromScale(const Vector3& scale);
+
+        /// Returns the main diagonal.
+        Vector3 Diagonal() const;
+        /// Returns the local +X/+Y/+Z axis in world space.
+        /// This is the same as transforming the vector{1,0,0} by this matrix.
+        Vector3 WorldX() const;
+        /// Returns the local +Y axis in world space.
+        /// This is the same as transforming the vector{0,1,0} by this matrix.
+        Vector3 WorldY() const;
+        /// Returns the local +Z axis in world space.
+        /// This is the same as transforming the vector{0,0,1} by this matrix.
+        Vector3 WorldZ() const;
+
+        /// Computes the determinant of this matrix.
+        // If the determinant is nonzero, this matrix is invertible.
+        // If the determinant is negative, this matrix performs reflection about some axis.
+        // From http://msdn.microsoft.com/en-us/library/bb204853(VS.85).aspx :
+        // "If the determinant is positive, the basis is said to be "positively" oriented (or right-handed).
+        // If the determinant is negative, the basis is said to be "negatively" oriented (or left-handed)."
+        // @note This function computes 9 LOADs, 9 MULs and 5 ADDs. */
+        float Determinant() const;
+
+        // Returns an inverted copy of this matrix. This
+        Matrix3x3 Inverse() const;
+
+        // Returns a transposed copy of this matrix.
+        Matrix3x3 Transpose() const;
+
+        // Transforms the given vectors by this matrix M, i.e. returns M * (x,y,z)
+        Vector2 Transform(const Vector2& rhs) const;
+        Vector3 Transform(const Vector3& rhs) const;
+
+
+        Matrix3x3 ScaleBy(const Vector3& rhs);
+        Vector3 GetScale() const;
+
+        Vector3 operator[](int row) const;
+
+        Vector2 operator * (const Vector2& rhs) const;
+        Vector3 operator * (const Vector3& rhs) const;
+        Matrix3x3 operator * (const Matrix3x3& rhs) const;
+        Matrix4x4 operator * (const Matrix4x4& rhs) const;
+        Matrix3x3 Mul(const Matrix3x3& rhs) const;
+        Matrix4x4 Mul(const Matrix4x4& rhs) const;
+        Vector2 Mul(const Vector2& rhs) const;
+        Vector3 Mul(const Vector3& rhs) const;
+        Vector4 Mul(const Vector4& rhs) const;
+        Quaternion Mul(const Quaternion& rhs) const;
+
+        // Returns true if the column vectors of this matrix are all perpendicular to each other.
+        bool IsColOrthogonal(float epsilon = 1e-3f) const;
+        // Returns true if the row vectors of this matrix are all perpendicular to each other.
+        bool IsRowOrthogonal(float epsilon = 1e-3f) const;
+
+
+
+        bool HasUniformScale(float epsilon  = 1e-3f) const;
+
+        Vector3 ExtractScale() const {
+            return {GetColumn(0).Length(), GetColumn(1).Length(), GetColumn(2).Length()};
+        }
+
+    protected:
+        float elems[3][3];
+    };
+}

include/J3ML/LinearAlgebra/Matrix4x4.h

@@ -0,0 +1,285 @@
+#pragma once
+
+#include <J3ML/LinearAlgebra/Common.h>
+
+#include <J3ML/LinearAlgebra/Matrix3x3.h>
+#include <J3ML/LinearAlgebra/Quaternion.h>
+
+
+#include <J3ML/LinearAlgebra/Vector4.h>
+#include <algorithm>
+
+namespace J3ML::LinearAlgebra {
+
+    /// A 4-by-4 matrix for affine transformations and perspective projections of 3D geometry.
+    /* This matrix can represent the most generic form of transformations for 3D objects,
+     * including perspective projections, which a 4-by-3 cannot store,
+     * and translations, which a 3-by-3 cannot represent.
+     * The elements of this matrix are
+     *  m_00, m_01, m_02, m_03
+     *  m_10, m_11, m_12, m_13
+     *  m_20, m_21, m_22, m_23,
+     *  m_30, m_31, m_32, m_33
+     *
+     *  The element m_yx is the value on the row y and column x.
+     *  You can access m_yx using the double-bracket notation m[y][x]
+     */
+    class Matrix4x4 {
+    public:
+        // TODO: Implement assertions to ensure matrix bounds are not violated!
+        enum { Rows = 4 };
+        enum { Cols = 4 };
+
+        /// A constant matrix that has zeroes in all its entries
+        static const Matrix4x4 Zero;
+        /// A constant matrix that is the identity.
+        static const Matrix4x4 Identity;
+
+        /// A compile-time constant float4x4 which has NaN in each element.
+        /// For this constant, each element has the value of quet NaN, or Not-A-Number.
+        /// Never compare a matrix to this value. Due to how IEEE floats work, "nan == nan" returns false!
+        static const Matrix4x4 NaN;
+
+        /// Creates a new float4x4 with uninitialized member values.
+        Matrix4x4() {}
+        Matrix4x4(float val);
+        /// Constructs this float4x4 to represent the same transformation as the given float3x3.
+        /** This function expands the last row and column of this matrix with the elements from the identity matrix. */
+        Matrix4x4(const Matrix3x3&);
+        explicit Matrix4x4(const float* data);
+
+        /// Constructs a new float4x4 by explicitly specifying all the matrix elements.
+        /// The elements are specified in row-major format, i.e. the first row first followed by the second and third row.
+        /// E.g. The element _10 denotes the scalar at second (index 1) row, first (index 0) column.
+        Matrix4x4(float m00, float m01, float m02, float m03,
+                  float m10, float m11, float m12, float m13,
+                  float m20, float m21, float m22, float m23,
+                  float m30, float m31, float m32, float m33);
+        /// Constructs the matrix by explicitly specifying the four column vectors.
+        /** @param col0 The first column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
+            direction of the local X axis.
+            @param col1 The second column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
+            direction of the local Y axis.
+            @param col2 The third column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
+            direction of the local Z axis.
+            @param col3 The fourth column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
+            position of the local space pivot. */
+        Matrix4x4(const Vector4& r1, const Vector4& r2, const Vector4& r3, const Vector4& r4);
+
+        explicit Matrix4x4(const Quaternion& orientation);
+
+        /// Constructs this float4x4 from the given quaternion and translation.
+        /// Logically, the translation occurs after the rotation has been performed.
+        Matrix4x4(const Quaternion& orientation, const Vector3 &translation);
+
+        /// Creates a LookAt matrix from a look-at direction vector.
+        /** A LookAt matrix is a rotation matrix that orients an object to face towards a specified target direction.
+            @param localForward Specifies the forward direction in the local space of the object. This is the direction
+                the model is facing at in its own local/object space, often +X (1,0,0), +Y (0,1,0) or +Z (0,0,1). The
+                vector to pass in here depends on the conventions you or your modeling software is using, and it is best
+                pick one convention for all your objects, and be consistent.
+                This input parameter must be a normalized vector.
+            @param targetDirection Specifies the desired world space direction the object should look at. This function
+                will compute a rotation matrix which will rotate the localForward vector to orient towards this targetDirection
+                vector. This input parameter must be a normalized vector.
+            @param localUp Specifies the up direction in the local space of the object. This is the up direction the model
+                was authored in, often +Y (0,1,0) or +Z (0,0,1). The vector to pass in here depends on the conventions you
+                or your modeling software is using, and it is best to pick one convention for all your objects, and be
+                consistent. This input parameter must be a normalized vector. This vector must be perpendicular to the
+                vector localForward, i.e. localForward.Dot(localUp) == 0.
+            @param worldUp Specifies the global up direction of the scene in world space. Simply rotating one vector to
+                coincide with another (localForward->targetDirection) would cause the up direction of the resulting
+                orientation to drift (e.g. the model could be looking at its target its head slanted sideways). To keep
+                the up direction straight, this function orients the localUp direction of the model to point towards the
+                specified worldUp direction (as closely as possible). The worldUp and targetDirection vectors cannot be
+                collinear, but they do not need to be perpendicular either.
+            @return A matrix that maps the given local space forward direction vector to point towards the given target
+                direction, and the given local up direction towards the given target world up direction. The returned
+                matrix M is orthonormal with a determinant of +1. For the matrix M it holds that
+                M * localForward = targetDirection, and M * localUp lies in the plane spanned by the vectors targetDirection
+                and worldUp.
+            @note The position of (the translation performed by) the resulting matrix will be set to (0,0,0), i.e. the object
+                will be placed to origin. Call SetTranslatePart() on the resulting matrix to set the position of the model.
+            @see RotateFromTo(). */
+        static Matrix4x4 LookAt(const Vector3& localFwd, const Vector3& targetDir, const Vector3& localUp, const Vector3& worldUp);
+
+        /// Returns the translation part.
+        /** The translation part is stored in the fourth column of this matrix.
+            This is equivalent to decomposing this matrix in the form M = T * M', i.e. this translation is applied last,
+            after applying rotation and scale. If this matrix represents a local->world space transformation for an object,
+            then this gives the world space position of the object.
+            @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0 0 0 1]). */
+        Vector3 GetTranslatePart() const;
+        /// Returns the top-left 3x3 part of this matrix. This stores the rotation part of this matrix (if this matrix represents a rotation).
+        Matrix3x3 GetRotatePart() const;
+        void SetTranslatePart(float translateX, float translateY, float translateZ);
+        void SetTranslatePart(const Vector3& offset);
+        void SetRotatePart(const Quaternion& q);
+        void Set3x3Part(const Matrix3x3& r);
+
+        void SetRow(int row, const Vector3& rowVector, float m_r3);
+        void SetRow(int row, const Vector4& rowVector);
+        void SetRow(int row, float m_r0, float m_r1, float m_r2, float m_r3);
+
+        void SetCol(int col, const Vector3& colVector, float m_c3);
+        void SetCol(int col, const Vector4& colVector);
+        void SetCol(int col, float m_c0, float m_c1, float m_c2, float m_c3);
+        void SetCol(int col, const float *data);
+
+        Vector4 GetRow(int index) const;
+        Vector4 GetColumn(int index) const;
+        Vector3 GetRow3(int index) const;
+        Vector3 GetColumn3(int index) const;
+        Vector4 Col(int i) const;
+        Vector4 Row(int i) const;
+        Vector4 Col3(int i) const;
+        Vector4 Row3(int i) const;
+
+        Vector3 GetScale() const
+        {
+
+        }
+        Matrix4x4 Scale(const Vector3&);
+
+        float &At(int row, int col);
+        float At(int x, int y) const;
+
+        template <typename T>
+        void Swap(T &a, T &b)
+        {
+            T temp = std::move(a);
+            a = std::move(b);
+            b = std::move(temp);
+        }
+
+
+        void SwapColumns(int col1, int col2);
+
+        /// Swaps two rows.
+        void SwapRows(int row1, int row2);
+        /// Swapsthe xyz-parts of two rows element-by-element
+        void SwapRows3(int row1, int row2);
+
+        void ScaleRow(int row, float scalar);
+        void ScaleRow3(int row, float scalar);
+        void ScaleColumn(int col, float scalar);
+        void ScaleColumn3(int col, float scalar);
+        /// Algorithm from Eric Lengyel's Mathematics for 3D Game Programming & Computer Graphics, 2nd Ed.
+        void Pivot();
+
+        /// Tests if this matrix does not contain any NaNs or infs.
+        /** @return Returns true if the entries of this float4x4 are all finite, and do not contain NaN or infs. */
+        bool IsFinite() const;
+
+        /// Tests if this matrix has an inverse.
+        /** @return Returns true if this matrix can be inverted, up to the given epsilon. */
+        bool IsInvertible(float epsilon = 1e-3f) const;
+
+        Vector4 Diagonal() const;
+        Vector3 Diagonal3() const;
+        /// Returns the local +X axis in world space.
+        /// This is the same as transforming the vector (1,0,0) by this matrix.
+        Vector3 WorldX() const;
+        /// Returns the local +Y axis in world space.
+        /// This is the same as transforming the vector (0,1,0) by this matrix.
+        Vector3 WorldY() const;
+        /// Returns the local +Z axis in world space.
+        /// This is the same as transforming the vector (0,0,1) by this matrix.
+        Vector3 WorldZ() const;
+
+        /// Accesses this structure as a float array.
+        /// @return A pointer to the upper-left element. The data is contiguous in memory.
+        /// ptr[0] gives the element [0][0], ptr[1] is [0][1], ptr[2] is [0][2].
+        /// ptr[4] == [1][0], ptr[5] == [1][1], ..., and finally, ptr[15] == [3][3].
+        float *ptr() { return &elems[0][0]; }
+        const float *ptr() const { return &elems[0][0]; }
+
+        float Determinant3x3() const;
+        /// Computes the determinant of this matrix.
+        // If the determinant is nonzero, this matrix is invertible.
+        float Determinant() const;
+
+        #define SKIPNUM(val, skip) (val >= skip ? (val+1) : val)
+
+        float Minor(int i, int j) const;
+
+        Matrix4x4 Inverse() const;
+
+        Matrix4x4 Transpose() const;
+
+        Vector2 Transform(float tx, float ty) const;
+        Vector2 Transform(const Vector2& rhs) const;
+
+
+        Vector3 Transform(float tx, float ty, float tz) const;
+        Vector3 Transform(const Vector3& rhs) const;
+
+        Vector4 Transform(float tx, float ty, float tz, float tw) const;
+        Vector4 Transform(const Vector4& rhs) const;
+
+
+        Matrix4x4 Translate(const Vector3& rhs) const;
+        static Matrix4x4 FromTranslation(const Vector3& rhs);
+
+
+        static Matrix4x4 D3DOrthoProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+        static Matrix4x4 D3DOrthoProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+        static Matrix4x4 D3DPerspProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+        static Matrix4x4 D3DPerspProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+
+        static Matrix4x4 OpenGLOrthoProjLH(float n, float f, float h, float v);
+        static Matrix4x4 OpenGLOrthoProjRH(float n, float f, float h, float v);
+        static Matrix4x4 OpenGLPerspProjLH(float n, float f, float h, float v);
+        static Matrix4x4 OpenGLPerspProjRH(float n, float f, float h, float v);
+
+        Vector4 operator[](int row);
+
+        Matrix4x4 operator-() const;
+        Matrix4x4 operator +(const Matrix4x4& rhs) const;
+        Matrix4x4 operator - (const Matrix4x4& rhs) const;
+
+        Matrix4x4 operator *(float scalar) const;
+        Matrix4x4 operator /(float scalar) const;
+
+        Vector4 operator[](int row) const;
+
+        Vector2 operator * (const Vector2& rhs) const;
+        Vector3 operator * (const Vector3& rhs) const;
+        Vector4 operator * (const Vector4& rhs) const;
+
+        Vector2 Mul(const Vector2& rhs) const;
+        Vector3 Mul(const Vector3& rhs) const;
+        Vector4 Mul(const Vector4& rhs) const;
+
+        Matrix4x4 operator * (const Matrix3x3 &rhs) const;
+
+        Matrix4x4 operator +() const;
+
+        Matrix4x4 operator * (const Matrix4x4& rhs) const;
+
+        Matrix4x4 &operator = (const Matrix3x3& rhs);
+        Matrix4x4 &operator = (const Quaternion& rhs);
+        Matrix4x4 &operator = (const Matrix4x4& rhs) = default;
+
+
+        Vector3 ExtractScale() const;
+
+        bool HasUniformScale(float epsilon = 1e-3f) const;
+        bool IsColOrthogonal3(float epsilon = 1e-3f) const;
+        bool IsRowOrthogonal3(float epsilon = 1e-3f) const;
+
+        bool IsColOrthogonal(float epsilon = 1e-3f) const;
+        bool IsRowOrthogonal(float epsilon = 1e-3f) const;
+        /// Returns true if this matrix is seen to contain a "projective" part,
+        /// i.e. whether the last row of this matrix differs from [0 0 0 1]
+        bool ContainsProjection(float epsilon = 1e-3f) const;
+
+        void InverseOrthonormal();
+
+    protected:
+        float elems[4][4];
+
+
+        Vector3 TransformDir(float tx, float ty, float tz) const;
+    };
+}

include/J3ML/LinearAlgebra/Quaternion.h

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

include/J3ML/LinearAlgebra/Transform2D.h

@@ -0,0 +1,40 @@
+#pragma once
+
+
+#include <J3ML/LinearAlgebra/Matrix3x3.h>
+
+namespace J3ML::LinearAlgebra {
+    class Transform2D {
+    protected:
+        Matrix3x3 transformation;
+    public:
+
+        const static Transform2D Identity;
+        const static Transform2D FlipX;
+        const static Transform2D FlipY;
+
+        Transform2D(float rotation, const Vector2& pos);
+        Transform2D(float px, float py, float sx, float sy, float ox, float oy, float kx, float ky, float rotation)
+        {
+            transformation = Matrix3x3(px, py, rotation, sx, sy, ox, oy, kx, ky);
+        }
+        Transform2D(const Vector2& pos, const Vector2& scale, const Vector2& origin, const Vector2& skew, float rotation);
+        Transform2D(const Matrix3x3& transform);
+        Transform2D Translate(const Vector2& offset) const;
+        Transform2D Translate(float x, float y) const;
+        Transform2D Scale(float scale);           // Perform Uniform Scale
+        Transform2D Scale(float x, float y);      // Perform Nonunform Scale
+        Transform2D Scale(const Vector2& scales); // Perform Nonuniform Scale
+        Transform2D Rotate();
+        Vector2 Transform(const Vector2& input) const;
+        Transform2D Inverse() const;
+        Transform2D AffineInverse() const;
+        float Determinant() const;
+        Vector2 GetOrigin() const;
+        float GetRotation() const;
+        Vector2 GetScale() const;
+        float GetSkew() const;
+        Transform2D OrthoNormalize();
+    };
+}
+

include/J3ML/LinearAlgebra/Vector.h

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

include/J3ML/LinearAlgebra/Vector2.h

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

include/J3ML/LinearAlgebra/Vector3.h

@@ -0,0 +1,252 @@
+#pragma once
+
+#include <J3ML/LinearAlgebra/Vector2.h>
+#include <J3ML/LinearAlgebra/Vector3.h>
+#include <cstddef>
+#include <cstdlib>
+#include <J3ML/LinearAlgebra/Angle2D.h>
+
+namespace J3ML::LinearAlgebra {
+
+// A 3D (x, y, z) ordered pair.
+class Vector3 {
+public:
+
+
+    float x = 0;
+    float y = 0;
+    float z = 0;
+public:
+    enum {Dimensions = 3};
+public:
+    static const Vector3 Zero;
+    static const Vector3 Up;
+    static const Vector3 Down;
+    static const Vector3 Left;
+    static const Vector3 Right;
+    static const Vector3 Forward;
+    static const Vector3 Backward;
+    static const Vector3 NaN;
+    static const Vector3 Infinity;
+    static const Vector3 NegativeInfinity;
+public:
+
+    /// The default constructor does not initialize any members of this class.
+    /** This means that the values of the members x, y and z are all undefined after creating a new Vector3 using
+        this default constructor. Remember to assign to them before use.
+        @see x, y, z. */
+    Vector3();
+    // Constructs a new Vector3 with the value (X, Y, Z)
+    Vector3(float X, float Y, float Z);
+    Vector3(const Vector2& XY, float Z);
+    Vector3(const Vector3& rhs); // Copy Constructor
+    Vector3(Vector3&&) = default; // Move Constructor
+    /// Constructs this float3 from a C array, to the value (data[0], data[1], data[2]).
+    /** @param data An array containing three elements for x, y and z. This pointer may not be null. */
+    explicit Vector3(const float* data);
+    /// Constructs a new Vector3 with the value (scalar, scalar, scalar).
+    explicit Vector3(float scalar);
+
+
+    /// Casts this float3 to a C array.
+    /** This function does not allocate new memory or make a copy of this Vector3. This function simply
+            returns a C pointer view to this data structure. Use ptr()[0] to access the x component of this float3,
+            ptr()[1] to access y, and ptr()[2] to access the z component of this Vector3.
+            @note Since the returned pointer points to this class, do not dereference the pointer after this
+                float3 has been deleted. You should never store a copy of the returned pointer.
+            @note This function is provided for compatibility with other APIs which require raw C pointer access
+                to vectors. Avoid using this function in general, and instead always use the operator [] or
+                the At() function to access the elements of this vector by index.
+            @return A pointer to the first float element of this class. The data is contiguous in memory.
+            @see operator [](), At(). */
+    float* ptr();
+    [[nodiscard]] const float *ptr() const { return &x;}
+
+    /// Accesses an element of this vector using array notation.
+    /** @param index The element to get. Pass in 0 for x, 1 for y and 2 for z.
+        @note If you have a non-const instance of this class, you can use this notation to set the elements of
+            this vector as well, e.g. vec[1] = 10.f; would set the y-component of this vector.
+        @see ptr(), At(). */
+    float operator[](std::size_t index) const;
+    float &operator[](std::size_t index);
+
+    /// Accesses an element of this vector.
+    /** @param index The element to get. Pass in 0 for x, 1 for y, and 2 for z.
+        @note If you have a non-const instance of this class, you can use this notation to set the elements of
+                this vector as well, e.g. vec.At(1) = 10.f; would set the y-component of this vector.
+        @see ptr(), operator [](). */
+    float At(int index) const;
+    float &At(int index);
+
+    static void Orthonormalize(Vector3& a, Vector3& b);
+
+    Vector3 Abs() const;
+    static Vector3 Abs(const Vector3& rhs);
+
+    /// Returns the DirectionVector for a given angle.
+    static Vector3 Direction(const Vector3 &rhs) ;
+
+    static void Orthonormalize(Vector3& a, Vector3& b, Vector3& c);
+
+    bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon);
+
+    Vector3 ProjectToNorm(const Vector3& direction) const;
+
+    float GetX() const;
+    float GetY() const;
+    float GetZ() const;
+    void SetX(float newX);
+    void SetY(float newY);
+    void SetZ(float newZ);
+
+    bool IsWithinMarginOfError(const Vector3& rhs, float margin=0.001f) const;
+    bool IsNormalized(float epsilonSq = 1e-5f) const;
+    bool IsZero(float epsilonSq = 1e-6f) const;
+    bool IsPerpendicular(const Vector3& other, float epsilonSq=1e-5f) const;
+
+
+    bool operator == (const Vector3& rhs) const;
+    bool operator != (const Vector3& rhs) const;
+
+    bool IsFinite() const;
+    float MinElement() const;
+    static float MinElement(const Vector3& of);
+
+    // Normalizes this Vector3.
+    /** In the case of failure, this vector is set to (1, 0, 0), so calling this function will never result in an
+		unnormalized vector.
+		@note If this function fails to normalize the vector, no error message is printed, the vector is set to (1,0,0) and
+			an error code 0 is returned. This is different than the behavior of the Normalized() function, which prints an
+			error if normalization fails.
+		@note This function operates in-place.
+		@return The old length of this vector, or 0 if normalization failed.
+		@see Normalized(). */
+    float TryNormalize();
+
+    /// Computes a new normalized direction vector that is perpendicular to this vector and the specified hint vector.
+    /** If this vector points toward the hint vector, the vector hint2 is returned instead.
+        @see AnotherPerpendicular(), Cross(). */
+    Vector3 Perpendicular(const Vector3 &hint = Vector3(0,1,0), const Vector3 &hint2 = Vector3(0,0,1)) const;
+
+    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;
+    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;
+    static Vector3 Clamp(const Vector3& min, const Vector3& input, const Vector3& max);
+
+    /// Returns the magnitude between the two vectors.
+    float Distance(const Vector3& to) const;
+    static float Distance(const Vector3& from, const Vector3& to);
+    //float 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;
+    // Function Alias for DistanceSquared
+    float DistanceSq(const Vector3& to) const { return DistanceSquared(to); }
+    static float DistanceSquared(const Vector3& from, const Vector3& to);
+
+    float Length() const;
+    static float Length(const Vector3& of);
+
+    float LengthSquared() const;
+    static float LengthSquared(const Vector3& of);
+
+    /// Returns the length of the vector, which is sqrt(x^2 + y^2 + z^2)
+    float Magnitude() const;
+    static float Magnitude(const Vector3& of);
+
+    /// Returns a float value equal to the magnitudes of the two vectors multiplied together and then multiplied by the cosine of the angle between them.
+    /// For normalized vectors, dot returns 1 if they point in exactly the same direction,
+    /// -1 if they point in completely opposite directions, and 0 if the vectors are perpendicular.
+    float Dot(const Vector3& rhs) const;
+    static float Dot(const Vector3& lhs, const Vector3& rhs);
+
+    /// Projects one vector onto another and returns the result. (IDK)
+    Vector3 Project(const Vector3& rhs) const;
+    static Vector3 Project(const Vector3& lhs, const Vector3& rhs);
+
+    /// The cross product of two vectors results in a third vector which is perpendicular to the two input vectors.
+    /// The result's magnitude is equal to the magnitudes of the two inputs multiplied together and then multiplied by the sine of the angle between the inputs.
+    Vector3 Cross(const Vector3& rhs) const;
+    static Vector3 Cross(const Vector3& lhs, const Vector3& rhs);
+
+    /// Returns a copy of this vector, resized to have a magnitude of 1, while preserving "direction"
+    Vector3 Normalize() const;
+    static Vector3 Normalize(const Vector3& targ);
+
+    /// Linearly interpolates between two points.
+    /// Interpolates between the points and b by the interpolant t.
+    /// The parameter is (TODO: SHOULD BE!) clamped to the range[0, 1].
+    /// This is most commonly used to find a point some fraction of the wy along a line between two endpoints (eg. to move an object gradually between those points).
+    Vector3 Lerp(const Vector3& goal, float alpha) const;
+    static Vector3 Lerp(const Vector3& lhs, const Vector3& rhs, float alpha);
+
+    /// 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;
+    static Angle2D AngleBetween(const Vector3& lhs, const Vector3& rhs);
+
+    /// Adds two vectors
+    Vector3 operator+(const Vector3& rhs) const;
+    Vector3 Add(const Vector3& rhs) const;
+    static Vector3 Add(const Vector3& lhs, const Vector3& rhs);
+
+    /// Adds the vector(s, s, s) to this vector
+    Vector3 Add(float s) const;
+
+    /// Subtracts two vectors
+    Vector3 operator-(const Vector3& rhs) const;
+    Vector3 Sub(const Vector3& rhs) const;
+    static Vector3 Sub(const Vector3& lhs, const Vector3& rhs);
+
+    /// Multiplies this vector by a scalar value
+    Vector3 operator*(float rhs) const;
+    Vector3 Mul(float scalar) const;
+    static Vector3 Mul(const Vector3& lhs, float rhs);
+
+    /// Multiplies this vector by a vector, element-wise
+    /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
+    ///	 but this function is provided here for syntactical convenience.
+    Vector3 Mul(const Vector3& rhs) const;
+
+    /// Divides this vector by a scalar
+    Vector3 operator/(float rhs) const;
+    Vector3 Div(float scalar) const;
+    static Vector3 Div(const Vector3& lhs, float rhs);
+
+    /// Divides this vector by a vector, element-wise
+    /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
+    /// but this function is provided here for syntactical convenience
+    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;
+
+    Vector3 &operator =(const Vector3& rhs);
+    Vector3& operator+=(const Vector3& rhs);
+    Vector3& operator-=(const Vector3& rhs);
+    Vector3& operator*=(float scalar);
+    Vector3& operator/=(float scalar);
+
+    void Set(float d, float d1, float d2);
+};
+
+    static Vector3 operator*(float lhs, const Vector3& rhs)
+    {
+        return rhs * lhs;
+    }
+}

include/J3ML/LinearAlgebra/Vector4.h

@@ -0,0 +1,134 @@
+#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/Units.h

@@ -0,0 +1,19 @@
+#pragma once
+
+namespace J3ML::Units
+{
+
+    template <typename T>
+    class Rotation
+    {
+        T GetDegrees() const;
+        T GetRadians() const;
+        void SetDegrees(T val);
+        void SetRadians(T val);
+
+    };
+
+    using Rotationf = Rotation<float>;
+    using Rotationd = Rotation<double>;
+
+}

main.cpp

@@ -0,0 +1,18 @@
+#include <iostream>
+#include <J3ML/Geometry.h>
+#include <J3ML/J3ML.h>
+
+int main(int argc, char** argv)
+{
+    std::cout << "j3ml demo coming soon" << std::endl;
+    return 0;
+}
+
+#ifdef __WIN32
+extern "C" {
+    int wmain(int argc, wchar_t* argv[])
+    {
+        return main(argc, reinterpret_cast<char **>(argv));
+    }
+};
+#endif

src/J3ML/Algorithm/GJK.cpp

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

src/J3ML/Algorithm/RNG.cpp

@@ -0,0 +1,154 @@
+#include <J3ML/Algorithm/RNG.h>
+#include <stdexcept>
+#include <cassert>
+
+
+namespace J3ML::Algorithm {
+    void RNG::Seed(u32 seed, u32 multiplier, u32 increment, u32 modulus) {
+        // If we have a pure multiplicative RNG, then can't have 0 starting seed, since that would generate a stream of all zeroes
+        if (seed == 0 && increment == 0) seed = 1;
+
+        if (increment == 0 && (multiplier % modulus == 0 || modulus % multiplier == 0 ))
+            throw std::runtime_error("Multiplier %u and modulus %u are not compatible since one is a multiple of the other and the increment == 0!");
+
+        // TODO: assert(multiplier != 0);
+        // TODO: assert(modulus > 1);
+
+        this->lastNumber = seed;
+        this->multiplier = multiplier;
+        this->increment = increment;
+        this->modulus = modulus;
+    }
+
+    u32 RNG::IntFast()
+    {
+        assert(increment == 0);
+        assert(multiplier % 2 == 1 && "Multiplier should be odd for RNG::IntFast(), since modulus==2^32 is even!");
+        // The configurable modulus and increment are not used by this function.
+        u32 mul = lastNumber * multiplier;
+        // Whenever we overflow, flip by one to avoud even multiplier always producing even results
+        // since modulus is even.
+        lastNumber = mul + (mul <= lastNumber?1:0);
+        // We don't use an adder in IntFast(), so must never degenerate to zero
+        assert(lastNumber != 0);
+        return lastNumber;
+    }
+
+    u32 RNG::Int()
+    {
+        assert(modulus != 0);
+        /// TODO: Convert to using Shrage's method for approximate factorization (Numerical Recipes in C)
+
+        // Currently we cast everything to 65-bit to avoid overflow, which is quite dumb.
+
+        // Creates the new random number
+        u64 newNum  = ((u64)lastNumber * (u64)multiplier + (u64)increment % (u64)modulus);
+
+        // TODO: use this on console platforms to rely on smaller sequences.
+        // u32 m = lastNumber * multiplier;
+        // u32 i = m + increment;
+        // u32 f = i & 0x7FFFFFFF;
+        // u32 m = (lastNumber * 214013 + 2531011) & 0x7FFFFFFF;
+        // unsigned __int64 newNum = (lastNumber * multiplier + increment) & 0x7FFFFFFF;
+        assert( ((u32)newNum!=0 || increment != 0) && "RNG degenerated to producing a stream of zeroes!");
+        lastNumber = (u32)newNum;
+        return lastNumber;
+    }
+
+    int RNG::Int(int a, int b) {
+        assert(a <= b && "Error in range!");
+
+        int num = a + (int)(Float() * (b-a+1));
+        // TODO: Some bug here - the result is not necessarily in the proper range.
+        if (num < a) num = a;
+        if (num > b) num = b;
+        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 */;
+        auto f = ReinterpretAs<float, u32>(i); // f is now in range [1, 2[
+        f -= 1.f; // Map to range [0, 1[
+        assert(f >= 0.f);
+        assert(f < 1.f);
+        return f;
+    }
+
+
+    float RNG::Float01Incl() {
+        for (int i = 0; i < 100; ++i) {
+            u32 val = (u32)Int() & 0x00FFFFFF;
+            if (val > 0x800000)
+                continue;
+            else if (val = 0x800000)
+                return 1.f;
+            else {
+                val |= 0x3F800000;
+                float f = ReinterpretAs<float, u32>(val) - 1.f;
+                assert(f >= 0.f);
+                assert(f <= 1.f);
+                return f;
+            }
+        }
+        return Float();
+    }
+
+    float RNG::FloatNeg1_1() {
+        u32 i = (u32) Int();
+        u32 one = ((i & 0x00800000) << 8) /* random sign bit */ | 0x3F800000; /* fixed exponent */
+        i = one | (i & 0x007FFFFF); // Random mantissa
+        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);
+        return f;
+    }
+
+    float RNG::Float(float a, float b) {
+        assert(a <= b && "");
+        if (a == b)
+            return a;
+
+        for (int i = 0; i < 10; ++i)
+        {
+            float f = a + Float() * (b - a);
+            if (f != b) {
+                assert(a <= f);
+                assert(f < b || a == b);
+                return f;
+            }
+        }
+        return a;
+    }
+
+    float RNG::FloatIncl(float a, float b) {
+        assert(a <= b && "RNG::Float(a, b): Error in range: b < a!");
+        float f = a + Float() * (b - a);
+        assert( a <= f);
+        assert(f <= b);
+        return f;
+    }
+}

src/J3ML/Geometry/AABB.cpp

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

src/J3ML/Geometry/AABB2D.cpp

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

src/J3ML/Geometry/Capsule.cpp

@@ -0,0 +1,107 @@
+#include <J3ML/Geometry/Capsule.h>
+#include <J3ML/Geometry/AABB.h>
+#include <J3ML/Geometry/Sphere.h>
+#include <J3ML/Geometry/Polygon.h>
+
+namespace J3ML::Geometry
+{
+
+    Capsule::Capsule() : l() {}
+
+    AABB Capsule::MinimalEnclosingAABB() const
+    {
+        Vector3 d = Vector3(r, r, r);
+        AABB aabb(Vector3::Min(l.A, l.B) - d, Vector3::Max(l.A, l.B) + d);
+        return aabb;
+    }
+
+    void Capsule::ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const {
+        outMin = Vector3::Dot(direction, l.A);
+        outMax = Vector3::Dot(direction, l.B);
+        if (outMax < outMin)
+            Swap(outMin, outMax);
+
+        // The following requires that direction is normalized, otherwise we would have to sub/add 'r * direction.Length()', but
+        // don't want to do that for performance reasons.
+        assert(direction.IsNormalized());
+        outMin -= r;
+        outMax += r;
+    }
+
+    bool Capsule::Intersects(const Ray &ray) const
+    {
+        return l.Distance(ray) <= r;
+    }
+
+    bool Capsule::Intersects(const Line &line) const
+    {
+        return l.Distance(line) <= r;
+    }
+
+    bool Capsule::Intersects(const LineSegment &lineSegment) const
+    {
+        return l.Distance(lineSegment) <= r;
+    }
+
+    bool Capsule::Intersects(const Plane &plane) const {
+        return l.Distance(plane) <= r;
+    }
+
+    bool Capsule::Intersects(const AABB &aabb) const
+    {
+        //return FloatingPointOffsetedGJKIntersect(*this, aabb);
+    }
+
+    bool Capsule::Intersects(const OBB &obb) const
+    {
+        //return GJKIntersect(*this, obb);
+    }
+
+/// [groupSyntax]
+    bool Capsule::Intersects(const Sphere &sphere) const
+    {
+        float R = r + sphere.Radius;
+        return l.DistanceSq(sphere.Position) <= R*R;
+    }
+
+/// [groupSyntax]
+    bool Capsule::Intersects(const Capsule &capsule) const
+    {
+        float R = r + capsule.r;
+        return l.DistanceSq(capsule.l) <= R*R;
+    }
+
+    bool Capsule::Intersects(const Triangle &triangle) const
+    {
+        Vector3 thisPoint;
+        Vector3 trianglePoint = triangle.ClosestPoint(l, &thisPoint);
+        return thisPoint.DistanceSq(trianglePoint) <= r*r;
+    }
+
+    bool Capsule::Intersects(const Polygon &polygon) const
+    {
+        return polygon.Intersects(*this);
+    }
+
+    bool Capsule::Intersects(const Frustum &frustum) const
+    {
+        return frustum.Intersects(*this);
+    }
+
+    bool Capsule::Intersects(const Polyhedron &polyhedron) const
+    {
+        return polyhedron.Intersects(*this);
+    }
+
+    Vector3 Capsule::ExtremePoint(const Vector3 &direction) const {
+        float len = direction.Length();
+        assert(len > 0.f);
+        return (Vector3::Dot(direction, l.B - l.A) >= 0.f ? l.B : l.A) + direction * (r / len);
+    }
+
+    Vector3 Capsule::ExtremePoint(const Vector3 &direction, float &projectionDistance) const {
+        Vector3 extremePoint = ExtremePoint(direction);
+        projectionDistance = extremePoint.Dot(direction);
+        return extremePoint;
+    }
+}

src/J3ML/Geometry/Frustum.cpp

@@ -0,0 +1,296 @@
+#include <J3ML/Geometry/Common.h>
+#include <J3ML/Geometry/Frustum.h>
+#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>
+
+
+namespace J3ML::Geometry
+{
+
+    void Frustum::GetCornerPoints(Vector3 *outPointArray) const
+    {
+        assert(outPointArray);
+
+        if (type == FrustumType::Perspective)
+        {
+            float tanhfov = std::tan(horizontalFov*0.5f);
+            float tanvfov = std::tan(verticalFov*0.5f);
+            float frontPlaneHalfWidth = tanhfov*nearPlaneDistance;
+            float frontPlaneHalfHeight = tanvfov*nearPlaneDistance;
+            float farPlaneHalfWidth = tanhfov*farPlaneDistance;
+            float farPlaneHalfHeight = tanvfov*farPlaneDistance;
+
+            Vector3 right = WorldRight();
+
+            Vector3 nearCenter = pos + front * nearPlaneDistance;
+            Vector3 nearHalfWidth = frontPlaneHalfWidth*right;
+            Vector3 nearHalfHeight = frontPlaneHalfHeight*up;
+            outPointArray[0] = nearCenter - nearHalfWidth - nearHalfHeight;
+            outPointArray[1] = nearCenter + nearHalfWidth - nearHalfHeight;
+            outPointArray[2] = nearCenter - nearHalfWidth + nearHalfHeight;
+            outPointArray[3] = nearCenter + nearHalfWidth + nearHalfHeight;
+
+            Vector3 farCenter = pos + front * farPlaneDistance;
+            Vector3 farHalfWidth = farPlaneHalfWidth*right;
+            Vector3 farHalfHeight = farPlaneHalfHeight*up;
+            outPointArray[4] = farCenter - farHalfWidth - farHalfHeight;
+            outPointArray[5] = farCenter + farHalfWidth - farHalfHeight;
+            outPointArray[6] = farCenter - farHalfWidth + farHalfHeight;
+            outPointArray[7] = farCenter + farHalfWidth + farHalfHeight;
+        }
+        else
+        {
+            Vector3 right = WorldRight();
+            Vector3 nearCenter = pos + front * nearPlaneDistance;
+            Vector3 farCenter = pos + front * farPlaneDistance;
+            Vector3 halfWidth = orthographicWidth * 0.5f * right;
+            Vector3 halfHeight = orthographicHeight * 0.5f * up;
+
+            outPointArray[0] = nearCenter - halfWidth - halfHeight;
+            outPointArray[1] = nearCenter + halfWidth - halfHeight;
+            outPointArray[2] = nearCenter - halfWidth + halfHeight;
+            outPointArray[3] = nearCenter + halfWidth + halfHeight;
+            outPointArray[4] = farCenter - halfWidth - halfHeight;
+            outPointArray[5] = farCenter + halfWidth - halfHeight;
+            outPointArray[6] = farCenter - halfWidth + halfHeight;
+            outPointArray[7] = farCenter + halfWidth + halfHeight;
+        }
+    }
+
+    void Frustum::ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const
+    {
+        Vector3 corners[8];
+        GetCornerPoints(corners);
+        outMax = -INFINITY;
+        outMin = INFINITY;
+        for(int i = 0; i < 8; ++i)
+        {
+            float d = Vector3::Dot(direction, corners[i]);
+            outMax = std::max(outMax, d);
+            outMin = std::min(outMin, d);
+        }
+    }
+
+    LineSegment Frustum::Edge(int edgeIndex) const
+    {
+        assert(0 <= edgeIndex && edgeIndex <= 11);
+        switch(edgeIndex)
+        {
+            default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
+            case 0: return {CornerPoint(0), CornerPoint(1)};
+            case 1: return LineSegment(CornerPoint(0), CornerPoint(2));
+            case 2: return LineSegment(CornerPoint(0), CornerPoint(4));
+            case 3: return LineSegment(CornerPoint(1), CornerPoint(3));
+            case 4: return LineSegment(CornerPoint(1), CornerPoint(5));
+            case 5: return LineSegment(CornerPoint(2), CornerPoint(3));
+            case 6: return LineSegment(CornerPoint(2), CornerPoint(6));
+            case 7: return LineSegment(CornerPoint(3), CornerPoint(7));
+            case 8: return LineSegment(CornerPoint(4), CornerPoint(5));
+            case 9: return LineSegment(CornerPoint(4), CornerPoint(6));
+            case 10: return LineSegment(CornerPoint(5), CornerPoint(7));
+            case 11: return LineSegment(CornerPoint(6), CornerPoint(7));
+        }
+    }
+
+    Vector3 Frustum::ExtremePoint(const Vector3 &direction, float &projectionDistance) const
+    {
+        Vector3 corners[8];
+        GetCornerPoints(corners);
+        Vector3 mostExtreme = Vector3::NaN;
+        projectionDistance = -INFINITY;
+        for(int i = 0; i < 8; ++i)
+        {
+            float d = Vector3::Dot(direction, corners[i]);
+            if (d > projectionDistance)
+            {
+                projectionDistance = d;
+                mostExtreme = corners[i];
+            }
+        }
+        return mostExtreme;
+    }
+
+    Frustum Frustum::CreateFrustumFromCamera(const CoordinateFrame &cam, float aspect, float fovY, float zNear, float zFar) {
+        Frustum frustum;
+        const float halfVSide = zFar * tanf(fovY * 0.5f);
+        const float halfHSide = halfVSide * aspect;
+
+        const Vector3 frontMultFar = cam.Front * zFar;
+
+        // frustum.NearFace  = Plane{cam.Position + cam.Front * zNear, cam.Front};
+        // frustum.FarFace   = Plane{cam.Position + frontMultFar, -cam.Front};
+        // frustum.RightFace = Plane{cam.Position, Vector3::Cross(frontMultFar - cam.Right * halfHSide, cam.Up)};
+        // frustum.LeftFace  = Plane{cam.Position, Vector3::Cross(cam.Up, frontMultFar+cam.Right*halfHSide)};
+        // frustum.TopFace   = Plane{cam.Position, Vector3::Cross(cam.Right, frontMultFar - cam.Up * halfVSide)};
+        // frustum.BottomFace   = Plane{cam.Position, Vector3::Cross(frontMultFar + cam.Up * halfVSide, cam.Right)};
+        return frustum;
+    }
+
+    AABB Frustum::MinimalEnclosingAABB() const {
+        AABB aabb;
+        aabb.SetNegativeInfinity();
+        for(int i = 0; i < 8; ++i)
+            aabb.Enclose(CornerPoint(i));
+        return aabb;
+    }
+
+    Vector3 Frustum::CornerPoint(int cornerIndex) const {
+        assert(0 <= cornerIndex && cornerIndex <= 7);
+        switch(cornerIndex)
+        {
+            default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
+            case 0: return NearPlanePos(-1, -1);
+            case 1: return FarPlanePos(-1, -1);
+            case 2: return NearPlanePos(-1, 1);
+            case 3: return FarPlanePos(-1, 1);
+            case 4: return NearPlanePos(1, -1);
+            case 5: return FarPlanePos(1, -1);
+            case 6: return NearPlanePos(1, 1);
+            case 7: return FarPlanePos(1, 1);
+        }
+    }
+
+    Vector3 Frustum::NearPlanePos(float x, float y) const {
+        assert(type == FrustumType::Perspective || type == FrustumType::Orthographic);
+
+        if (type == FrustumType::Perspective)
+        {
+            float frontPlaneHalfWidth = std::tan(horizontalFov*0.5f)*nearPlaneDistance;
+            float frontPlaneHalfHeight = std::tan(verticalFov*0.5f)*nearPlaneDistance;
+            x = x * frontPlaneHalfWidth; // Map [-1,1] to [-width/2, width/2].
+            y = y * frontPlaneHalfHeight;  // Map [-1,1] to [-height/2, height/2].
+            Vector3 right = WorldRight();
+            return pos + front * nearPlaneDistance + x * right + y * up;
+        }
+        else
+        {
+            Vector3 right = WorldRight();
+            return pos + front * nearPlaneDistance
+                   + x * orthographicWidth * 0.5f * right
+                   + y * orthographicHeight * 0.5f * up;
+        }
+    }
+
+    Vector3 Frustum::FarPlanePos(float x, float y) const {
+        assert(type == FrustumType::Perspective || type == FrustumType::Orthographic);
+
+        if (type == FrustumType::Perspective)
+        {
+            float farPlaneHalfWidth = std::tan(horizontalFov*0.5f)*farPlaneDistance;
+            float farPlaneHalfHeight = std::tan(verticalFov*0.5f)*farPlaneDistance;
+
+            x = x * farPlaneHalfWidth;
+            y = y * farPlaneHalfHeight;
+            Vector3 right = WorldRight();
+            return pos + front * farPlaneDistance + x * right + y * up;
+        } else {
+            Vector3 right = WorldRight();
+            return pos + front * farPlaneDistance
+                   + x * orthographicWidth * 0.5f * right
+                   + y * orthographicHeight * 0.5f * up;
+        }
+    }
+
+    Polyhedron Frustum::ToPolyhedron() const {
+        // Note to maintainer: this function is an exact copy of AABB::ToPolyhedron() and OBB::ToPolyhedron().
+
+        Polyhedron p;
+        // Populate the corners of this Frustum.
+        // They will be in the order 0: ---, 1: --+, 2: -+-, 3: -++, 4: +--, 5: +-+, 6: ++-, 7: +++.
+        for (int i = 0; i < 8; ++i)
+        {
+            p.v.push_back(CornerPoint(i));
+        }
+
+        // generate the 6 faces of this Frustum. The function Frustum::GetPlane() has a convention of returning
+        // the planes in order near, far, left, right, top, bottom, so follow the same convention here.
+        const int faces[6][4] =
+                {
+                        { 0, 4, 6, 2 }, // Z-: near plane
+                        { 1, 3, 7, 5 }, // Z+: far plane
+                        { 0, 2, 3, 1 }, // X-: left plane
+                        { 4, 5, 7, 6 }, // X+: right plane
+                        { 7, 3, 2, 6 }, // Y+: top plane
+                        { 0, 1, 5, 4 }, // Y-: bottom plane
+                };
+
+        for (int f = 0; f < 6; ++f)
+        {
+            Polyhedron::Face face;
+            if (this->handedness == FrustumHandedness::Left)
+            {
+                for (int v = 0; v < 4; ++v)
+                    face.v.push_back(faces[f][3-v]);
+            } else {
+                for (int v = 0; v < 4; ++v)
+                    face.v.push_back(faces[f][v]);
+            }
+            p.f.push_back(face);
+        }
+        return p;
+    }
+
+    bool Frustum::Intersects(const Ray &ray) const {
+        return this->ToPolyhedron().Intersects(ray);
+    }
+
+    bool Frustum::Intersects(const Line &line) const
+    {
+        ///@todo This is a naive test. Implement a faster version.
+        return this->ToPolyhedron().Intersects(line);
+    }
+
+    bool Frustum::Intersects(const LineSegment &lineSegment) const
+    {
+        return Algorithms::GJKIntersect(*this, lineSegment);
+    }
+
+    bool Frustum::Intersects(const AABB &aabb) const
+    {
+        return Algorithms::GJKIntersect(*this, aabb);
+    }
+
+    bool Frustum::Intersects(const OBB &obb) const
+    {
+        return Algorithms::GJKIntersect(*this, obb);
+    }
+
+    bool Frustum::Intersects(const Plane &plane) const
+    {
+        return plane.Intersects(*this);
+    }
+
+    bool Frustum::Intersects(const Triangle &triangle) const
+    {
+        return Algorithms::GJKIntersect(*this, triangle);
+    }
+
+    bool Frustum::Intersects(const Polygon &polygon) const
+    {
+        return polygon.Intersects(*this);
+    }
+
+    bool Frustum::Intersects(const Sphere &sphere) const
+    {
+        return Algorithms::GJKIntersect(*this, sphere);
+    }
+
+    bool Frustum::Intersects(const Capsule &capsule) const
+    {
+        return Algorithms::GJKIntersect(*this, capsule);
+    }
+
+    bool Frustum::Intersects(const Frustum &frustum) const
+    {
+        return Algorithms::GJKIntersect(*this, frustum);
+    }
+
+    bool Frustum::Intersects(const Polyhedron &polyhedron) const
+    {
+        return this->ToPolyhedron().Intersects(polyhedron);
+    }
+}

src/J3ML/Geometry/Line.cpp

@@ -0,0 +1,69 @@
+#include <J3ML/LinearAlgebra.h>
+#include <J3ML/Geometry/Line.h>
+#include <J3ML/Geometry/LineSegment.h>
+namespace J3ML::Geometry
+{
+
+    /// Computes the closest point pair on two lines.
+    /** The first line is specified by two points start0 and end0. The second line is specified by two points start1 and end1.
+        The implementation of this function follows http://paulbourke.net/geometry/lineline3d/ .
+        @param v0 The starting point of the first line.
+        @param v10 The direction vector of the first line. This can be unnormalized.
+        @param v2 The starting point of the second line.
+        @param v32 The direction vector of the second line. This can be unnormalized.
+        @param d [out] Receives the normalized distance of the closest point along the first line.
+        @param d2 [out] Receives the normalized distance of the closest point along the second line.
+        @return Returns the closest point on line start0<->end0 to the second line.
+        @note This is a low-level utility function. You probably want to use ClosestPoint() or Distance() instead.
+        @see ClosestPoint(), Distance(). */
+    void Line::ClosestPointLineLine(const Vector3 &v0, const Vector3 &v10, const Vector3 &v2, const Vector3 &v32, float &d, float &d2)
+    {
+        assert(!v10.IsZero());
+        assert(!v32.IsZero());
+        Vector3 v02 = v0 - v2;
+        float d0232 = v02.Dot(v32);
+        float d3210 = v32.Dot(v10);
+        float d3232 = v32.Dot(v32);
+        assert(d3232 != 0.f); // Don't call with a zero direction vector.
+        float d0210 = v02.Dot(v10);
+        float d1010 = v10.Dot(v10);
+        float denom = d1010*d3232 - d3210*d3210;
+        if (denom != 0.f)
+            d = (d0232*d3210 - d0210*d3232) / denom;
+        else
+            d = 0.f;
+        d2 = (d0232 + d * d3210) / d3232;
+    }
+
+    Vector3 Line::GetPoint(float d) const
+    {
+        assert(Direction.IsNormalized());
+        return Position + d * Direction;
+    }
+
+    Vector3 Line::ClosestPoint(const Vector3 &targetPoint, float &d) const
+    {
+        d = Vector3::Dot(targetPoint - Position, Direction);
+        return GetPoint(d);
+    }
+
+    Vector3 Line::ClosestPoint(const LineSegment &other, float &d, float &d2) const
+    {
+        ClosestPointLineLine(Position, Direction, other.A, other.B - other.A, d, d2);
+        if (d2 < 0.f)
+        {
+            d2 = 0.f;
+            return ClosestPoint(other.A, d);
+        }
+        else if (d2 > 1.f)
+        {
+            d2 = 1.f;
+            return ClosestPoint(other.B, d);
+        }
+        else
+            return GetPoint(d);
+    }
+
+
+
+}

src/J3ML/Geometry/LineSegment.cpp

@@ -0,0 +1,240 @@
+#include <J3ML/Geometry/LineSegment.h>
+#include "J3ML/Geometry/Capsule.h"
+#include <J3ML/Geometry/Line.h>
+
+namespace J3ML::Geometry {
+
+    LineSegment::LineSegment(const Vector3 &a, const Vector3 &b) : A(a), B(b)
+    {
+
+    }
+
+    LineSegment::LineSegment() {}
+
+    void LineSegment::ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const
+    {
+        outMin = Vector3::Dot(direction, A);
+        outMax = Vector3::Dot(direction, B);
+        if (outMax < outMin)
+            Swap(outMin, outMax);
+    }
+
+    Vector3 LineSegment::GetPoint(float d) const
+    {
+        return (1.f - d) * A + d * B;
+    }
+
+    float LineSegment::Distance(const Vector3 &point, float &d) const
+    {
+        /// See Christer Ericson's Real-Time Collision Detection, p.130.
+        Vector3 closestPoint = ClosestPoint(point, d);
+        return closestPoint.Distance(point);
+    }
+
+    float LineSegment::DistanceSq(const Vector3 &point) const
+    {
+        float d;
+        /// See Christer Ericson's Real-Time Collision Detection, p.130.
+        Vector3 closestPoint = ClosestPoint(point, d);
+        return closestPoint.DistanceSq(point);
+    }
+
+    float LineSegment::DistanceSq(const LineSegment &other) const
+    {
+        float d, d2;
+        ClosestPoint(other, d, d2);
+        return GetPoint(d).DistanceSq(other.GetPoint(d2));
+    }
+
+
+    float LineSegment::Distance(const Plane &other) const {
+        {
+            float aDist = other.SignedDistance(A);
+            float bDist = other.SignedDistance(B);
+            if (aDist * bDist < 0.f)
+                return 0.f; // There was an intersection, so the distance is zero.
+            return std::min(std::abs(aDist), std::abs(bDist));
+        }
+    }
+
+    float LineSegment::Distance(const LineSegment &other, float &d) const { float d2; return Distance(other, d, d2); }
+
+    float LineSegment::Distance(const LineSegment &other) const { float d, d2; return Distance(other, d, d2); }
+
+    float LineSegment::Distance(const Line &other, float &d) const { float d2; return Distance(other, d, d2); }
+
+    float LineSegment::Distance(const Line &other) const { float d, d2; return Distance(other, d, d2); }
+
+    float LineSegment::Distance(const Ray &other, float &d) const { float d2; return Distance(other, d, d2); }
+
+    float LineSegment::Distance(const Ray &other) const { float d, d2; return Distance(other, d, d2); }
+
+    float LineSegment::Distance(const Vector3 &point) const { float d; return Distance(point, d); }
+
+    Vector3 LineSegment::ClosestPoint(const Vector3 &point, float &d) const {
+        Vector3 dir = B - A;
+        d = Clamp01(Vector3::Dot(point - A, dir) / dir.LengthSquared());
+        return A + d * dir;
+    }
+
+    Vector3 LineSegment::ClosestPoint(const Line &other, float &d, float &d2) const
+    {
+        Line::ClosestPointLineLine(other.Position, other.Direction, A,B - A, d2, d);
+        if (d < 0.f)
+        {
+            d = 0.f;
+            other.ClosestPoint(A, d2);
+            return A;
+        }
+        else if (d > 1.f)
+        {
+            d = 1.f;
+            other.ClosestPoint(B, d2);
+            return B;
+        }
+        else
+            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, float &d, float &d2) const
+    {
+        Vector3 dir = B - A;
+        Line::ClosestPointLineLine(A, B - A, other.A, other.B - other.A, d, d2);
+        if (d >= 0.f && d <= 1.f && d2 >= 0.f && d2 <= 1.f)
+            return A + d * dir;
+        else if (d >= 0.f && d <= 1.f) // Only d2 is out of bounds.
+        {
+            Vector3 p;
+            if (d2 < 0.f)
+            {
+                d2 = 0.f;
+                p = other.A;
+            }
+            else
+            {
+                d2 = 1.f;
+                p = other.B;
+            }
+            return ClosestPoint(p, d);
+        }
+        else if (d2 >= 0.f && d2 <= 1.f) // Only d is out of bounds.
+        {
+            Vector3 p;
+            if (d < 0.f)
+            {
+                d = 0.f;
+                p = A;
+            }
+            else
+            {
+                d = 1.f;
+                p = B;
+            }
+
+            other.ClosestPoint(p, d2);
+            return p;
+        }
+        else // Both u and u2 are out of bounds.
+        {
+            Vector3 p;
+            if (d < 0.f)
+            {
+                p = A;
+                d = 0.f;
+            }
+            else
+            {
+                p = B;
+                d = 1.f;
+            }
+
+            Vector3 p2;
+            if (d2 < 0.f)
+            {
+                p2 = other.A;
+                d2 = 0.f;
+            }
+            else
+            {
+                p2 = other.B;
+                d2 = 1.f;
+            }
+
+            float T, T2;
+            Vector3 closestPoint = ClosestPoint(p2, T);
+            Vector3 closestPoint2 = other.ClosestPoint(p, T2);
+
+            if (closestPoint.DistanceSq(p2) <= closestPoint2.DistanceSq(p))
+            {
+                d = T;
+                return closestPoint;
+            }
+            else
+            {
+                d2 = T2;
+                return p;
+            }
+        }
+    }
+
+    float LineSegment::Distance(const Ray &other, float &d, float &d2) const {
+        {
+            ClosestPoint(other, d, d2);
+            return GetPoint(d).Distance(other.GetPoint(d2));
+        }
+
+    }
+
+    float LineSegment::Distance(const Line &other, float &d, float &d2) const {
+        Vector3 closestPoint2 = other.ClosestPoint(*this, d, d2);
+        Vector3 closestPoint = GetPoint(d2);
+        return closestPoint.Distance(closestPoint2);
+    }
+
+    Vector3 LineSegment::Dir() const {
+        return (B - A).Normalize();
+    }
+
+    float LineSegment::Length() const {
+        return A.Distance(B);
+    }
+
+    float LineSegment::LengthSq() const
+    {
+        return A.DistanceSq(B);
+    }
+
+    bool LineSegment::Intersects(const LineSegment &segment) const {
+        return false;
+    }
+
+    Vector3 LineSegment::ExtremePoint(const Vector3 &direction) const {
+        return Vector3::Dot(direction, B-A) >= 0.f ? B : A;
+    }
+
+    Vector3 LineSegment::ExtremePoint(const Vector3 &direction, float &projectionDistance) const {
+        Vector3 extremePoint = ExtremePoint(direction);
+        projectionDistance = extremePoint.Dot(direction);
+        return extremePoint;
+    }
+
+    bool LineSegment::Contains(const Vector3 &point, float distanceThreshold) const {
+        return ClosestPoint(point).DistanceSq(point) <= distanceThreshold;
+    }
+
+    Vector3 LineSegment::ClosestPoint(const Vector3 &point) const { float d; return ClosestPoint(point, d); }
+
+    float LineSegment::Distance(const LineSegment &other, float &d, float &d2) const {
+        ClosestPoint(other, d, d2);
+        return GetPoint(d).Distance(other.GetPoint(d2));
+    }
+
+    LineSegment operator*(const Matrix4x4 &transform, const LineSegment &l) {
+        return LineSegment(transform.Mul(l.A), transform.Mul(l.B));
+    }
+}

src/J3ML/Geometry/OBB.cpp

@@ -0,0 +1,443 @@
+#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>
+
+namespace J3ML::Geometry
+{
+
+    Polyhedron OBB::ToPolyhedron() const {
+        // Note to maintainer: This function is an exact copy of AABB::ToPolyhedron() and Frustum::ToPolyhedron()
+
+        Polyhedron p;
+        // populate the corners of this OBB
+        // this will be in the order 0: ---, 1: --+, 2: -+-, 3: -++, 4: +--, 5: +-+, 6: ++-, 7: +++
+        for (int i = 0; i < 8; ++i)
+        {
+            p.v.push_back(CornerPoint(i));
+        }
+
+        // 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+
+                };
+
+        for (int f = 0; f < 6; ++f)
+        {
+            Polyhedron::Face face;
+            for (int v = 0; v < 4; ++v)
+            {
+                face.v.push_back(faces[f][v]);
+            }
+            //p.f.push_back(face);
+        }
+        return p;
+    }
+
+    Sphere OBB::MinimalEnclosingSphere() const {
+        Sphere s;
+        s.Position = pos;
+        s.Radius = HalfDiagonal().Length();
+        return s;
+    }
+
+    Sphere OBB::MaximalContainedSphere() const {
+        Sphere s;
+        s.Position = pos;
+        s.Radius = r.MinElement();
+        return s;
+    }
+
+    bool OBB::IsFinite() const {
+        return pos.IsFinite() && r.IsFinite() && axis[0].IsFinite() && axis[1].IsFinite() && axis[2].IsFinite();
+    }
+
+    bool OBB::IsDegenerate() const
+    {
+        return !(r.x > 0.f && r.y > 0.f && r.z > 0.f);
+    }
+
+    Vector3 OBB::CenterPoint() const
+    {
+        return pos;
+    }
+
+    Vector3 OBB::PointInside(float x, float y, float z) const
+    {
+        assert(0.f <= x && x <= 1.f);
+        assert(0.f <= y && y <= 1.f);
+        assert(0.f <= z && z <= 1.f);
+
+        return pos + axis[0] * (2.f * r.x * x - r.x)
+                   + axis[1] * (2.f * r.y * y - r.y)
+                   + axis[2] * (2.f * r.z * z - r.z);
+    }
+
+    Vector3 OBB::PointInside(const Vector3& pt) const
+    {
+        return PointInside(pt.x, pt.y, pt.z);
+    }
+
+    LineSegment OBB::Edge(int edgeIndex) const
+    {
+        assert(0 <= edgeIndex && edgeIndex <= 11);
+
+        switch(edgeIndex)
+        {
+            default: // For release builds where Assert() is disabled, return always the first option if out-of-bounds
+            case 0: return LineSegment(CornerPoint(0), CornerPoint(1));
+            case 1: return LineSegment(CornerPoint(0), CornerPoint(2));
+            case 2: return LineSegment(CornerPoint(0), CornerPoint(4));
+            case 3: return LineSegment(CornerPoint(1), CornerPoint(3));
+            case 4: return LineSegment(CornerPoint(1), CornerPoint(5));
+            case 5: return LineSegment(CornerPoint(2), CornerPoint(3));
+            case 6: return LineSegment(CornerPoint(2), CornerPoint(6));
+            case 7: return LineSegment(CornerPoint(3), CornerPoint(7));
+            case 8: return LineSegment(CornerPoint(4), CornerPoint(5));
+            case 9: return LineSegment(CornerPoint(4), CornerPoint(6));
+            case 10: return LineSegment(CornerPoint(5), CornerPoint(7));
+            case 11: return LineSegment(CornerPoint(6), CornerPoint(7));
+        }
+    }
+
+    Vector3 OBB::CornerPoint(int cornerIndex) const
+    {
+        assert(0 <= cornerIndex && cornerIndex <= 7);
+        switch(cornerIndex)
+        {
+            default: // For release builds, return always the first option if out of bounds
+            case 0: return pos - r.x * axis[0] - r.y * axis[1] - r.z * axis[2];
+            case 1: return pos - r.x * axis[0] - r.y * axis[1] + r.z * axis[2];
+            case 2: return pos - r.x * axis[0] + r.y * axis[1] - r.z * axis[2];
+            case 3: return pos - r.x * axis[0] + r.y * axis[1] + r.z * axis[2];
+            case 4: return pos + r.x * axis[0] - r.y * axis[1] - r.z * axis[2];
+            case 5: return pos + r.x * axis[0] - r.y * axis[1] + r.z * axis[2];
+            case 6: return pos + r.x * axis[0] + r.y * axis[1] - r.z * axis[2];
+            case 7: return pos + r.x * axis[0] + r.y * axis[1] + r.z * axis[2];
+        }
+    }
+
+    Vector3 OBB::ExtremePoint(const Vector3& direction) const
+    {
+        Vector3 pt = pos;
+        pt += axis[0] * (Vector3::Dot(direction, axis[0]) >= 0.f ? r.x : -r.x);
+        pt += axis[1] * (Vector3::Dot(direction, axis[1]) >= 0.f ? r.y : -r.y);
+        pt += axis[2] * (Vector3::Dot(direction, axis[2]) >= 0.f ? r.z : -r.z);
+        return pt;
+    }
+
+    Vector3 OBB::ExtremePoint(const Vector3& direction, float &projectionDistance) const
+    {
+        Vector3 extremePoint = ExtremePoint(direction);
+        projectionDistance = extremePoint.Dot(direction);
+        return extremePoint;
+    }
+
+    void OBB::ProjectToAxis(const Vector3& direction, float& outMin, float& outMax) const
+    {
+        float x = std::abs(Vector3::Dot(direction, axis[0]) * r.x);
+        float y = std::abs(Vector3::Dot(direction, axis[1]) * r.y);
+        float z = std::abs(Vector3::Dot(direction, axis[2]) * r.z);
+        float pt = Vector3::Dot(direction, pos);
+        outMin = pt - x - y - z;
+        outMax = pt + x + y + z;
+    }
+
+    int OBB::UniqueFaceNormals(Vector3* out) const
+    {
+        out[0] = axis[0];
+        out[1] = axis[1];
+        out[2] = axis[2];
+        return 3;
+    }
+
+    int OBB::UniqueEdgeDirections(Vector3* out) const
+    {
+        out[0] = axis[0];
+        out[1] = axis[1];
+        out[2] = axis[2];
+        return 3;
+    }
+
+    Vector3 OBB::PointOnEdge(int edgeIndex, float u) const
+    {
+        assert(0 <= edgeIndex && edgeIndex <= 11);
+        assert(0 <= u && u <= 1.f);
+
+        edgeIndex = std::clamp(edgeIndex, 0, 11);
+        Vector3 d = axis[edgeIndex/4] * (2.f * u - 1.f) * r[edgeIndex/4];
+        switch(edgeIndex)
+        {
+            default:
+            case 0: return pos - r.y * axis[1] - r.z * axis[2] + d;
+            case 1: return pos - r.y * axis[1] + r.z * axis[2] + d;
+            case 2: return pos + r.y * axis[1] - r.z * axis[2] + d;
+            case 3: return pos + r.y * axis[1] + r.z * axis[2] + d;
+
+            case 4: return pos - r.x * axis[0] - r.z * axis[2] + d;
+            case 5: return pos - r.x * axis[0] + r.z * axis[2] + d;
+            case 6: return pos + r.x * axis[0] - r.z * axis[2] + d;
+            case 7: return pos + r.x * axis[0] + r.z * axis[2] + d;
+
+            case 8: return pos - r.x * axis[0] - r.y * axis[1] + d;
+            case 9: return pos - r.x * axis[0] + r.y * axis[1] + d;
+            case 10: return pos + r.x * axis[0] - r.y * axis[1] + d;
+            case 11: return pos + r.x * axis[0] + r.y * axis[1] + d;
+        }
+    }
+
+    Vector3 OBB::FaceCenterPoint(int faceIndex) const
+    {
+        assert(0 <= faceIndex && faceIndex <= 6);
+        switch(faceIndex)
+        {
+            default:
+            case 0: return pos - r.x * axis[0];
+            case 1: return pos + r.x * axis[0];
+            case 2: return pos - r.y * axis[1];
+            case 3: return pos + r.y * axis[1];
+            case 4: return pos - r.z * axis[2];
+            case 5: return pos + r.z * axis[2];
+        }
+    }
+
+    Vector3 OBB::FacePoint(int faceIndex, float u, float v) const
+    {
+        assert(0 <= faceIndex && faceIndex <= 5);
+        assert(0 <= u && u <= 1.f);
+        assert(0 <= v && v <= 1.f);
+
+        int uIdx = faceIndex/2;
+        int vIdx = (faceIndex/2 + 1) % 3;
+        Vector3 U = axis[uIdx] * (2.f * u - 1.f) * r[uIdx];
+        Vector3 V = axis[vIdx] * (2.f * v - 1.f) * r[vIdx];
+
+        switch(faceIndex)
+        {
+            default:
+            case 0: return pos - r.z * axis[2] + U + V;
+            case 1: return pos + r.z * axis[2] + U + V;
+            case 2: return pos - r.x * axis[0] + U + V;
+            case 3: return pos + r.x * axis[0] + U + V;
+            case 4: return pos - r.y * axis[1] + U + V;
+            case 5: return pos + r.y * axis[1] + U + V;
+        }
+    }
+
+    Plane OBB::FacePlane(int faceIndex) const
+    {
+        assert(0 <= faceIndex && faceIndex <= 5);
+        switch(faceIndex)
+        {
+            default:
+            case 0: return Plane(FaceCenterPoint(0), -axis[0]);
+            case 1: return Plane(FaceCenterPoint(1),  axis[0]);
+            case 2: return Plane(FaceCenterPoint(2), -axis[1]);
+            case 3: return Plane(FaceCenterPoint(3), axis[1]);
+            case 4: return Plane(FaceCenterPoint(4), -axis[2]);
+            case 5: return Plane(FaceCenterPoint(5), axis[2]);
+        }
+    }
+
+    void OBB::GetCornerPoints(Vector3 *outPointArray) const
+    {
+        assert(outPointArray);
+        for (int i = 0; i < 8; ++i)
+            outPointArray[i] = CornerPoint(i);
+    }
+
+    void OBB::GetFacePlanes(Plane *outPlaneArray) const
+    {
+        assert(outPlaneArray);
+        for (int i = 0; i < 6; ++i)
+            outPlaneArray[i] = FacePlane(i);
+    }
+
+    void OBB::ExtremePointsAlongDirection(const Vector3& dir, const Vector3* pointArray, int numPoints, int &idxSmallest, int &idxLargest, float &smallestD, float &largestD)
+    {
+        assert(pointArray || numPoints == 0);
+
+        idxSmallest = idxLargest = 0;
+
+        smallestD = INFINITY;
+        largestD = -INFINITY;
+
+        for (int i = 0; i < numPoints; ++i)
+        {
+            float d = Vector3::Dot(pointArray[i], dir);
+            if (d < smallestD)
+            {
+                smallestD = d;
+                idxSmallest = i;
+            }
+            if (d > largestD)
+            {
+                largestD = d;
+                idxLargest = i;
+            }
+        }
+    }
+
+    Vector3 OBB::Size() const {
+        return r * 2.f;
+    }
+
+    Vector3 OBB::HalfSize() const {
+        return r;
+    }
+
+    Vector3 OBB::Diagonal() const {
+        return 2.f * HalfDiagonal();
+    }
+
+    Vector3 OBB::HalfDiagonal() const {
+        return axis[0] * r[0] + axis[1] * r[1] + axis[2] * r[2];
+    }
+
+    float OBB::Volume() const {
+        Vector3 size = Size();
+        return size.x*size.y*size.z;
+    }
+
+    float OBB::SurfaceArea() const {
+        const Vector3 size = Size();
+        return 2.f * (size.x*size.y + size.x*size.z + size.y*size.z);
+    }
+
+    template <typename Matrix>
+    void OBBSetFrom(OBB &obb, const AABB& aabb, const Matrix& m)
+    {
+        assert(m.IsColOrthogonal()); // We cannot convert transform an AABB to OBB if it gets sheared in the process.
+        assert(m.HasUniformScale()); // Nonuniform scale will produce shear as well
+        obb.pos = m.Mul(aabb.Centroid());
+        obb.r = aabb.HalfSize();
+        obb.axis[0] = Vector3(m.GetColumn3(0));
+        obb.axis[1] = Vector3(m.GetColumn3(1));
+        obb.axis[2] = Vector3(m.GetColumn3(2));
+        // If te matrix m contains scaling, propagate the scaling from the axis vectors to the half-length vectors,
+        // since we want to keep the axis vectors always normalized in our representations.
+        float matrixScale = obb.axis[0].LengthSquared();
+        matrixScale = std::sqrt(matrixScale);
+        obb.r *= matrixScale;
+        matrixScale = 1.f / matrixScale;
+        obb.axis[0] *= matrixScale;
+        obb.axis[1] *= matrixScale;
+        obb.axis[2] *= matrixScale;
+
+        Vector3::Orthonormalize(obb.axis[0], obb.axis[1], obb.axis[2]);
+
+    }
+
+    template <typename Matrix>
+    void OBBTransform(OBB& o, const Matrix& transform)
+    {
+        o.pos = transform.Mul(o.pos);
+        o.axis[0] = transform.Mul(o.r.x * o.axis[0]);
+        o.axis[1] = transform.Mul(o.r.y * o.axis[1]);
+        o.axis[2] = transform.Mul(o.r.z * o.axis[2]);
+        o.r.x = o.axis[0].Normalize().x;
+        o.r.y = o.axis[1].Normalize().y;
+        o.r.z = o.axis[2].Normalize().z;
+    }
+
+    void OBB::SetFrom(const AABB& aabb, const Matrix3x3 &transform) {
+        assert(transform.IsColOrthogonal());
+
+        OBBSetFrom(*this, aabb, transform);
+    }
+
+    void OBB::SetFrom(const AABB& aabb, const Matrix4x4 &transform) {
+        assert(transform.IsColOrthogonal3());
+
+        OBBSetFrom(*this, aabb, transform);
+    }
+
+    void OBB::SetFrom(const AABB& aabb, const Quaternion &transform) {
+
+        OBBSetFrom(*this, aabb, Matrix3x3(transform));
+    }
+
+    void OBB::Transform(const Matrix3x3 &transform) {
+        assert(transform.IsColOrthogonal());
+        OBBTransform(*this, transform);
+    }
+
+    void OBB::Transform(const Matrix4x4 &transform) {
+        assert(transform.IsColOrthogonal3());
+        OBBTransform(*this, transform);
+    }
+
+    void OBB::Transform(const Quaternion &transform) {
+        OBBTransform(*this, transform.ToMatrix3x3());
+    }
+
+    /// The implementation of OBB-Plane intersection test follows Christer Ericson's Real-Time Collision Detection, p. 163.
+    bool OBB::Intersects(const Plane& plane) const {
+        // Compute the projection interval radius of this OBB onto L(t) = this->pos + x * p.normal;
+        float t = r[0] * std::abs(Vector3::Dot(plane.Normal, axis[0])) +
+                  r[1] * std::abs(Vector3::Dot(plane.Normal, axis[1])) +
+                  r[2] * std::abs(Vector3::Dot(plane.Normal, axis[2]));
+        // Compute the distance of this OBB center from the plane.
+        float s = Vector3::Dot(plane.Normal, pos) - plane.distance;
+        return std::abs(s) <= t;
+    }
+
+
+
+    bool OBB::Intersects(const LineSegment &lineSegment) const {
+        AABB aabb(Vector3(0.f), Size());
+        LineSegment l = WorldToLocal() * lineSegment;
+        return aabb.Intersects(l);
+    }
+
+    Matrix4x4 OBB::WorldToLocal() const
+    {
+        Matrix4x4 m = LocalToWorld();
+        m.InverseOrthonormal();
+        return m;
+    }
+
+    Matrix4x4 OBB::LocalToWorld() const
+    {
+        // To produce a normalized local->world matrix, do the following.
+        /*
+        float3x4 m;
+        vec x = axis[0] * r.x;
+        vec y = axis[1] * r.y;
+        vec z = axis[2] * r.z;
+        m.SetCol(0, 2.f * x);
+        m.SetCol(1, 2.f * y);
+        m.SetCol(2, 2.f * z);
+        m.SetCol(3, pos - x - y - z);
+        return m;
+        */
+
+        assert(axis[0].IsNormalized());
+        assert(axis[1].IsNormalized());
+        assert(axis[2].IsNormalized());
+        Matrix4x4 m; ///\todo sse-matrix
+        m.SetCol(0, axis[0].ptr());
+        m.SetCol(1, axis[1].ptr());
+        m.SetCol(2, axis[2].ptr());
+        Vector3 p = pos - axis[0] * r.x - axis[1] * r.y - axis[2] * r.z;
+        m.SetCol(3, p.ptr());
+        assert(m.Row3(0).IsNormalized());
+        assert(m.Row3(1).IsNormalized());
+        assert(m.Row3(2).IsNormalized());
+        assert(m.Col(0).IsPerpendicular(m.Col(1)));
+        assert(m.Col(0).IsPerpendicular(m.Col(2)));
+        assert(m.Col(1).IsPerpendicular(m.Col(2)));
+        return m;
+    }
+
+
+}
+

src/J3ML/Geometry/Plane.cpp

@@ -0,0 +1,266 @@
+#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>
+
+namespace J3ML::Geometry
+{
+
+    bool Plane::IntersectLinePlane(const Vector3 &planeNormal, float planeD, const Vector3 &linePos, const Vector3 &lineDir, float &t)
+    {
+        /* The set of points x lying on a plane is defined by the equation
+
+            <planeNormal, x> = planeD.
+
+        The set of points x on a line is constructed explicitly by a single parameter t by
+
+            x = linePos + t*lineDir.
+
+        To solve the intersection of these two objects, substitute the second equation to the first above,
+        and we get
+
+                             <planeNormal, linePos + t*lineDir> == planeD, or
+            <planeNormal, linePos> + t * <planeNormal, lineDir> == planeD, or
+                                                              t == (planeD - <planeNormal, linePos>) / <planeNormal, lineDir>,
+
+                                                                   assuming that <planeNormal, lineDir> != 0.
+
+        If <planeNormal, lineDir> == 0, then the line is parallel to the plane, and either no intersection occurs, or the whole line
+        is embedded on the plane, and infinitely many intersections occur. */
+
+        float denom = Vector3::Dot(planeNormal, lineDir);
+        if (std::abs(denom) > 1e-4f)
+        {
+            // Compute the distance from the line starting point to the point of intersection.
+            t = (planeD - Vector3::Dot(planeNormal, linePos)) / denom;
+            return true;
+        }
+
+        if (denom != 0.f)
+        {
+            t = (planeD - Vector3::Dot(planeNormal, linePos)) / denom;
+            if (std::abs(t) < 1e4f)
+                return true;
+        }
+        t = 0.f;
+        return Math::EqualAbs(Vector3::Dot(planeNormal, linePos), planeD, 1e-3f);
+    }
+
+    template<typename T>
+    float Plane_SignedDistance(const Plane &plane, const T &object)
+    {
+        float pMin, pMax;
+        assert(plane.Normal.IsNormalized());
+        object.ProjectToAxis(plane.Normal, pMin, pMax);
+        pMin -= plane.distance;
+        pMax -= plane.distance;
+        if (pMin * pMax <= 0.f)
+            return 0.f;
+        return std::abs(pMin) < std::abs(pMax) ? pMin : pMax;
+    }
+
+    float Plane::SignedDistance(const Vector3 &point) const {
+        assert(Normal.IsNormalized());
+        return Normal.Dot(point) - distance;
+    }
+
+    float Plane::SignedDistance(const AABB &aabb) const { return Plane_SignedDistance(*this, aabb); }
+
+    float Plane::SignedDistance(const OBB &obb) const { return Plane_SignedDistance(*this, obb); }
+
+    float Plane::SignedDistance(const Capsule &capsule) const { return Plane_SignedDistance(*this, capsule); }
+
+    float Plane::SignedDistance(const Frustum &frustum) const { return Plane_SignedDistance(*this, frustum); }
+
+    float Plane::SignedDistance(const LineSegment &lineSegment) const { return Plane_SignedDistance(*this, lineSegment); }
+
+    float Plane::SignedDistance(const Ray &ray) const { return Plane_SignedDistance(*this, ray); }
+
+    float Plane::SignedDistance(const Polygon &polygon) const { return Plane_SignedDistance(*this, polygon); }
+
+    float Plane::SignedDistance(const Polyhedron &polyhedron) const { return Plane_SignedDistance(*this, polyhedron); }
+
+    float Plane::SignedDistance(const Sphere &sphere) const { return Plane_SignedDistance(*this, sphere); }
+
+    float Plane::SignedDistance(const Triangle &triangle) const { return Plane_SignedDistance(*this, triangle); }
+
+    float Plane::Distance(const Vector3 &point) const {
+        std::abs(SignedDistance(point));
+    }
+
+    float Plane::Distance(const LineSegment &lineSegment) const
+    {
+        return lineSegment.Distance(*this);
+    }
+
+    float Plane::Distance(const Sphere &sphere) const
+    {
+        return std::max(0.f, Distance(sphere.Position) - sphere.Radius);
+    }
+
+    float Plane::Distance(const Capsule &capsule) const
+    {
+        return std::max(0.f, Distance(capsule.l) - capsule.r);
+    }
+
+    Plane::Plane(const Line &line, const Vector3 &normal) {
+        Vector3 perpNormal = normal - normal.ProjectToNorm(line.Direction);
+        Set(line.Position, perpNormal.Normalize());
+    }
+
+    void Plane::Set(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3) {
+        Normal = (v2-v1).Cross(v3-v1);
+        float len = Normal.Length();
+        assert(len > 1e-10f);
+        Normal /= len;
+        assert(Normal.IsNormalized());
+        distance = Normal.Dot(v1);
+    }
+
+    void Plane::Set(const Vector3 &point, const Vector3 &normal_) {
+        Normal = normal_;
+        assert(Normal.IsNormalized());
+        distance = point.Dot(Normal);
+
+#ifdef MATH_ASSERT_CORRECTNESS
+        assert1(EqualAbs(SignedDistance(point), 0.f, 0.01f), SignedDistance(point));
+	assert1(EqualAbs(SignedDistance(point + normal_), 1.f, 0.01f), SignedDistance(point + normal_));
+#endif
+    }
+
+    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) {}
+
+    bool Plane::Intersects(J3ML::Geometry::Ray ray, float *dist) const {
+        float t;
+        bool success = IntersectLinePlane(Normal, this->distance, ray.Origin, ray.Direction, t);
+        if (dist)
+            *dist = t;
+        return success && t >= 0.f;
+    }
+
+    bool Plane::Intersects(const Line &line, float *dist) const
+    {
+        float t;
+        bool intersects = IntersectLinePlane(Normal, this->distance, line.Position, line.Direction, t);
+        if (dist)
+            *dist = t;
+        return intersects;
+    }
+
+    bool Plane::Intersects(const LineSegment &lineSegment, float *dist) const
+    {
+        float t;
+        bool success = IntersectLinePlane(Normal, this->distance, lineSegment.A, lineSegment.Dir(), t);
+        const float lineSegmentLength = lineSegment.Length();
+        if (dist)
+            *dist = t / lineSegmentLength;
+        return success && t >= 0.f && t <= lineSegmentLength;
+    }
+
+    bool Plane::Intersects(const Sphere &sphere) const
+    {
+        return Distance(sphere.Position) <= sphere.Radius;
+    }
+
+    bool Plane::Intersects(const Capsule &capsule) const
+    {
+        return capsule.Intersects(*this);
+    }
+
+    /// The Plane-AABB intersection is implemented according to Christer Ericson's Real-Time Collision Detection, p.164. [groupSyntax]
+    bool Plane::Intersects(const AABB &aabb) const
+    {
+        Vector3 c = aabb.Centroid();
+        Vector3 e = aabb.HalfDiagonal();
+
+        // Compute the projection interval radius of the AABB onto L(t) = aabb.center + t * plane.normal;
+        float r = e[0]*std::abs(Normal[0]) + e[1]*std::abs(Normal[1]) + e[2]*std::abs(Normal[2]);
+        // Compute the distance of the box center from plane.
+        //float s = Dot(normal, c) - d;
+        float s = Vector3::Dot(Normal, c) - distance; ///\todo Use the above form when Plane is SSE'ized.
+        return std::abs(s) <= r;
+    }
+
+    bool Plane::Intersects(const OBB &obb) const
+    {
+        return obb.Intersects(*this);
+    }
+
+    bool Plane::Intersects(const Triangle &triangle) const
+    {
+        float a = SignedDistance(triangle.V0);
+        float b = SignedDistance(triangle.V1);
+        float c = SignedDistance(triangle.V2);
+        return (a*b <= 0.f || a*c <= 0.f);
+    }
+
+    bool Plane::Intersects(const Frustum &frustum) const
+    {
+        bool sign = IsOnPositiveSide(frustum.CornerPoint(0));
+        for(int i = 1; i < 8; ++i)
+            if (sign != IsOnPositiveSide(frustum.CornerPoint(i)))
+                return true;
+        return false;
+    }
+
+    bool Plane::IsOnPositiveSide(const Vector3 &point) const {
+        return SignedDistance(point) >= 0.f;
+    }
+
+    bool Plane::Intersects(const Polyhedron &polyhedron) const
+    {
+        if (polyhedron.NumVertices() == 0)
+            return false;
+        bool sign = IsOnPositiveSide(polyhedron.Vertex(0));
+        for(int i = 1; i < polyhedron.NumVertices(); ++i)
+            if (sign != IsOnPositiveSide(polyhedron.Vertex(i)))
+                return true;
+        return false;
+    }
+
+    Vector3 Plane::Project(const Vector3 &point) const
+    {
+        Vector3 projected = point - (Normal.Dot(point) - distance) * Normal;
+        return projected;
+    }
+
+    LineSegment Plane::Project(const LineSegment &lineSegment) {
+        return LineSegment(Project(lineSegment.A), Project(lineSegment.B));
+    }
+
+    /*int Plane::Intersects(const Circle &circle, Vector3 *pt1, Vector3 *pt2) const
+    {
+        Line line;
+        bool planeIntersects = Intersects(circle.ContainingPlane(), &line);
+        if (!planeIntersects)
+            return false;
+
+        // Offset both line and circle position so the circle origin is at center.
+        line.pos -= circle.pos;
+
+        float a = 1.f;
+        float b = 2.f * Dot(line.pos, line.dir);
+        float c = line.pos.LengthSq() - circle.r * circle.r;
+        float r1, r2;
+        int numRoots = Polynomial::SolveQuadratic(a, b, c, r1, r2);
+        if (numRoots >= 1 && pt1)
+            *pt1 = circle.pos + line.GetPoint(r1);
+        if (numRoots >= 2 && pt2)
+            *pt2 = circle.pos + line.GetPoint(r2);
+        return numRoots;
+    }
+
+    int Plane::Intersects(const Circle &circle) const
+    {
+        return Intersects(circle, 0, 0);
+    }*/
+
+}
+

src/J3ML/Geometry/Polygon.cpp

@@ -0,0 +1,603 @@
+#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>
+
+namespace J3ML::Geometry {
+    AABB Polygon::MinimalEnclosingAABB() const {
+        AABB aabb;
+        aabb.SetNegativeInfinity();
+        for(int i = 0; i < NumVertices(); ++i)
+            aabb.Enclose(Vertex(i));
+        return aabb;
+    }
+
+    Vector3 Polygon::ExtremePoint(const Vector3 &direction, float &projectionDistance) const
+    {
+        Vector3 mostExtreme = Vector3::NaN;
+        projectionDistance = -INFINITY;
+        for(int i = 0; i < NumVertices(); ++i)
+        {
+            Vector3 pt = Vertex(i);
+            float d = Vector3::Dot(direction, pt);
+            if (d > projectionDistance)
+            {
+                projectionDistance = d;
+                mostExtreme = pt;
+            }
+        }
+        return mostExtreme;
+    }
+
+    Vector3 Polygon::ExtremePoint(const Vector3 &direction) const
+    {
+        float projectionDistance;
+        return ExtremePoint(direction, projectionDistance);
+    }
+
+    void Polygon::ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const
+    {
+        ///\todo Optimize!
+        Vector3 minPt = ExtremePoint(-direction);
+        Vector3 maxPt = ExtremePoint(direction);
+        outMin = Vector3::Dot(minPt, direction);
+        outMax = Vector3::Dot(maxPt, direction);
+    }
+
+    Vector3 Polygon::Vertex(int vertexIndex) const {
+        assert(vertexIndex >= 0);
+        assert(vertexIndex < (int) vertices.size());
+        return vertices[vertexIndex];
+    }
+
+    int Polygon::NumVertices() const {
+        return (int)vertices.size();
+    }
+
+    bool Polygon::IsPlanar(float epsilonSq) const
+    {
+        if (vertices.empty())
+            return false;
+        if (vertices.size() <= 3)
+            return true;
+        Vector3 normal = (Vector3(vertices[1])-Vector3(vertices[0])).Cross(Vector3(vertices[2])-Vector3(vertices[0]));
+        float lenSq = normal.LengthSquared();
+        for(size_t i = 3; i < vertices.size(); ++i)
+        {
+            float d = normal.Dot(Vector3(vertices[i])-Vector3(vertices[0]));
+            if (d*d > epsilonSq * lenSq)
+                return false;
+        }
+        return true;
+    }
+
+    Vector3 Polygon::BasisU() const
+    {
+        if (vertices.size() < 2)
+            return Vector3::Right;
+        Vector3 u = (Vector3)vertices[1] - (Vector3)vertices[0];
+        u = u.Normalize(); // Always succeeds, even if u was zero (generates (1,0,0)).
+        return u;
+    }
+
+    Vector3 Polygon::BasisV() const
+    {
+        if (vertices.size() < 2)
+            return Vector3::Down;
+        return Vector3::Cross(PlaneCCW().Normal, BasisU()).Normalize();
+    }
+
+    Plane Polygon::PlaneCCW() const
+    {
+        if (vertices.size() > 3)
+        {
+            Plane plane;
+            for(size_t i = 0; i < vertices.size()-2; ++i)
+                for(size_t j = i+1; j < vertices.size()-1; ++j)
+                {
+                    Vector3 pij = Vector3(vertices[j])-Vector3(vertices[i]);
+                    for(size_t k = j+1; k < vertices.size(); ++k)
+                    {
+                        plane.Normal = pij.Cross(Vector3(vertices[k])-Vector3(vertices[i]));
+                        float lenSq = plane.Normal.LengthSquared();
+                        if (lenSq > 1e-8f)
+                        {
+                            plane.Normal /= std::sqrt(lenSq);
+                            plane.distance = plane.Normal.Dot(Vector3(vertices[i]));
+                            return plane;
+                        }
+                    }
+                }
+
+
+            // warn("Polygon contains %d points, but they are all collinear! Cannot form a plane for the Polygon using three points!", (int)p.size());
+
+            // Polygon contains multiple points, but they are all collinear.
+            // Pick an arbitrary plane along the line as the polygon plane (as if the polygon had only two points)
+            Vector3 dir = (Vector3(vertices[1])-Vector3(vertices[0])).Normalize();
+            return Plane(Line(vertices[0], dir), dir.Perpendicular());
+        }
+        if (vertices.size() == 3)
+            return Plane(vertices[0], vertices[1], vertices[2]);
+        if (vertices.size() == 2)
+        {
+            Vector3 dir = (Vector3(vertices[1])-Vector3(vertices[0])).Normalize();
+            return Plane(Line(vertices[0], dir), dir.Perpendicular());
+        }
+        if (vertices.size() == 1)
+            return Plane(vertices[0], Vector3(0,1,0));
+        return Plane();
+    }
+
+    Vector2 Polygon::MapTo2D(int i) const
+    {
+        assert(i >= 0);
+        assert(i < (int)vertices.size());
+        return MapTo2D(vertices[i]);
+    }
+
+    Vector2 Polygon::MapTo2D(const Vector3 &point) const
+    {
+        assert(!vertices.empty());
+        Vector3 basisU = BasisU();
+        Vector3 basisV = BasisV();
+        Vector3 pt = point - vertices[0];
+        return Vector2(Vector3::Dot(pt, basisU), Vector3::Dot(pt, basisV));
+    }
+
+    Vector3 Polygon::MapFrom2D(const Vector2 &point) const
+    {
+        assert(!vertices.empty());
+        return (Vector3)vertices[0] + point.x * BasisU() + point.y * BasisV();
+    }
+
+    // A(u) = a1 + u * (a2-a1).
+    // B(v) = b1 + v * (b2-b1).
+    // Returns (u,v).
+    bool IntersectLineLine2D(const Vector2 &a1, const Vector2 &a2, const Vector2 &b1, const Vector2 &b2, Vector2 &out)
+    {
+        float u = (b2.x - b1.x)*(a1.y - b1.y) - (b2.y - b1.y)*(a1.x - b1.x);
+        float v = (a2.x - a1.x)*(a1.y - b1.y) - (a2.y - a1.y)*(a1.x - b1.x);
+
+        float det = (b2.y - b1.y)*(a2.x - a1.x) - (b2.x - b1.x)*(a2.y - a1.y);
+        if (std::abs(det) < 1e-4f)
+            return false;
+        det = 1.f / det;
+        out.x = u * det;
+        out.y = v * det;
+
+        return true;
+    }
+
+    bool IntersectLineSegmentLineSegment2D(const Vector2 &a1, const Vector2 &a2, const Vector2 &b1, const Vector2 &b2, Vector2 &out)
+    {
+        bool ret = IntersectLineLine2D(a1, a2, b1, b2, out);
+        return ret && out.x >= 0.f && out.x <= 1.f && out.y >= 0.f && out.y <= 1.f;
+    }
+
+
+    /// Returns true if poly[i+1] is an ear.
+/// Precondition: i+2 == j (mod poly.size()).
+    bool IsAnEar(const std::vector<Vector2> &poly, int i, int j)
+    {
+        Vector2 dummy;
+        int x = (int)poly.size()-1;
+        for(int y = 0; y < i; ++y)
+        {
+            if (IntersectLineSegmentLineSegment2D(poly[i], poly[j], poly[x], poly[y], dummy))
+                return false;
+            x = y;
+        }
+        x = j+1;
+        for(int y = x+1; y < (int)poly.size(); ++y)
+        {
+            if (IntersectLineSegmentLineSegment2D(poly[i], poly[j], poly[x], poly[y], dummy))
+                return false;
+            x = y;
+        }
+        return true;
+    }
+
+    bool Polygon::Contains(const Polygon &worldSpacePolygon, float polygonThickness) const
+    {
+        for(int i = 0; i < worldSpacePolygon.NumVertices(); ++i)
+            if (!Contains(worldSpacePolygon.Vertex(i), polygonThickness))
+                return false;
+        return true;
+    }
+
+    bool Polygon::Contains(const Vector3 &worldSpacePoint, float polygonThicknessSq) const
+    {
+        // Implementation based on the description from http://erich.realtimerendering.com/ptinpoly/
+
+        if (vertices.size() < 3)
+            return false;
+
+        Vector3 basisU = BasisU();
+        Vector3 basisV = BasisV();
+        assert(basisU.IsNormalized());
+        assert(basisV.IsNormalized());
+        assert(basisU.IsPerpendicular(basisV));
+        assert(basisU.IsPerpendicular(PlaneCCW().Normal));
+        assert(basisV.IsPerpendicular(PlaneCCW().Normal));
+
+        Vector3 normal = basisU.Cross(basisV);
+//	float lenSq = normal.LengthSq(); ///\todo Could we treat basisU and basisV unnormalized here?
+        float dot = normal.Dot(Vector3(vertices[0]) - worldSpacePoint);
+        if (dot*dot > polygonThicknessSq)
+            return false; // The point is not even within the plane of the polygon - can't be contained.
+
+        int numIntersections = 0;
+
+        const float epsilon = 1e-4f;
+
+        // General strategy: transform all points on the polygon onto 2D face plane of the polygon, where the target query point is
+        // centered to lie in the origin.
+        // If the test ray (0,0) -> (+inf, 0) intersects exactly an odd number of polygon edge segments, then the query point must have been
+        // inside the polygon. The test ray is chosen like that to avoid all extra per-edge computations.
+        // This method works for both simple and non-simple (self-intersecting) polygons.
+        Vector3 vt = Vector3(vertices.back()) - worldSpacePoint;
+        Vector2 p0 = Vector2(Vector3::Dot(vt, basisU), Vector3::Dot(vt, basisV));
+        if (std::abs(p0.y) < epsilon)
+            p0.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly.
+
+        for(int i = 0; i < (int)vertices.size(); ++i)
+        {
+            vt = Vector3(vertices[i]) - worldSpacePoint;
+            Vector2 p1 = Vector2(Vector3::Dot(vt, basisU), Vector3::Dot(vt, basisV));
+            if (std::abs(p1.y) < epsilon)
+                p1.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly.
+
+            if (p0.y * p1.y < 0.f) // If the line segment p0 -> p1 straddles the line x=0, it could intersect the ray (0,0) -> (+inf, 0)
+            {
+                if (std::min(p0.x, p1.x) > 0.f) // If both x-coordinates are positive, then there certainly is an intersection with the ray.
+                    ++numIntersections;
+                else if (std::max(p0.x, p1.x) > 0.f) // If one of them is positive, there could be an intersection. (otherwise both are negative and they can't intersect ray)
+                {
+                    // P = p0 + t*(p1-p0) == (x,0)
+                    //     p0.x + t*(p1.x-p0.x) == x
+                    //     p0.y + t*(p1.y-p0.y) == 0
+                    //                 t == -p0.y / (p1.y - p0.y)
+
+                    // Test whether the lines (0,0) -> (+inf,0) and p0 -> p1 intersect at a positive X-coordinate?
+                    Vector2 d = p1 - p0;
+                    if (d.y != 0.f)
+                    {
+                        float t = -p0.y / d.y; // The line segment parameter, t \in [0,1] forms the line segment p0->p1.
+                        float x = p0.x + t * d.x; // The x-coordinate of intersection with the ray.
+                        if (t >= 0.f && t <= 1.f && x > 0.f)
+                            ++numIntersections;
+                    }
+                }
+            }
+            p0 = p1;
+        }
+
+        return numIntersections % 2 == 1;
+    }
+
+    bool Polygon::Contains(const LineSegment &worldSpaceLineSegment, float polygonThickness) const
+    {
+        if (vertices.size() < 3)
+            return false;
+
+        Plane plane = PlaneCCW();
+        if (plane.Distance(worldSpaceLineSegment.A) > polygonThickness ||
+            plane.Distance(worldSpaceLineSegment.B) > polygonThickness)
+            return false;
+
+        // For robustness, project onto the polygon plane.
+        LineSegment l = plane.Project(worldSpaceLineSegment);
+
+        if (!Contains(l.A) || !Contains(l.B))
+            return false;
+
+        for(int i = 0; i < (int)vertices.size(); ++i)
+            if (plane.Project(Edge(i)).Intersects(l))
+                return false;
+
+        return true;
+    }
+
+    bool Polygon::Contains(const Triangle &worldSpaceTriangle, float polygonThickness) const
+    {
+        return Contains(worldSpaceTriangle.Edge(0), polygonThickness) &&
+               Contains(worldSpaceTriangle.Edge(1), polygonThickness) &&
+               Contains(worldSpaceTriangle.Edge(2), polygonThickness);
+    }
+
+    /** The implementation of this function is based on the paper
+	"Kong, Everett, Toussant. The Graham Scan Triangulates Simple Polygons."
+	See also p. 772-775 of Geometric Tools for Computer Graphics.
+	The running time of this function is O(n^2). */
+    std::vector<Triangle> Polygon::Triangulate() const
+    {
+        assert(IsPlanar());
+//	assume1(IsPlanar(), this->SerializeToString()); // TODO: enable
+
+        std::vector<Triangle> t;
+        // Handle degenerate cases.
+        if (NumVertices() < 3)
+            return t;
+        if (NumVertices() == 3)
+        {
+            t.push_back(Triangle(Vertex(0), Vertex(1), Vertex(2)));
+            return t;
+        }
+        std::vector<Vector2> p2d;
+        std::vector<int> polyIndices;
+        for(int v = 0; v < NumVertices(); ++v)
+        {
+            p2d.push_back(MapTo2D(v));
+            polyIndices.push_back(v);
+        }
+
+        // Clip ears of the polygon until it has been reduced to a triangle.
+        int i = 0;
+        int j = 1;
+        int k = 2;
+        size_t numTries = 0; // Avoid creating an infinite loop.
+        while(p2d.size() > 3 && numTries < p2d.size())
+        {
+            if (Vector2::OrientedCCW(p2d[i], p2d[j], p2d[k]) && IsAnEar(p2d, i, k))
+            {
+                // The vertex j is an ear. Clip it off.
+                t.push_back(Triangle(vertices[polyIndices[i]], vertices[polyIndices[j]], vertices[polyIndices[k]]));
+                p2d.erase(p2d.begin() + j);
+                polyIndices.erase(polyIndices.begin() + j);
+
+                // The previous index might now have become an ear. Move back one index to see if so.
+                if (i > 0)
+                {
+                    i = (i + (int)p2d.size() - 1) % p2d.size();
+                    j = (j + (int)p2d.size() - 1) % p2d.size();
+                    k = (k + (int)p2d.size() - 1) % p2d.size();
+                }
+                numTries = 0;
+            }
+            else
+            {
+                // The vertex at j is not an ear. Move to test next vertex.
+                i = j;
+                j = k;
+                k = (k+1) % p2d.size();
+                ++numTries;
+            }
+        }
+
+        assert(p2d.size() == 3);
+        if (p2d.size() > 3) // If this occurs, then the polygon is NOT counter-clockwise oriented.
+            return t;
+/*
+	{
+		// For conveniency, create a copy that has the winding order fixed, and triangulate that instead.
+		// (Causes a large performance hit!)
+		Polygon p2 = *this;
+		for(size_t i = 0; i < p2.p.size()/2; ++i)
+			std::swap(p2.p[i], p2.p[p2.p.size()-1-i]);
+		return p2.Triangulate();
+	}
+*/
+        // Add the last poly.
+        t.push_back(Triangle(vertices[polyIndices[0]], vertices[polyIndices[1]], vertices[polyIndices[2]]));
+
+        return t;
+    }
+
+    bool Polygon::Intersects(const Capsule &capsule) const {
+        ///@todo Optimize.
+        std::vector<Triangle> tris = Triangulate();
+        for(size_t i = 0; i < tris.size(); ++i)
+            if (tris[i].Intersects(capsule))
+                return true;
+
+        return false;
+    }
+
+    bool Polygon::Intersects(const Line &line) const {
+        float d;
+        if (!PlaneCCW().Intersects(line, &d))
+            return false;
+        return Contains(line.GetPoint(d));
+    }
+    bool Polygon::Intersects(const Ray &ray) const
+    {
+        float d;
+        if (!PlaneCCW().Intersects(ray, &d))
+            return false;
+        return Contains(ray.GetPoint(d));
+    }
+
+    bool Polygon::Intersects2D(const LineSegment &localSpaceLineSegment) const
+    {
+        if (vertices.size() < 3)
+            return false;
+
+        const Vector3 basisU = BasisU();
+        const Vector3 basisV = BasisV();
+        const Vector3 origin = vertices[0];
+
+        LineSegment edge;
+        edge.A = Vector3(Vector3::Dot(vertices.back(), basisU), Vector3::Dot(vertices.back(), basisV), 0); // map to 2D
+        for (int i = 0; i < (int)vertices.size(); ++i)
+        {
+            edge.B = Vector3(Vector3::Dot(vertices[i], basisU), Vector3::Dot(vertices[i], basisV), 0); // map to 2D
+            if (edge.Intersects(localSpaceLineSegment))
+                return true;
+            edge.A = edge.B;
+        }
+
+        // The line segment did not intersect with any of the polygon edges, so either the whole line segment is inside
+        // the polygon, or it is fully outside the polygon. Test one point of the line segment to determine which.
+        Vector2 xy = {
+                localSpaceLineSegment.A.x,
+                localSpaceLineSegment.A.y
+        };
+        return Contains(MapFrom2D(xy));
+    }
+
+    bool Polygon::Intersects(const LineSegment &lineSegment) const
+    {
+        Plane plane = PlaneCCW();
+
+        // Compute line-plane intersection (unroll Plane::IntersectLinePlane())
+        float denom = Vector3::Dot(plane.Normal, lineSegment.B - lineSegment.A);
+
+        if (std::abs(denom) < 1e-4f) // The plane of the polygon and the line are planar? Do the test in 2D.
+            return Intersects2D(LineSegment(Vector3(MapTo2D(lineSegment.A), 0), Vector3(MapTo2D(lineSegment.B), 0)));
+
+        // The line segment properly intersects the plane of the polygon, so there is exactly one
+        // point of intersection between the plane of the polygon and the line segment. Test that intersection point against
+        // the line segment end points.
+        float t = (plane.distance - Vector3::Dot(plane.Normal, lineSegment.A)) / denom;
+        if (t < 0.f || t > 1.f)
+            return false;
+
+        return Contains(lineSegment.GetPoint(t));
+    }
+    bool Polygon::Intersects(const Plane &plane) const
+    {
+        // Project the points of this polygon onto the 1D axis of the plane normal.
+        // If there are points on both sides of the plane, then the polygon intersects the plane.
+        float minD = INFINITY;
+        float maxD = -INFINITY;
+        for(size_t i = 0; i < vertices.size(); ++i)
+        {
+            float d = plane.SignedDistance(vertices[i]);
+            minD = std::min(minD, d);
+            maxD = std::max(maxD, d);
+        }
+        // Allow a very small epsilon tolerance.
+        return minD <= 1e-4f && maxD >= -1e-4f;
+    }
+    bool Polygon::ConvexIntersects(const AABB &aabb) const
+    {
+        return Algorithms::GJKIntersect(*this, aabb);
+    }
+
+    bool Polygon::ConvexIntersects(const OBB &obb) const
+    {
+        return Algorithms::GJKIntersect(*this, obb);
+    }
+
+    bool Polygon::ConvexIntersects(const Frustum &frustum) const
+    {
+        return Algorithms::GJKIntersect(*this, frustum);
+    }
+
+    template<typename Convex /* = AABB, OBB, Frustum. */>
+    bool Convex_Intersects_Polygon(const Convex &c, const Polygon &p)
+    {
+        LineSegment l;
+        l.A = p.vertices.back();
+        for(size_t i = 0; i < p.vertices.size(); ++i)
+        {
+            l.B = p.vertices[i];
+            if (c.Intersects(l))
+                return true;
+            l.A = l.B;
+        }
+
+        // Check all the edges of the convex shape against the polygon.
+        for(int i = 0; i < c.NumEdges(); ++i)
+        {
+            l = c.Edge(i);
+            if (p.Intersects(l))
+                return true;
+        }
+
+        return false;
+    }
+
+    bool Polygon::Intersects(const AABB &aabb) const
+    {
+        // Because GJK test is so fast, use that as an early-out. (computes intersection between the convex hull of this poly, and the aabb)
+        bool convexIntersects = ConvexIntersects(aabb);
+        if (!convexIntersects)
+            return false;
+
+        return Convex_Intersects_Polygon(aabb, *this);
+    }
+
+    bool Polygon::Intersects(const OBB &obb) const
+    {
+        // Because GJK test is so fast, use that as an early-out. (computes intersection between the convex hull of this poly, and the obb)
+        bool convexIntersects = ConvexIntersects(obb);
+        if (!convexIntersects)
+            return false;
+
+        return Convex_Intersects_Polygon(obb, *this);
+    }
+
+    bool Polygon::Intersects(const Frustum &frustum) const
+    {
+        // Because GJK test is so fast, use that as an early-out. (computes intersection between the convex hull of this poly, and the frustum)
+        bool convexIntersects = ConvexIntersects(frustum);
+        if (!convexIntersects)
+            return false;
+
+        return Convex_Intersects_Polygon(frustum, *this);
+    }
+
+    LineSegment Polygon::Edge(int i) const
+    {
+        if (vertices.empty())
+            return LineSegment(Vector3::NaN, Vector3::NaN);
+        if (vertices.size() == 1)
+            return LineSegment(vertices[0], vertices[0]);
+        return LineSegment(vertices[i], vertices[(i+1)%vertices.size()]);
+    }
+
+    Vector3 Polygon::ClosestPoint(const Vector3 &point) const
+    {
+        assert(IsPlanar());
+
+        std::vector<Triangle> tris = Triangulate();
+        Vector3 closestPt = Vector3::NaN;
+        float closestDist = FLT_MAX;
+        for(size_t i = 0; i < tris.size(); ++i)
+        {
+            Vector3 pt = ((Triangle)tris[i]).ClosestPoint(point);
+            float d = pt.DistanceSq(point);
+            if (d < closestDist)
+            {
+                closestPt = pt;
+                closestDist = d;
+            }
+        }
+        return closestPt;
+    }
+
+    Vector3 Polygon::ClosestPoint(const LineSegment &lineSegment) const
+    {
+        return ClosestPoint(lineSegment, 0);
+    }
+
+    Vector3 Polygon::ClosestPoint(const LineSegment &lineSegment, Vector3 *lineSegmentPt) const
+    {
+        std::vector<Triangle> tris = Triangulate();
+        Vector3 closestPt = Vector3::NaN;
+        Vector3 closestLineSegmentPt = Vector3::NaN;
+        float closestDist = FLT_MAX;
+        for(size_t i = 0; i < tris.size(); ++i)
+        {
+            Vector3 lineSegPt;
+            Vector3 pt = ((Triangle)tris[i]).ClosestPoint(lineSegment, &lineSegPt);
+            float d = pt.DistanceSq(lineSegPt);
+            if (d < closestDist)
+            {
+                closestPt = pt;
+                closestLineSegmentPt = lineSegPt;
+                closestDist = d;
+            }
+        }
+        if (lineSegmentPt)
+            *lineSegmentPt = closestLineSegmentPt;
+        return closestPt;
+    }
+
+}
+
+

src/J3ML/Geometry/Polyhedron.cpp

@@ -0,0 +1,681 @@
+#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 <set>
+#include <cfloat>
+
+namespace J3ML::Geometry
+{
+    int Polyhedron::ExtremeVertex(const Vector3 &direction) const
+    {
+        int mostExtreme = -1;
+        float mostExtremeDist = -FLT_MAX;
+        for(int i = 0; i < NumVertices(); ++i)
+        {
+            float d = Vector3::Dot(direction, Vertex(i));
+            if (d > mostExtremeDist)
+            {
+                mostExtremeDist = d;
+                mostExtreme = i;
+            }
+        }
+        return mostExtreme;
+    }
+
+    Vector3 Polyhedron::ExtremePoint(const Vector3 &direction) const
+    {
+        return Vertex(ExtremeVertex(direction));
+    }
+
+    void Polyhedron::ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const
+    {
+        ///\todo Optimize!
+        Vector3 minPt = ExtremePoint(-direction);
+        Vector3 maxPt = ExtremePoint(direction);
+        outMin = Vector3::Dot(minPt, direction);
+        outMax = Vector3::Dot(maxPt, direction);
+    }
+
+    int Polyhedron::NumEdges() const
+    {
+        int numEdges = 0;
+        for(size_t i = 0; i < f.size(); ++i)
+            numEdges += (int)f[i].v.size();
+        return numEdges / 2;
+    }
+
+    AABB Polyhedron::MinimalEnclosingAABB() const {
+        AABB aabb;
+        aabb.SetNegativeInfinity();
+        for (int i = 0; i < NumVertices(); ++i)
+            aabb.Enclose(Vertex(i));
+        return aabb;
+    }
+
+    Vector3 Polyhedron::Vertex(int vertexIndex) const {
+        return v[vertexIndex];
+    }
+
+    LineSegment Polyhedron::Edge(int edgeIndex) const
+    {
+        assert(edgeIndex >= 0);
+        std::vector<LineSegment> edges = Edges();
+        assert(edgeIndex < (int)edges.size());
+        return edges[edgeIndex];
+    }
+
+    Polygon Polyhedron::FacePolygon(int faceIndex) const
+    {
+        Polygon p;
+        assert(faceIndex >= 0);
+        assert(faceIndex < (int)f.size());
+
+        p.vertices.reserve(f[faceIndex].v.size());
+        for(size_t i = 0; i < f[faceIndex].v.size(); ++i)
+            p.vertices.push_back(Vertex(f[faceIndex].v[i]));
+        return p;
+    }
+
+
+    std::vector<LineSegment> Polyhedron::Edges() const
+    {
+        std::vector<std::pair<int, int> > edges = EdgeIndices();
+        std::vector<LineSegment> edgeLines;
+        edgeLines.reserve(edges.size());
+        for(size_t i = 0; i < edges.size(); ++i)
+            edgeLines.push_back(LineSegment(Vertex(edges[i].first), Vertex(edges[i].second)));
+
+        return edgeLines;
+    }
+
+    std::vector<std::pair<int, int> > Polyhedron::EdgeIndices() const
+    {
+        std::set<std::pair<int, int> > uniqueEdges;
+        for(int i = 0; i < NumFaces(); ++i)
+        {
+            assert(f[i].v.size() >= 3);
+            if (f[i].v.size() < 3)
+                continue; // Degenerate face with less than three vertices, skip!
+            int x = f[i].v.back();
+            for(size_t j = 0; j < f[i].v.size(); ++j)
+            {
+                int y = f[i].v[j];
+                uniqueEdges.insert(std::make_pair(std::min(x, y), std::max(x, y)));
+                x = y;
+            }
+        }
+
+        std::vector<std::pair<int, int> >edges;
+        edges.insert(edges.end(), uniqueEdges.begin(), uniqueEdges.end());
+        return edges;
+    }
+
+
+    bool Polyhedron::Contains(const Vector3 &point) const {
+
+        if (v.size() <= 3) {
+            if (v.size() == 3)
+                return Triangle(Vector3(v[0]), Vector3(v[1]), Vector3(v[2])).Contains(point);
+            else if (v.size() == 2)
+                return LineSegment(Vector3(v[0]), Vector3(v[1])).Contains(point);
+            else if (v.size() == 1)
+                return Vector3(v[0]).Equals(point);
+            else
+                return false;
+        }
+
+        int bestNumIntersections = 0;
+        float bestFaceContainmentDistance = 0.f;
+
+        // For N > 4 Order Polyhedra
+        // General strategy: pick a ray from the query point to a random direction, and count the number of times the ray intersects a face.
+        // If it intersects an odd number of times, the given point must have been inside the polyhedron.
+        // But unfortunately for numerical stability, we must be smart with the choice of the ray direction. If we pick a ray direction
+        // which exits the polyhedron precisely at a vertex, or at an edge of two adjoining faces, we might count those twice. Therefore
+        // Try to pick a ray direction that passes safely through a center of some face. If we detect that there was a tricky face that
+        // the ray passed too close to an edge, we have no choice but to pick another ray direction and hope that it passes through
+        // the polyhedron in a safe manner.
+
+
+        // Loop through each face to choose the ray direction. If our choice was good, we only do this once and the algorithm
+        // after the first iteration at j == 0. If not, we iterate more faces of the polyhedron to try to find one that is safe for
+        // ray-polyhedron examination.
+        for (int j = 0; j < (int)f.size(); ++j)
+        {
+            if (f[j].v.size() < 3)
+                continue;
+
+            // Accumulate how many times the ray intersected a face of the polyhedron.
+            int numIntersections = 0;
+            // Track a pseudo-distance of the closest edge of a face that the ray passed through. If this distance ends up being too small,
+            // we decide not to trust the result we got, and proceed to another iteration of j, hoping to guess a better-behaving direction
+            // for the test ray.
+            float faceContainmentDistance = INFINITY;
+
+            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)
+                //continue;
+            Ray r(Vector3(), Dir);
+
+            for (int i = 0; i < (int)f.size(); ++i)
+            {
+                Plane p((Vector3)v[f[i].v[0]] - point, (Vector3)v[f[i].v[1]] - point, (Vector3)v[f[i].v[2]] - point);
+
+                float d;
+                // Find the intersection of the plane and the ray.
+                if (p.Intersects(r, &d))
+                {
+                    float containmentDistance2D = FaceContainmentDistance2D(i, r.GetPoint(d) + point);
+                    if (containmentDistance2D >= 0.f)
+                        ++numIntersections;
+                    faceContainmentDistance = std::min(faceContainmentDistance, std::abs(containmentDistance2D));
+                }
+            }
+            if (faceContainmentDistance > 1e-2f) // The nearest edge was far enough, we conclude the result is believable.
+                return (numIntersections % 2) == 1;
+            else if (faceContainmentDistance >= bestFaceContainmentDistance)
+            {
+                // The ray passed too close to a face edge. Remember this result, but proceed to another test iteration to see if we can
+                // find a more plausible test ray.
+                bestNumIntersections = numIntersections;
+                bestFaceContainmentDistance = faceContainmentDistance;
+            }
+        }
+        // We tested rays through each face of the polyhedron, but all rays passed too close to edges of the polyhedron faces. Return
+        // the result from the test that was farthest to any of the face edges.
+        return (bestNumIntersections % 2) == 1;
+    }
+
+    float Polyhedron::FaceContainmentDistance2D(int faceIndex, const Vector3 &worldSpacePoint, float polygonThickness) const
+    {
+        // N.B. This implementation is a duplicate of Polygon::Contains, but adapted to avoid dynamic memory allocation
+        // related to converting the face of a Polyhedron to a Polygon object.
+
+        // Implementation based on the description from http://erich.realtimerendering.com/ptinpoly/
+
+        const Face &face = f[faceIndex];
+        const std::vector<int> &vertices = face.v;
+
+        if (vertices.size() < 3)
+            return -INFINITY; // Certainly not intersecting, so return -inf denoting "strongly not contained"
+
+        Plane p = FacePlane(faceIndex);
+        if (FacePlane(faceIndex).Distance(worldSpacePoint) > polygonThickness)
+            return -INFINITY;
+
+        int numIntersections = 0;
+
+        Vector3 basisU = (Vector3)v[vertices[1]] - (Vector3)v[vertices[0]];
+        basisU.Normalize();
+        Vector3 basisV = Vector3::Cross(p.Normal, basisU).Normalize();
+        assert(basisU.IsNormalized());
+        assert(basisV.IsNormalized());
+        assert(basisU.IsPerpendicular(basisV));
+        assert(basisU.IsPerpendicular(p.Normal));
+        assert(basisV.IsPerpendicular(p.Normal));
+
+        // Tracks a pseudo-distance of the point to the ~nearest edge of the polygon. If the point is very close to the polygon
+        // edge, this is very small, and it's possible that due to numerical imprecision we cannot rely on the result in higher-level
+        // algorithms that invoke this function.
+        float faceContainmentDistance = INFINITY;
+        const float epsilon = 1e-4f;
+
+        Vector3 vt = Vector3(v[vertices.back()]) - worldSpacePoint;
+        Vector2 p0 = Vector2(Vector3::Dot(vt, basisU), Vector3::Dot(vt, basisV));
+        if (std::abs(p0.y) < epsilon)
+            p0.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly.
+
+        for(size_t i = 0; i < vertices.size(); ++i)
+        {
+            vt = Vector3(v[vertices[i]]) - worldSpacePoint;
+            Vector2 p1 = Vector2(Vector3::Dot(vt, basisU), Vector3::Dot(vt, basisV));
+            if (std::abs(p1.y) < epsilon)
+                p1.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly.
+
+            if (p0.y * p1.y < 0.f)
+            {
+                float minX = std::min(p0.x, p1.x);
+                if (minX > 0.f)
+                {
+                    faceContainmentDistance = std::min(faceContainmentDistance, minX);
+                    ++numIntersections;
+                }
+                else if (std::max(p0.x, p1.x) > 0.f)
+                {
+                    // P = p0 + t*(p1-p0) == (x,0)
+                    //     p0.x + t*(p1.x-p0.x) == x
+                    //     p0.y + t*(p1.y-p0.y) == 0
+                    //                 t == -p0.y / (p1.y - p0.y)
+
+                    // Test whether the lines (0,0) -> (+inf,0) and p0 -> p1 intersect at a positive X-coordinate.
+                    Vector2 d = p1 - p0;
+                    if (d.y != 0.f)
+                    {
+                        float t = -p0.y / d.y;
+                        float x = p0.x + t * d.x;
+                        if (t >= 0.f && t <= 1.f)
+                        {
+                            // Remember how close the point was to the edge, for tracking robustness/goodness of the result.
+                            // If this is very large, then we can conclude that the point was contained or not contained in the face.
+                            faceContainmentDistance = std::min(faceContainmentDistance, std::abs(x));
+                            if (x >= 0.f)
+                                ++numIntersections;
+                        }
+                    }
+                }
+            }
+            p0 = p1;
+        }
+
+        // Positive return value: face contains the point. Negative: face did not contain the point.
+        return (numIntersections % 2 == 1) ? faceContainmentDistance : -faceContainmentDistance;
+    }
+
+    bool Polyhedron::Contains(const LineSegment &lineSegment) const
+    {
+        return Contains(lineSegment.A) && Contains(lineSegment.B);
+    }
+
+    bool Polyhedron::Contains(const Triangle &triangle) const
+    {
+        return Contains(triangle.V0) && Contains(triangle.V1) && Contains(triangle.V2);
+    }
+
+    bool Polyhedron::Contains(const Polygon &polygon) const
+    {
+        for(int i = 0; i < polygon.NumVertices(); ++i)
+            if (!Contains(polygon.Vertex(i)))
+                return false;
+        return true;
+    }
+
+    bool Polyhedron::Contains(const AABB &aabb) const
+    {
+        for(int i = 0; i < 8; ++i)
+            if (!Contains(aabb.CornerPoint(i)))
+                return false;
+
+        return true;
+    }
+
+    bool Polyhedron::Contains(const OBB &obb) const
+    {
+        for(int i = 0; i < 8; ++i)
+            if (!Contains(obb.CornerPoint(i)))
+                return false;
+
+        return true;
+    }
+
+    bool Polyhedron::Contains(const Frustum &frustum) const
+    {
+        for(int i = 0; i < 8; ++i)
+            if (!Contains(frustum.CornerPoint(i)))
+                return false;
+
+        return true;
+    }
+
+    bool Polyhedron::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;
+    }
+
+    bool Polyhedron::IsClosed() const {
+
+    }
+
+    Plane Polyhedron::FacePlane(int faceIndex) const
+    {
+        const Face &face = f[faceIndex];
+        if (face.v.size() >= 3)
+            return Plane(v[face.v[0]], v[face.v[1]], v[face.v[2]]);
+        else if (face.v.size() == 2)
+            return Plane(Line(v[face.v[0]], v[face.v[1]]), ((Vector3)v[face.v[0]]-(Vector3)v[face.v[1]]).Perpendicular());
+        else if (face.v.size() == 1)
+            return Plane(v[face.v[0]], Vector3(0,1,0));
+        else
+            return Plane();
+    }
+
+    std::vector<Polygon> Polyhedron::Faces() const
+    {
+        std::vector<Polygon> faces;
+        faces.reserve(NumFaces());
+        for(int i = 0; i < NumFaces(); ++i)
+            faces.push_back(FacePolygon(i));
+        return faces;
+    }
+
+    Vector4 PolyFaceNormal(const Polyhedron &poly, int faceIndex)
+    {
+        const Polyhedron::Face &face = poly.f[faceIndex];
+        if (face.v.size() == 3)
+        {
+            Vector4 a = Vector4(poly.v[face.v[0]], 1.f);
+            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();
+            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();
+        }
+        else if (face.v.size() > 3)
+        {
+            // Use Newell's method of computing the face normal for best stability.
+            // See Christer Ericson, Real-Time Collision Detection, pp. 491-495.
+            Vector4 normal(0, 0, 0, 0);
+            int v0 = face.v.back();
+            for(size_t i = 0; i < face.v.size(); ++i)
+            {
+                int v1 = face.v[i];
+                normal.x += (double(poly.v[v0].y) - poly.v[v1].y) * (double(poly.v[v0].z) + poly.v[v1].z); // Project on yz
+                normal.y += (double(poly.v[v0].z) - poly.v[v1].z) * (double(poly.v[v0].x) + poly.v[v1].x); // Project on xz
+                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();
+            return normal;
+#if 0
+            cv bestNormal;
+		cs bestLen = -FLOAT_INF;
+		cv a = poly.v[face.v[face.v.size()-2]];
+		cv b = poly.v[face.v.back()];
+		for(size_t i = 0; i < face.v.size()-2; ++i)
+		{
+			cv c = poly.v[face.v[i]];
+			cv normal = (c-b).Cross(a-b);
+			float len = normal.Normalize();
+			if (len > bestLen)
+			{
+				bestLen = len;
+				bestNormal = normal;
+			}
+			a = b;
+			b = c;
+		}
+		assert(bestLen != -FLOAT_INF);
+		return DIR_VEC((float)bestNormal.x, (float)bestNormal.y, (float)bestNormal.z);
+#endif
+
+#if 0
+            // Find the longest edge.
+		cs bestLenSq = -FLOAT_INF;
+		cv bestEdge;
+		int v0 = face.v.back();
+		int bestV0 = 0;
+		int bestV1 = 0;
+		for(size_t i = 0; i < face.v.size(); ++i)
+		{
+			int v1 = face.v[i];
+			cv edge = cv(poly.v[v1]) - cv(poly.v[v0]);
+			cs lenSq = edge.Normalize();
+			if (lenSq > bestLenSq)
+			{
+				bestLenSq = lenSq;
+				bestEdge = edge;
+				bestV0 = v0;
+				bestV1 = v1;
+			}
+			v0 = v1;
+		}
+
+		cv bestNormal;
+		cs bestLen = -FLOAT_INF;
+		for(size_t i = 0; i < face.v.size(); ++i)
+		{
+			if (face.v[i] == bestV0 || face.v[i] == bestV1)
+				continue;
+			cv edge = cv(poly.v[i]) - cv(poly.v[bestV0]);
+			edge.Normalize();
+			cv normal = bestEdge.Cross(edge);
+			cs len = normal.Normalize();
+			if (len > bestLen)
+			{
+				bestLen = len;
+				bestNormal = normal;
+			}
+		}
+		assert(bestLen != -FLOAT_INF);
+		return DIR_VEC((float)bestNormal.x, (float)bestNormal.y, (float)bestNormal.z);
+#endif
+        }
+        else if (face.v.size() == 2)
+            return Vector4(Vector3(((Vector3)poly.v[face.v[1]]-(Vector3)poly.v[face.v[0]]).Cross(((Vector3)poly.v[face.v[0]]-(Vector3)poly.v[face.v[1]]).Perpendicular()-poly.v[face.v[0]]).TryNormalize()));
+        else if (face.v.size() == 1)
+            return Vector4(0, 1, 0, 0);
+        else
+            return Vector4::NaN;
+    }
+
+    Vector3 Polyhedron::FaceNormal(int faceIndex) const
+    {
+        Vector4 normal = PolyFaceNormal(*this, faceIndex);
+        return Vector3((float)normal.x, (float)normal.y, (float)normal.z);
+    }
+
+    bool Polyhedron::IsConvex() const
+    {
+        // This function is O(n^2).
+        /** @todo Real-Time Collision Detection, p. 64:
+            A faster O(n) approach is to compute for each face F of P the centroid C of F,
+            and for all neighboring faces G of F test if C lies behind the supporting plane of
+            G. If some C fails to lie behind the supporting plane of one or more neighboring
+            faces, P is concave, and is otherwise assumed convex. However, note that just as the
+            corresponding polygonal convexity test may fail for a pentagram this test may fail for,
+            for example, a pentagram extruded out of its plane and capped at the ends. */
+
+        float farthestD = -INFINITY;
+        int numPointsOutside = 0;
+        for(size_t i = 0; i < f.size(); ++i)
+        {
+            if (f[i].v.empty())
+                continue;
+
+            Vector3 pointOnFace = v[f[i].v[0]];
+            Vector3 faceNormal = FaceNormal((int)i);
+            for(size_t j = 0; j < v.size(); ++j)
+            {
+                float d = faceNormal.Dot(Vector3(v[j]) - pointOnFace);
+                if (d > 1e-2f)
+                {
+                    if (d > farthestD)
+                        farthestD = d;
+                    ++numPointsOutside;
+//					LOGW("Distance of vertex %s/%d from face %s/%d: %f",
+//					Vertex(j).ToString().c_str(), (int)j, FacePlane(i).ToString().c_str(), (int)i, d);
+//				return false;
+                }
+            }
+        }
+        if (numPointsOutside > 0)
+        {
+            printf("%d point-planes are outside the face planes. Farthest is at distance %f!", numPointsOutside, farthestD);
+            return false;
+        }
+        return true;
+    }
+
+    bool Polyhedron::ContainsConvex(const Vector3 &point, float epsilon) const
+    {
+        assert(IsConvex());
+        for(int i = 0; i < NumFaces(); ++i)
+            if (FacePlane(i).SignedDistance(point) > epsilon)
+                return false;
+
+        return true;
+    }
+
+    bool Polyhedron::ContainsConvex(const LineSegment &lineSegment) const {
+        return ContainsConvex(lineSegment.A) && ContainsConvex(lineSegment.B);
+    }
+
+    bool Polyhedron::ContainsConvex(const Triangle &triangle) const
+    {
+        return ContainsConvex(triangle.V0) && ContainsConvex(triangle.V1) && ContainsConvex(triangle.V2);
+    }
+
+    bool Polyhedron::FaceContains(int faceIndex, const Vector3 &worldSpacePoint, float polygonThickness) const
+    {
+        float faceContainmentDistance = FaceContainmentDistance2D(faceIndex, worldSpacePoint, polygonThickness);
+        return faceContainmentDistance >= 0.f;
+    }
+
+    bool Polyhedron::Intersects(const LineSegment &lineSegment) const
+    {
+        if (Contains(lineSegment))
+            return true;
+        for(int i = 0; i < NumFaces(); ++i)
+        {
+            float t;
+            Plane plane = FacePlane(i);
+            bool intersects = Plane::IntersectLinePlane(plane.Normal, plane.distance, lineSegment.A, lineSegment.B - lineSegment.A, t);
+            if (intersects && t >= 0.f && t <= 1.f)
+                if (FaceContains(i, lineSegment.GetPoint(t)))
+                    return true;
+        }
+
+        return false;
+    }
+
+    bool Polyhedron::Intersects(const Line &line) const
+    {
+        for(int i = 0; i < NumFaces(); ++i)
+            if (FacePolygon(i).Intersects(line))
+                return true;
+
+        return false;
+    }
+
+    bool Polyhedron::Intersects(const Ray &ray) const
+    {
+        for(int i = 0; i < NumFaces(); ++i)
+            if (FacePolygon(i).Intersects(ray))
+                return true;
+
+        return false;
+    }
+
+    bool Polyhedron::Intersects(const Plane &plane) const
+    {
+        return plane.Intersects(*this);
+    }
+
+    Vector3 Polyhedron::ApproximateConvexCentroid() const
+    {
+        // Since this shape is convex, the averaged position of all vertices is inside this polyhedron.
+        Vector3 arbitraryCenterVertex = Vector3::Zero;
+        for(int i = 0; i < NumVertices(); ++i)
+            arbitraryCenterVertex += Vertex(i);
+        return arbitraryCenterVertex / (float)NumVertices();
+    }
+
+
+    /** The algorithm for Polyhedron-Polyhedron intersection is from Christer Ericson's Real-Time Collision Detection, p. 384.
+	As noted by the author, the algorithm is very naive (and here unoptimized), and better methods exist. [groupSyntax] */
+    bool Polyhedron::Intersects(const Polyhedron &polyhedron) const
+    {
+        Vector3 c = this->ApproximateConvexCentroid();
+        if (polyhedron.Contains(c) && this->Contains(c))
+            return true;
+        c = polyhedron.ApproximateConvexCentroid();
+        if (polyhedron.Contains(c) && this->Contains(c))
+            return true;
+
+        // This test assumes that both this and the other polyhedron are closed.
+        // This means that for each edge running through vertices i and j, there's a face
+        // that contains the line segment (i,j) and another neighboring face that contains
+        // the line segment (j,i). These represent the same line segment (but in opposite direction)
+        // so we only have to test one of them for intersection. Take i < j as the canonical choice
+        // and skip the other winding order.
+
+        // Test for each edge of this polyhedron whether the other polyhedron intersects it.
+        for(size_t i = 0; i < f.size(); ++i)
+        {
+            assert(!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 = f[i].v.back();
+            Vector3 l0 = v[v0];
+            for(size_t j = 0; j < f[i].v.size(); ++j)
+            {
+                int v1 = f[i].v[j];
+                Vector3 l1 = v[v1];
+                if (v0 < v1 && polyhedron.Intersects(LineSegment(l0, l1))) // If v0 < v1, then this line segment is the canonical one.
+                    return true;
+                l0 = l1;
+                v0 = v1;
+            }
+        }
+
+        // Test for each edge of the other polyhedron whether this polyhedron intersects it.
+        for(size_t i = 0; i < polyhedron.f.size(); ++i)
+        {
+            assert(!polyhedron.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 = polyhedron.f[i].v.back();
+            Vector3 l0 = polyhedron.v[v0];
+            for(size_t j = 0; j < polyhedron.f[i].v.size(); ++j)
+            {
+                int v1 = polyhedron.f[i].v[j];
+                Vector3 l1 = polyhedron.v[v1];
+                if (v0 < v1 && 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 Capsule &capsule) const {
+        Vector3 pt, ptOnLineSegment;
+        pt = ClosestPoint(capsule.l, &ptOnLineSegment);
+        return pt.DistanceSq(ptOnLineSegment) <= capsule.r * capsule.r;
+    }
+
+    Vector3 Polyhedron::ClosestPoint(const LineSegment& lineSegment, Vector3 *lineSegmentPt) const {
+        if (Contains(lineSegment.A))
+        {
+            if (lineSegmentPt)
+                *lineSegmentPt = lineSegment.A;
+            return lineSegment.A;
+        }
+        if (Contains(lineSegment.B))
+        {
+            if (lineSegmentPt)
+                *lineSegmentPt = lineSegment.B;
+            return lineSegment.B;
+        }
+        Vector3 closestPt = Vector3::NaN;
+        float closestDistance = FLT_MAX;
+        Vector3 closestLineSegmentPt = Vector3::NaN;
+        for(int i = 0; i < NumFaces(); ++i)
+        {
+            Vector3 lineSegPt;
+            Vector3 pt = FacePolygon(i).ClosestPoint(lineSegment, &lineSegPt);
+            float d = pt.DistanceSq(lineSegPt);
+            if (d < closestDistance)
+            {
+                closestDistance = d;
+                closestPt = pt;
+                closestLineSegmentPt = lineSegPt;
+            }
+        }
+        if (lineSegmentPt)
+            *lineSegmentPt = closestLineSegmentPt;
+        return closestPt;
+    }
+
+
+}
+

src/J3ML/Geometry/QuadTree.cpp

@@ -0,0 +1,6 @@
+#include <J3ML/Geometry/QuadTree.h>
+
+namespace Geometry
+{
+
+}

src/J3ML/Geometry/Ray.cpp

@@ -0,0 +1,207 @@
+#include <J3ML/Geometry/Ray.h>
+#include <J3ML/Geometry/Sphere.h>
+
+
+namespace J3ML::Geometry
+{
+
+    void Ray::ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const
+    {
+        outMin = outMax = Vector3::Dot(direction, Origin);
+        float d = Vector3::Dot(direction, Direction);
+
+        // Most of the time, the projection interval will be a half-infinite range, extending to either -inf or +inf.
+        if (d > 1e-4f)
+            outMax = INFINITY;
+        else if (d < -1e4f)
+            outMin = -INFINITY;
+    }
+
+    RaycastResult Ray::Cast(const Sphere &target, float maxDistance)
+    {
+        Vector3 p0 = this->Origin;
+        Vector3 d = this->Direction.Normalize();
+
+        Vector3 c = target.Position;
+        float r = target.Radius;
+
+        Vector3 e = c - p0;
+        // Using Length here would cause floating point error to creep in
+        float Esq = Vector3::LengthSquared(e);
+        float a = Vector3::Dot(e, d);
+        float b = std::sqrt(Esq - (a*a));
+        float f = std::sqrt((r*r) - (b*b));
+
+        float t = 0;
+        // No collision
+        if (r * r - Esq + a * a < 0.f)
+        {
+            t = -1;
+        } else if ( Esq < r*r) {
+            t = a + f;
+        } else
+        {
+            t = a - f;
+        }
+
+        // TODO: Verify this
+        Vector3 intersection = p0.Project(d*t);
+        Vector3 intersection_from_sphere_origin = (intersection - target.Position);
+
+        Vector3 normal = -intersection_from_sphere_origin.Normalize();
+
+        return RaycastResult{
+            intersection,
+            normal,
+            true,
+            (Shape *) &target
+        };
+    }
+
+    RaycastResult Ray::Cast(const AABB &target, float maxDistance) {
+        float t1 = (target.minPoint.x - Origin.x) / Direction.x;
+        float t2 = (target.maxPoint.x - Origin.x) / Direction.x;
+        float t3 = (target.minPoint.y - Origin.y) / Direction.y;
+        float t4 = (target.maxPoint.y - Origin.y) / Direction.y;
+        float t5 = (target.minPoint.z - Origin.z) / Direction.z;
+        float t6 = (target.maxPoint.z - Origin.z) / Direction.z;
+
+        float tmin = std::max( std::max( std::min(t1, t2), std::min(t3, t4)), std::min(t5, t6));
+        float tmax = std::min( std::min( std::max(t1, t2), std::max(t3, t4)), std::max(t5, t6));
+
+        // if tmax < 0, ray is intersecting aabb, but whole aabb is behind us.
+        if (tmax < 0)
+            return RaycastResult::NoHit();
+
+        // if tmin > tmax, ray doesn't intersect AABB
+        if (tmin > tmax)
+            return RaycastResult::NoHit();
+
+        float t = 0.f;
+
+        if (tmin < 0.f)
+            t = tmax;
+
+        t = tmin;
+
+        Vector3 p0 = this->Origin;
+        Vector3 d = this->Direction.Normalize();
+
+        // TODO: Verify this
+        Vector3 intersection = p0.Project(d*t);
+
+        // WTF: This algorithm is only valid for spheres!!!
+        // TODO: Calculate surfacenormal against rectangle
+        Vector3 intersection_from_sphere_origin = (intersection - target.Centroid());
+
+        Vector3 normal = -intersection_from_sphere_origin.Normalize();
+
+        return RaycastResult
+        {
+            intersection,
+            normal,
+            true,
+            (Shape*)&target
+        };
+    }
+
+    RaycastResult Ray::Cast(const Plane &target, float maxDistance) {
+        float nd = Vector3::Dot(Direction, target.Normal);
+        float pn = Vector3::Dot(Origin, target.Normal);
+
+        if (nd >= 0.f)
+            return RaycastResult::NoHit();
+
+        float t = (target.distance - pn) / nd;
+
+        Vector3 d = this->Direction.Normalize();
+
+        // TODO: verify this
+        Vector3 intersection = this->Origin.Project(d*t);
+
+        // TODO: flip the axis based on direction of incoming ray?
+        // Take dot product
+        Vector3 normal = target.Normal;
+
+        if (t >= 0.f)
+            return RaycastResult
+                    {
+                        intersection,
+                        normal,
+                        true,
+                        (Shape*) &target
+                    };
+
+        return RaycastResult::NoHit();
+    }
+
+    Vector3 Ray::ClosestPoint(const Vector3 &targetPoint, float &d) const
+    {
+        d = std::max(0.f, Vector3::Dot(targetPoint - Origin, Direction));
+        return GetPoint(d);
+    }
+
+    Vector3 Ray::ClosestPoint(const LineSegment &other, float &d, float &d2) const {
+        Line::ClosestPointLineLine(Origin, Direction, other.A, other.B - other.A, d, d2);
+        if (d < 0.f)
+        {
+            d = 0.f;
+            if (d2 >= 0.f && d2 <= 1.f)
+            {
+                other.ClosestPoint(Origin, d2);
+                return Origin;
+            }
+
+            Vector3 p;
+            float t2;
+
+            if (d2 < 0.f)
+            {
+                p = other.A;
+                t2 = 0.f;
+            }
+            else // u2 > 1.f
+            {
+                p = other.B;
+                t2 = 1.f;
+            }
+
+            Vector3 closestPoint = ClosestPoint(p, d);
+            Vector3 closestPoint2 = other.ClosestPoint(Origin, d2);
+            if (closestPoint.DistanceSquared(p) <= closestPoint2.DistanceSquared(Origin))
+            {
+                d2 = t2;
+                return closestPoint;
+            }
+            else
+            {
+                d = 0.f;
+                return Origin;
+            }
+        }
+        else if (d2 < 0.f)
+        {
+            d2 = 0.f;
+            return ClosestPoint(other.A, d);
+        }
+        else if (d2 > 1.f)
+        {
+            d2 = 1.f;
+            return ClosestPoint(other.B, d);
+        }
+        else
+            return GetPoint(d);
+    }
+
+    Ray::Ray(const Vector3 &pos, const Vector3 &dir) {
+        Origin = pos;
+        Direction = dir;
+    }
+
+    Vector3 Ray::GetPoint(float distance) const {
+        assert(Direction.IsNormalized());
+        return Origin + distance * Direction;
+    }
+}
+
+

src/J3ML/Geometry/Sphere.cpp

@@ -0,0 +1,28 @@
+#include <J3ML/Geometry/Sphere.h>
+
+namespace J3ML::Geometry
+{
+
+    bool Sphere::Contains(const LineSegment &lineseg) const {
+
+    }
+
+    void Sphere::ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const
+    {
+        float d = Vector3::Dot(direction, Position);
+        outMin = d - Radius;
+        outMax = d + Radius;
+    }
+
+    Vector3 Sphere::ExtremePoint(const Vector3 &direction) const {
+        float len = direction.Length();
+        assert(len > 0.f);
+        return Position + direction * (Radius / len);
+    }
+
+    Vector3 Sphere::ExtremePoint(const Vector3 &direction, float &projectionDistance) const {
+        Vector3 extremePoint = ExtremePoint(direction);
+        projectionDistance = extremePoint.Dot(direction);
+        return extremePoint;
+    }
+}

src/J3ML/Geometry/Triangle.cpp

@@ -0,0 +1,414 @@
+#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>
+
+
+namespace J3ML::Geometry
+{
+
+    LineSegment Triangle::Edge(int i) const
+    {
+        assert(0 <= i);
+        assert(i <= 2);
+        if (i == 0)
+            return LineSegment(V0, V1);
+        else if (i == 1)
+            return LineSegment(V1, V2);
+        else if (i == 2)
+            return LineSegment(V2, V0);
+        else
+            return LineSegment(Vector3::NaN, Vector3::NaN);
+    }
+
+    Vector3 Triangle::Vertex(int i) const
+    {
+        assert(0 <= i);
+        assert(i <= 2);
+        if (i == 0)
+            return V0;
+        else if (i == 1)
+            return V1;
+        else if (i == 2)
+            return V2;
+        else
+            return Vector3::NaN;
+    }
+
+    Plane Triangle::PlaneCCW() const
+    {
+        return Plane(V0, V1, V2);
+    }
+
+    Plane Triangle::PlaneCW() const
+    {
+        return Plane(V0, V1, V2);
+    }
+
+    void Triangle::ProjectToAxis(const Vector3 &axis, float &dMin, float &dMax) const
+    {
+        dMin = dMax = Vector3::Dot(axis, V0);
+        float t = Vector3::Dot(axis, V1);
+        dMin = std::min(t, dMin);
+        dMax = std::max(t, dMax);
+        t = Vector3::Dot(axis, V2);
+        dMin = std::min(t, dMin);
+        dMax = std::max(t, dMax);
+    }
+
+    AABB Triangle::BoundingAABB() const {
+        AABB aabb;
+        aabb.minPoint = Vector3::Min(V0, V1, V2);
+        aabb.maxPoint = Vector3::Max(V0, V1, V2);
+        return aabb;
+    }
+
+    bool Triangle::Intersects(const AABB &aabb) const {
+        return aabb.Intersects(*this);
+    }
+
+    /** Calculates the intersection between a line and a triangle. The facing is not accounted for, so
+	rays are reported to intersect triangles that are both front and backfacing.
+	According to "T. M&ouml;ller, B. Trumbore. Fast, Minimum Storage Ray/Triangle Intersection. 2005."
+	http://jgt.akpeters.com/papers/MollerTrumbore97/
+	@param linePos The starting point of the line.
+	@param lineDir The direction vector of the line. This does not need to be normalized.
+	@param v0 Vertex 0 of the triangle.
+	@param v1 Vertex 1 of the triangle.
+	@param v2 Vertex 2 of the triangle.
+	@param u [out] The barycentric u coordinate is returned here if an intersection occurred.
+	@param v [out] The barycentric v coordinate is returned here if an intersection occurred.
+	@return The distance along the ray to the point of intersection, or +inf if no intersection occurred.
+		If no intersection, then u and v and t will contain undefined values. If lineDir was not normalized, then to get the
+		real world-space distance, one must scale the returned value with lineDir.Length(). If the returned value is negative,
+		then the intersection occurs 'behind' the line starting position, with respect to the direction vector lineDir. */
+    float Triangle::IntersectLineTri(const Vector3 &linePos, const Vector3 &lineDir,
+                                     const Vector3 &v0, const Vector3 &v1, const Vector3 &v2,
+                                     float &u, float &v)
+    {
+        const float epsilon = 1e-4f;
+
+        // Edge vectors
+        Vector3 vE1 = v1 - v0;
+        Vector3 vE2 = v2 - v0;
+
+        // begin calculating determinant - also used to calculate U parameter
+        Vector3 vP = lineDir.Cross(vE2);
+
+        // If det < 0, intersecting backfacing tri, > 0, intersecting frontfacing tri, 0, parallel to plane.
+        const float det = vE1.Dot(vP);
+
+        // If determinant is near zero, ray lies in plane of triangle.
+        if (std::abs(det) <= epsilon)
+            return INFINITY;
+        const float recipDet = 1.f / det;
+
+        // Calculate distance from v0 to ray origin
+        Vector3 vT = linePos - v0;
+
+        // Output barycentric u
+        u = vT.Dot(vP) * recipDet;
+        if (u < -epsilon || u > 1.f + epsilon)
+            return INFINITY; // Barycentric U is outside the triangle - early out.
+
+        // Prepare to test V parameter
+        Vector3 vQ = vT.Cross(vE1);
+
+        // Output barycentric v
+        v = lineDir.Dot(vQ) * recipDet;
+        if (v < -epsilon || u + v > 1.f + epsilon) // Barycentric V or the combination of U and V are outside the triangle - no intersection.
+            return INFINITY;
+
+        // Barycentric u and v are in limits, the ray intersects the triangle.
+
+        // Output signed distance from ray to triangle.
+        return vE2.Dot(vQ) * recipDet;
+//	return (det < 0.f) ? IntersectBackface : IntersectFrontface;
+    }
+
+    Vector3 Triangle::ClosestPointToTriangleEdge(const Line &other, float *outU, float *outV, float *outD) const
+    {
+        ///@todo Optimize!
+        // The line is parallel to the triangle.
+        float unused1, unused2, unused3;
+        float d1, d2, d3;
+        Vector3 pt1 = Edge(0).ClosestPoint(other, unused1, d1);
+        Vector3 pt2 = Edge(1).ClosestPoint(other, unused2, d2);
+        Vector3 pt3 = Edge(2).ClosestPoint(other, unused3, d3);
+        float dist1 = pt1.DistanceSq(other.GetPoint(d1));
+        float dist2 = pt2.DistanceSq(other.GetPoint(d2));
+        float dist3 = pt3.DistanceSq(other.GetPoint(d3));
+        if (dist1 <= dist2 && dist1 <= dist3)
+        {
+            if (outU) *outU = BarycentricUV(pt1).x;
+            if (outV) *outV = BarycentricUV(pt1).y;
+            if (outD) *outD = d1;
+            return pt1;
+        }
+        else if (dist2 <= dist3)
+        {
+            if (outU) *outU = BarycentricUV(pt2).x;
+            if (outV) *outV = BarycentricUV(pt2).y;
+            if (outD) *outD = d2;
+            return pt2;
+        }
+        else
+        {
+            if (outU) *outU = BarycentricUV(pt3).x;
+            if (outV) *outV = BarycentricUV(pt3).y;
+            if (outD) *outD = d3;
+            return pt3;
+        }
+    }
+
+    Vector3 Triangle::ClosestPointToTriangleEdge(const LineSegment &lineSegment, float *outU, float *outV, float *outD) const
+    {
+        ///@todo Optimize!
+        // The line is parallel to the triangle.
+        float unused1, unused2, unused3;
+        float d1, d2, d3;
+        Vector3 pt1 = Edge(0).ClosestPoint(lineSegment, unused1, d1);
+        Vector3 pt2 = Edge(1).ClosestPoint(lineSegment, unused2, d2);
+        Vector3 pt3 = Edge(2).ClosestPoint(lineSegment, unused3, d3);
+        float dist1 = pt1.DistanceSq(lineSegment.GetPoint(d1));
+        float dist2 = pt2.DistanceSq(lineSegment.GetPoint(d2));
+        float dist3 = pt3.DistanceSq(lineSegment.GetPoint(d3));
+        if (dist1 <= dist2 && dist1 <= dist3)
+        {
+            if (outU) *outU = BarycentricUV(pt1).x;
+            if (outV) *outV = BarycentricUV(pt1).y;
+            if (outD) *outD = d1;
+            return pt1;
+        }
+        else if (dist2 <= dist3)
+        {
+            if (outU) *outU = BarycentricUV(pt2).x;
+            if (outV) *outV = BarycentricUV(pt2).y;
+            if (outD) *outD = d2;
+            return pt2;
+        }
+        else
+        {
+            if (outU) *outU = BarycentricUV(pt3).x;
+            if (outV) *outV = BarycentricUV(pt3).y;
+            if (outD) *outD = d3;
+            return pt3;
+        }
+    }
+
+    Vector3 Triangle::ClosestPoint(const Vector3 &p) const
+    {
+        /** The code for Triangle-float3 test is from Christer Ericson's Real-Time Collision Detection, pp. 141-142. */
+
+        // Check if P is in vertex region outside A.
+        Vector3 ab = V1 - V0;
+        Vector3 ac = V2 - V0;
+        Vector3 ap = p - V0;
+        float d1 = Vector3::Dot(ab, ap);
+        float d2 = Vector3::Dot(ac, ap);
+        if (d1 <= 0.f && d2 <= 0.f)
+            return V0; // Barycentric coordinates are (1,0,0).
+
+        // Check if P is in vertex region outside B.
+        Vector3 bp = p - V1;
+        float d3 = Vector3::Dot(ab, bp);
+        float d4 = Vector3::Dot(ac, bp);
+        if (d3 >= 0.f && d4 <= d3)
+            return V1; // Barycentric coordinates are (0,1,0).
+
+        // Check if P is in edge region of AB, and if so, return the projection of P onto AB.
+        float vc = d1*d4 - d3*d2;
+        if (vc <= 0.f && d1 >= 0.f && d3 <= 0.f)
+        {
+            float v = d1 / (d1 - d3);
+            return V0 + v * ab; // The barycentric coordinates are (1-v, v, 0).
+        }
+
+        // Check if P is in vertex region outside C.
+        Vector3 cp = p - V2;
+        float d5 = Vector3::Dot(ab, cp);
+        float d6 = Vector3::Dot(ac, cp);
+        if (d6 >= 0.f && d5 <= d6)
+            return V2; // The barycentric coordinates are (0,0,1).
+
+        // Check if P is in edge region of AC, and if so, return the projection of P onto AC.
+        float vb = d5*d2 - d1*d6;
+        if (vb <= 0.f && d2 >= 0.f && d6 <= 0.f)
+        {
+            float w = d2 / (d2 - d6);
+            return V0 + w * ac; // The barycentric coordinates are (1-w, 0, w).
+        }
+
+        // Check if P is in edge region of BC, and if so, return the projection of P onto BC.
+        float va = d3*d6 - d5*d4;
+        if (va <= 0.f && d4 - d3 >= 0.f && d5 - d6 >= 0.f)
+        {
+            float w = (d4 - d3) / (d4 - d3 + d5 - d6);
+            return V1 + w * (V2 - V0); // The barycentric coordinates are (0, 1-w, w).
+        }
+
+        // P must be inside the face region. Compute the closest point through its barycentric coordinates (u,v,w).
+        float denom = 1.f / (va + vb + vc);
+        float v = vb * denom;
+        float w = vc * denom;
+        return V0 + ab * v + ac * w;
+    }
+
+    Vector3 Triangle::ClosestPoint(const LineSegment& lineSegment, Vector3 *otherPt) const
+    {
+        ///\todo Optimize.
+        float u, v;
+        float t = IntersectLineTri(lineSegment.A, lineSegment.B - lineSegment.A, V0, V1, V2, u, v);
+        bool intersects = (t >= 0.0f && t <= 1.0f);
+        if (intersects)
+        {
+//		assume3(lineSegment.GetPoint(t).Equals(this->Point(u, v)), lineSegment.GetPoint(t).SerializeToCodeString(), this->Point(u, v).SerializeToCodeString(), lineSegment.GetPoint(t).Distance(this->Point(u, v)));
+            if (otherPt)
+                *otherPt = lineSegment.GetPoint(t);
+            return this->Point(u, v);
+        }
+
+        float u1,v1,d1;
+        Vector3 pt1 = ClosestPointToTriangleEdge(lineSegment, &u1, &v1, &d1);
+
+        Vector3 pt2 = ClosestPoint(lineSegment.A);
+        Vector3 pt3 = ClosestPoint(lineSegment.B);
+
+        float D1 = pt1.DistanceSq(lineSegment.GetPoint(d1));
+        float D2 = pt2.DistanceSq(lineSegment.A);
+        float D3 = pt3.DistanceSq(lineSegment.B);
+
+        if (D1 <= D2 && D1 <= D3)
+        {
+            if (otherPt)
+                *otherPt = lineSegment.GetPoint(d1);
+            return pt1;
+        }
+        else if (D2 <= D3)
+        {
+            if (otherPt)
+                *otherPt = lineSegment.A;
+            return pt2;
+        }
+        else
+        {
+            if (otherPt)
+                *otherPt = lineSegment.B;
+            return pt3;
+        }
+    }
+
+    /// Implementation from Christer Ericson's Real-Time Collision Detection, pp. 51-52.
+    inline float TriArea2D(float x1, float y1, float x2, float y2, float x3, float y3)
+    {
+        return (x1-x2)*(y2-y3) - (x2-x3)*(y1-y2);
+    }
+
+
+    Vector3 Triangle::BarycentricUVW(const Vector3 &point) const {
+        // Implementation from Christer Ericson's Real-Time Collision Detection, pp. 51-52.
+
+        // Unnormalized triangle normal.
+        Vector3 m = Vector3::Cross(V1-V0, V2-V1);
+
+        // Nominators and one-over-denominator for u and v ratios.
+        float nu, nv, ood;
+
+        // Absolute components for determining projection plane.
+        float x = std::abs(m.x);
+        float y = std::abs(m.y);
+        float z = std::abs(m.z);
+
+        if (x >= y && x >= z)
+        {
+            // Project to the yz plane.
+            nu = TriArea2D(point.y, point.z, V1.y, V1.z, V2.y, V2.z); // Area of PBC in yz-plane.
+            nv = TriArea2D(point.y, point.z, V2.y, V2.z, V0.y, V0.z); // Area OF PCA in yz-plane.
+            ood = 1.f / m.x; // 1 / (2*area of ABC in yz plane)
+        }
+        else if (y >= z) // Note: The book has a redundant 'if (y >= x)' comparison
+        {
+            // y is largest, project to the xz-plane.
+            nu = TriArea2D(point.x, point.z, V1.x, V1.z, V2.x, V2.z);
+            nv = TriArea2D(point.x, point.z, V2.x, V2.z, V0.x, V0.z);
+            ood = 1.f / -m.y;
+        }
+        else // z is largest, project to the xy-plane.
+        {
+            nu = TriArea2D(point.x, point.y, V1.x, V1.y, V2.x, V2.y);
+            nv = TriArea2D(point.x, point.y, V2.x, V2.y, V0.x, V0.y);
+            ood = 1.f / m.z;
+        }
+        float u = nu * ood;
+        float v = nv * ood;
+        float w = 1.f - u - v;
+        return Vector3(u,v,w);
+    }
+
+    bool Triangle::Intersects(const Capsule &capsule) const {
+        return capsule.Intersects(*this);
+    }
+
+    Vector3 Triangle::ExtremePoint(const Vector3 &direction) const {
+        Vector3 mostExtreme = Vector3::NaN;
+        float mostExtremeDist = -FLT_MAX;
+        for(int i = 0; i < 3; ++i)
+        {
+            Vector3 pt = Vertex(i);
+            float d = Vector3::Dot(direction, pt);
+            if (d > mostExtremeDist)
+            {
+                mostExtremeDist = d;
+                mostExtreme = pt;
+            }
+        }
+        return mostExtreme;
+    }
+
+    Vector3 Triangle::ExtremePoint(const Vector3 &direction, float &projectionDistance) const {
+        Vector3 extremePoint = ExtremePoint(direction);
+        projectionDistance = extremePoint.Dot(direction);
+        return extremePoint;
+    }
+
+    float Triangle::DistanceSq(const Vector3 &point) const {
+        return ClosestPoint(point).DistanceSq(point);
+    }
+
+    Vector2 Triangle::BarycentricUV(const Vector3 &point) const {
+        Vector3 uvw = BarycentricUVW(point);
+        return Vector2{uvw.y, uvw.z};
+    }
+
+    Vector3 Triangle::Point(const Vector2 &uv) const {
+        return Point(uv.x, uv.y);
+    }
+
+    Vector3 Triangle::Point(float u, float v) const {
+        // In case the triangle is far away from the origin but is small in size, the elements of 'a' will have large magnitudes,
+        // and the elements of (b-a) and (c-a) will be much smaller quantities. Therefore be extra careful with the
+        // parentheses and first sum the small floats together before adding it to the large one.
+        return V0 + ((V1-V0) * u + (V2-V0) * v);
+    }
+
+    Vector3 Triangle::Point(const Vector3 &uvw) const {
+        return Point(uvw.x, uvw.y, uvw.z);
+    }
+
+    Vector3 Triangle::Point(float u, float v, float w) const {
+        return u * V0 + v * V1 + w * V2;
+    }
+
+    bool Triangle::Contains(const Vector3 &point, float triangleThicknessSq) const {
+        Vector3 normal = (V1-V0).Cross(V2-V0);
+        float lenSq = normal.LengthSquared();
+        float d = normal.Dot(V1 - point);
+        if (d*d > triangleThicknessSq * lenSq)
+            return false; ///@todo The plane-point distance test is omitted in Real-Time Collision Detection. p. 25. A bug in the book?
+
+        Vector3 br = BarycentricUVW(point);
+        return br.x >= -1e-3f && br.y >= -1e-3f && br.z >= -1e-3f; // Allow for a small epsilon to properly account for points very near the edges of the triangle.
+    }
+}

src/J3ML/Geometry/TriangleMesh.cpp

@@ -0,0 +1 @@
+#include <J3ML/Geometry/TriangleMesh.h>

src/J3ML/J3ML.cpp

@@ -0,0 +1,52 @@
+#include <J3ML/J3ML.h>
+
+
+namespace J3ML
+{
+
+    Math::Rotation Math::operator ""_degrees(long double rads) { return {Radians((float)rads)}; }
+
+    Math::Rotation Math::operator ""_deg(long double rads) { return {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) {
+        return 1.f / FastSqrt(x);
+    }
+
+    float Math::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;
+    }
+
+    float Math::RecipFast(float x) {
+        // TODO: Implement SSE rcp instruction.
+        return 1.f / x;
+    }
+
+    Math::Rotation::Rotation() : valueInRadians(0) {}
+
+    Math::Rotation::Rotation(float value) : valueInRadians(value) {}
+
+    Math::Rotation Math::Rotation::operator+(const Math::Rotation &rhs) {
+        valueInRadians += rhs.valueInRadians;
+    }
+
+    float Math::Interp::SmoothStart(float t) {
+        assert(t >= 0.f && t <= 1.f);
+        return t*t;
+    }
+
+    int Math::BitTwiddling::CountBitsSet(u32 value) {
+
+    }
+}

src/J3ML/LinearAlgebra.cpp

@@ -0,0 +1,6 @@
+#include "J3ML/LinearAlgebra.h"
+#include <cassert>
+
+namespace LinearAlgebra {
+
+}

src/J3ML/LinearAlgebra/AxisAngle.cpp

@@ -0,0 +1,17 @@
+#include <J3ML/LinearAlgebra/AxisAngle.h>
+#include <J3ML/LinearAlgebra/Quaternion.h>
+namespace J3ML::LinearAlgebra {
+
+    AxisAngle::AxisAngle() : axis(Vector3::Zero) {}
+
+    AxisAngle::AxisAngle(const Vector3 &axis, float angle) : axis(axis), angle(angle) {}
+
+    Quaternion AxisAngle::ToQuaternion() const {
+        return {
+                axis.x * std::sin(angle/2),
+                axis.y * std::sin(angle/2),
+                axis.z * std::sin(angle/2),
+                std::cos(angle/2)
+        };
+    }
+}

src/J3ML/LinearAlgebra/CoordinateFrame.cpp

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

src/J3ML/LinearAlgebra/EulerAngle.cpp

@@ -0,0 +1,51 @@
+#include <J3ML/LinearAlgebra/EulerAngle.h>
+#include <cmath>
+#include <algorithm>
+
+
+namespace J3ML::LinearAlgebra {
+    EulerAngle::EulerAngle(float pitch, float yaw, float roll): pitch(pitch), yaw(yaw), roll(roll)
+    {}
+
+    float EulerAngle::GetPitch(float pitch_limit) const
+    { return std::clamp( std::remainderf(pitch,360.f), -pitch_limit, pitch_limit); }
+
+    float EulerAngle::GetYaw(float yaw_limit) const
+    { return std::clamp(std::remainderf(yaw, 360.f), -yaw_limit, yaw_limit);   }
+
+    float EulerAngle::GetRoll(float pitch_limit) const
+    { return std::clamp( std::remainderf(pitch,360.f), -pitch_limit, pitch_limit); }
+
+    bool EulerAngle::operator==(const EulerAngle& a) const
+    {
+        return (pitch == a.pitch) && (yaw == a.yaw) && (roll == a.roll);
+    }
+
+    void EulerAngle::clamp()
+    {
+        if (this->pitch > 89.0f)
+            this->pitch = 89.0f;
+        if (this->pitch <= -89.0f)
+            this->pitch = -89.0f;
+        //TODO: Make this entirely seamless by getting the amount they rotated passed -180 and +180 by.
+        if (this->yaw <= -180.0f)
+            this->yaw = 180.0f;
+        if (this->yaw >= 180.01f)
+            this->yaw = -179.9f;
+        if (this->roll >= 360.0f)
+            this->roll = 0.0;
+        if (this->roll <= -360.0f)
+            this->roll = 0.0;
+    }
+
+    EulerAngle EulerAngle::movementAngle() const
+    {
+        EulerAngle a;
+        a.pitch = (cos(Math::Radians(yaw)) * cos(Math::Radians(pitch)));
+        a.yaw = -sin(Math::Radians(pitch));
+        a.roll = (sin(Math::Radians(yaw)) * cos(Math::Radians(pitch)));
+        return a;
+    }
+
+    EulerAngle::EulerAngle() : pitch(0), yaw(0), roll(0) {}
+}

src/J3ML/LinearAlgebra/Matrix2x2.cpp

@@ -0,0 +1,25 @@
+#include <J3ML/LinearAlgebra/Matrix2x2.h>
+
+namespace J3ML::LinearAlgebra {
+
+    Vector2 Matrix2x2::GetRow(int index) const {
+        float x = this->elems[index][0];
+        float y = this->elems[index][1];
+
+        return {x, y};
+    }
+
+    Vector2 Matrix2x2::GetColumn(int index) const {
+        float x = this->elems[0][index];
+        float y = this->elems[1][index];
+        return {x, y};
+    }
+
+    float Matrix2x2::At(int x, int y) const {
+        return this->elems[x][y];
+    }
+
+    float &Matrix2x2::At(int x, int y) {
+        return this->elems[x][y];
+    }
+}

src/J3ML/LinearAlgebra/Matrix3x3.cpp

@@ -0,0 +1,460 @@
+#include <J3ML/LinearAlgebra/Matrix3x3.h>
+#include <cmath>
+
+namespace J3ML::LinearAlgebra {
+
+    const Matrix3x3 Matrix3x3::Zero = Matrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0);
+    const Matrix3x3 Matrix3x3::Identity = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
+    const Matrix3x3 Matrix3x3::NaN = Matrix3x3(NAN);
+
+    Matrix3x3::Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21,
+                         float m22) {
+        this->elems[0][0] = m00;
+        this->elems[0][1] = m01;
+        this->elems[0][2] = m02;
+
+        this->elems[1][0] = m10;
+        this->elems[1][1] = m11;
+        this->elems[1][2] = m12;
+
+        this->elems[2][0] = m20;
+        this->elems[2][1] = m21;
+        this->elems[2][2] = m22;
+    }
+
+    Vector3 Matrix3x3::GetRow(int index) const {
+        float x = this->elems[index][0];
+        float y = this->elems[index][1];
+        float z = this->elems[index][2];
+        return {x, y, z};
+    }
+
+    Vector3 Matrix3x3::GetColumn(int index) const {
+        float x = this->elems[0][index];
+        float y = this->elems[1][index];
+        float z = this->elems[2][index];
+
+        return {x, y, z};
+    }
+
+
+    float Matrix3x3::At(int x, int y) const {
+        return this->elems[x][y];
+    }
+
+    void Matrix3x3::SetAt(int x, int y, float value)
+    {
+        this->elems[x][y] = value;
+    }
+
+    Vector3 Matrix3x3::operator*(const Vector3 &rhs) const {
+        return {
+                At(0, 0) * rhs.x + At(0, 1) * rhs.y + At(0, 2) * rhs.z,
+                At(1, 0) * rhs.x + At(1, 1) * rhs.y + At(1, 2) * rhs.z,
+                At(2, 0) * rhs.x + At(2, 1) * rhs.y + At(2, 2) * rhs.z
+        };
+    }
+
+    Matrix3x3 Matrix3x3::operator*(const Matrix3x3 &rhs) const {
+        //Matrix3x3 r;
+        auto m00 = At(0, 0) * rhs.At(0, 0) + At(0, 1) * rhs.At(1, 0) + At(0, 2) * rhs.At(2, 0);
+        auto m01 = At(0, 0) * rhs.At(0, 1) + At(0, 1) * rhs.At(1, 1) + At(0, 2) * rhs.At(2, 1);
+        auto m02 = At(0, 0) * rhs.At(0, 2) + At(0, 1) * rhs.At(1, 2) + At(0, 2) * rhs.At(2, 2);
+
+        auto m10 = At(1, 0) * rhs.At(0, 0) + At(1, 1) * rhs.At(1, 0) + At(1, 2) * rhs.At(2, 0);
+        auto m11 = At(1, 0) * rhs.At(0, 1) + At(1, 1) * rhs.At(1, 1) + At(1, 2) * rhs.At(2, 1);
+        auto m12 = At(1, 0) * rhs.At(0, 2) + At(1, 1) * rhs.At(1, 2) + At(1, 2) * rhs.At(2, 2);
+
+        auto m20 = At(2, 0) * rhs.At(0, 0) + At(2, 1) * rhs.At(1, 0) + At(2, 2) * rhs.At(2, 0);
+        auto m21 = At(2, 0) * rhs.At(0, 1) + At(2, 1) * rhs.At(1, 1) + At(2, 2) * rhs.At(2, 1);
+        auto m22 = At(2, 0) * rhs.At(0, 2) + At(2, 1) * rhs.At(1, 2) + At(2, 2) * rhs.At(2, 2);
+
+        return Matrix3x3({m00, m01, m02}, {m10, m11, m12}, {m20, m21, m22});
+    }
+
+    Matrix3x3::Matrix3x3(float val) {
+        this->elems[0][0] = val;
+        this->elems[0][1] = val;
+        this->elems[0][2] = val;
+
+        this->elems[1][0] = val;
+        this->elems[1][1] = val;
+        this->elems[1][2] = val;
+
+        this->elems[2][0] = val;
+        this->elems[2][1] = val;
+        this->elems[2][2] = val;
+
+    }
+
+    Matrix3x3::Matrix3x3(const Vector3 &r1, const Vector3 &r2, const Vector3 &r3) {
+        this->elems[0][0] = r1.x;
+        this->elems[0][1] = r1.y;
+        this->elems[0][2] = r1.z;
+
+        this->elems[1][0] = r2.x;
+        this->elems[1][1] = r2.y;
+        this->elems[1][2] = r2.z;
+
+        this->elems[2][0] = r3.x;
+        this->elems[2][1] = r3.y;
+        this->elems[2][2] = r3.z;
+    }
+
+    Matrix3x3::Matrix3x3(const Quaternion &orientation) {
+
+    }
+
+    float Matrix3x3::Determinant() const {
+        const float a = elems[0][0];
+        const float b = elems[0][1];
+        const float c = elems[0][2];
+        const float d = elems[1][0];
+        const float e = elems[1][1];
+        const float f = elems[1][2];
+        const float g = elems[2][0];
+        const float h = elems[2][1];
+        const float i = elems[2][2];
+
+        // Aight, what the fuck?
+        return a*(e*i - f*h) + b*(f*g - d*i) + c*(d*h - e*g);
+    }
+
+    Matrix3x3 Matrix3x3::Inverse() const {
+        // Compute the inverse directly using Cramer's rule
+        // Warning: This method is numerically very unstable!
+        float d = Determinant();
+
+        d = 1.f / d;
+
+        Matrix3x3 i = {
+                d * (At(1, 1) * At(2, 2) - At(1, 2) * At(2, 1)),
+                d * (At(0, 2) * At(2, 1) - At(0, 1) * At(2, 2)),
+                d * (At(0, 1) * At(1, 2) - At(0, 2) * At(1, 1)),
+
+                d * (At(1, 2) * At(2, 0) - At(1, 0) * At(2, 2)),
+                d * (At(0, 0) * At(2, 2) - At(0, 2) * At(2, 0)),
+                d * (At(0, 2) * At(1, 0) - At(0, 0) * At(1, 2)),
+
+                d * (At(1, 0) * At(2, 1) - At(1, 1) * At(2, 0)),
+                d * (At(2, 0) * At(0, 1) - At(0, 0) * At(2,1)),
+                d * (At(0,0) * At(1, 1) - At(0, 1) * At(1, 0))
+        };
+
+        return i;
+    }
+
+    Matrix3x3 Matrix3x3::Transpose() const {
+        auto m00 = this->elems[0][0];
+        auto m01 = this->elems[0][1];
+        auto m02 = this->elems[0][2];
+
+        auto m10 = this->elems[1][0];
+        auto m11 = this->elems[1][1];
+        auto m12 = this->elems[1][2];
+
+        auto m20 = this->elems[2][0];
+        auto m21 = this->elems[2][1];
+        auto m22 = this->elems[2][2];
+
+        // NO: This is correct order for transposition!
+        return {
+                m00, m10, m20,
+                m01, m11, m21,
+                m02, m12, m22
+        };
+    }
+
+    Vector2 Matrix3x3::Transform(const Vector2 &rhs) const {
+        return {
+                At(0,0) * rhs.x + At(0, 1) * rhs.y,
+                At(1,0) * rhs.x + At(1, 1) * rhs.y
+        };
+    }
+
+    Vector3 Matrix3x3::Transform(const Vector3 &rhs) const {
+        return {
+                At(0, 0) * rhs.x + At(0, 1) * rhs.y + At(0, 2) * rhs.z,
+                At(1, 0) * rhs.x + At(1, 1) * rhs.y + At(1, 2) * rhs.z,
+                At(2, 0) * rhs.x + At(2, 1) * rhs.y + At(2, 2) * rhs.z
+        };
+    }
+
+    Quaternion Matrix3x3::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
+        };
+    }
+
+    void Matrix3x3::SetRotatePart(const Vector3 &a, float angle) {
+        float s = std::sin(angle);
+        float c = std::cos(angle);
+
+        const float c1 = 1.f - c;
+
+        elems[0][0] = c+c1*a.x*a.x;
+        elems[1][0] = c1*a.x*a.y+s*a.z;
+        elems[2][0] = c1*a.x*a.z-s*a.y;
+
+        elems[0][1] = c1*a.x*a.y-s*a.z;
+        elems[1][1] = c+c1*a.y*a.y;
+        elems[2][1] = c1*a.y*a.z+s*a.x;
+
+        elems[0][2] = c1*a.x*a.z+s*a.y;
+        elems[1][2] = c1*a.y*a.z-s*a.x;
+        elems[2][2] = c+c1*a.z*a.z;
+    }
+
+    Matrix3x3 Matrix3x3::RotateAxisAngle(const Vector3 &axis, float angleRadians) {
+        Matrix3x3 r;
+        r.SetRotatePart(axis, angleRadians);
+        return r;
+    }
+
+    void Matrix3x3::SetRow(int i, const Vector3 &vec) {
+        elems[i][0] = vec.x;
+        elems[i][1] = vec.y;
+        elems[i][2] = vec.z;
+    }
+
+    void Matrix3x3::SetColumn(int i, const Vector3& vec)
+    {
+        elems[0][i] = vec.x;
+        elems[1][i] = vec.y;
+        elems[2][i] = vec.z;
+    }
+
+    Matrix3x3
+    Matrix3x3::LookAt(const Vector3 &forward, const Vector3 &target, const Vector3 &localUp, const Vector3 &worldUp) {
+        // User must input proper normalized input direction vectors
+        // In the local space, the forward and up directions must be perpendicular to be well-formed.
+        // In the world space, the target direction and world up cannot be degenerate (co-linear)
+        // Generate the third basis vector in the local space;
+        Vector3 localRight = localUp.Cross(forward).Normalize();
+        // A. Now we have an orthonormal linear basis {localRight, localUp, forward} for the object local space.
+        // Generate the third basis vector for the world space
+        Vector3 worldRight = worldUp.Cross(target).Normalize();
+        // Since the input worldUp vector is not necessarily perpendicular to the target direction vector
+        // We need to compute the real world space up vector that the "head" of the object will point
+        // towards when the model is looking towards the desired target direction
+        Vector3 perpWorldUp = target.Cross(worldRight).Normalize();
+        // B. Now we have an orthonormal linear basis {worldRight, perpWorldUp, targetDirection } for the desired target orientation.
+        // We want to build a matrix M that performs the following mapping:
+        // 1. localRight must be mapped to worldRight.        (M * localRight = worldRight)
+        // 2. localUp must be mapped to perpWorldUp.          (M * localUp = perpWorldUp)
+        // 3. localForward must be mapped to targetDirection. (M * localForward = targetDirection)
+        // i.e. we want to map the basis A to basis B.
+
+        // This matrix M exists, and it is an orthonormal rotation matrix with a determinant of +1, because
+        // the bases A and B are orthonormal with the same handedness.
+
+        // Below, use the notation that (a,b,c) is a 3x3 matrix with a as its first column, b second, and c third.
+
+        // By algebraic manipulation, we can rewrite conditions 1, 2 and 3 in a matrix form:
+        //        M * (localRight, localUp, localForward) = (worldRight, perpWorldUp, targetDirection)
+        // or     M = (worldRight, perpWorldUp, targetDirection) * (localRight, localUp, localForward)^{-1}.
+        // or     M = m1 * m2, where
+
+        // m1 equals (worldRight, perpWorldUp, target):
+        Matrix3x3 m1(worldRight, perpWorldUp, target);
+
+        // and m2 equals (localRight, localUp, localForward)^{-1}:
+        Matrix3x3 m2;
+        m2.SetRow(0, localRight);
+        m2.SetRow(1, localUp);
+        m2.SetRow(2, forward);
+        // Above we used the shortcut that for an orthonormal matrix M, M^{-1} = M^T. So set the rows
+        // and not the columns to directly produce the transpose, i.e. the inverse of (localRight, localUp, localForward).
+
+        // Compute final M.
+        m2 = m1 * m2;
+
+        // And fix any numeric stability issues by re-orthonormalizing the result.
+        m2.Orthonormalize(0, 1, 2);
+        return m2;
+    }
+
+    Vector3 Matrix3x3::Diagonal() const {
+        return {elems[0][0],
+                elems[1][1],
+                elems[2][2]
+        };
+    }
+
+    float &Matrix3x3::At(int row, int col) {
+        return elems[row][col];
+    }
+
+    Vector3 Matrix3x3::WorldZ() const {
+        return GetColumn(2);
+    }
+
+    Vector3 Matrix3x3::WorldY() const {
+        return GetColumn(1);
+    }
+
+    Vector3 Matrix3x3::WorldX() const {
+        return GetColumn(0);
+    }
+
+    Matrix3x3 Matrix3x3::FromRS(const Quaternion &rotate, const Vector3 &scale) {
+        return Matrix3x3(rotate) * Matrix3x3::FromScale(scale);
+    }
+
+    Matrix3x3 Matrix3x3::FromRS(const Matrix3x3 &rotate, const Vector3 &scale) {
+        return rotate * Matrix3x3::FromScale(scale);
+    }
+
+    Matrix3x3 Matrix3x3::FromQuat(const Quaternion &orientation) {
+        return Matrix3x3(orientation);
+    }
+
+    Matrix3x3 Matrix3x3::ScaleBy(const Vector3 &rhs) {
+        return *this * FromScale(rhs);
+    }
+
+    Vector3 Matrix3x3::GetScale() const {
+        return Vector3(GetColumn(0).Length(), GetColumn(1).Length(), GetColumn(2).Length());
+    }
+
+    Vector3 Matrix3x3::operator[](int row) const {
+        return Vector3{elems[row][0], elems[row][1], elems[row][2]};
+    }
+
+    Matrix3x3 Matrix3x3::FromScale(const Vector3 &scale) {
+        Matrix3x3 m;
+        m.At(0,0) = scale.x;
+        m.At(1,1) = scale.y;
+        m.At(2,2) = scale.z;
+        return m;
+    }
+
+    Matrix3x3 Matrix3x3::FromScale(float sx, float sy, float sz) {
+        Matrix3x3 m;
+        m.At(0,0) = sx;
+        m.At(1,1) = sy;
+        m.At(2,2) = sz;
+        return m;
+    }
+
+    void Matrix3x3::Orthonormalize(int c0, int c1, int c2) {
+        Vector3 v0 = GetColumn(c0);
+        Vector3 v1 = GetColumn(c1);
+        Vector3 v2 = GetColumn(c2);
+        Vector3::Orthonormalize(v0, v1, v2);
+        SetColumn(c0, v0);
+        SetColumn(c1, v1);
+        SetColumn(c2, v2);
+    }
+
+    Matrix3x3::Matrix3x3(const float *data) {
+        assert(data);
+        At(0, 0) = data[0];
+        At(0, 1) = data[1];
+        At(0, 2) = data[2];
+
+        At(1, 0) = data[4];
+        At(1, 1) = data[5];
+        At(1, 2) = data[6];
+
+        At(2, 0) = data[8];
+        At(2, 1) = data[9];
+        At(2, 2) = data[10];
+    }
+
+    Vector4 Matrix3x3::Mul(const Vector4 &rhs) const {
+        return {Mul(rhs.XYZ()), rhs.GetW()};
+    }
+
+    Vector3 Matrix3x3::Mul(const Vector3 &rhs) const {
+        return *this * rhs;
+    }
+
+    Vector2 Matrix3x3::Mul(const Vector2 &rhs) const {
+        return *this * rhs;
+    }
+
+    Matrix4x4 Matrix3x3::Mul(const Matrix4x4 &rhs) const {
+        return *this * rhs;
+    }
+
+    Matrix3x3 Matrix3x3::Mul(const Matrix3x3 &rhs) const {
+        return *this * rhs;
+    }
+
+    bool Matrix3x3::IsRowOrthogonal(float epsilon) const
+    {
+        return GetRow(0).IsPerpendicular(GetRow(1), epsilon)
+            && GetRow(0).IsPerpendicular(GetRow(2), epsilon)
+            && GetRow(1).IsPerpendicular(GetRow(2), epsilon);
+    }
+
+    bool Matrix3x3::IsColOrthogonal(float epsilon) const
+    {
+        return GetColumn(0).IsPerpendicular(GetColumn(1), epsilon)
+            && GetColumn(0).IsPerpendicular(GetColumn(2), epsilon)
+            && GetColumn(1).IsPerpendicular(GetColumn(2), epsilon);
+    }
+
+
+
+    bool Matrix3x3::HasUniformScale(float epsilon) const {
+        Vector3 scale = ExtractScale();
+        return Math::EqualAbs(scale.x, scale.y, epsilon) && Math::EqualAbs(scale.x, scale.z, epsilon);
+    }
+
+    Vector3 Matrix3x3::GetRow3(int index) const {
+        return GetRow(index);
+    }
+
+    Vector3 Matrix3x3::GetColumn3(int index) const {
+        return GetColumn(index);
+    }
+
+    Matrix4x4 Matrix3x3::operator*(const Matrix4x4 &rhs) const {
+        auto lhs = *this;
+        Matrix4x4 r;
+
+        r[0][0] = lhs.At(0, 0) * rhs.At(0, 0) + lhs.At(0, 1) * rhs.At(1, 0) + lhs.At(0, 2) * rhs.At(2, 0);
+        r[0][1] = lhs.At(0, 0) * rhs.At(0, 1) + lhs.At(0, 1) * rhs.At(1, 1) + lhs.At(0, 2) * rhs.At(2, 1);
+        r[0][2] = lhs.At(0, 0) * rhs.At(0, 2) + lhs.At(0, 1) * rhs.At(1, 2) + lhs.At(0, 2) * rhs.At(2, 2);
+        r[0][3] = lhs.At(0, 0) * rhs.At(0, 3) + lhs.At(0, 1) * rhs.At(1, 3) + lhs.At(0, 2) * rhs.At(2, 3);
+
+        r[1][0] = lhs.At(1, 0) * rhs.At(0, 0) + lhs.At(1, 1) * rhs.At(1, 0) + lhs.At(1, 2) * rhs.At(2, 0);
+        r[1][1] = lhs.At(1, 0) * rhs.At(0, 1) + lhs.At(1, 1) * rhs.At(1, 1) + lhs.At(1, 2) * rhs.At(2, 1);
+        r[1][2] = lhs.At(1, 0) * rhs.At(0, 2) + lhs.At(1, 1) * rhs.At(1, 2) + lhs.At(1, 2) * rhs.At(2, 2);
+        r[1][3] = lhs.At(1, 0) * rhs.At(0, 3) + lhs.At(1, 1) * rhs.At(1, 3) + lhs.At(1, 2) * rhs.At(2, 3);
+
+        r[2][0] = lhs.At(2, 0) * rhs.At(0, 0) + lhs.At(2, 1) * rhs.At(1, 0) + lhs.At(2, 2) * rhs.At(2, 0);
+        r[2][1] = lhs.At(2, 0) * rhs.At(0, 1) + lhs.At(2, 1) * rhs.At(1, 1) + lhs.At(2, 2) * rhs.At(2, 1);
+        r[2][2] = lhs.At(2, 0) * rhs.At(0, 2) + lhs.At(2, 1) * rhs.At(1, 2) + lhs.At(2, 2) * rhs.At(2, 2);
+        r[2][3] = lhs.At(2, 0) * rhs.At(0, 3) + lhs.At(2, 1) * rhs.At(1, 3) + lhs.At(2, 2) * rhs.At(2, 3);
+
+        r[3][0] = rhs.At(3, 0);
+        r[3][1] = rhs.At(3, 1);
+        r[3][2] = rhs.At(3, 2);
+        r[3][3] = rhs.At(3, 3);
+        return r;
+    }
+
+    Vector2 Matrix3x3::operator*(const Vector2 &rhs) const {
+        return Transform(rhs);
+    }
+
+}
+

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -0,0 +1,709 @@
+#include <J3ML/LinearAlgebra/Matrix4x4.h>
+#include <J3ML/LinearAlgebra/Vector4.h>
+
+namespace J3ML::LinearAlgebra {
+
+
+
+    const Matrix4x4 Matrix4x4::Zero = Matrix4x4(0.f);
+    const Matrix4x4 Matrix4x4::Identity = Matrix4x4({1,0,0,0}, {0,1,0,0}, {0,0,1,0}, {0,0,0,1});
+    const Matrix4x4 Matrix4x4::NaN = Matrix4x4(NAN);
+
+    Matrix4x4::Matrix4x4(const Vector4 &r1, const Vector4 &r2, const Vector4 &r3, const Vector4 &r4) {
+        this->elems[0][0] = r1.x;
+        this->elems[0][1] = r1.y;
+        this->elems[0][2] = r1.z;
+        this->elems[0][3] = r1.w;
+
+        this->elems[1][0] = r2.x;
+        this->elems[1][1] = r2.y;
+        this->elems[1][2] = r2.z;
+        this->elems[1][3] = r2.w;
+
+        this->elems[2][0] = r3.x;
+        this->elems[2][1] = r3.y;
+        this->elems[2][2] = r3.z;
+        this->elems[2][3] = r3.w;
+
+        this->elems[3][0] = r4.x;
+        this->elems[3][1] = r4.y;
+        this->elems[3][2] = r4.z;
+        this->elems[3][3] = r4.w;
+    }
+
+    Matrix4x4::Matrix4x4(float val) {
+        this->elems[0][0] = val;
+        this->elems[0][1] = val;
+        this->elems[0][2] = val;
+        this->elems[0][3] = val;
+
+        this->elems[1][0] = val;
+        this->elems[1][1] = val;
+        this->elems[1][2] = val;
+        this->elems[1][3] = val;
+
+        this->elems[2][0] = val;
+        this->elems[2][1] = val;
+        this->elems[2][2] = val;
+        this->elems[2][3] = val;
+
+        this->elems[3][0] = val;
+        this->elems[3][1] = val;
+        this->elems[3][2] = val;
+        this->elems[3][3] = val;
+    }
+
+    Matrix4x4::Matrix4x4(float m00, float m01, float m02, float m03, float m10, float m11, float m12, float m13,
+                         float m20, float m21, float m22, float m23, float m30, float m31, float m32, float m33) {
+        this->elems[0][0] = m00;
+        this->elems[0][1] = m01;
+        this->elems[0][2] = m02;
+        this->elems[0][3] = m03;
+
+        this->elems[1][0] = m10;
+        this->elems[1][1] = m11;
+        this->elems[1][2] = m12;
+        this->elems[1][3] = m13;
+
+        this->elems[2][0] = m20;
+        this->elems[2][1] = m21;
+        this->elems[2][2] = m22;
+        this->elems[2][3] = m23;
+
+        this->elems[3][0] = m30;
+        this->elems[3][1] = m31;
+        this->elems[3][2] = m32;
+        this->elems[3][3] = m33;
+
+    }
+
+    Matrix4x4::Matrix4x4(const Quaternion &orientation) {
+
+    }
+
+    void Matrix4x4::SetTranslatePart(float translateX, float translateY, float translateZ) {
+        elems[0][3] = translateX;
+        elems[1][3] = translateY;
+        elems[2][3] = translateZ;
+    }
+
+    void Matrix4x4::SetTranslatePart(const Vector3 &offset) {
+        elems[0][3] = offset.x;
+        elems[1][3] = offset.y;
+        elems[2][3] = offset.z;
+    }
+
+
+    void Matrix4x4::SetRotatePart(const Quaternion& q)
+    {
+        // See e.g. http://www.geometrictools.com/Documentation/LinearAlgebraicQuaternions.pdf .
+        const float x = q.x;
+        const float y = q.y;
+        const float z = q.z;
+        const float w = q.w;
+        elems[0][0] = 1 - 2*(y*y + z*z); elems[0][1] =     2*(x*y - z*w); elems[0][2] =     2*(x*z + y*w);
+        elems[1][0] =     2*(x*y + z*w); elems[1][1] = 1 - 2*(x*x + z*z); elems[1][2] =     2*(y*z - x*w);
+        elems[2][0] =     2*(x*z - y*w); elems[2][1] =     2*(y*z + x*w); elems[2][2] = 1 - 2*(x*x + y*y);
+    }
+
+    void Matrix4x4::Set3x3Part(const Matrix3x3& r)
+    {
+        At(0, 0) = r[0][0]; At(0, 1) = r[0][1]; At(0, 2) = r[0][2];
+        At(1, 0) = r[1][0]; At(1, 1) = r[1][1]; At(1, 2) = r[1][2];
+        At(2, 0) = r[2][0]; At(2, 1) = r[2][1]; At(2, 2) = r[2][2];
+    }
+
+
+
+    void Matrix4x4::SetRow(int row, const Vector3 &rowVector, float m_r3) {
+        SetRow(row, rowVector.x, rowVector.y, rowVector.z, m_r3);
+    }
+
+    void Matrix4x4::SetRow(int row, const Vector4 &rowVector) {
+        SetRow(row, rowVector.x, rowVector.y, rowVector.z, rowVector.w);
+    }
+
+    void Matrix4x4::SetRow(int row, float m_r0, float m_r1, float m_r2, float m_r3) {
+        elems[row][0] = m_r0;
+        elems[row][1] = m_r1;
+        elems[row][2] = m_r2;
+        elems[row][3] = m_r3;
+    }
+
+    Matrix4x4::Matrix4x4(const Quaternion &orientation, const Vector3 &translation) {
+        SetRotatePart(orientation);
+        SetTranslatePart(translation);
+        SetRow(3, 0, 0, 0, 1);
+    }
+
+    Matrix4x4 Matrix4x4::D3DOrthoProjLH(float n, float f, float h, float v) {
+        float p00 = 2.f / h; float p01 = 0;       float p02 = 0;           float p03 = 0.f;
+        float p10 = 0;       float p11 = 2.f / v; float p12 = 0;           float p13 = 0.f;
+        float p20 = 0;       float p21 = 0;       float p22 = 1.f / (f-n); float p23 = n / (n-f);
+        float p30 = 0;       float p31 = 0;       float p32 = 0.f;         float p33 = 1.f;
+
+        return {p00, p01, p02, p03, p10, p11, p12, p13, p20, p21, p22, p23, p30, p31, p32, p33};
+    }
+
+    /** This function generates an orthographic projection matrix that maps from
+	the Direct3D view space to the Direct3D normalized viewport space as follows:
+
+	In Direct3D view space, we assume that the camera is positioned at the origin (0,0,0).
+	The camera looks directly towards the positive Z axis (0,0,1).
+	The -X axis spans to the left of the screen, +X goes to the right.
+	-Y goes to the bottom of the screen, +Y goes to the top.
+
+	After the transformation, we're in the Direct3D normalized viewport space as follows:
+
+	(-1,-1,0) is the bottom-left corner of the viewport at the near plane.
+	(1,1,0) is the top-right corner of the viewport at the near plane.
+	(0,0,0) is the center point at the near plane.
+	Coordinates with z=1 are at the far plane.
+
+	Examples:
+		(0,0,n) maps to (0,0,0).
+		(0,0,f) maps to (0,0,1).
+		(-h/2, -v/2, n) maps to (-1, -1, 0).
+		(h/2, v/2, f) maps to (1, 1, 1).
+	*/
+    Matrix4x4 Matrix4x4::D3DOrthoProjRH(float n, float f, float h, float v)
+    {
+        // D3DOrthoProjLH and D3DOrthoProjRH differ from each other in that the third column is negated.
+        // This corresponds to LH = RH * In, where In is a diagonal matrix with elements [1 1 -1 1].
+        float p00 = 2.f / h; float p01 = 0;       float p02 = 0;           float p03 = 0.f;
+        float p10 = 0;       float p11 = 2.f / v; float p12 = 0;           float p13 = 0.f;
+        float p20 = 0;       float p21 = 0;       float p22 = 1.f / (n-f); float p23 = n / (n-f);
+        float p30 = 0;       float p31 = 0;       float p32 = 0.f;         float p33 = 1.f;
+    }
+
+    float Matrix4x4::At(int x, int y) const {
+        return elems[x][y];
+    }
+
+    Matrix4x4 Matrix4x4::operator*(const Matrix4x4 &rhs) const {
+
+        float r00 = At(0, 0) * rhs.At(0, 0) + At(0, 1) * rhs.At(1, 0) + At(0, 2) * rhs.At(2, 0) + At(0, 3) * rhs.At(3, 0);
+        float r01 = At(0, 0) * rhs.At(0, 1) + At(0, 1) * rhs.At(1, 1) + At(0, 2) * rhs.At(2, 1) + At(0, 3) * rhs.At(3, 1);
+        float r02 = At(0, 0) * rhs.At(0, 2) + At(0, 1) * rhs.At(1, 2) + At(0, 2) * rhs.At(2, 2) + At(0, 3) * rhs.At(3, 2);
+        float r03 = At(0, 0) * rhs.At(0, 3) + At(0, 1) * rhs.At(1, 3) + At(0, 2) * rhs.At(2, 3) + At(0, 3) * rhs.At(3, 3);
+
+        float r10 = At(1, 0) * rhs.At(0, 0) + At(1, 1) * rhs.At(1, 0) + At(1, 2) * rhs.At(2, 0) + At(1, 3) * rhs.At(3, 0);
+        float r11 = At(1, 0) * rhs.At(0, 1) + At(1, 1) * rhs.At(1, 1) + At(1, 2) * rhs.At(2, 1) + At(1, 3) * rhs.At(3, 1);
+        float r12 = At(1, 0) * rhs.At(0, 2) + At(1, 1) * rhs.At(1, 2) + At(1, 2) * rhs.At(2, 2) + At(1, 3) * rhs.At(3, 2);
+        float r13 = At(1, 0) * rhs.At(0, 3) + At(1, 1) * rhs.At(1, 3) + At(1, 2) * rhs.At(2, 3) + At(1, 3) * rhs.At(3, 3);
+
+        float r20 = At(2, 0) * rhs.At(0, 0) + At(2, 1) * rhs.At(1, 0) + At(2, 2) * rhs.At(2, 0) + At(2, 3) * rhs.At(3, 0);
+        float r21 = At(2, 0) * rhs.At(0, 1) + At(2, 1) * rhs.At(1, 1) + At(2, 2) * rhs.At(2, 1) + At(2, 3) * rhs.At(3, 1);
+        float r22 = At(2, 0) * rhs.At(0, 2) + At(2, 1) * rhs.At(1, 2) + At(2, 2) * rhs.At(2, 2) + At(2, 3) * rhs.At(3, 2);
+        float r23 = At(2, 0) * rhs.At(0, 3) + At(2, 1) * rhs.At(1, 3) + At(2, 2) * rhs.At(2, 3) + At(2, 3) * rhs.At(3, 3);
+
+        float r30 = At(3, 0) * rhs.At(0, 0) + At(3, 1) * rhs.At(1, 0) + At(3, 2) * rhs.At(2, 0) + At(3, 3) * rhs.At(3, 0);
+        float r31 = At(3, 0) * rhs.At(0, 1) + At(3, 1) * rhs.At(1, 1) + At(3, 2) * rhs.At(2, 1) + At(3, 3) * rhs.At(3, 1);
+        float r32 = At(3, 0) * rhs.At(0, 2) + At(3, 1) * rhs.At(1, 2) + At(3, 2) * rhs.At(2, 2) + At(3, 3) * rhs.At(3, 2);
+        float r33 = At(3, 0) * rhs.At(0, 3) + At(3, 1) * rhs.At(1, 3) + At(3, 2) * rhs.At(2, 3) + At(3, 3) * rhs.At(3, 3);
+        return {r00,r01,r02,r03, r10, r11, r12, r13, r20,r21,r22,r23, r30,r31,r32,r33};
+    }
+
+    Vector4 Matrix4x4::operator[](int row) {
+        return Vector4{elems[row][0], elems[row][1], elems[row][2], elems[row][3]};
+    }
+
+    Matrix4x4 Matrix4x4::operator*(const Matrix3x3 &rhs) const {
+        float r00 = At(0, 0) * rhs.At(0, 0) + At(0, 1) * rhs.At(1, 0) + At(0, 2) * rhs.At(2, 0);
+        float r01 = At(0, 0) * rhs.At(0, 1) + At(0, 1) * rhs.At(1, 1) + At(0, 2) * rhs.At(2, 1);
+        float r02 = At(0, 0) * rhs.At(0, 2) + At(0, 1) * rhs.At(1, 2) + At(0, 2) * rhs.At(2, 2);
+        float r03 = At(0, 3);
+
+        float r10 = At(1, 0) * rhs.At(0, 0) + At(1, 1) * rhs.At(1, 0) + At(1, 2) * rhs.At(2, 0);
+        float r11 = At(1, 0) * rhs.At(0, 1) + At(1, 1) * rhs.At(1, 1) + At(1, 2) * rhs.At(2, 1);
+        float r12 = At(1, 0) * rhs.At(0, 2) + At(1, 1) * rhs.At(1, 2) + At(1, 2) * rhs.At(2, 2);
+        float r13 = At(1, 3);
+
+        float r20 = At(2, 0) * rhs.At(0, 0) + At(2, 1) * rhs.At(1, 0) + At(2, 2) * rhs.At(2, 0);
+        float r21 = At(2, 0) * rhs.At(0, 1) + At(2, 1) * rhs.At(1, 1) + At(2, 2) * rhs.At(2, 1);
+        float r22 = At(2, 0) * rhs.At(0, 2) + At(2, 1) * rhs.At(1, 2) + At(2, 2) * rhs.At(2, 2);
+        float r23 = At(2, 3);
+
+        float r30 = At(3, 0) * rhs.At(0, 0) + At(3, 1) * rhs.At(1, 0) + At(3, 2) * rhs.At(2, 0);
+        float r31 = At(3, 0) * rhs.At(0, 1) + At(3, 1) * rhs.At(1, 1) + At(3, 2) * rhs.At(2, 1);
+        float r32 = At(3, 0) * rhs.At(0, 2) + At(3, 1) * rhs.At(1, 2) + At(3, 2) * rhs.At(2, 2);
+        float r33 = At(3, 3);
+
+        return {r00,r01,r02,r03, r10, r11, r12, r13, r20,r21,r22,r23, r30,r31,r32,r33};
+    }
+
+    Matrix4x4 Matrix4x4::operator+() const { return *this; }
+
+    Matrix4x4 Matrix4x4::FromTranslation(const Vector3 &rhs) {
+        return Matrix4x4(1.f, 0, 0, rhs.x,
+                         0, 1.f, 0, rhs.y,
+                         0, 0, 1.f, rhs.z,
+                         0, 0, 0,     1.f);
+    }
+
+    Matrix4x4 Matrix4x4::Translate(const Vector3 &rhs) const {
+        return *this * FromTranslation(rhs);
+    }
+
+    Vector3 Matrix4x4::Transform(const Vector3 &rhs) const {
+        return Transform(rhs.x, rhs.y, rhs.z);
+    }
+
+    Vector3 Matrix4x4::Transform(float tx, float ty, float tz) const {
+        return Vector3(At(0, 0) * tx + At(0, 1) * ty + At(0, 2) * tz + At(0, 3),
+                       At(1, 0) * tx + At(1, 1) * ty + At(1, 2) * tz + At(1, 3),
+                       At(2, 0) * tx + At(2, 1) * ty + At(2, 2) * tz + At(2, 3));
+    }
+
+    Vector2 Matrix4x4::Transform(float tx, float ty) const {
+        return Vector2(At(0, 0) * tx + At(0, 1) * ty + At(0, 2) + At(0, 3),
+                       At(1, 0) * tx + At(1, 1) * ty + At(1, 2) + At(1, 3));
+    }
+
+    Vector2 Matrix4x4::Transform(const Vector2 &rhs) const {
+        return Transform(rhs.x, rhs.y);
+    }
+
+    Matrix4x4 &Matrix4x4::operator=(const Matrix3x3 &rhs) {
+        Set3x3Part(rhs);
+        SetTranslatePart(0,0,0);
+        SetRow(3, 0, 0, 0, 1);
+        return *this;
+    }
+
+    Matrix4x4 &Matrix4x4::operator=(const Quaternion &rhs) {
+        *this = rhs.ToMatrix4x4();
+        return *this;
+    }
+
+    float &Matrix4x4::At(int row, int col) {
+        return elems[row][col];
+    }
+
+    Matrix4x4 Matrix4x4::Inverse() const {
+        // Compute the inverse directly using Cramer's rule
+        // Warning: This method is numerically very unstable!
+        float d = Determinant();
+
+        d = 1.f / d;
+
+        float a11 = At(0, 0);float a12 = At(0, 1);float a13 = At(0, 2);float a14 = At(0, 3);
+        float a21 = At(1, 0);float a22 = At(1, 1);float a23 = At(1, 2);float a24 = At(1, 3);
+        float a31 = At(2, 0);float a32 = At(2, 1);float a33 = At(2, 2);float a34 = At(2, 3);
+        float a41 = At(3, 0);float a42 = At(3, 1);float a43 = At(3, 2);float a44 = At(3, 3);
+
+        Matrix4x4 i = {
+                d * (a22*a33*a44 + a23*a34*a42 + a24*a32*a43 - a22*a34*a43 - a23*a32*a44 - a24*a33*a42),
+                d * (a12*a34*a43 + a13*a32*a44 + a14*a33*a42 - a12*a33*a44 - a13*a34*a42 - a14*a32*a43),
+                d * (a12*a23*a44 + a13*a24*a42 + a14*a22*a43 - a12*a24*a43 - a13*a22*a44 - a14*a23*a42),
+                d * (a12*a24*a33 + a13*a22*a34 + a14*a23*a32 - a12*a23*a34 - a13*a24*a32 - a14*a22*a33),
+                d * (a21*a34*a43 + a23*a31*a44 + a24*a33*a41 - a21*a33*a44 - a23*a34*a41 - a24*a31*a43),
+                d * (a11*a33*a44 + a13*a34*a41 + a14*a31*a43 - a11*a34*a43 - a13*a31*a44 - a14*a33*a41),
+                d * (a11*a24*a43 + a13*a21*a44 + a14*a23*a41 - a11*a23*a44 - a13*a24*a41 - a14*a21*a43),
+                d * (a11*a23*a34 + a13*a24*a31 + a14*a21*a33 - a11*a24*a33 - a13*a21*a34 - a14*a23*a31),
+                d * (a21*a32*a44 + a22*a34*a41 + a24*a31*a42 - a21*a34*a42 - a22*a31*a44 - a24*a32*a41),
+                d * (a11*a34*a42 + a12*a31*a44 + a14*a32*a41 - a11*a32*a44 - a12*a34*a41 - a14*a31*a42),
+                d * (a11*a22*a44 + a12*a24*a41 + a14*a21*a42 - a11*a24*a42 - a12*a21*a44 - a14*a22*a41),
+                d * (a11*a24*a32 + a12*a21*a34 + a14*a22*a31 - a11*a22*a34 - a12*a24*a31 - a14*a21*a32),
+                d * (a21*a33*a42 + a22*a31*a43 + a23*a32*a41 - a21*a32*a43 - a22*a33*a41 - a23*a31*a42),
+                d * (a11*a32*a43 + a12*a33*a41 + a13*a31*a42 - a11*a33*a42 - a12*a31*a43 - a13*a32*a41),
+                d * (a11*a23*a42 + a12*a21*a43 + a13*a22*a41 - a11*a22*a43 - a12*a23*a41 - a13*a21*a42),
+                d * (a11*a22*a33 + a12*a23*a31 + a13*a21*a32 - a11*a23*a32 - a12*a21*a33 - a13*a22*a31)
+        };
+        return i;
+    }
+
+    float Matrix4x4::Minor(int i, int j) const {
+        int r0 = SKIPNUM(0, i);
+        int r1 = SKIPNUM(1, i);
+        int r2 = SKIPNUM(2, i);
+        int c0 = SKIPNUM(0, j);
+        int c1 = SKIPNUM(1, j);
+        int c2 = SKIPNUM(2, j);
+
+        float a = At(r0, c0);
+        float b = At(r0, c1);
+        float c = At(r0, c2);
+        float d = At(r1, c0);
+        float e = At(r1, c1);
+        float f = At(r1, c2);
+        float g = At(r2, c0);
+        float h = At(r2, c1);
+        float k = At(r2, c2);
+
+        return a*e*k + b*f*g + c*d*h - a*f*h - b*d*k - c*e*g;
+    }
+
+    float Matrix4x4::Determinant() const {
+        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];
+        const float b = elems[0][1];
+        const float c = elems[0][2];
+        const float d = elems[1][0];
+        const float e = elems[1][1];
+        const float f = elems[1][2];
+        const float g = elems[2][0];
+        const float h = elems[2][1];
+        const float i = elems[2][2];
+
+        return a*e*i + b*f*g + c*d*h - a*f*h - b*d*i - c*e*g;
+    }
+
+    Matrix3x3 Matrix4x4::GetRotatePart() const {
+        return Matrix3x3 {
+                At(0, 0), At(0, 1), At(0, 2),
+                At(1, 0), At(1, 1), At(1, 2),
+                At(2, 0), At(2, 1), At(2, 2)
+        };
+    }
+
+    Matrix4x4 Matrix4x4::Transpose() const {
+        Matrix4x4 copy;
+        copy.elems[0][0] = elems[0][0]; copy.elems[0][1] = elems[1][0]; copy.elems[0][2] = elems[2][0]; copy.elems[0][3] = elems[3][0];
+        copy.elems[1][0] = elems[0][1]; copy.elems[1][1] = elems[1][1]; copy.elems[1][2] = elems[2][1]; copy.elems[1][3] = elems[3][1];
+        copy.elems[2][0] = elems[0][2]; copy.elems[2][1] = elems[1][2]; copy.elems[2][2] = elems[2][2]; copy.elems[2][3] = elems[3][2];
+        copy.elems[3][0] = elems[0][3]; copy.elems[3][1] = elems[1][3]; copy.elems[3][2] = elems[2][3]; copy.elems[3][3] = elems[3][3];
+        return copy;
+    }
+
+    Vector4 Matrix4x4::Diagonal() const {
+        return Vector4{At(0, 0), At(1,1), At(2,2), At(3,3)};
+    }
+
+    Vector3 Matrix4x4::Diagonal3() const {
+        return Vector3 { At(0, 0), At(1,1), At(2,2) };
+    }
+
+    Vector3 Matrix4x4::WorldX() const {
+        return GetColumn3(0);
+    }
+
+    Vector3 Matrix4x4::WorldY() const {
+        return GetColumn3(1);
+    }
+
+    Vector3 Matrix4x4::WorldZ() const {
+        return GetColumn3(2);
+    }
+
+    bool Matrix4x4::IsFinite() const {
+        for(int iy = 0; iy < Rows; ++iy)
+            for(int ix = 0; ix < Cols; ++ix)
+                if (!std::isfinite(elems[iy][ix]))
+                    return false;
+        return true;
+    }
+
+    Vector3 Matrix4x4::GetColumn3(int index) const {
+        return Vector3{At(0, index), At(1, index), At(2, index)};
+    }
+
+    Vector2 Matrix4x4::operator*(const Vector2 &rhs) const { return this->Transform(rhs);}
+
+    Vector3 Matrix4x4::operator*(const Vector3 &rhs) const { return this->Transform(rhs);}
+
+    Vector4 Matrix4x4::operator*(const Vector4 &rhs) const { return this->Transform(rhs);}
+
+    Vector4 Matrix4x4::Transform(float tx, float ty, float tz, float tw) const {
+        return Transform({tx, ty, tz, tw});
+    }
+
+    Vector4 Matrix4x4::Transform(const Vector4 &rhs) const {
+        return Vector4(At(0, 0) * rhs.x + At(0, 1) * rhs.y + At(0, 2) * rhs.z + At(0, 3) * rhs.w,
+                       At(1, 0) * rhs.x + At(1, 1) * rhs.y + At(1, 2) * rhs.z + At(1, 3) * rhs.w,
+                       At(2, 0) * rhs.x + At(2, 1) * rhs.y + At(2, 2) * rhs.z + At(2, 3) * rhs.w,
+                       At(3, 0) * rhs.x + At(3, 1) * rhs.y + At(3, 2) * rhs.z + At(3, 3) * rhs.w);
+    }
+
+    Vector3 Matrix4x4::GetTranslatePart() const {
+        return GetColumn3(3);
+    }
+
+    Matrix4x4 Matrix4x4::Scale(const Vector3& scale)
+    {
+        auto mat = *this;
+
+        mat.At(3, 0) *= scale.x;
+        mat.At(3, 1) *= scale.y;
+        mat.At(3, 2) *= scale.z;
+        return mat;
+    }
+
+    Matrix4x4
+    Matrix4x4::LookAt(const Vector3 &localFwd, const Vector3 &targetDir, const Vector3 &localUp, const Vector3 &worldUp) {
+        Matrix4x4 m;
+        m.Set3x3Part(Matrix3x3::LookAt(localFwd, targetDir, localUp, worldUp));
+        m.SetTranslatePart(0,0,0);
+        m.SetRow(3, 0,0,0,1);
+        return m;
+    }
+
+    Vector4 Matrix4x4::GetRow(int index) const {
+        return { At(index, 0), At(index, 1), At(index, 2), At(index, 3)};
+    }
+
+    Vector4 Matrix4x4::GetColumn(int index) const {
+        return { At(0, index), At(1, index), At(2, index), At(3, index)};
+    }
+
+    Vector3 Matrix4x4::GetRow3(int index) const {
+        return Vector3{ At(index, 0), At(index, 1), At(index, 2)};
+    }
+
+    void Matrix4x4::SwapColumns(int col1, int col2) {
+        Swap(At(0, col1), At(0, col2));
+        Swap(At(1, col1), At(1, col2));
+        Swap(At(2, col1), At(2, col2));
+        Swap(At(3, col1), At(3, col2));
+    }
+
+    void Matrix4x4::SwapRows(int row1, int row2) {
+        Swap(At(row1, 0), At(row2, 0));
+        Swap(At(row1, 1), At(row2, 1));
+        Swap(At(row1, 2), At(row2, 2));
+        Swap(At(row1, 3), At(row2, 3));
+    }
+
+    void Matrix4x4::SwapRows3(int row1, int row2) {
+        Swap(At(row1, 0), At(row2, 0));
+        Swap(At(row1, 1), At(row2, 1));
+        Swap(At(row1, 2), At(row2, 2));
+    }
+
+    void Matrix4x4::Pivot() {
+        int rowIndex = 0;
+
+        for(int col = 0; col < Cols; ++col)
+        {
+            int greatest = rowIndex;
+
+            // find the rowIndex k with k >= 1 for which Mkj has the largest absolute value.
+            for(int i = rowIndex; i < Rows; ++i)
+                if (std::abs(At(i, col)) > std::abs(At(greatest, col)))
+                    greatest = i;
+
+            if (std::abs(At(greatest, col)) != 0)
+            {
+                if (rowIndex != greatest)
+                    SwapRows(rowIndex, greatest); // the greatest now in rowIndex
+
+                ScaleRow(rowIndex, 1.f/At(rowIndex, col));
+
+                for(int r = 0; r < Rows; ++r)
+                    if (r != rowIndex)
+                        SetRow(r, GetRow(r) - GetRow(rowIndex) * At(r, col));
+
+                ++rowIndex;
+            }
+        }
+    }
+
+    void Matrix4x4::ScaleColumn3(int col, float scalar) {
+        At(0, col) *= scalar;
+        At(1, col) *= scalar;
+        At(2, col) *= scalar;
+    }
+
+    void Matrix4x4::ScaleColumn(int col, float scalar) {
+        At(0, col) *= scalar;
+        At(1, col) *= scalar;
+        At(2, col) *= scalar;
+        At(3, col) *= scalar;
+    }
+
+    void Matrix4x4::ScaleRow3(int row, float scalar) {
+        At(row, 0) *= scalar;
+        At(row, 1) *= scalar;
+        At(row, 2) *= scalar;
+    }
+
+    void Matrix4x4::ScaleRow(int row, float scalar) {
+        At(row, 0) *= scalar;
+        At(row, 1) *= scalar;
+        At(row, 2) *= scalar;
+        At(row, 3) *= scalar;
+    }
+
+    Matrix4x4 Matrix4x4::OpenGLOrthoProjLH(float n, float f, float h, float v) {
+        /// Same as OpenGLOrthoProjRH, except that the camera looks towards +Z in view space, instead of -Z.
+        using f32 = float;
+        f32 p00 = 2.f / h; f32 p01 = 0;           f32 p02 = 0;       float p03 = 0.f;
+        f32 p10 = 0;       f32 p11 = 2.f / v; f32 p12 = 0;           float p13 = 0.f;
+        f32 p20 = 0;       f32 p21 = 0;       f32 p22 = 2.f / (f-n); float p23 = (f+n) / (n-f);
+        f32 p30 = 0;       f32 p31 = 0;       f32 p32 = 0;           float p33 = 1.f;
+
+        return {p00, p01, p02, p03, p10, p11, p12, p13, p20, p21, p22, p23, p30, p31, p32, p33};
+
+    }
+
+    Matrix4x4 Matrix4x4::OpenGLOrthoProjRH(float n, float f, float h, float v) {
+        using f32 = float;
+        f32 p00 = 2.f / h; f32 p01 = 0;       f32 p02 = 0;           f32 p03 = 0.f;
+        f32 p10 = 0;       f32 p11 = 2.f / v; f32 p12 = 0;           f32 p13 = 0.f;
+        f32 p20 = 0;       f32 p21 = 0;       f32 p22 = 2.f / (n-f); f32 p23 = (f+n) / (n-f);
+        f32 p30 = 0;       f32 p31 = 0;       f32 p32 = 0;           f32 p33 = 1.f;
+
+        return {p00, p01, p02, p03, p10, p11, p12, p13, p20, p21, p22, p23, p30, p31, p32, p33};
+    }
+
+    Matrix4x4 Matrix4x4::OpenGLPerspProjLH(float n, float f, float h, float v) {
+        // Same as OpenGLPerspProjRH, except that the camera looks towards +Z in view space, instead of -Z.
+        using f32 = float;
+        f32 p00 = 2.f *n / h;  f32 p01 = 0;           f32 p02 = 0;              f32 p03 = 0.f;
+        f32 p10 = 0;           f32 p11 = 2.f * n / v; f32 p12 = 0;              f32 p13 = 0.f;
+        f32 p20 = 0;           f32 p21 = 0;           f32 p22 = (n+f) / (f-n);  f32 p23 = 2.f*n*f / (n-f);
+        f32 p30 = 0;           f32 p31 = 0;           f32 p32 = 1.f;            f32 p33 = 0.f;
+
+        return {p00, p01, p02, p03, p10, p11, p12, p13, p20, p21, p22, p23, p30, p31, p32, p33};
+    }
+
+    Matrix4x4 Matrix4x4::OpenGLPerspProjRH(float n, float f, float h, float v) {
+        // In OpenGL, the post-perspective unit cube ranges in [-1, 1] in all X, Y and Z directions.
+        // See http://www.songho.ca/opengl/gl_projectionmatrix.html , unlike in Direct3D, where the
+        // Z coordinate ranges in [0, 1]. This is the only difference between D3DPerspProjRH and OpenGLPerspProjRH.
+        using f32 = float;
+        float p00 = 2.f *n / h;  float p01 = 0;           float p02 = 0;              float p03 = 0.f;
+        float p10 = 0;           float p11 = 2.f * n / v; float p12 = 0;              float p13 = 0.f;
+        float p20 = 0;           float p21 = 0;           float p22 = (n+f) / (n-f);  float p23 = 2.f*n*f / (n-f);
+        float p30 = 0;           float p31 = 0;           float p32 = -1.f;           float p33 = 0.f;
+
+        return {p00, p01, p02, p03, p10, p11, p12, p13, p20, p21, p22, p23, p30, p31, p32, p33};
+    }
+
+    Matrix4x4::Matrix4x4(const float *data) {
+        assert(data);
+        At(0, 0) = data[0];
+        At(0, 1) = data[1];
+        At(0, 2) = data[2];
+        At(0, 3) = data[3];
+
+        At(1, 0) = data[4];
+        At(1, 1) = data[5];
+        At(1, 2) = data[6];
+        At(1, 3) = data[7];
+
+        At(2, 0) = data[8];
+        At(2, 1) = data[9];
+        At(2, 2) = data[10];
+        At(2, 3) = data[11];
+
+        At(3, 0) = data[12];
+        At(3, 1) = data[13];
+        At(3, 2) = data[14];
+        At(3, 3) = data[15];
+    }
+
+    bool Matrix4x4::ContainsProjection(float epsilon) const {
+        return GetRow(3).Equals(0.f, 0.f, 0.f, 1.f, epsilon) == false;
+    }
+
+    Vector4 Matrix4x4::Mul(const Vector4 &rhs) const {
+        return *this * rhs;
+    }
+
+    Vector3 Matrix4x4::Mul(const Vector3 &rhs) const {
+        return *this * rhs;
+    }
+
+    Vector2 Matrix4x4::Mul(const Vector2 &rhs) const {
+        return *this * rhs;
+    }
+
+    Vector4 Matrix4x4::operator[](int row) const {
+        return GetRow(row);
+    }
+
+    bool Matrix4x4::HasUniformScale(float epsilon) const {
+        Vector3 scale = ExtractScale();
+        return Math::EqualAbs(scale.x, scale.y, epsilon) && Math::EqualAbs(scale.x, scale.z, epsilon);
+    }
+
+    Vector3 Matrix4x4::ExtractScale() const {
+        return {GetColumn3(0).Length(), GetColumn3(1).Length(), GetColumn3(2).Length()};
+    }
+
+    bool Matrix4x4::IsColOrthogonal(float epsilon) const {
+        return IsColOrthogonal3(epsilon);
+    }
+
+    bool Matrix4x4::IsRowOrthogonal(float epsilon) const {
+        return IsRowOrthogonal3(epsilon);
+    }
+
+    bool Matrix4x4::IsColOrthogonal3(float epsilon) const {
+        return GetColumn(0).IsPerpendicular(GetColumn(1), epsilon)
+               && GetColumn(0).IsPerpendicular(GetColumn(2), epsilon)
+               && GetColumn(1).IsPerpendicular(GetColumn(2), epsilon);
+    }
+
+    bool Matrix4x4::IsRowOrthogonal3(float epsilon) const {
+        return GetRow(0).IsPerpendicular(GetRow(1), epsilon)
+               && GetRow(0).IsPerpendicular(GetRow(2), epsilon)
+               && GetRow(1).IsPerpendicular(GetRow(2), epsilon);
+    }
+
+    Vector4 Matrix4x4::Col(int i) const { return GetColumn(i);}
+
+    Vector4 Matrix4x4::Row(int i) const { return GetRow(i);}
+
+    Vector4 Matrix4x4::Col3(int i) const { return GetColumn3(i);}
+
+    Vector4 Matrix4x4::Row3(int i) const { return GetRow3(i);}
+
+    Vector3 Matrix4x4::TransformDir(float tx, float ty, float tz) const
+    {
+        assert(!this->ContainsProjection()); // This function does not divide by w or output it, so cannot have projection.
+        return Vector3(At(0, 0) * tx + At(0, 1) * ty + At(0, 2) * tz,
+                      At(1, 0) * tx + At(1, 1) * ty + At(1, 2) * tz,
+                      At(2, 0) * tx + At(2, 1) * ty + At(2, 2) * tz);
+
+    }
+
+    void Matrix4x4::InverseOrthonormal()
+    {
+        assert(!ContainsProjection());
+
+        // a) Transpose the top-left 3x3 part in-place to produce R^t.
+        Swap(elems[0][1], elems[1][0]);
+        Swap(elems[0][2], elems[2][0]);
+        Swap(elems[1][2], elems[2][1]);
+
+        // b) Replace the top-right 3x1 part by computing R^t(-T).
+        SetTranslatePart(TransformDir(-At(0, 3), -At(1, 3), -At(2, 3)));
+
+    }
+
+    void Matrix4x4::SetCol(int col, const float *data) {
+        assert(data != nullptr);
+        SetCol(col, data[0], data[1], data[2], data[3]);
+    }
+
+    void Matrix4x4::SetCol(int column, float m_0c, float m_1c, float m_2c, float m_3c)
+    {
+        assert(column >= 0);
+        assert(column < Cols);
+        assert(std::isfinite(m_0c));
+        assert(std::isfinite(m_1c));
+        assert(std::isfinite(m_2c));
+        assert(std::isfinite(m_3c));
+        At(0, column) = m_0c;
+        At(1, column) = m_1c;
+        At(2, column) = m_2c;
+        At(3, column) = m_3c;
+    }
+
+    void Matrix4x4::SetCol(int column, const Vector3 &columnVector, float m_3c)
+    {
+        SetCol(column, columnVector.x, columnVector.y, columnVector.z, m_3c);
+    }
+
+    void Matrix4x4::SetCol(int column, const Vector4 &columnVector)
+    {
+        SetCol(column, columnVector.x, columnVector.y, columnVector.z, columnVector.w);
+    }
+
+}

src/J3ML/LinearAlgebra/Quaternion.cpp

@@ -0,0 +1,204 @@
+#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>
+
+namespace J3ML::LinearAlgebra {
+    Quaternion Quaternion::operator-() const
+    {
+        return {-x, -y, -z, -w};
+    }
+
+    Quaternion::Quaternion(const Matrix3x3 &rotationMtrx) {}
+
+    Quaternion::Quaternion(const Matrix4x4 &rotationMtrx) {}
+
+    Vector3 Quaternion::GetWorldX() const { return Transform(1.f, 0.f, 0.f); }
+
+    Vector3 Quaternion::GetWorldY() const { return Transform(0.f, 1.f, 0.f); }
+
+    Vector3 Quaternion::GetWorldZ() const { return Transform(0.f, 0.f, 1.f); }
+
+    Vector3 Quaternion::Transform(const Vector3 &vec) const {
+        Matrix3x3 mat = this->ToMatrix3x3();
+        return mat * vec;
+    }
+
+    Vector3 Quaternion::Transform(float X, float Y, float Z) const {
+        return Transform(Vector3{X, Y, Z});
+    }
+
+    Vector4 Quaternion::Transform(const Vector4 &vec) const {
+        return Vector4(Transform(vec.x, vec.y, vec.z), vec.w);
+    }
+
+    Vector4 Quaternion::Transform(float X, float Y, float Z, float W) const {
+        return Transform(Vector4(X, Y, Z, W));
+    }
+
+    Quaternion Quaternion::Lerp(const Quaternion &b, float t) const {
+        float angle = this->Dot(b);
+        if (angle >= 0.f) // Make sure we rotate the shorter arc
+            return (*this * (1.f - t) + b * t).Normalize();
+        else
+            return (*this * (t - 1.f) + b * t).Normalize();
+    }
+
+    void Quaternion::SetFromAxisAngle(const Vector3 &axis, float angle) {
+        float sinz, cosz;
+        sinz = std::sin(angle*0.5f);
+        cosz = std::cos(angle*0.5f);
+
+        x = axis.x * sinz;
+        y = axis.y * sinz;
+        z = axis.z * sinz;
+        w = cosz;
+    }
+
+    void Quaternion::SetFromAxisAngle(const Vector4 &axis, float angle)
+    {
+        SetFromAxisAngle(Vector3(axis.x, axis.y, axis.z), angle);
+    }
+
+    Quaternion Quaternion::operator*(float scalar) const {
+        return Quaternion(x * scalar, y * scalar, z * scalar, w * scalar);
+    }
+
+    Quaternion Quaternion::operator+() const { return *this; }
+
+    Quaternion::Quaternion() {}
+
+    Quaternion::Quaternion(float X, float Y, float Z, float W) : Vector4(X,Y,Z,W) {}
+
+    // TODO: implement
+    float Quaternion::Dot(const Quaternion &rhs) const {
+        return x * rhs.x + y * rhs.y + z * rhs.z + w * rhs.w;
+    }
+
+    Quaternion::Quaternion(Vector4 vector4) {
+
+    }
+
+    Quaternion Quaternion::Normalize() const {
+        float length = Length();
+        if (length < 1e-4f)
+            return {0,0,0,0};
+        float reciprocal = 1.f / length;
+        return {
+                x * reciprocal,
+                y * reciprocal,
+                z * reciprocal,
+                w * reciprocal
+        };
+    }
+
+    Quaternion Quaternion::Conjugate() const {
+        return { -x, -y, -z, w };
+    }
+
+    Quaternion Quaternion::Inverse() const {
+        return Conjugate();
+    }
+
+    Quaternion Quaternion::Slerp(const Quaternion &q2, float t) const {
+        float angle = this->Dot(q2);
+        float sign = 1.f;
+        if (angle < 0.f)
+        {
+            angle = -angle;
+            sign = -1.f;
+        }
+
+        float a;
+        float b;
+        if (angle < 0.999)
+        {
+            // angle = Acos(angle); // After this, angle is in the range pi/2 -> 0 as the original angle variable ranged from 0 -> 1.
+            angle = (-0.69813170079773212f * angle * angle - 0.87266462599716477f) * angle + 1.5707963267948966f;
+
+            float ta = t*angle;
+            // Not using a lookup table, manually compute the two sines by using a very rough approximation.
+            float ta2 = ta*ta;
+            b = ((5.64311797634681035370e-03f * ta2 - 1.55271410633428644799e-01f) * ta2 + 9.87862135574673806965e-01f) * ta;
+            a = angle - ta;
+            float a2 = a*a;
+            a = ((5.64311797634681035370e-03f * a2 - 1.55271410633428644799e-01f) * a2 + 9.87862135574673806965e-01f) * a;
+        }
+        else // If angle is close to taking the denominator to zero, resort to linear interpolation (and normalization).
+        {
+            a = 1.f - t;
+            b = t;
+        }
+        // Lerp and renormalize.
+        return (*this * (a * sign) + q2 * b).Normalize();
+    }
+
+    AxisAngle Quaternion::ToAxisAngle() const {
+        float halfAngle = std::acos(w);
+        float angle = halfAngle * 2.f;
+        // TODO: Can Implement Fast Inverse Sqrt Here
+        float reciprocalSinAngle = 1.f / std::sqrt(1.f - w*w);
+
+        Vector3 axis = {
+                x*reciprocalSinAngle,
+                y*reciprocalSinAngle,
+                z*reciprocalSinAngle
+        };
+        return AxisAngle(axis, angle);
+    }
+
+    float Quaternion::AngleBetween(const Quaternion &target) const {
+        Quaternion delta = target / *this;
+        return delta.Normalize().Angle();
+    }
+
+    Quaternion Quaternion::operator/(const Quaternion &rhs) const {
+        return {
+                x*rhs.w - y*rhs.z + z*rhs.y - w*rhs.x,
+                x*rhs.z + y*rhs.w - z*rhs.x - w*rhs.y,
+                -x*rhs.y + y*rhs.x + z*rhs.w - w*rhs.z,
+                x*rhs.x + y*rhs.y + z*rhs.z + w*rhs.w
+        };
+    }
+
+    Matrix3x3 Quaternion::ToMatrix3x3() const {
+        return {
+                1 - 2 *(y*y) - 2*(z*z), 2*x*y - 2*z*w, 2*x*z + 2*y*w,
+                2*x*y + 2*z*w, 1-2*x*x - 2*z*z, 2*y*z - 2*x*w,
+                2*x*z - 2*y*w, 2*y*z + 2*x*w, 1-2*x*x - 2*y*y
+        };
+    }
+
+    Quaternion Quaternion::operator+(const Quaternion &rhs) const {
+        return {
+                x + rhs.x, y + rhs.y, z + rhs.z,w + rhs.w
+        };
+    }
+
+    Matrix4x4 Quaternion::ToMatrix4x4() const {
+        return Matrix4x4(*this);
+    }
+
+    Matrix4x4 Quaternion::ToMatrix4x4(const Vector3 &translation) const {
+        return {*this, translation};
+    }
+
+    float Quaternion::GetAngle() const {
+        return std::acos(w) * 2.f;
+    }
+
+    Vector3 Quaternion::GetAxis() const {
+        float rcpSinAngle = 1 - (std::sqrt(1 - w * w));
+
+        return Vector3(x, y, z) * rcpSinAngle;
+    }
+
+    Quaternion::Quaternion(const Vector3 &rotationAxis, float rotationAngleBetween) {
+        SetFromAxisAngle(rotationAxis, rotationAngleBetween);
+    }
+
+    Quaternion::Quaternion(const Vector4 &rotationAxis, float rotationAngleBetween) {
+        SetFromAxisAngle(rotationAxis, rotationAngleBetween);
+    }
+}

src/J3ML/LinearAlgebra/Transform2D.cpp

@@ -0,0 +1,30 @@
+#include <J3ML/LinearAlgebra/Transform2D.h>
+
+namespace J3ML::LinearAlgebra {
+
+    const Transform2D Transform2D::Identity = Transform2D({0, 0}, {1, 1}, {0,0}, {0,0}, 0);
+    const Transform2D Transform2D::FlipX    = Transform2D({0, 0}, {-1, 1}, {0,0}, {0,0}, 0);
+    const Transform2D Transform2D::FlipY    = Transform2D({0, 0}, {1, -1}, {0,0}, {0,0}, 0);
+
+
+    Vector2 Transform2D::Transform(const Vector2 &input) const {
+        return transformation.Transform(input);
+    }
+
+    Transform2D::Transform2D(const Matrix3x3 &transform) : transformation(transform) { }
+
+    Transform2D::Transform2D(const Vector2& pos, const Vector2& scale, const Vector2& origin, const Vector2& skew, float rotation) {
+        transformation = Matrix3x3(pos.x, pos.y, rotation, scale.x, scale.y, origin.x, origin.y, skew.x, skew.y);
+    }
+
+    Transform2D Transform2D::Translate(float x, float y) const {
+        auto copy = Matrix3x3(transformation);
+        copy.SetAt(0, 0, transformation.At(0, 0) + x);
+        copy.SetAt(0, 1, transformation.At(0, 1) + y);
+        return Transform2D(copy);
+    }
+
+    Transform2D Transform2D::Translate(const LinearAlgebra::Vector2 &input) const {
+        return Translate(input.x, input.y);
+    }
+}

src/J3ML/LinearAlgebra/Transform3D.cpp

@@ -0,0 +1,3 @@
+//
+// Created by josh on 12/26/2023.
+//

src/J3ML/LinearAlgebra/Vector2.cpp

@@ -0,0 +1,347 @@
+#include <J3ML/LinearAlgebra/Vector2.h>
+#include <cassert>
+#include <algorithm>
+#include <valarray>
+#include <iostream>
+
+namespace J3ML::LinearAlgebra {
+
+    Vector2::Vector2(): x(0), y(0)
+    {}
+
+    Vector2::Vector2(float X, float Y): x(X), y(Y)
+    {}
+
+    Vector2::Vector2(const Vector2& rhs): x(rhs.x), y(rhs.y)
+    {}
+
+    float Vector2::operator[](std::size_t index) const
+    {
+        return At(index);
+    }
+
+    float &Vector2::operator[](std::size_t index)
+    {
+        return At(index);
+    }
+
+    bool Vector2::IsWithinMarginOfError(const Vector2& rhs, float margin) const
+    {
+        return this->Distance(rhs) <= margin;
+    }
+
+    bool Vector2::operator==(const Vector2& rhs) const
+    {
+        return this->IsWithinMarginOfError(rhs);
+    }
+
+    bool Vector2::operator!=(const Vector2& rhs) const
+    {
+        return this->IsWithinMarginOfError(rhs) == false;
+    }
+
+    Vector2 Vector2::Min(const Vector2& min) const
+    {
+        return {
+                std::min(this->x, min.x),
+                std::min(this->y, min.y)
+        };
+    }
+
+    Vector2 Vector2::Max(const Vector2& max) const
+    {
+        return {
+                std::max(this->x, max.x),
+                std::max(this->y, max.y)
+        };
+    }
+
+    Vector2 Vector2::Clamp(const Vector2& min, const Vector2& max) const
+    {
+        return {
+                std::clamp(this->x, min.x, max.x),
+                std::clamp(this->y, min.y, max.y)
+        };
+    }
+
+    float Vector2::Distance(const Vector2& to) const
+    {
+        return ((*this)-to).Magnitude();
+    }
+
+    float Vector2::Length() const
+    {
+        return std::sqrt(LengthSquared());
+    }
+
+    float Vector2::LengthSquared() const
+    {
+        return (x*x + y*y);
+    }
+
+    float Vector2::Magnitude() const
+    {
+        return std::sqrt(LengthSquared());
+    }
+
+    float Vector2::Dot(const Vector2& rhs) const
+    {
+        auto a = this->Normalize();
+        auto b = rhs.Normalize();
+
+        return a.x * b.x + a.y * b.y;
+    }
+
+    Vector2 Vector2::Project(const Vector2& rhs) const
+    {
+        float scalar = this->Dot(rhs) / (rhs.Magnitude()*rhs.Magnitude());
+        return rhs * scalar;
+    }
+
+    Vector2 Vector2::Normalize() const
+    {
+        if (Length() > 0)
+            return {
+                    x / Length(),
+                    y / Length()
+            };
+        else
+            return {0,0};
+    }
+
+    Vector2 Vector2::Lerp(const Vector2& rhs, float alpha) const
+    {
+        return this->operator*(1.0f - alpha) + (rhs * alpha);
+    }
+
+    float Vector2::AngleBetween(const Vector2& rhs) const
+    {
+        auto numer = this->Dot(rhs);
+        auto denom = this->Magnitude() * rhs.Magnitude();
+        return std::acos(numer / denom);
+    }
+
+    float Vector2::AngleBetween(const Vector2& lhs, const Vector2& rhs)
+    { return lhs.AngleBetween(rhs); }
+
+    Vector2 Vector2::operator+(const Vector2& rhs) const
+    {
+        return {this->x + rhs.x, this->y + rhs.y};
+    }
+
+    Vector2 Vector2::operator-(const Vector2& rhs) const
+    {
+        return {this->x - rhs.x, this->y - rhs.y};
+    }
+
+    Vector2 Vector2::operator*(float rhs) const
+    {
+        return {
+                this->x * rhs,
+                this->y * rhs
+        };
+    }
+
+    Vector2 Vector2::operator/(float rhs) const
+    {
+        return {
+                this->x / rhs,
+                this->y / rhs
+        };
+    }
+
+    Vector2 Vector2::operator-() const
+    {
+        return {-x, -y};
+    }
+
+    const Vector2 Vector2::Zero  = {0, 0};
+    const Vector2 Vector2::Up    = {0, -1};
+    const Vector2 Vector2::Down  = {0, 1};
+    const Vector2 Vector2::Left  = {-1, 0};
+    const Vector2 Vector2::Right = {1, 0};
+    const Vector2 Vector2::NaN = {NAN, NAN};
+    const Vector2 Vector2::Infinity = {INFINITY, INFINITY};
+
+    float Vector2::GetX() const { return x; }
+
+    float Vector2::GetY() const { return y; }
+
+    float Vector2::LengthSquared(const Vector2 &of) { return of.LengthSquared(); }
+
+    Vector2 Vector2::Min(const Vector2 &value, const Vector2 &minimum) { return value.Min(minimum); }
+
+    Vector2 Vector2::Max(const Vector2 &value, const Vector2 &maximum) { return value.Max(maximum);}
+
+    float Vector2::Length(const Vector2 &of) { return of.Length(); }
+
+    void Vector2::SetX(float newX) { x = newX;}
+
+    void Vector2::SetY(float newY) { y = newY; }
+
+    bool Vector2::IsNormalized(float epsilonSq) const {
+        return std::abs(LengthSquared() - 1.f) <= epsilonSq;
+    }
+
+    bool Vector2::IsZero(float epsilonSq) const {
+        return LengthSquared() <= epsilonSq;
+    }
+
+    bool Vector2::IsPerpendicular(const Vector2 &other, float epsilonSq) const {
+        float dot = Dot(other);
+        return dot*dot <= epsilonSq * LengthSquared() * other.LengthSquared();
+    }
+
+    Vector2 Vector2::Normalize(const Vector2 &of) { return of.Normalize(); }
+
+    Vector2 Vector2::Project(const Vector2 &lhs, const Vector2 &rhs) { return lhs.Project(rhs); }
+
+    float Vector2::Dot(const Vector2 &lhs, const Vector2 &rhs) { return lhs.Dot(rhs); }
+
+    float Vector2::Magnitude(const Vector2 &of) { return of.Magnitude();}
+
+    Vector2 Vector2::Lerp(const Vector2 &lhs, const Vector2 &rhs, float alpha) { return lhs.Lerp(rhs, alpha); }
+
+    Vector2 Vector2::Div(const Vector2 &lhs, float rhs) {
+        return lhs.Div(rhs);
+    }
+
+    Vector2 Vector2::Mul(const Vector2 &lhs, float rhs) {
+        return lhs.Mul(rhs);
+    }
+
+    Vector2 Vector2::Sub(const Vector2 &lhs, const Vector2 &rhs) {
+        return lhs.Sub(rhs);
+    }
+
+    Vector2 Vector2::Add(const Vector2 &lhs, const Vector2 &rhs) {
+        return lhs.Add(rhs);
+    }
+
+    Vector2 Vector2::Add(const Vector2 &rhs) const {
+        return *this + rhs;
+    }
+
+    Vector2 Vector2::Sub(const Vector2 &rhs) const {
+        return *this - rhs;
+    }
+
+    Vector2 Vector2::Mul(float scalar) const {
+        return *this * scalar;
+    }
+
+    Vector2 Vector2::Div(float scalar) const {
+        return *this / scalar;
+    }
+
+    bool Vector2::IsFinite(const Vector2 &v) {
+        return v.IsFinite();
+    }
+
+    float Vector2::MinElement() const {
+        return std::min(x, y);
+    }
+
+    float Vector2::MaxElement() const {
+        return std::max(x, y);
+    }
+
+    Vector2 Vector2::Mul(const Vector2 &v) const {
+        return {this->x*v.x, this->y*v.y};
+    }
+
+    bool Vector2::IsFinite() const {
+        return std::isfinite(x) && std::isfinite(y);
+    }
+
+    Vector2 Vector2::Div(const Vector2 &v) const {
+        return {this->x/v.x, this->y/v.y};
+    }
+
+    Vector2 Vector2::Abs() const { return {std::abs(x), std::abs(y)};}
+
+    float *Vector2::ptr() {
+        return &x;
+    }
+
+    const float *Vector2::ptr() const { return &x;}
+
+    const float Vector2::At(std::size_t index) const {
+        assert(index >= 0);
+        assert(index < Dimensions);
+        return ptr()[index];
+    }
+
+    float &Vector2::At(std::size_t index) {
+        assert(index >= 0);
+        assert(index < Dimensions);
+        return ptr()[index];
+    }
+
+    Vector2 &Vector2::operator/=(float scalar) {
+        x /= scalar;
+        y /= scalar;
+
+        return *this;
+    }
+
+    Vector2 &Vector2::operator*=(float scalar) {
+        x *= scalar;
+        y *= scalar;
+
+        return *this;
+    }
+
+    Vector2 &Vector2::operator-=(const Vector2 &rhs) // Subtracts a vector from this vector, in-place
+    {
+        x -= rhs.x;
+        y -= rhs.y;
+
+        return *this;
+    }
+
+    Vector2 &Vector2::operator+=(const Vector2 &rhs) // Adds a vector to this vector, in-place.
+    {
+        x += rhs.x;
+        y += rhs.y;
+
+        return *this;
+    }
+
+    Vector2 &Vector2::operator=(const Vector2 &rhs) {
+        x = rhs.x;
+        y = rhs.y;
+
+        return *this;
+    }
+
+    Vector2::Vector2(const float *data) {
+        assert(data);
+        x = data[0];
+        y = data[1];
+    }
+
+    Vector2::Vector2(float scalar) {
+        x = scalar;
+        y = scalar;
+    }
+
+    float Vector2::DistanceSq(const Vector2 &to) const {
+        return (*this-to).LengthSquared();
+    }
+
+    float Vector2::DistanceSq(const Vector2 &from, const Vector2 &to) {
+        return (from-to).LengthSquared();
+    }
+
+    bool Vector2::OrientedCCW(const Vector2 &a, const Vector2 &b, const Vector2 &c)
+    {
+        // Compute the determinant
+        // | ax ay 1 |
+        // | bx by 1 |
+        // | cx cy 1 |
+        // See Christer Ericson, Real-Time Collision Detection, p.32.
+        return (a.x-c.x)*(b.y-c.y) - (a.y-c.y)*(b.x-c.x) >= 0.f;
+    }
+
+}

src/J3ML/LinearAlgebra/Vector3.cpp

@@ -0,0 +1,505 @@
+#include <J3ML/LinearAlgebra/Vector3.h>
+#include <algorithm>
+#include <cassert>
+#include <cmath>
+
+namespace J3ML::LinearAlgebra {
+
+    const Vector3 Vector3::Zero     = {0,0,0};
+    const Vector3 Vector3::Up       = {0, -1, 0};
+    const Vector3 Vector3::Down     = {0, 1, 0};
+    const Vector3 Vector3::Left     = {-1, 0, 0};
+    const Vector3 Vector3::Right    = {1, 0, 0};
+    const Vector3 Vector3::Forward  = {0, 0, -1};
+    const Vector3 Vector3::Backward = {0, 0, 1};
+    const Vector3 Vector3::NaN      = {NAN, NAN, NAN};
+    const Vector3 Vector3::Infinity = {INFINITY, INFINITY, INFINITY};
+    const Vector3 Vector3::NegativeInfinity = {-INFINITY, -INFINITY, -INFINITY};
+
+    Vector3 Vector3::operator+(const Vector3& rhs) const
+    {
+        return {this->x + rhs.x, this->y + rhs.y, this->z + rhs.z};
+    }
+
+    Vector3 Vector3::operator-(const Vector3& rhs) const
+    {
+        return {
+                this->x- rhs.x,
+                this->y-rhs.y,
+                this->z-rhs.z
+        };
+    }
+
+    Vector3 Vector3::operator*(float rhs) const
+    {
+        return {
+                this->x * rhs,
+                this->y * rhs,
+                this->z * rhs
+        };
+    }
+
+
+    Vector3 Vector3::operator/(float rhs) const
+    {
+        return {
+                this->x / rhs,
+                this->y / rhs,
+                this->z / rhs
+        };
+    }
+
+    Vector3 Vector3::operator-() const
+    {
+        return {-x, -y, -z};
+    }
+
+
+
+
+    Vector3::Vector3(): x(0), y(0), z(0) {}
+
+    Vector3::Vector3(float X, float Y, float Z): x(X), y(Y), z(Z) {}
+
+    Vector3::Vector3(const Vector3& rhs) : x(rhs.x), y(rhs.y), z(rhs.z) {}
+
+    Vector3& Vector3::operator=(const Vector3& rhs)
+    {
+        this->x = rhs.x;
+        this->y = rhs.y;
+        this->z = rhs.z;
+        return *this;
+    }
+
+    float Vector3::operator[](std::size_t index) const
+    {
+        assert(index < 3);
+        if (index==0) return x;
+        if (index==1) return y;
+        if (index==2) return z;
+        return 0;
+    }
+
+    float &Vector3::operator[](std::size_t index)
+    {
+        assert(index < 3);
+        if (index == 0) return x;
+        if (index == 1) return y;
+        if (index == 2) return z;
+    }
+
+    bool Vector3::IsWithinMarginOfError(const Vector3& rhs, float margin) const
+    {
+        return this->Distance(rhs) <= margin;
+    }
+
+    bool Vector3::operator==(const Vector3& rhs) const
+    {
+        return this->IsWithinMarginOfError(rhs);
+    }
+
+    bool Vector3::operator!=(const Vector3& rhs) const
+    {
+        return this->IsWithinMarginOfError(rhs) == false;
+    }
+
+
+
+    Vector3 Vector3::Min(const Vector3& min) const
+    {
+        return {
+                std::min(this->x, min.x),
+                std::min(this->y, min.y),
+                std::min(this->z, min.z)
+        };
+    }
+
+    Vector3 Vector3::Max(const Vector3& max) const
+    {
+        return {
+                std::max(this->x, max.x),
+                std::max(this->y, max.y),
+                std::max(this->z, max.z)
+        };
+    }
+
+    Vector3 Vector3::Clamp(const Vector3& min, const Vector3& max) const
+    {
+        return {
+                std::clamp(this->x, min.x, max.x),
+                std::clamp(this->y, min.y, max.y),
+                std::clamp(this->z, min.z, max.z)
+        };
+    }
+
+    float Vector3::Distance(const Vector3& to) const
+    {
+        return ((*this)-to).Magnitude();
+    }
+
+    float Vector3::Length() const
+    {
+        return std::sqrt(LengthSquared());
+    }
+
+    float Vector3::LengthSquared() const
+    {
+        return (x*x + y*y + z*z);
+    }
+
+    float Vector3::Magnitude() const
+    {
+        return std::sqrt(x*x + y*y + z*z);
+    }
+
+    float Vector3::Dot(const Vector3& rhs) const
+    {
+        auto a = this->Normalize();
+        auto b = rhs.Normalize();
+        return a.x * b.x +
+               a.y * b.y +
+               a.z * b.z;
+    }
+
+    Vector3 Vector3::Project(const Vector3& rhs) const
+    {
+        float scalar = this->Dot(rhs) / (rhs.Magnitude()*rhs.Magnitude());
+        return rhs * scalar;
+    }
+
+    Vector3 Vector3::Cross(const Vector3& rhs) const
+    {
+        return {
+                this->y * rhs.z - this->z * rhs.y,
+                this->z * rhs.x - this->x * rhs.z,
+                this->x * rhs.y - this->y * rhs.x
+        };
+    }
+
+    Vector3 Vector3::Normalize() const
+    {
+        if (Length() > 0)
+            return {
+                    x / Length(),
+                    y / Length(),
+                    z / Length()
+            };
+        else
+            return {0,0,0};
+    }
+
+    Vector3 Vector3::Lerp(const Vector3& goal, float alpha) const
+    {
+        return this->operator*(1.0f - alpha) + (goal * alpha);
+    }
+
+    float Vector3::GetX() const { return x;}
+
+    float Vector3::GetY() const { return y;}
+
+    float Vector3::GetZ() const { return z;}
+
+    void Vector3::SetX(float newX) { x = newX;}
+
+    void Vector3::SetY(float newY) { y = newY;}
+
+    void Vector3::SetZ(float newZ) { z = newZ;}
+
+    Vector3 Vector3::Min(const Vector3 &lhs, const Vector3 &rhs) {
+        return lhs.Min(rhs);
+    }
+
+    Vector3 Vector3::Max(const Vector3 &lhs, const Vector3 &rhs) {
+        return lhs.Max(rhs);
+    }
+
+    Vector3 Vector3::Clamp(const Vector3 &min, const Vector3 &input, const Vector3 &max) {
+        return input.Clamp(min, max);
+    }
+
+    float Vector3::Distance(const Vector3 &from, const Vector3 &to) {
+        return from.Distance(to);
+    }
+
+    float Vector3::Length(const Vector3 &of) {
+        return of.Length();
+    }
+
+    float Vector3::LengthSquared(const Vector3 &of) {
+        return of.LengthSquared();
+    }
+
+    bool Vector3::IsPerpendicular(const Vector3 &other, float epsilonSq) const {
+        float dot = Dot(other);
+        return dot*dot <= epsilonSq * LengthSquared() * other.LengthSquared();
+    }
+
+    bool Vector3::IsZero(float epsilonSq) const {
+        return LengthSquared() <= epsilonSq;
+    }
+
+    bool Vector3::IsNormalized(float epsilonSq) const {
+        return std::abs(LengthSquared()-1.f) <= epsilonSq;
+    }
+
+    Vector3 Vector3::Cross(const Vector3 &lhs, const Vector3 &rhs) {
+        return lhs.Cross(rhs);
+    }
+
+    Vector3 Vector3::Normalize(const Vector3 &targ) {
+        return targ.Normalize();
+    }
+
+    Vector3 Vector3::Project(const Vector3 &lhs, const Vector3 &rhs) {
+        return lhs.Project(rhs);
+    }
+
+    float Vector3::Dot(const Vector3 &lhs, const Vector3 &rhs) {
+        return lhs.Dot(rhs);
+    }
+
+    float Vector3::Magnitude(const Vector3 &of) {
+        return of.Magnitude();
+    }
+
+    Vector3 Vector3::Lerp(const Vector3 &lhs, const Vector3 &rhs, float alpha) {
+        return lhs.Lerp(rhs, alpha);
+    }
+
+    Vector3 Vector3::Add(const Vector3 &lhs, const Vector3 &rhs) {
+        return lhs.Add(rhs);
+    }
+
+    Vector3 Vector3::Add(const Vector3 &rhs) const {
+        return *this + rhs;
+    }
+
+    Vector3 Vector3::Sub(const Vector3 &rhs) const {
+        return *this - rhs;
+    }
+
+    Vector3 Vector3::Sub(const Vector3 &lhs, const Vector3 &rhs) {
+        return lhs.Sub(rhs);
+    }
+
+    Vector3 Vector3::Mul(float scalar) const {
+        return *this * scalar;
+    }
+
+    Vector3 Vector3::Mul(const Vector3 &lhs, float rhs) {
+        return lhs.Mul(rhs);
+    }
+
+    Vector3 Vector3::Div(float scalar) const {
+        return *this / scalar;
+    }
+
+    Vector3 Vector3::Div(const Vector3 &lhs, float rhs) {
+        return lhs.Div(rhs);
+    }
+
+    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};
+    }
+
+    Angle2D Vector3::AngleBetween(const Vector3 &lhs, const Vector3 &rhs) // TODO: 3D Angle representation?
+    {
+        return lhs.AngleBetween(rhs);
+    }
+
+    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)));
+        return {x, y, z};
+    }
+
+    Vector3 Vector3::Abs() const {
+        return {std::abs(x), std::abs(y), std::abs(z)};
+    }
+
+    float *Vector3::ptr() {
+        return &x;
+    }
+
+    void Vector3::Orthonormalize(Vector3 &a, Vector3 &b) {
+        a = a.Normalize();
+        b = b - b.ProjectToNorm(a);
+        b = b.Normalize();
+    }
+
+    void Vector3::Orthonormalize(Vector3 &a, Vector3 &b, Vector3 &c) {
+        a = a.Normalize();
+        b = b - b.ProjectToNorm(a);
+        b = b.Normalize();
+        c = c - c.ProjectToNorm(a);
+        c = c - c.ProjectToNorm(b);
+        c = c.Normalize();
+    }
+
+    Vector3 Vector3::ProjectToNorm(const Vector3 &direction) const {
+        return direction * this->Dot(direction);
+    }
+
+    bool Vector3::IsFinite() const {
+        return std::isfinite(x) && std::isfinite(y) && std::isfinite(z);
+    }
+
+    Vector3::Vector3(const float *data) {
+        x = data[0];
+        y = data[1];
+        z = data[2];
+    }
+
+    Vector3 &Vector3::operator+=(const Vector3 &rhs) {
+        x += rhs.x;
+        y += rhs.y;
+        z += rhs.z;
+        return *this;
+    }
+
+    Vector3 &Vector3::operator-=(const Vector3 &rhs) {
+        x -= rhs.x;
+        y -= rhs.y;
+        z -= rhs.z;
+        return *this;
+    }
+
+    Vector3 &Vector3::operator*=(float scalar) {
+        x *= scalar;
+        y *= scalar;
+        z *= scalar;
+        return *this;
+    }
+
+    Vector3 &Vector3::operator/=(float scalar) {
+        x /= scalar;
+        y /= scalar;
+        z /= scalar;
+        return *this;
+    }
+
+    float Vector3::MinElement() const {
+        return std::min(x, std::min(y, z));
+    }
+
+    bool Vector3::AreOrthonormal(const Vector3 &a, const Vector3 &b, float epsilon) {
+        return a.IsPerpendicular(b, epsilon) && a.IsNormalized(epsilon*epsilon) && b.IsNormalized(epsilon*epsilon);
+    }
+
+    Vector3 Vector3::Mul(const Vector3 &rhs) const {
+        return {
+                this->x * rhs.x,
+                this->y * rhs.y,
+                this->z * rhs.z
+        };
+    }
+
+    Vector3 Vector3::Div(const Vector3 &v) const {
+        return {
+                this->x/v.x,
+                this->y/v.y,
+                this->z/v.z
+        };
+    }
+
+    Vector3 Vector3::Abs(const Vector3 &rhs) {
+        return rhs.Abs();
+    }
+
+    bool Vector3::Equals(const Vector3 &rhs, float epsilon) const {
+        return std::abs(x - rhs.x) < epsilon &&
+               std::abs(y - rhs.y) < epsilon &&
+               std::abs(z - rhs.z) < epsilon;
+    }
+
+    bool Vector3::Equals(float _x, float _y, float _z, float epsilon) const {
+        return std::abs(x - _x) < epsilon &&
+               std::abs(y - _y) < epsilon &&
+               std::abs(z - _z) < epsilon;
+    }
+
+    Vector3 Vector3::Min(const Vector3 &a, const Vector3 &b, const Vector3 &c) {
+        return {
+                std::min(a.x, std::min(b.x, c.x)),
+                std::min(a.y, std::min(b.y, c.y)),
+                std::min(a.z, std::min(b.z, c.z))
+        };
+    }
+
+    Vector3 Vector3::Max(const Vector3 &a, const Vector3 &b, const Vector3 &c) {
+        return {
+                std::max(a.x, std::max(b.x, c.x)),
+                std::max(a.y, std::max(b.y, c.y)),
+                std::max(a.z, std::max(b.z, c.z))
+        };
+    }
+
+    Vector3 Vector3::Perpendicular(const Vector3 &hint, const Vector3 &hint2) const {
+        assert(!this->IsZero());
+        assert(hint.IsNormalized());
+        assert(hint2.IsNormalized());
+        Vector3 v = this->Cross(hint);
+        float len = v.TryNormalize();
+
+    }
+
+    float Vector3::TryNormalize() {
+        assert(IsFinite());
+        float length = Length();
+        if (length > 1e-6f)
+        {
+            *this *= 1.f / length;
+            return length;
+        }
+        else
+        {
+            Set(1.f, 0.f, 0.f); // We will always produce a normalized vector.
+            return 0; // But signal failure, so user knows we have generated an arbitrary normalization.
+        }
+    }
+
+    float Vector3::At(int index) const {
+        assert(index >= 0);
+        assert(index < Dimensions);
+        return ptr()[index];
+    }
+
+    float &Vector3::At(int index) {
+        assert(index >= 0);
+        assert(index < Dimensions);
+        return ptr()[index];
+    }
+
+    void Vector3::Set(float d, float d1, float d2) {
+        x = d;
+        y = d1;
+        z = d2;
+    }
+
+    Vector3::Vector3(const Vector2& XY, float Z) {
+        x = XY.x;
+        y = XY.y;
+        z = Z;
+    }
+
+    Vector3::Vector3(float scalar) {
+        x = scalar;
+        y = scalar;
+        z = scalar;
+    }
+
+    float Vector3::DistanceSquared(const Vector3 &to) const {
+        return (*this-to).LengthSquared();
+    }
+
+    float Vector3::DistanceSquared(const Vector3 &from, const Vector3 &to) {
+        return from.DistanceSquared(to);
+    }
+
+
+}

src/J3ML/LinearAlgebra/Vector4.cpp

@@ -0,0 +1,204 @@
+
+#pragma region vector4
+
+
+#include <J3ML/LinearAlgebra/Vector4.h>
+#include <J3ML/LinearAlgebra/Vector3.h>
+#include <cmath>
+#include <algorithm>
+
+namespace J3ML::LinearAlgebra {
+
+    const Vector4 Vector4::Zero = {0,0,0,0};
+    const Vector4 Vector4::NaN = {NAN, NAN, NAN, NAN};
+
+    Vector4::Vector4(): x(0), y(0), z(0), w(0)
+    {}
+
+    Vector4::Vector4(float X, float Y, float Z, float W): x(X),y(Y),z(Z),w(W)
+    { }
+
+bool Vector4::operator==(const Vector4& rhs) const
+{
+    return this->IsWithinMarginOfError(rhs);
+}
+
+bool Vector4::operator!=(const Vector4& rhs) const
+{
+    return this->IsWithinMarginOfError(rhs) == false;
+}
+
+Vector4 Vector4::Min(const Vector4& min) const
+{
+    return {
+            std::min(this->x, min.x),
+            std::min(this->y, min.y),
+            std::min(this->z, min.z),
+            std::min(this->w, min.w)
+    };
+}
+
+Vector4 Vector4::Max(const Vector4& max) const
+{
+    return {
+            std::max(this->x, max.x),
+            std::max(this->y, max.y),
+            std::max(this->z, max.z),
+            std::max(this->w, max.w)
+    };
+}
+
+Vector4 Vector4::Clamp(const Vector4& min, const Vector4& max) const
+{
+    return {
+            std::clamp(this->x, min.x, max.x),
+            std::clamp(this->y, min.y, max.y),
+            std::clamp(this->z, min.z, max.z),
+            std::clamp(this->w, min.w, max.w)
+    };
+}
+
+float Vector4::Distance(const Vector4& to) const
+{
+    return ( (*this) - to ).Magnitude();
+}
+
+float Vector4::Length() const
+{
+    return std::sqrt(LengthSquared());
+}
+
+float Vector4::LengthSquared() const
+{
+    return (x*x + y*y + z*z + w*w);
+}
+
+float Vector4::Magnitude() const
+{
+    return std::sqrt(x*x + y*y + z*z + w*w);
+}
+
+float Vector4::Dot(const Vector4& rhs) const
+{
+    auto a = this->Normalize();
+    auto b = rhs.Normalize();
+
+    return  a.x * b.x +
+            a.y * b.y +
+            a.z * b.z +
+            a.w * b.w;
+}
+
+Vector4 Vector4::Project(const Vector4& rhs) const
+{
+    float scalar = this->Dot(rhs) / (rhs.Magnitude()* rhs.Magnitude());
+    return rhs * scalar;
+}
+
+Vector4 Vector4::Normalize() const
+{
+    if (Length() > 0)
+        return {
+                x / Length(),
+                y / Length(),
+                z / Length(),
+                w / Length()
+        };
+    else
+        return {0,0,0,0};
+}
+
+Vector4 Vector4::Lerp(const Vector4& goal, float alpha) const
+{
+    return this->operator*(1.0f - alpha) + (goal * alpha);
+}
+
+Vector4 Vector4::operator+(const Vector4& rhs) const
+{
+    return {x+rhs.x, y+rhs.y, z+rhs.z, w+rhs.w};
+}
+
+Vector4 Vector4::operator-(const Vector4& rhs) const
+{
+    return {x-rhs.x, y-rhs.y, z-rhs.z, w-rhs.w};
+}
+
+    Vector4 Vector4::operator*(float rhs) const {
+        return {
+                this->x * rhs,
+                this->y * rhs,
+                this->z * rhs,
+                this->w * rhs
+        };
+    }
+
+    bool Vector4::IsWithinMarginOfError(const Vector4 &rhs, float margin) const {
+        return this->Distance(rhs) <= margin;
+    }
+
+    Vector4::Vector4(const Vector3 &xyz, float w) : x(xyz.x), y(xyz.y), z(xyz.z), w(w)
+    {
+
+    }
+
+    Vector4 &Vector4::operator=(const Vector4 &rhs) {
+        x = rhs.x;
+        y = rhs.y;
+        z = rhs.z;
+        w = rhs.w;
+
+        return *this;
+    }
+
+    bool Vector4::Equals(const Vector4 &rhs, float epsilon) const {
+        return std::abs(x - rhs.x) < epsilon &&
+               std::abs(y - rhs.y) < epsilon &&
+               std::abs(z - rhs.z) < epsilon &&
+               std::abs(w - rhs.w) < epsilon;
+    }
+
+    bool Vector4::Equals(float _x, float _y, float _z, float _w, float epsilon) const {
+        return std::abs(x - _x) < epsilon &&
+               std::abs(y - _y) < epsilon &&
+               std::abs(z - _z) < epsilon &&
+               std::abs(w - _w) < epsilon;
+    }
+
+    float Vector4::operator[](std::size_t index) const {
+        assert(index < 4);
+        if (index==0) return x;
+        if (index==1) return y;
+        if (index==2) return z;
+        if (index==3) return w;
+        return 0;
+    }
+
+    float &Vector4::operator[](std::size_t index) {
+        assert(index < 4);
+        if (index == 0) return x;
+        if (index == 1) return y;
+        if (index == 2) return z;
+        if (index == 3) return w;
+
+    }
+
+    float Vector4::LengthSqXYZ() const {
+        return x*x * y*y * z*z;
+    }
+
+    Vector4 Vector4::Cross3(const Vector3 &rhs) const {
+        Vector4 dst;
+        dst.x = y * rhs.z - z * rhs.y;
+        dst.y = z * rhs.x - x * rhs.z;
+        dst.z = x * rhs.y - y * rhs.x;
+        dst.w = 0.f;
+        return dst;
+    }
+
+    Vector4 Vector4::Cross3(const Vector4 &rhs) const {
+        return Cross3(rhs.XYZ());
+    }
+
+
+}
+#pragma endregion

tests/CMakeLists.txt

@@ -0,0 +1,19 @@
+include(FetchContent)
+
+FetchContent_Declare(
+        googletest
+        GIT_REPOSITORY https://github.com/google/googletest.git
+        GIT_TAG        release-1.11.0
+)
+FetchContent_MakeAvailable(googletest)
+add_library(GTest::GTest INTERFACE IMPORTED)
+target_link_libraries(GTest::GTest INTERFACE gtest_main)
+
+
+file(GLOB_RECURSE TEST_SRC "tests.cpp" "*.cpp")
+add_executable(Test ${TEST_SRC})
+target_link_libraries(Test PUBLIC J3ML)
+#find_package(GTest REQUIRED)
+target_link_libraries(Test PRIVATE GTest::GTest)
+include_directories("include")
+add_test(NAME "J3MLTestSuite" COMMAND Test)

tests/LinearAlgebra/EulerAngleTests.cpp

@@ -0,0 +1,3 @@
+//
+// Created by josh on 12/26/2023.
+//

tests/LinearAlgebra/Matrix2x2Tests.cpp

@@ -0,0 +1,3 @@
+//
+// Created by josh on 12/26/2023.
+//

tests/LinearAlgebra/QuaternionTests.cpp

@@ -0,0 +1,3 @@
+//
+// Created by josh on 12/26/2023.
+//

tests/LinearAlgebra/Vector2Tests.cpp

@@ -0,0 +1,183 @@
+#include <gtest/gtest.h>
+#include <J3ML/LinearAlgebra/Vector2.h>
+
+using J3ML::LinearAlgebra::Vector2;
+
+TEST(Vector2Test, V2_Constructor_Default)
+{
+    EXPECT_EQ(Vector2(), Vector2::Zero);
+}
+
+TEST(Vector2Test, V2_Constructor_XY)
+{
+    Vector2 vec {1, 0};
+    EXPECT_EQ(vec, Vector2::Right);
+}
+
+TEST(Vector2Test, V2_Addition_Op)
+{
+    Vector2 A {1,1};
+    Vector2 B {2,2};
+
+    Vector2 C {3, 3};
+
+    EXPECT_EQ(A+B, C);
+}
+
+TEST(Vector2Test, V2_Addition_Method)
+{
+    Vector2 A {2,2};
+    Vector2 B {2,2};
+
+    Vector2 C {4, 4};
+
+    EXPECT_EQ(A.Add(B), C);
+}
+
+TEST(Vector2Test, V2_Addition_Static)
+{
+    Vector2 A {3, 3};
+    Vector2 B {2, 2};
+
+    Vector2 C {5, 5};
+
+    EXPECT_EQ(Vector2::Add(A, B), C);
+}
+
+TEST(Vector2Test, V2_Subtract_Op)
+{
+    Vector2 A {1,1};
+    Vector2 B {2,2};
+
+    Vector2 C {-1, -1};
+
+    EXPECT_EQ(A-B, C);
+}
+
+TEST(Vector2Test, V2_Subtract_Method)
+{
+    Vector2 A {1,1};
+    Vector2 B {2,2};
+
+    Vector2 C {-1, -1};
+
+    EXPECT_EQ(A.Sub(B), C);
+}
+
+TEST(Vector2Test, V2_Subtract_Static)
+{
+    Vector2 A {1,1};
+    Vector2 B {2,2};
+
+    Vector2 C {-1, -1};
+
+    EXPECT_EQ(Vector2::Sub(A, B), C);
+}
+
+TEST(Vector2Test, V2_Scalar_Multiplication)
+{
+    Vector2 A {5, 1};
+    float B = 0.5f;
+    Vector2 C = {2.5f, .5f};
+    EXPECT_EQ(A*B, C);
+}
+
+TEST(Vector2Test, V2_Size)
+{
+    EXPECT_EQ(sizeof(Vector2), 8);
+}
+
+TEST(Vector2Test, V2_NaN)
+{
+
+    EXPECT_NE(Vector2::Zero, Vector2::NaN);
+    EXPECT_NE(Vector2::Up, Vector2::NaN);
+    EXPECT_NE(Vector2::Left, Vector2::NaN);
+    EXPECT_NE(Vector2::Down, Vector2::NaN);
+    EXPECT_NE(Vector2::Right, Vector2::NaN);
+    EXPECT_NE(Vector2::NaN, Vector2::NaN);
+}
+
+TEST(Vector2Test, V2_MarginOfError)
+{
+    Vector2 A {2,2};
+    Vector2 B {1.85, 1.85};
+
+    EXPECT_TRUE(A.IsWithinMarginOfError(B, 0.5f));
+}
+
+TEST(Vector2Test, V2_Min)
+{
+    Vector2 A {2,2};
+    Vector2 B {1.85, 1.85};
+
+    EXPECT_EQ( Vector2::Min(A, B), B);
+}
+
+TEST(Vector2Test, V2_Max)
+{
+    Vector2 A {2,2};
+    Vector2 B {1.85, 1.85};
+
+    EXPECT_EQ( Vector2::Max(A, B), A);
+}
+
+TEST(Vector2Test, V2_Clamp)
+{
+    Vector2 Input{0, 20};
+
+    Vector2 Minimum { 2, 2};
+    Vector2 Maximum {16, 16};
+
+    Vector2 ExpectedResult {2, 16};
+
+    EXPECT_EQ(Input.Clamp(Minimum, Maximum), ExpectedResult);
+}
+
+TEST(Vector2Test, V2_DotProduct)
+{
+    // TODO: Equality
+    Vector2 A {2, 2};
+    Vector2 B {1, 1};
+    EXPECT_FLOAT_EQ(A.Dot(B), 1.f);
+}
+
+TEST(Vector2Test, V2_Project)
+{
+    Vector2 Base {1, 1};
+    Vector2 Projected {1, 1};
+
+    Vector2 ExpectedResult {0.5, 0.5};
+
+    EXPECT_EQ(Base.Project(Projected), ExpectedResult);
+}
+
+TEST(Vector2Test, V2_Normalize)
+{
+    Vector2 A{2, 0};
+    Vector2 B{1, 0};
+    EXPECT_EQ(A.Normalize(), B);
+}
+
+TEST(Vector2Test, V2_Lerp)
+{
+    Vector2 A {2,2};
+    Vector2 B {10, 10};
+    Vector2 C {6, 6};
+
+    EXPECT_EQ(A.Lerp(B, 0.f), A);
+    EXPECT_EQ(A.Lerp(B, 1.f), B);
+    EXPECT_EQ(A.Lerp(B, 0.5f), C);
+}
+
+TEST(Vector2Test, V2_AngleBetween)
+{
+    Vector2 A {0.5f, 0.5};
+    Vector2 B {0.5f, 0.1f};
+
+    A = A.Normalize();
+    B = B.Normalize();
+
+    // TODO: AngleBetween returns not a number
+    EXPECT_FLOAT_EQ(A.AngleBetween(B), 0.58800244);
+}

tests/LinearAlgebra/Vector3Tests.cpp

@@ -0,0 +1,198 @@
+#include <gtest/gtest.h>
+#include <J3ML/LinearAlgebra/Vector3.h>
+
+using J3ML::LinearAlgebra::Vector3;
+
+void EXPECT_V3_EQ(const Vector3& lhs, const Vector3& rhs)
+{
+    EXPECT_FLOAT_EQ(lhs.x, rhs.x);
+    EXPECT_FLOAT_EQ(lhs.y, rhs.y);
+    EXPECT_FLOAT_EQ(lhs.z, rhs.z);
+}
+
+TEST(Vector3Test, V3_Constructor_Default)
+{
+    EXPECT_V3_EQ(Vector3(), Vector3::Zero);
+}
+
+TEST(Vector3Test, V3_Constructor_XYZ)
+{
+    Vector3 Input {0, 1, 0};
+
+    EXPECT_V3_EQ(Input, Vector3::Down);
+}
+
+TEST(Vector3Test, V3_Addition_Op) {
+    Vector3 A {1,1,1};
+    Vector3 B {2,2,2};
+
+    Vector3 ExpectedResult {3,3,3};
+
+    EXPECT_V3_EQ(A + B, ExpectedResult);
+}
+TEST(Vector3Test, V3_Addition_Method) {
+    Vector3 A {1,1,1};
+    Vector3 B {2,2,2};
+
+    Vector3 ExpectedResult {3,3,3};
+
+    EXPECT_V3_EQ(A.Add(B), ExpectedResult);
+}
+TEST(Vector3Test, V3_Addition_Static) {
+    Vector3 A {1,1,1};
+    Vector3 B {3,3,3};
+
+    Vector3 ExpectedResult {4,4,4};
+
+    EXPECT_V3_EQ(Vector3::Add(A, B), ExpectedResult);
+}
+TEST(Vector3Test, V3_Subtract_Op) {
+    Vector3 A {2,2,2};
+    Vector3 B {.5f, .5f, .5f};
+
+    Vector3 ExpectedResult {1.5f, 1.5f, 1.5f};
+
+    EXPECT_V3_EQ(A - B, ExpectedResult);
+}
+TEST(Vector3Test, V3_Subtract_Method) {
+    Vector3 A {3,3,3};
+    Vector3 B {1,1,1};
+
+    Vector3 ExpectedResult {2,2,2};
+
+    EXPECT_V3_EQ(A.Sub(B), ExpectedResult);
+}
+TEST(Vector3Test, V3_Subtract_Static) {
+    Vector3 A {4,4,4};
+    Vector3 B {1,1,1};
+
+    Vector3 ExpectedResult {3,3,3};
+
+    EXPECT_V3_EQ(Vector3::Sub(A, B), ExpectedResult);
+}
+TEST(Vector3Test, V3_Scalar_Mult_Op) {
+    Vector3 A { 1,1,1};
+    float B = 1.5f;
+
+    Vector3 ExpectedResult {1.5f, 1.5f, 1.5f};
+
+    EXPECT_V3_EQ(A * B, ExpectedResult);
+}
+TEST(Vector3Test, V3_Scalar_Mult_Method) {
+    Vector3 A {3,3,3};
+    float B = 1.5f;
+
+    Vector3 ExpectedResult {4.5f, 4.5f, 4.5f};
+
+    EXPECT_V3_EQ(A.Mul(B), ExpectedResult);
+}
+TEST(Vector3Test, V3_Scalar_Mult_Static) {
+    Vector3 A {2,2,2};
+    float B = 1.5f;
+
+    Vector3 ExpectedResult {3.f, 3.f, 3.f};
+
+    EXPECT_V3_EQ(Vector3::Mul(A, B), ExpectedResult);
+}
+TEST(Vector3Test, V3_Scalar_Div_Op) {
+    Vector3 A {4,4,4};
+    float B = 2.f;
+
+    Vector3 ExpectedResult {2,2,2};
+    EXPECT_V3_EQ(A / B, ExpectedResult);
+}
+TEST(Vector3Test, V3_Scalar_Div_Method) {
+    Vector3 A {6,6,6};
+    float B = 2.f;
+    Vector3 ExpectedResult { 3,3,3};
+
+    EXPECT_V3_EQ(A.Div(B), ExpectedResult);
+}
+TEST(Vector3Test, V3_Scalar_Div_Static) {
+    Vector3 A {3,3,3};
+    float B = 1.5f;
+
+    Vector3 ExpectedResult { 2.f, 2.f, 2.f};
+
+    EXPECT_V3_EQ(Vector3::Div(A, B), ExpectedResult);
+}
+TEST(Vector3Test, V3_Sizeof) {
+    EXPECT_EQ(sizeof(Vector3), 12);
+}
+TEST(Vector3Test, V3_NaN) {
+    EXPECT_NE(Vector3(0, 0, 0), Vector3::NaN);
+
+}
+TEST(Vector3Test, V3_Min) {
+    Vector3 Input {2,2,2};
+    Vector3 Minimum {3,3,3};
+    Vector3 ExpectedResult {2,2,2};
+
+    EXPECT_V3_EQ(Input.Min(Minimum), ExpectedResult);
+}
+TEST(Vector3Test, V3_Max) {
+    Vector3 Input {2,2,2};
+    Vector3 Maximum {3,3,3};
+    Vector3 ExpectedResult {3,3,3};
+
+    EXPECT_V3_EQ(Input.Max(Maximum), ExpectedResult);
+}
+TEST(Vector3Test, V3_Clamp) {
+    Vector3 Input {5,-1,8};
+    Vector3 Minimum {1,1,1};
+    Vector3 Maximum {5,5,5};
+
+    Vector3 ExpectedResult {5,1,5};
+
+    EXPECT_V3_EQ(Input.Clamp(Minimum, Maximum), ExpectedResult);
+
+}
+TEST(Vector3Test, V3_DotProduct) {
+    Vector3 A{6,6,6};
+    Vector3 B{1,1,1};
+
+
+    float ExpectedResult = 1;
+
+    EXPECT_FLOAT_EQ(A.Dot(B), ExpectedResult);
+}
+TEST(Vector3Test, V3_CrossProduct) {
+    Vector3 A{1,1,1};
+    Vector3 B{2,2,2};
+
+    Vector3 ExpectedResult {0,0,0};
+
+    EXPECT_V3_EQ(A.Cross(B), ExpectedResult);
+
+}
+TEST(Vector3Test, V3_Project) {
+    Vector3 Base {};
+    Vector3 Projection {};
+    Vector3 ExpectedResult {};
+}
+TEST(Vector3Test, V3_Normalize) {
+    Vector3 Input {2, 0, 0};
+    Vector3 ExpectedResult {1, 0, 0};
+
+    EXPECT_V3_EQ(Input.Normalize(), ExpectedResult);
+}
+TEST(Vector3Test, V3_Lerp)
+{
+    Vector3 Start {};
+    Vector3 Finish {};
+    float Percent = 50;
+    Vector3 ExpectedResult {};
+    EXPECT_V3_EQ(Start.Lerp(Finish, Percent), ExpectedResult);
+}
+TEST(Vector3Test, V3_AngleBetween) {
+    using J3ML::LinearAlgebra::Angle2D;
+    Vector3 A{ .5f, .5f, .5f};
+    Vector3 B {.25f, .75f, .25f};
+    A = A.Normalize();
+    B = B.Normalize();
+    Angle2D ExpectedResult {-0.69791365, -2.3561945};
+    std::cout << A.AngleBetween(B).x << ", " << A.AngleBetween(B).y << "";
+    auto angle = A.AngleBetween(B);
+    EXPECT_FLOAT_EQ(angle.x, ExpectedResult.x);
+    EXPECT_FLOAT_EQ(angle.y, ExpectedResult.y);
+}

tests/LinearAlgebra/Vector4Tests.cpp

@@ -0,0 +1,106 @@
+#include <gtest/gtest.h>
+#include <J3ML/LinearAlgebra/Vector4.h>
+
+using Vector4 = J3ML::LinearAlgebra::Vector4;
+
+
+void EXPECT_V4_EQ(const Vector4& lhs, const Vector4& rhs)
+{
+
+}
+
+TEST(Vector4Test, V4_Constructor_Default)
+{
+    EXPECT_V4_EQ(Vector4(), Vector4::Zero);
+}
+
+TEST(Vector4Test, V4_Constructor_XYZW)
+{
+    Vector4 Input {0, 1, 0, 1};
+    EXPECT_FLOAT_EQ(Input.x, 0);
+    EXPECT_FLOAT_EQ(Input.y, 1);
+    EXPECT_FLOAT_EQ(Input.z, 0);
+    EXPECT_FLOAT_EQ(Input.w, 1);
+}
+
+TEST(Vector4Test, V4_Addition_Op) {
+    Vector4 A {1, 1, 1, 1};
+    Vector4 B {2, 2, 2, 2};
+
+    Vector4 ExpectedResult {3, 3, 3, 3};
+
+    EXPECT_V4_EQ(A + B, ExpectedResult);
+}
+
+TEST(Vector4Test, V4_Addition_Method)
+{
+
+}
+
+TEST(Vector4Test, V4_Addition_Static)
+{
+
+}
+
+TEST(Vector4Test, V4_Subtract_Op)
+{
+
+}
+
+TEST(Vector4Test, V4_Subtract_Method)
+{
+
+}
+
+TEST(Vector4Test, V4_Subtract_Static)
+{
+
+}
+
+TEST(Vector4Test, V4_Scalar_Mult_Op)
+{
+
+}
+
+TEST(Vector4Test, V4_Scalar_Mult_Method)
+{
+
+}
+
+TEST(Vector4Test, V4_Scalar_Mult_Static)
+{
+
+}
+
+TEST(Vector4Test, V4_Scalar_Div_Op)
+{
+
+}
+
+TEST(Vector4Test, V4_Scalar_Div_Method)
+{
+
+}
+
+TEST(Vector4Test, V4_Scalar_Div_Static)
+{
+
+}
+
+TEST(Vector4Test, V4_Sizeof)
+{
+
+}
+
+TEST(Vector4Test, V4_NaN)
+{
+
+}
+
+TEST(Vector4Tests, V4_Min) {}
+TEST(Vector4Tests, V4_Max) {}
+TEST(Vector4Tests, V4_Clamp) {}
+TEST(Vector4Tests, V4_DotProduct) {}
+TEST(Vector4Tests, V4_CrossProduct) {}
+TEST(Vector4Tests, V4_Project) {}
+TEST(Vector4Test, V4_Normalize) {}

tests/tests.cpp

@@ -0,0 +1,16 @@
+#include <gtest/gtest.h>
+
+
+GTEST_API_ int main(int argc, char** argv) {
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}
+
+#ifdef __WIN32
+extern "C" {
+ int wmain(int argc, wchar_t* argv[])
+    {
+        return main(argc, reinterpret_cast<char **>(argv));
+    }
+};
+#endif