Unfinished Work

CMakeLists.txt

@@ -29,35 +29,8 @@ file(GLOB_RECURSE J3ML_SRC     "src/J3ML/*.c" "src/J3ML/*.cpp")
 include_directories("include")
 
 add_library(J3ML SHARED ${J3ML_SRC}
-        src/J3ML/LinearAlgebra/AxisAngle.cpp
-        include/J3ML/LinearAlgebra/Vector.h
-        include/J3ML/Geometry/Plane.h
-        include/J3ML/Geometry/AABB.h
-        include/J3ML/Geometry/Frustum.h
-        include/J3ML/Geometry/OBB.h
-        include/J3ML/Geometry/Capsule.h
-        include/J3ML/Geometry/Sphere.h
-        include/J3ML/Geometry/Ray.h
-        include/J3ML/Geometry/QuadTree.h
-        include/J3ML/Geometry/LineSegment.h
-        include/J3ML/Geometry/TriangleMesh.h
-        include/J3ML/Geometry/Polygon.h
-        include/J3ML/Geometry/Triangle.h
-        include/J3ML/Geometry/Triangle2D.h
-        src/J3ML/Geometry/AABB.cpp
-        src/J3ML/Geometry/Plane.cpp
-        src/J3ML/Geometry/Sphere.cpp
-        src/J3ML/Geometry/Frustum.cpp
-        src/J3ML/Geometry/OBB.cpp
-        src/J3ML/Geometry/Ray.cpp
-        src/J3ML/Geometry/Capsule.cpp
-        src/J3ML/Geometry/TriangleMesh.cpp
-        src/J3ML/Geometry/QuadTree.cpp
-        src/J3ML/Geometry/LineSegment.cpp
-        include/J3ML/Geometry/AABB2D.h
-        src/J3ML/Geometry/Polygon.cpp
-        include/J3ML/Geometry/Polyhedron.h
-        src/J3ML/Geometry/Polyhedron.cpp)
+        include/J3ML/Geometry/Common.h
+        src/J3ML/Geometry/Triangle.cpp)
 set_target_properties(J3ML PROPERTIES LINKER_LANGUAGE CXX)
 
 install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME})

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/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

@@ -1,28 +1,22 @@
-#include <J3ML/LinearAlgebra/Vector2.h>
-#include <J3ML/LinearAlgebra/Vector3.h>
+#include <J3ML/LinearAlgebra.h>
 
 #pragma once
 
-namespace Geometry {
-    using Vector2 = LinearAlgebra::Vector2;
-    using Vector3 = LinearAlgebra::Vector3;
+#include <J3ML/Geometry/AABB2D.h>
+#include <J3ML/Geometry/Plane.h>
+#include <J3ML/Geometry/Sphere.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>
 
-    class LineSegment2D
-    {
-        Vector2 A;
-        Vector2 B;
-    };
-
-    class Rectangle;
-    class OBB2D;
-    class Line2D;
-    class Ray2D;
-    class Triangle2D;
-    class Polygon2D;
-
-
-    struct IntersectionResult2D {};
-
-    bool Intersects2D(LineSegment2D seg, Rectangle rect);
-    IntersectionResult2D GetIntersection2D(LineSegment2D seg, Rectangle rect);
-}
+using namespace J3ML::Geometry;

include/J3ML/Geometry/AABB.h

@@ -1,27 +1,17 @@
 #pragma once
 
-#include <J3ML/LinearAlgebra/Vector3.h>
-#include <J3ML/Geometry/Plane.h>
-#include <J3ML/Geometry/Sphere.h>
-#include <J3ML/Geometry/OBB.h>
-#include <J3ML/Geometry/LineSegment.h>
-#include <J3ML/Geometry/Triangle.h>
-#include <J3ML/Geometry/Polygon.h>
-#include <J3ML/Geometry/Frustum.h>
-#include <J3ML/Geometry/Capsule.h>
-#include <J3ML/Geometry/Ray.h>
-#include <J3ML/Geometry/TriangleMesh.h>
-#include <J3ML/Geometry/Polyhedron.h>
+#include <J3ML/LinearAlgebra.h>
+#include <J3ML/Geometry/Common.h>
+#include <J3ML/Geometry/Shape.h>
+#include "J3ML/Algorithm/RNG.h"
 
 
-// TODO: Fix circular include between AABB and OBB
-
-
-
-namespace Geometry
+namespace J3ML::Geometry
 {
 
-    using namespace LinearAlgebra;
+
+    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
@@ -31,53 +21,95 @@ namespace Geometry
     // 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
-    {
-        static AABB FromCenterAndSize(const Vector3 FromSize);
-        float MinX();
-        // Returns the smallest sphere that contains this AABB.
-        // This function computes the minimal volume sphere that contains all the points inside this AABB
+    class AABB : public Shape {
+    public:
+        Vector3 minPoint;
+        Vector3 maxPoint;
+
+        AABB();
+
+        AABB(const Vector3& min, const Vector3& max);
+
+        static int NumFaces() { return 6; }
+
+        static int NumEdges() { return 12; }
+
+        static int NumVertices() { return 8; }
+
+        static AABB FromCenterAndSize(const Vector3 &center, const Vector3 &size);
+
+        float MinX() const;
+
+        float MinY() const;
+
+        float MinZ() const;
+
+        float MaxX() const;
+
+        float MaxY() const;
+
+        float MaxZ() const;
+
+        /// Returns the smallest sphere that contains this AABB.
+        /// This function computes the minimal volume sphere that contains all the points inside this AABB
         Sphere MinimalEnclosingSphere() const;
-        // Returns the largest sphere that can fit inside this AABB
-        // This function computes the largest sphere that can fit inside this AABB.
+
+        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;
-        Vector3 GetCentroid() 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);
-        Vector3 ExtremePoint(const Vector3& direction) const;
-        Vector3 ExtremePoint(const Vector3& direction, float projectionDistance);
+
+        Vector3 CornerPoint(int cornerIndex) const;
+
+        Vector3 ExtremePoint(const Vector3 &direction) const;
+
+        Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance);
+
         Vector3 PointOnEdge(int edgeIndex, float u) const;
+
         Vector3 FaceCenterPoint(int faceIndex) const;
+
         Vector3 FacePoint(int faceIndex, float u, float v) const;
+
         Vector3 FaceNormal(int faceIndex) const;
-        Plane FacePlane(int faceIndex);
-        static AABB MinimalEnclosingAABB(const Vector3* pointArray, int numPoints);
-        Vector3 GetSize();
-        Vector3 GetVolume();
-        float GetVolumeCubed();
-        float GetSurfaceArea();
-        Vector3 GetRandomPointInside();
-        Vector3 GetRandomPointOnSurface();
-        Vector3 GetRandomPointOnEdge();
-        Vector3 GetRandomCornerPoint();
+
+        Plane FacePlane(int faceIndex) 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);
-        OBB Transform(const Matrix4x4& transform);
-        OBB Transform(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& triange) 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;
@@ -92,5 +124,33 @@ namespace Geometry
         TriangleMesh Triangulate(int numFacesX, int numFacesY, int numFacesZ, bool ccwIsFrontFacing) const;
         AABB Intersection(const AABB& rhs) const;
         bool IntersectLineAABB(const Vector3& linePos, const Vector3& lineDir, float tNear, float tFar) const;
+
+
+        void SetFrom(const Vector3 *pVector3, int i);
+
+        void SetFromCenterAndSize(const Vector3 &center, const Vector3 &size);
+
+        void SetFrom(const OBB &obb);
+
+        void SetFrom(const Sphere &s);
+
+        Vector3 GetRandomPointInside(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;
     };
+
+
 }

include/J3ML/Geometry/AABB2D.h

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

include/J3ML/Geometry/Capsule.h

@@ -1,27 +1,31 @@
 #pragma once
 
 #include "LineSegment.h"
+#include "Shape.h"
 #include <J3ML/LinearAlgebra/Vector3.h>
+#include <J3ML/Geometry/Common.h>
 
-namespace Geometry
+namespace J3ML::Geometry
 {
     using namespace LinearAlgebra;
-    class Capsule
+    class Capsule : public Shape
     {
+    public:
         // Specifies the two inner points of this capsule
         LineSegment l;
         // Specifies the radius of this capsule
         float r;
 
-        Capsule() {}
+        Capsule();
         Capsule(const LineSegment& endPoints, float radius);
         Capsule(const Vector3& bottomPt, const Vector3& topPt, float radius);
-        bool IsDegenerate()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;
     };
 }

include/J3ML/Geometry/Common.h

@@ -0,0 +1,30 @@
+#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 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

@@ -3,11 +3,16 @@
 //
 #pragma once
 
+#include <J3ML/Geometry/Common.h>
 #include "Plane.h"
+#include "Shape.h"
+#include <J3ML/LinearAlgebra.h>
 
-namespace Geometry
+namespace J3ML::Geometry
 {
 
+    using J3ML::LinearAlgebra::CoordinateFrame;
+
     enum class FrustumType
     {
         Invalid,
@@ -24,7 +29,7 @@ namespace Geometry
         Perspective
     };
 
-    class Frustum {
+    class Frustum : public Shape {
     public:
         Plane TopFace;
         Plane BottomFace;
@@ -32,24 +37,9 @@ namespace Geometry
         Plane LeftFace;
         Plane FarFace;
         Plane NearFace;
-        static Frustum CreateFrustumFromCamera(const Camera& cam, float aspect, float fovY, float zNear, float zFar);
-
+        static Frustum CreateFrustumFromCamera(const CoordinateFrame& cam, float aspect, float fovY, float zNear, float zFar);
+        AABB MinimalEnclosingAABB() const;
     };
 
-    Frustum Frustum::CreateFrustumFromCamera(const Camera &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;
-    }
 
 }

include/J3ML/Geometry/LineSegment.h

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

include/J3ML/Geometry/OBB.h

@@ -1,22 +1,29 @@
 #pragma once
 
+#include <J3ML/Geometry/Common.h>
 #include <J3ML/Geometry/AABB.h>
-#include "Polyhedron.h"
+#include <J3ML/Geometry/Polyhedron.h>
 
 
-
-namespace Geometry {
-    class OBB
+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 direc tion vectors for the local axes
+        // 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;  }
@@ -24,8 +31,13 @@ namespace Geometry {
         inline static int NumVertices() { return 8; }
 
         Polyhedron ToPolyhedron() const;
-
-        AABB MinimalEnclosingAABB() const;
+        //PBVolume<6> ToPBVolume() const;
+        AABB MinimalEnclosingAABB() const
+        {
+            AABB aabb;
+            aabb.SetFrom(*this);
+            return aabb;
+        }
 
         Sphere MinimalEnclosingSphere() const;
         Sphere MaximalContainedSphere() const;
@@ -33,6 +45,9 @@ namespace Geometry {
         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;
@@ -40,9 +55,44 @@ namespace Geometry {
 
         Vector3 AnyPointFast() const;
 
-        float Volume();
-        float SurfaceArea();
-        LineSegment Edge(int edgeIndex) const;
+        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);
     };
 }

include/J3ML/Geometry/Plane.h

@@ -1,13 +1,20 @@
 #pragma once
 #include <J3ML/LinearAlgebra/Vector3.h>
+#include "Shape.h"
 
-using namespace LinearAlgebra;
 
-class Plane
+namespace J3ML::Geometry
 {
-public:
-    Vector3 Position;
-    Vector3 Normal;
-    float distance = 0.f;
+    using J3ML::LinearAlgebra::Vector3;
 
-};
+    class Plane : public Shape
+    {
+    public:
+        Plane() : Shape() {}
+        Plane(const Vector3& pos, const Vector3& norm)
+        : Shape(), Position(pos), Normal(norm) {}
+        Vector3 Position;
+        Vector3 Normal;
+        float distance = 0.f;
+    };
+}

include/J3ML/Geometry/Polygon.h

@@ -1,7 +1,28 @@
 #pragma once
 
-namespace Geometry {
-    class Polygon {
+#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;
+        AABB MinimalEnclosingAABB() const;
+        int NumVertices() const
+        {
+            return (int)vertices.size();
+        }
+        Vector3 Vertex(int vertexIndex) const
+        {
+            assert(vertexIndex >= 0);
+            assert(vertexIndex < (int) vertices.size());
+            return vertices[vertexIndex];
+        }
+    protected:
+    private:
     };
 }

include/J3ML/Geometry/Polyhedron.h

@@ -1,8 +1,46 @@
 #pragma once
 
-namespace Geometry
-{
-    class Polyhedron {
+#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;
+
+    protected:
+    private:
     };
 }

include/J3ML/Geometry/QuadTree.h

@@ -5,11 +5,22 @@
 #include <J3ML/LinearAlgebra/Vector2.h>
 #include "AABB2D.h"
 
-namespace Geometry {
+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;

include/J3ML/Geometry/Ray.h

@@ -5,12 +5,56 @@
 #pragma once
 
 #include <J3ML/LinearAlgebra/Vector3.h>
+#include <vector>
+#include "TriangleMesh.h"
+#include "Frustum.h"
+#include "OBB.h"
 
-namespace Geometry
+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);
     };
 }

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

@@ -1,9 +1,71 @@
 #pragma once
 
-namespace Geometry
+#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
 {
-    class Sphere
+    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;
+        Sphere() {}
+        Sphere(const Vector3& pos, float radius) : Position(pos), Radius(radius) {}
+        void Translate(const Vector3& offset)
+        {
+            Position = Position + offset;
+        }
+        void Transform(const Matrix3x3& transform)
+        {
+            Position = transform * Position;
+        }
+        void Transform(const Matrix4x4& transform)
+        {
+            Position = transform * Position;
+        }
+        inline float Cube(float inp) const
+        {
+            return inp*inp*inp;
+        }
+        float Volume() const
+        {
+            return 4.f * M_PI * Cube(Radius) / 3.f;
+        }
+        float SurfaceArea() const
+        {
+            return 4.f * M_PI * Cube(Radius) / 3.f;
+        }
+        bool IsFinite() const
+        {
+            return Position.IsFinite() && std::isfinite(Radius);
+        }
+        bool IsDegenerate()
+        {
+            return !(Radius > 0.f) || !Position.IsFinite();
+        }
+        bool Contains(const Vector3& point) const
+        {
+            return Position.DistanceSquared(point) <= Radius*Radius;
+        }
+        bool Contains(const Vector3& point, float epsilon) const
+        {
+            return Position.DistanceSquared(point) <= Radius*Radius + epsilon;
+        }
+        bool Contains(const LineSegment& lineseg)  const
+        {
+
+        }
+        TriangleMesh GenerateUVSphere() const {}
+        TriangleMesh GenerateIcososphere() const {}
 
     };
 }

include/J3ML/Geometry/Triangle.h

@@ -1,9 +1,20 @@
 #pragma once
 
-namespace Geometry
+#include <J3ML/Geometry/Common.h>
+#include <J3ML/LinearAlgebra.h>
+
+namespace J3ML::Geometry
 {
     class Triangle
     {
+    public:
+        Vector3 V0;
+        Vector3 V1;
+        Vector3 V2;
 
+        bool Intersects(const AABB& aabb) const;
+        AABB BoundingAABB() const;
     };
+
+
 }

include/J3ML/Geometry/Triangle2D.h

@@ -1,8 +1,9 @@
-//
-// Created by dawsh on 1/25/24.
-//
+#pragma once
 
-#ifndef J3ML_TRIANGLE2D_H
-#define J3ML_TRIANGLE2D_H
 
-#endif //J3ML_TRIANGLE2D_H
+namespace J3ML::Geometry
+{
+    class Triangle2D {
+    public:
+    };
+}

include/J3ML/Geometry/TriangleMesh.h

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

include/J3ML/J3ML.h

@@ -1,17 +1,242 @@
+#pragma once
+
+
+
 //
 // Created by josh on 12/25/2023.
 //
 #include <cstdint>
+#include <cmath>
+#include <stdfloat>
+#include <string>
+#include <cassert>
 
-namespace J3ML
+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);
+
+    // 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

@@ -1,76 +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
-#include <cstdint>
-#include <cmath>
-#include <cstdlib>
-#include <algorithm>
-#include <functional>
-
-
-namespace Math
-{
-    const float Pi = M_PI;
-    inline float Radians(float degrees) { return degrees * (Pi/180.f); }
-    inline float Degrees(float radians) { return radians * (180.f/Pi); }
-
-
-    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);
-
-}
-
-
-// Dawsh Linear Algebra Library - Everything you need for 3D math
-namespace 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;
-}
 
 // TODO: Enforce Style Consistency (Function Names use MicroSoft Case)
-
-// Note: Josh Linear Algebra Types are designed as Immutable Data Types
-// x, y, z, w, etc. values should not and can not be set directly.
-// rather, you just construct a new type and assign it.
-// This might sound ass-backwards for many object types.
-// But mathematically, a vector or matrix is defined by it's size, and values.
-// Changing the value of one axis fundamentally changes the definition of the vector/matrix.
-// So we enforce this conceptually at code level...
-// If you're wondering how it remains performant, it only heap-allocates a tiny space (4*n bytes for vectors) (4*n*m bytes for matrices)
-// Just Trust Me Bro - Josjh
-#define MUTABLE true // Toggle This For: Ugly math, ugly code, and an ugly genital infection!
-
-#if MUTABLE
-#define IMMUTABLE !MUTABLE
-#endif
-
-namespace LinearAlgebra
-{
+// 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

@@ -1,14 +1,22 @@
 #pragma once
 
-#include <J3ML/LinearAlgebra.h>
 
-namespace LinearAlgebra {
+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

@@ -1,9 +1,11 @@
 #pragma once
 
-#include <J3ML/LinearAlgebra.h>
+#include <J3ML/LinearAlgebra/EulerAngle.h>
+#include <J3ML/LinearAlgebra/Quaternion.h>
+#include <J3ML/LinearAlgebra/AxisAngle.h>
 #include <J3ML/LinearAlgebra/Vector3.h>
 
-namespace LinearAlgebra
+namespace J3ML::LinearAlgebra
 {
 
     /// Transitional datatype, not useful for internal representation of rotation

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

@@ -1,20 +1,17 @@
 #pragma once
 
-#include <J3ML/LinearAlgebra.h>
 
-namespace LinearAlgebra
+#include <J3ML/LinearAlgebra/Vector3.h>
+
+namespace J3ML::LinearAlgebra
 {
     /// The CFrame is fundamentally 4 vectors (position, forward, right, up vector)
     class CoordinateFrame
     {
-        Vector3 getPosition();
-        Vector3 getLookVector();
-        Vector3 getRightVector();
-        Vector3 getUpVector();
-        AxisAngle GetAxisAngle();
-        EulerAngle GetEulerAngleXYZ();
-        EulerAngle GetWorldAngleYZX();
+    public:
+        Vector3 Position;
+        Vector3 Front;
+        Vector3 Right;
+        Vector3 Up;
     };
-
-
 }

include/J3ML/LinearAlgebra/EulerAngle.h

@@ -1,8 +1,12 @@
 #pragma once
-#include <J3ML/LinearAlgebra.h>
-#include <J3ML/LinearAlgebra/Vector3.h>
 
-namespace LinearAlgebra {
+#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

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

@@ -1,9 +1,9 @@
 #pragma once
 
-#include <J3ML/LinearAlgebra.h>
+
 #include <J3ML/LinearAlgebra/Vector2.h>
 
-namespace LinearAlgebra {
+namespace J3ML::LinearAlgebra {
     class Matrix2x2 {
     public:
         enum { Rows = 3 };
@@ -16,9 +16,17 @@ namespace LinearAlgebra {
         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;

include/J3ML/LinearAlgebra/Matrix3x3.h

@@ -1,11 +1,15 @@
 #pragma once
 
-#include <J3ML/LinearAlgebra.h>
+#include <J3ML/LinearAlgebra/Common.h>
 #include <J3ML/LinearAlgebra/Vector2.h>
 #include <J3ML/LinearAlgebra/Vector3.h>
 #include <J3ML/LinearAlgebra/Quaternion.h>
 
-namespace LinearAlgebra {
+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.
@@ -22,6 +26,7 @@ namespace LinearAlgebra {
      *  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 };
@@ -36,6 +41,8 @@ namespace LinearAlgebra {
         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);
@@ -44,6 +51,11 @@ namespace LinearAlgebra {
         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);
@@ -51,32 +63,18 @@ namespace LinearAlgebra {
         /// Creates a new M3x3 that rotates about the given axis by the given angle
         static Matrix3x3 RotateAxisAngle(const Vector3& axis, float angleRadians);
 
-        static Matrix3x3 RotateFromTo(const Vector3& source, const Vector3& direction)
-        {
-
-        }
+        // 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)
-        {
-            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);
-        }
+        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)
-        {
-            return Matrix3x3(orientation);
-        }
+        static Matrix3x3 FromQuat(const Quaternion& orientation);
 
         Quaternion ToQuat() const;
 
@@ -86,20 +84,14 @@ namespace LinearAlgebra {
         // 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)
-        {
-            return Matrix3x3(rotate) * Matrix3x3::Scale(scale);
-        }
-        static Matrix3x3 FromRS(const Matrix3x3 &rotate, const Vector3& scale)
-        {
-            return rotate * Matrix3x3::Scale(scale);
-        }
+        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 Scale(float sx, float sy, float sz);
-        static Matrix3x3 Scale(const Vector3& scale);
+        static Matrix3x3 FromScale(float sx, float sy, float sz);
+        static Matrix3x3 FromScale(const Vector3& scale);
 
         /// Returns the main diagonal.
         Vector3 Diagonal() const;
@@ -132,9 +124,36 @@ namespace LinearAlgebra {
         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;
+        Vector4 operator * (const Vector4& 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

@@ -1,9 +1,15 @@
 #pragma once
 
-#include <J3ML/LinearAlgebra.h>
+#include <J3ML/LinearAlgebra/Common.h>
+
+#include <J3ML/LinearAlgebra/Matrix3x3.h>
 #include <J3ML/LinearAlgebra/Quaternion.h>
 
-namespace LinearAlgebra {
+
+#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,
@@ -12,7 +18,7 @@ namespace LinearAlgebra {
      * 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, am_23,
+     *  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.
@@ -20,106 +26,247 @@ namespace LinearAlgebra {
      */
     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
+        /// A constant matrix that has zeroes in all its entries
         static const Matrix4x4 Zero;
-        // A constant matrix that is the identity.
+        /// 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!
+        /// 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);
+                  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;
-        Matrix3x3 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)
-            };
-        }
+        /// 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);
 
-
-        Matrix4x4(const Quaternion& orientation, const Vector3& translation);
-
         Vector4 GetRow(int index) const;
         Vector4 GetColumn(int index) const;
-        float At(int x, int y) const
+        Vector3 GetRow3(int index) const;
+        Vector3 GetColumn3(int index) const;
+
+        Vector3 GetScale() const
         {
-            return elems[x][y];
+
+        }
+        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);
         }
 
-        Vector4 Diagonal() const;
-        Vector4 WorldX() const;
-        Vector4 WorldY() const;
-        Vector4 WorldZ() const;
 
+        void SwapColumns(int col1, int col2);
+
+        /// Swaps two rows.
+        void SwapRows(int row1, int row2);
+        /// Swapsthe xyz-parts of two rows element-by-element
+        void SwapRows3(int row1, int row2);
+
+        void ScaleRow(int row, float scalar);
+        void ScaleRow3(int row, float scalar);
+        void ScaleColumn(int col, float scalar);
+        void ScaleColumn3(int col, float scalar);
+        /// 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;
 
-        Matrix4x4 Inverse() 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 nearPlane, float farPlane, float hViewportSize, float vViewportSize);
-        static Matrix4x4 OpenGLOrthoProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
-        static Matrix4x4 OpenGLPerspProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
-        static Matrix4x4 OpenGLPerspProjRH(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);
 
-        Vector3 GetTranslationComponent() const;
-        Matrix3x3 GetRotationComponent() const;
+        Vector4 operator[](int row);
 
-        Vector4 GetRow() const;
-        Vector4 GetColumn() const;
+        Matrix4x4 operator-() const;
+        Matrix4x4 operator +(const Matrix4x4& rhs) const;
+        Matrix4x4 operator - (const Matrix4x4& rhs) const;
 
-        Vector4 operator[](int row)
-        {
-            return Vector4{elems[row][0], elems[row][1], elems[row][2], elems[row][3]};
-        }
+        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;
     protected:
         float elems[4][4];
 
-        void SetMatrixRotatePart(Matrix4x4 &m, const Quaternion &q);
+
     };
 }

include/J3ML/LinearAlgebra/Quaternion.h

@@ -1,58 +1,70 @@
 #pragma once
 
-#include <J3ML/LinearAlgebra.h>
 
+
+
+#include <J3ML/LinearAlgebra/Matrix3x3.h>
+#include <J3ML/LinearAlgebra/Matrix4x4.h>
 #include <J3ML/LinearAlgebra/Vector4.h>
 #include <J3ML/LinearAlgebra/AxisAngle.h>
-#include <J3ML/LinearAlgebra/Matrix3x3.h>
+//#include <J3ML/LinearAlgebra/AxisAngle.h>
+#include <cmath>
 
-namespace LinearAlgebra
+
+
+namespace J3ML::LinearAlgebra
 {
-    class Quaternion : public Vector4
-    {
+
+    class Matrix3x3;
+
+    class Quaternion : public Vector4 {
     public:
         Quaternion();
-        Quaternion(const Quaternion& rhs) = default;
-        explicit Quaternion(const Matrix3x3& rotationMtrx);
-        explicit Quaternion(const Matrix4x4& rotationMtrx);
+
+        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) { SetFromAxisAngle(rotationAxis, rotationAngleBetween); }
-        Quaternion(const Vector4& rotationAxis, float rotationAngleBetween) { SetFromAxisAngle(rotationAxis, rotationAngleBetween); }
+        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 rcpSinAngle = 1 - (std::sqrt(1 - w * w));
-
-            return Vector3(x, y, z) * rcpSinAngle;
-        }
-
-        float GetAngle() const
-        {
-            return std::acos(w) * 2.f;
-        }
+        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
@@ -89,7 +101,7 @@ namespace LinearAlgebra
         Quaternion operator - () const;
         float Dot(const Quaternion &quaternion) const;
 
-        float Angle() const;
+        float Angle() const { return acos(w) * 2.f;}
 
         float AngleBetween(const Quaternion& target) const;
 

include/J3ML/LinearAlgebra/Transform2D.h

@@ -1,9 +1,9 @@
 #pragma once
 
-#include <J3ML/LinearAlgebra.h>
+
 #include <J3ML/LinearAlgebra/Matrix3x3.h>
 
-namespace LinearAlgebra {
+namespace J3ML::LinearAlgebra {
     class Transform2D {
     protected:
         Matrix3x3 transformation;

include/J3ML/LinearAlgebra/Vector.h

@@ -1,13 +1,15 @@
 #pragma once
 
 
-template <typename T, int Dims>
-class templated_vector
+#include <cstdlib>
+
+namespace J3ML::LinearAlgebra
 {
+    template <uint DIMS, typename T>
+    class Vector {
+    public:
+        enum { Dimensions = DIMS};
+        T elems[DIMS];
+    };
 
-};
-
-
-using v2f = templated_vector<float, 2>;
-using v3f = templated_vector<float, 3>;
-using v4f = templated_vector<float, 4>;
+}

include/J3ML/LinearAlgebra/Vector2.h

@@ -1,15 +1,26 @@
+#pragma clang diagnostic push
+#pragma ide diagnostic ignored "modernize-use-nodiscard"
 #pragma once
-#include <J3ML/LinearAlgebra.h>
+#include <J3ML/J3ML.h>
 #include <cstddef>
 
-namespace LinearAlgebra {
+namespace J3ML::LinearAlgebra {
+    using namespace J3ML;
+
     /// A 2D (x, y) ordered pair.
     class Vector2 {
     public:
+
+        enum {Dimensions = 2};
+
         /// Default Constructor - Initializes values to zero
         Vector2();
         /// Constructs a new Vector2 with the value (X, Y)
         Vector2(float X, float Y);
+        /// Constructs this float2 from a C array, to the value (data[0], data[1]).
+        explicit Vector2(const float* data);
+        // Constructs a new Vector2 with the value {scalar, scalar}
+        explicit Vector2(float scalar);
         Vector2(const Vector2& rhs);   // Copy Constructor
         //Vector2(Vector2&&) = default;   // Move Constructor
 
@@ -19,19 +30,43 @@ namespace LinearAlgebra {
         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;
 
-        float operator[](std::size_t index);
+
         bool operator == (const Vector2& rhs) const;
         bool operator != (const Vector2& rhs) const;
 
@@ -65,6 +100,7 @@ namespace LinearAlgebra {
         float Magnitude() const;
         static float Magnitude(const Vector2& of);
 
+
         bool IsFinite() const;
         static bool IsFinite(const Vector2& v);
 
@@ -112,6 +148,10 @@ namespace LinearAlgebra {
         /// 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.
@@ -128,8 +168,9 @@ namespace LinearAlgebra {
         Vector2 operator +() const; // TODO: Implement
         Vector2 operator -() const;
         /// Assigns a vector to another
-        Vector2& operator+=(const Vector2& rhs); // Adds a vector to this vector, in-place.
-        Vector2& operator-=(const Vector2& rhs); // Subtracts a vector from this vector, in-place
+        Vector2 &operator =(const Vector2 &rhs);
+        Vector2& operator+=(const Vector2& rhs);
+        Vector2& operator-=(const Vector2& rhs);
         Vector2& operator*=(float scalar);
         Vector2& operator/=(float scalar);
 
@@ -138,4 +179,10 @@ namespace LinearAlgebra {
         float y = 0;
 
     };
-}
+
+    static Vector2 operator*(float lhs, const Vector2 &rhs)
+    {
+        return rhs * lhs;
+    }
+}
+#pragma clang diagnostic pop

include/J3ML/LinearAlgebra/Vector3.h

@@ -1,25 +1,26 @@
 #pragma once
 
+#include <J3ML/LinearAlgebra/Vector2.h>
 #include <J3ML/LinearAlgebra/Vector3.h>
 #include <cstddef>
 #include <cstdlib>
 #include <J3ML/LinearAlgebra/Angle2D.h>
 
-
-namespace LinearAlgebra {
+namespace J3ML::LinearAlgebra {
 
 // A 3D (x, y, z) ordered pair.
 class Vector3 {
 public:
 
+    enum {Dimensions = 3};
+
     // Default Constructor - Initializes to zero
     Vector3();
     // Constructs a new Vector3 with the value (X, Y, Z)
     Vector3(float X, float Y, float Z);
     Vector3(const Vector3& rhs); // Copy Constructor
     Vector3(Vector3&&) = default; // Move Constructor
-    Vector3& operator=(const Vector3& rhs);
-
+    explicit Vector3(const float* data);
 
     static const Vector3 Zero;
     static const Vector3 Up;
@@ -29,38 +30,24 @@ public:
     static const Vector3 Forward;
     static const Vector3 Backward;
     static const Vector3 NaN;
+    static const Vector3 Infinity;
+    static const Vector3 NegativeInfinity;
 
-    static void Orthonormalize(Vector3& a, Vector3& b)
-    {
-        a = a.Normalize();
-        b = b - b.ProjectToNorm(a);
-        b = b.Normalize();
-    }
+    float* ptr();
 
+    static void Orthonormalize(Vector3& a, Vector3& b);
 
-    //Returns the DirectionVector for a given angle.
+    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);
 
-    static void 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();
-    }
+    bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon);
 
-    bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon)
-    {
-
-    }
-
-    Vector3 ProjectToNorm(const Vector3& direction)
-    {
-        return direction * this->Dot(direction);
-    }
+    Vector3 ProjectToNorm(const Vector3& direction) const;
 
     float GetX() const;
     float GetY() const;
@@ -75,55 +62,65 @@ public:
     bool IsPerpendicular(const Vector3& other, float epsilonSq=1e-5f) const;
 
     float operator[](std::size_t index) const;
+    float &operator[](std::size_t index);
     bool operator == (const Vector3& rhs) const;
     bool operator != (const Vector3& rhs) const;
 
+    bool IsFinite() const;
+    float MinElement() const;
+    static float MinElement(const Vector3& of);
+
     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.
+    /// Returns the magnitude between the two vectors.
     float Distance(const Vector3& to) const;
     static float Distance(const Vector3& from, const Vector3& to);
 
+    float DistanceSquared(const Vector3& to) const;
+    static float DistanceSquared(const Vector3& from, const Vector3& to);
+
     float Length() const;
     static float Length(const Vector3& of);
 
     float LengthSquared() const;
     static float LengthSquared(const Vector3& of);
 
-    // Returns the length of the vector, which is sqrt(x^2 + y^2 + z^2)
+    /// 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.
+    /// 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)
+    /// 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.
+    /// 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"
+    /// 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).
+    /// 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);
 
@@ -136,28 +133,53 @@ public:
     Vector3 Add(const Vector3& rhs) const;
     static Vector3 Add(const Vector3& lhs, const Vector3& rhs);
 
-    // Subtracts two vectors
+    /// 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
+    /// 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);
 
-    // Divides this vector by a scalar
+    /// 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);
 
-    // Unary + operator
+    /// 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)
+    /// 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);
 public:
      float x = 0;
      float y = 0;
      float z = 0;
 };
+
+    static Vector3 operator*(float lhs, const Vector3& rhs)
+    {
+        return rhs * lhs;
+    }
 }

include/J3ML/LinearAlgebra/Vector4.h

@@ -1,9 +1,9 @@
 #pragma once
 
-#include <J3ML/LinearAlgebra.h>
 
+#include <J3ML/LinearAlgebra/Vector3.h>
 
-namespace LinearAlgebra {
+namespace J3ML::LinearAlgebra {
     class Vector4 {
     public:
         // Default Constructor
@@ -16,6 +16,15 @@ namespace LinearAlgebra {
         Vector4(Vector4&& move) = default;
         Vector4& operator=(const Vector4& rhs);
 
+        float* ptr()
+        {
+            return &x;
+        }
+        Vector3 XYZ() const
+        {
+            return {x, y, z};
+        }
+
         float GetX() const;
         float GetY() const;
         float GetZ() const;
@@ -40,6 +49,9 @@ namespace LinearAlgebra {
         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;
@@ -81,6 +93,8 @@ namespace LinearAlgebra {
 
         Vector4 operator+() const; // Unary + Operator
         Vector4 operator-() const; // Unary - Operator (Negation)
+
+
     public:
 #if MUTABLE
         float x;

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>;
+
+}

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

@@ -1,5 +1,622 @@
 #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 Geometry {
+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) {
+        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;
+    }
+
+    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;
+
+    }
+
+}

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

@@ -1,6 +1,15 @@
 #include <J3ML/Geometry/Capsule.h>
+#include <J3ML/Geometry/AABB.h>
 
-namespace Geometry
+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;
+    }
 }

src/J3ML/Geometry/Frustum.cpp

@@ -1 +1,21 @@
 #include <J3ML/Geometry/Frustum.h>
+#include <cmath>
+
+namespace J3ML::Geometry
+{
+    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;
+    }
+}

src/J3ML/Geometry/LineSegment.cpp

@@ -1,3 +1,11 @@
-//
-// Created by dawsh on 1/25/24.
-//
+#include <J3ML/Geometry/LineSegment.h>
+
+namespace J3ML::Geometry {
+
+    LineSegment::LineSegment(const Vector3 &a, const Vector3 &b) : A(a), B(b)
+    {
+
+    }
+
+    LineSegment::LineSegment() {}
+}

src/J3ML/Geometry/OBB.cpp

@@ -1,3 +1,383 @@
-//
-// Created by dawsh on 1/25/24.
-//
+#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());
+    }
+
+
+}

src/J3ML/Geometry/Polygon.cpp

@@ -1,5 +1,14 @@
 #include <J3ML/Geometry/Polygon.h>
+#include <J3ML/Geometry/AABB.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;
+    }
+}
 
-namespace Geometry {
 
-}

src/J3ML/Geometry/Polyhedron.cpp

@@ -1,6 +1,19 @@
 #include <J3ML/Geometry/Polyhedron.h>
+#include <J3ML/Geometry/AABB.h>
 
-namespace Geometry
+namespace J3ML::Geometry
 {
+    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];
+    }
+
+}
 
-}

src/J3ML/Geometry/Ray.cpp

@@ -1,6 +1,126 @@
 #include <J3ML/Geometry/Ray.h>
+#include <J3ML/Geometry/Sphere.h>
 
-namespace Geometry
+
+namespace J3ML::Geometry
 {
+    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();
+    }
+}
+
 
-}

src/J3ML/Geometry/Sphere.cpp

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

src/J3ML/Geometry/Triangle.cpp

@@ -0,0 +1,17 @@
+#include <J3ML/Geometry/Triangle.h>
+#include <J3ML/LinearAlgebra.h>
+#include <J3ML/Geometry/AABB.h>
+
+namespace J3ML::Geometry
+{
+    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);
+    }
+}

src/J3ML/J3ML.cpp

@@ -0,0 +1,47 @@
+#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;
+    }
+
+    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

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

src/J3ML/LinearAlgebra/AxisAngle.cpp

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

src/J3ML/LinearAlgebra/EulerAngle.cpp

@@ -3,50 +3,49 @@
 #include <algorithm>
 
 
-#pragma region EulerAngle
-namespace LinearAlgebra {
-EulerAngle::EulerAngle(float pitch, float yaw, float roll): pitch(pitch), yaw(yaw), roll(roll)
-{}
+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::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::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); }
+    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);
-}
+    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;
-}
+    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::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

@@ -1,6 +1,6 @@
 #include <J3ML/LinearAlgebra/Matrix2x2.h>
 
-namespace LinearAlgebra {
+namespace J3ML::LinearAlgebra {
 
     Vector2 Matrix2x2::GetRow(int index) const {
         float x = this->elems[index][0];
@@ -18,4 +18,8 @@ namespace LinearAlgebra {
     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

@@ -1,7 +1,7 @@
 #include <J3ML/LinearAlgebra/Matrix3x3.h>
 #include <cmath>
 
-namespace LinearAlgebra {
+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);
@@ -296,5 +296,135 @@ namespace LinearAlgebra {
         };
     }
 
+    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);
+    }
+
 }
 

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -1,8 +1,11 @@
 #include <J3ML/LinearAlgebra/Matrix4x4.h>
 #include <J3ML/LinearAlgebra/Vector4.h>
 
-namespace LinearAlgebra {
-    const Matrix4x4 Matrix4x4::Zero = Matrix4x4(0);
+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);
 
@@ -90,11 +93,8 @@ namespace LinearAlgebra {
         elems[2][3] = offset.z;
     }
 
-    void Matrix4x4::SetRotatePart(const Quaternion &q) {
-        SetMatrixRotatePart(*this, q);
-    }
 
-    void Matrix4x4::SetMatrixRotatePart(Matrix4x4 &m, const Quaternion& q)
+    void Matrix4x4::SetRotatePart(const Quaternion& q)
     {
         // See e.g. http://www.geometrictools.com/Documentation/LinearAlgebraicQuaternions.pdf .
         const float x = q.x;
@@ -106,6 +106,15 @@ namespace LinearAlgebra {
         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);
     }
@@ -167,5 +176,462 @@ namespace LinearAlgebra {
         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);
+    }
 
 }

src/J3ML/LinearAlgebra/Quaternion.cpp

@@ -1,9 +1,10 @@
 #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 LinearAlgebra {
+namespace J3ML::LinearAlgebra {
     Quaternion Quaternion::operator-() const
     {
         return {-x, -y, -z, -w};
@@ -46,7 +47,18 @@ namespace LinearAlgebra {
 
     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 {
@@ -163,4 +175,30 @@ namespace LinearAlgebra {
                 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

@@ -1,6 +1,6 @@
 #include <J3ML/LinearAlgebra/Transform2D.h>
 
-namespace LinearAlgebra {
+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);
@@ -8,7 +8,7 @@ namespace LinearAlgebra {
 
 
     Vector2 Transform2D::Transform(const Vector2 &input) const {
-        return transformation * input;
+        return transformation.Transform(input);
     }
 
     Transform2D::Transform2D(const Matrix3x3 &transform) : transformation(transform) { }

src/J3ML/LinearAlgebra/Vector2.cpp

@@ -4,7 +4,7 @@
 #include <valarray>
 #include <iostream>
 
-namespace LinearAlgebra {
+namespace J3ML::LinearAlgebra {
 
     Vector2::Vector2(): x(0), y(0)
     {}
@@ -15,12 +15,14 @@ namespace LinearAlgebra {
     Vector2::Vector2(const Vector2& rhs): x(rhs.x), y(rhs.y)
     {}
 
-    float Vector2::operator[](std::size_t index)
+    float Vector2::operator[](std::size_t index) const
     {
-        assert(index < 2);
-        if (index == 0) return x;
-        if (index == 1) return y;
-        return 0;
+        return At(index);
+    }
+
+    float &Vector2::operator[](std::size_t index)
+    {
+        return At(index);
     }
 
     bool Vector2::IsWithinMarginOfError(const Vector2& rhs, float margin) const
@@ -159,6 +161,7 @@ namespace LinearAlgebra {
     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; }
 
@@ -247,5 +250,87 @@ namespace LinearAlgebra {
         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();
+    }
 }

src/J3ML/LinearAlgebra/Vector3.cpp

@@ -3,9 +3,7 @@
 #include <cassert>
 #include <cmath>
 
-namespace LinearAlgebra {
-
-#pragma region vector3
+namespace J3ML::LinearAlgebra {
 
     const Vector3 Vector3::Zero     = {0,0,0};
     const Vector3 Vector3::Up       = {0, -1, 0};
@@ -15,6 +13,8 @@ namespace LinearAlgebra {
     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
     {
@@ -80,6 +80,14 @@ namespace LinearAlgebra {
         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;
@@ -95,6 +103,8 @@ namespace LinearAlgebra {
         return this->IsWithinMarginOfError(rhs) == false;
     }
 
+
+
     Vector3 Vector3::Min(const Vector3& min) const
     {
         return {
@@ -308,5 +318,126 @@ namespace LinearAlgebra {
         return {x, y, z};
     }
 
-#pragma endregion
+    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))
+        };
+    }
+
+
 }

src/J3ML/LinearAlgebra/Vector4.cpp

@@ -7,8 +7,10 @@
 #include <cmath>
 #include <algorithm>
 
-namespace LinearAlgebra {
+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)
     {}
@@ -139,6 +141,29 @@ Vector4 Vector4::operator-(const Vector4& rhs) const
 
     }
 
+    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;
+    }
+
 
 }
 #pragma endregion

tests/LinearAlgebra/Vector2Tests.cpp

@@ -1,7 +1,7 @@
 #include <gtest/gtest.h>
 #include <J3ML/LinearAlgebra/Vector2.h>
 
-using Vector2 = LinearAlgebra::Vector2;
+using J3ML::LinearAlgebra::Vector2;
 
 TEST(Vector2Test, V2_Constructor_Default)
 {

tests/LinearAlgebra/Vector3Tests.cpp

@@ -1,7 +1,7 @@
 #include <gtest/gtest.h>
 #include <J3ML/LinearAlgebra/Vector3.h>
 
-using Vector3 = LinearAlgebra::Vector3;
+using J3ML::LinearAlgebra::Vector3;
 
 void EXPECT_V3_EQ(const Vector3& lhs, const Vector3& rhs)
 {
@@ -185,11 +185,12 @@ TEST(Vector3Test, V3_Lerp)
     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();
-    LinearAlgebra::Angle2D ExpectedResult {-0.69791365, -2.3561945};
+    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);

tests/LinearAlgebra/Vector4Tests.cpp

@@ -1,7 +1,7 @@
 #include <gtest/gtest.h>
 #include <J3ML/LinearAlgebra/Vector4.h>
 
-using Vector4 = LinearAlgebra::Vector4;
+using Vector4 = J3ML::LinearAlgebra::Vector4;
 
 
 void EXPECT_V4_EQ(const Vector4& lhs, const Vector4& rhs)