Unfinished Work

Implement Vector4::Equals
Implement Vector3::Equals
2024-03-23 16:20:57 -04:00 · 2024-03-21 20:43:16 -04:00 · 2024-03-21 20:43:07 -04:00 · 2024-03-21 18:37:43 -04:00 · 2024-03-21 15:24:50 -04:00 · 2024-03-21 14:00:31 -04:00
40 changed files with 1749 additions and 226 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -29,41 +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/Algorithm/RNG.h
-        src/J3ML/Algorithm/RNG.cpp
-        include/J3ML/Algorithm/Spring.h
-        include/J3ML/Algorithm/DifferentialSolvers.h
-        include/J3ML/Units.h
-        src/J3ML/J3ML.cpp)
+        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})
--- a/include/J3ML/Geometry.h
+++ b/include/J3ML/Geometry.h
@@ -1,32 +1,22 @@
-#include <J3ML/LinearAlgebra/Vector2.h>
-#include <J3ML/LinearAlgebra/Vector3.h>
+#include <J3ML/LinearAlgebra.h>

 #pragma once

-namespace J3ML::Geometry {
-    using Vector2 = J3ML::LinearAlgebra::Vector2;
-    using Vector3 = J3ML::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 AABB;
-    class OBB;
-    class Capsule;
-    class Frustum;
-    class OBB2D;
-    class Line2D;
-    class Ray2D;
-    class Triangle2D;
-    class Polygon2D;
-
-
-    struct IntersectionResult2D {};
-
-    bool Intersects2D(LineSegment2D seg, Rectangle rect);
-    IntersectionResult2D GetIntersection2D(LineSegment2D seg, Rectangle rect);
-}
+using namespace J3ML::Geometry;
--- a/include/J3ML/Geometry/AABB.h
+++ b/include/J3ML/Geometry/AABB.h
@@ -1,30 +1,17 @@
 #pragma once

-#include <J3ML/LinearAlgebra/Vector3.h>
-
-#include "J3ML/LinearAlgebra.h"
-#include <J3ML/Geometry.h>
-
-#include <J3ML/Geometry/Plane.h>
-#include <J3ML/Geometry/Sphere.h>
-#include <J3ML/Geometry/OBB.h>
-#include <J3ML/Geometry/LineSegment.h>
-#include <J3ML/Geometry/Triangle.h>
-#include <J3ML/Geometry/Polygon.h>
-#include <J3ML/Geometry/Frustum.h>
-#include <J3ML/Geometry/Capsule.h>
-#include <J3ML/Geometry/Ray.h>
-#include <J3ML/Geometry/TriangleMesh.h>
-#include <J3ML/Geometry/Polyhedron.h>
-
-
-// TODO: Fix circular include between AABB and OBB
-
+#include <J3ML/LinearAlgebra.h>
+#include <J3ML/Geometry/Common.h>
+#include <J3ML/Geometry/Shape.h>
+#include "J3ML/Algorithm/RNG.h"


 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
@@ -34,11 +21,15 @@ namespace J3ML::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 {
+    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; }
@@ -102,23 +93,23 @@ namespace J3ML::Geometry
        static AABB MinimalEnclosingAABB(const Vector3 *pointArray, int numPoints);
        float GetVolume() const;
        float GetSurfaceArea() const;
-        Vector3 GetRandomPointInside();
-        Vector3 GetRandomPointOnSurface();
-        Vector3 GetRandomPointOnEdge();
-        Vector3 GetRandomCornerPoint();
+        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;
@@ -143,7 +134,10 @@ namespace J3ML::Geometry

        void SetFrom(const Sphere &s);

-        Vector3 GetRandomPointInside() const;
+        Vector3 GetRandomPointInside(RNG& rng) const;
+        Vector3 GetRandomPointOnSurface(RNG& rng) const;
+        Vector3 GetRandomPointOnEdge(RNG& rng) const;
+        Vector3 GetRandomCornerPoint(RNG& rng) const;

        void SetNegativeInfinity();

@@ -154,5 +148,9 @@ namespace J3ML::Geometry
        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;
    };
+
+
 }
--- a/include/J3ML/Geometry/AABB2D.h
+++ b/include/J3ML/Geometry/AABB2D.h
@@ -1,12 +1,13 @@
 #pragma once

 #include <J3ML/LinearAlgebra/Vector2.h>
+#include "Shape.h"

 namespace J3ML::Geometry
 {
    using LinearAlgebra::Vector2;
    // CaveGame AABB
-    class AABB2D
+    class AABB2D : public Shape2D
    {
    public:

@@ -17,90 +18,37 @@ namespace J3ML::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;

    };
 }
--- a/include/J3ML/Geometry/Capsule.h
+++ b/include/J3ML/Geometry/Capsule.h
@@ -1,13 +1,16 @@
 #pragma once

 #include "LineSegment.h"
+#include "Shape.h"
 #include <J3ML/LinearAlgebra/Vector3.h>
+#include <J3ML/Geometry/Common.h>

 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
@@ -23,5 +26,6 @@ namespace J3ML::Geometry
        Vector3 Center() const;
        Vector3 Centroid() const;
        Vector3 ExtremePoint(const Vector3& direction);
+        AABB MinimalEnclosingAABB() const;
    };
 }
--- a/include/J3ML/Geometry/Common.h
+++ b/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
+{
+
+}
--- a/include/J3ML/Geometry/Frustum.h
+++ b/include/J3ML/Geometry/Frustum.h
@@ -3,8 +3,10 @@
 //
 #pragma once

+#include <J3ML/Geometry/Common.h>
 #include "Plane.h"
-#include <J3ML/LinearAlgebra/CoordinateFrame.h>
+#include "Shape.h"
+#include <J3ML/LinearAlgebra.h>

 namespace J3ML::Geometry
 {
@@ -27,7 +29,7 @@ namespace J3ML::Geometry
        Perspective
    };

-    class Frustum {
+    class Frustum : public Shape {
    public:
        Plane TopFace;
        Plane BottomFace;
@@ -36,6 +38,7 @@ namespace J3ML::Geometry
        Plane FarFace;
        Plane NearFace;
        static Frustum CreateFrustumFromCamera(const CoordinateFrame& cam, float aspect, float fovY, float zNear, float zFar);
+        AABB MinimalEnclosingAABB() const;
    };


--- a/include/J3ML/Geometry/OBB.h
+++ b/include/J3ML/Geometry/OBB.h
@@ -1,31 +1,43 @@
 #pragma once

-#include <J3ML/Geometry.h>
+#include <J3ML/Geometry/Common.h>
 #include <J3ML/Geometry/AABB.h>
-#include <J3ML/Geometry/LineSegment.h>
 #include <J3ML/Geometry/Polyhedron.h>

+
 namespace J3ML::Geometry {
-    class OBB
+    /// 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 Geometry::AABB& aabb);
+        OBB(const AABB& aabb);
        inline static int NumFaces() { return 6;  }
        inline static int NumEdges() { return 12; }
        inline static int NumVertices() { return 8; }

        Polyhedron ToPolyhedron() const;
-
-        Geometry::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 J3ML::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 J3ML::Geometry {

        Vector3 AnyPointFast() const;

-        float Volume();
-        float SurfaceArea();
+        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);
    };
 }
--- a/include/J3ML/Geometry/Plane.h
+++ b/include/J3ML/Geometry/Plane.h
@@ -1,15 +1,18 @@
 #pragma once
 #include <J3ML/LinearAlgebra/Vector3.h>
-
+#include "Shape.h"


 namespace J3ML::Geometry
 {
    using J3ML::LinearAlgebra::Vector3;

-    class Plane
+    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;
--- a/include/J3ML/Geometry/Polygon.h
+++ b/include/J3ML/Geometry/Polygon.h
@@ -1,7 +1,28 @@
 #pragma once

-namespace J3ML::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:
    };
 }
--- a/include/J3ML/Geometry/Polyhedron.h
+++ b/include/J3ML/Geometry/Polyhedron.h
@@ -1,8 +1,46 @@
 #pragma once

+#include <J3ML/Geometry/Common.h>
+#include <J3ML/Geometry/Shape.h>
+#include <vector>
+#include <J3ML/LinearAlgebra/Vector3.h>
+
+
 namespace J3ML::Geometry
 {
-    class Polyhedron {
+    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:
    };
 }
--- a/include/J3ML/Geometry/Ray.h
+++ b/include/J3ML/Geometry/Ray.h
@@ -5,13 +5,56 @@
 #pragma once

 #include <J3ML/LinearAlgebra/Vector3.h>
+#include <vector>
+#include "TriangleMesh.h"
+#include "Frustum.h"
+#include "OBB.h"

 namespace J3ML::Geometry
 {
-    using LinearAlgebra::Vector3;
+    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);
    };
 }
--- a/include/J3ML/Geometry/Shape.h
+++ b/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:
+    };
+}
--- a/include/J3ML/Geometry/Sphere.h
+++ b/include/J3ML/Geometry/Sphere.h
@@ -5,6 +5,7 @@
 #include <J3ML/LinearAlgebra/Matrix4x4.h>
 #include <J3ML/Geometry/LineSegment.h>
 #include <J3ML/Geometry/TriangleMesh.h>
+#include <J3ML/Geometry/Shape.h>

 namespace J3ML::Geometry
 {
@@ -12,7 +13,7 @@ namespace J3ML::Geometry
    using J3ML::LinearAlgebra::Matrix4x4;

    // A mathematical representation of a 3-dimensional sphere
-    class Sphere
+    class Sphere : public Shape
    {
    public:
        Vector3 Position;
--- a/include/J3ML/Geometry/Triangle.h
+++ b/include/J3ML/Geometry/Triangle.h
@@ -1,9 +1,20 @@
 #pragma once

+#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;
    };
+
+
 }
--- a/include/J3ML/Geometry/Triangle2D.h
+++ b/include/J3ML/Geometry/Triangle2D.h
@@ -3,7 +3,6 @@

 namespace J3ML::Geometry
 {
-    class Shape2D {};
    class Triangle2D {
    public:
    };
--- a/include/J3ML/J3ML.h
+++ b/include/J3ML/J3ML.h
@@ -42,6 +42,7 @@ using namespace J3ML::SizedFloatTypes;
 namespace J3ML::Math
 {

+    bool EqualAbs(float a, float b, float epsilon = 1e-3f);

    // Coming soon: Units Namespace
    // For Dimensional Analysis
--- a/include/J3ML/LinearAlgebra/Matrix.h
+++ b/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];
+    };
+}
--- a/include/J3ML/LinearAlgebra/Matrix2x2.h
+++ b/include/J3ML/LinearAlgebra/Matrix2x2.h
@@ -16,9 +16,17 @@ namespace J3ML::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;
--- a/include/J3ML/LinearAlgebra/Matrix3x3.h
+++ b/include/J3ML/LinearAlgebra/Matrix3x3.h
@@ -1,6 +1,6 @@
 #pragma once

-
+#include <J3ML/LinearAlgebra/Common.h>
 #include <J3ML/LinearAlgebra/Vector2.h>
 #include <J3ML/LinearAlgebra/Vector3.h>
 #include <J3ML/LinearAlgebra/Quaternion.h>
@@ -26,6 +26,7 @@ namespace J3ML::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 };
@@ -40,6 +41,8 @@ namespace J3ML::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);
@@ -48,6 +51,10 @@ namespace J3ML::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;

@@ -56,10 +63,8 @@ namespace J3ML::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);
@@ -125,8 +130,30 @@ namespace J3ML::LinearAlgebra {

        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];
--- a/include/J3ML/LinearAlgebra/Matrix4x4.h
+++ b/include/J3ML/LinearAlgebra/Matrix4x4.h
@@ -46,6 +46,8 @@ namespace J3ML::LinearAlgebra {
        /// 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.
@@ -230,12 +232,16 @@ namespace J3ML::LinearAlgebra {
        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;
@@ -246,6 +252,18 @@ namespace J3ML::LinearAlgebra {
        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];

--- a/include/J3ML/LinearAlgebra/Vector.h
+++ b/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>;
+}
--- a/include/J3ML/LinearAlgebra/Vector2.h
+++ b/include/J3ML/LinearAlgebra/Vector2.h
@@ -30,6 +30,7 @@ namespace J3ML::LinearAlgebra {
        static const Vector2 Down;
        static const Vector2 Right;
        static const Vector2 NaN;
+        static const Vector2 Infinity;

        float GetX() const;
        float GetY() const;
--- a/include/J3ML/LinearAlgebra/Vector3.h
+++ b/include/J3ML/LinearAlgebra/Vector3.h
@@ -6,7 +6,6 @@
 #include <cstdlib>
 #include <J3ML/LinearAlgebra/Angle2D.h>

-
 namespace J3ML::LinearAlgebra {

 // A 3D (x, y, z) ordered pair.
@@ -21,7 +20,6 @@ public:
    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;
@@ -40,18 +38,14 @@ public:
    static void Orthonormalize(Vector3& a, Vector3& b);

    Vector3 Abs() const;
-
+    static Vector3 Abs(const Vector3& rhs);

    /// Returns the DirectionVector for a given angle.
    static Vector3 Direction(const Vector3 &rhs) ;

-
    static void Orthonormalize(Vector3& a, Vector3& b, Vector3& c);

-    bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon)
-    {
-
-    }
+    bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon);

    Vector3 ProjectToNorm(const Vector3& direction) const;

@@ -73,11 +67,15 @@ public:
    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;
@@ -148,26 +146,32 @@ public:
    /// 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
-    {
-
-    }
+    Vector3 Mul(const Vector3& rhs) const;

    /// Divides this vector by a scalar
    Vector3 operator/(float rhs) const;
    Vector3 Div(float scalar) const;
    static Vector3 Div(const Vector3& lhs, float rhs);

-
    /// Divides this vector by a vector, element-wise
    /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
    /// but this function is provided here for syntactical convenience
-    Vector2 Div(const Vector2& v) const;
+    Vector3 Div(const Vector3& v) const;

    /// Unary + operator
    Vector3 operator+() const; // TODO: Implement
    /// Unary - operator (Negation)
    Vector3 operator-() const;
+
+    bool Equals(const Vector3& rhs, float epsilon = 1e-3f) const;
+    bool Equals(float _x, float _y, float _z, float epsilon = 1e-3f) const;
+
+
+    Vector3 &operator =(const Vector3& rhs);
+    Vector3& operator+=(const Vector3& rhs);
+    Vector3& operator-=(const Vector3& rhs);
+    Vector3& operator*=(float scalar);
+    Vector3& operator/=(float scalar);
 public:
     float x = 0;
     float y = 0;
--- a/include/J3ML/LinearAlgebra/Vector4.h
+++ b/include/J3ML/LinearAlgebra/Vector4.h
@@ -20,6 +20,10 @@ namespace J3ML::LinearAlgebra {
        {
            return &x;
        }
+        Vector3 XYZ() const
+        {
+            return {x, y, z};
+        }

        float GetX() const;
        float GetY() const;
@@ -45,6 +49,9 @@ namespace J3ML::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;
--- a/src/J3ML/Geometry/AABB.cpp
+++ b/src/J3ML/Geometry/AABB.cpp
@@ -1,11 +1,42 @@
 #include <J3ML/Geometry/AABB.h>
 #include <cassert>
+#include <J3ML/Geometry/Plane.h>
+#include <J3ML/Geometry/Sphere.h>
+//#include <J3ML/Geometry/OBB.h>
+#include <J3ML/Geometry/LineSegment.h>
+#include <J3ML/Geometry/Triangle.h>
+#include <J3ML/Geometry/Polygon.h>
+#include <J3ML/Geometry/Frustum.h>
+#include <J3ML/Geometry/Capsule.h>
+#include <J3ML/Geometry/Ray.h>
+#include <J3ML/Geometry/TriangleMesh.h>
+#include <J3ML/Geometry/Polyhedron.h>
+#include <J3ML/Algorithm/RNG.h>

 namespace J3ML::Geometry {

+    /// See Christer Ericson's Real-time Collision Detection, p. 87, or
+    /// James Arvo's "Transforming Axis-aligned Bounding Boxes" in Graphics Gems 1, pp. 548-550.
+    /// http://www.graphicsgems.org/
+    template<typename Matrix>
+    void AABBTransformAsAABB(AABB &aabb, Matrix &m)
+    {
+        const Vector3 centerPoint = (aabb.minPoint + aabb.maxPoint) * 0.5f;
+        const Vector3 halfSize = centerPoint - aabb.minPoint;
+        Vector3 newCenter = m.Mul(centerPoint);
+
+
+        // The following is equal to taking the absolute value of the whole matrix m.
+        Vector3 newDir = Vector3(std::abs(m[0][0] * halfSize.x) + std::abs(m[0][1] * halfSize.y) + std::abs(m[0][2] * halfSize.z),
+                                 std::abs(m[1][0] * halfSize.x) + std::abs(m[1][1] * halfSize.y) + std::abs(m[1][2] * halfSize.z),
+                                 std::abs(m[2][0] * halfSize.x) + std::abs(m[2][1] * halfSize.y) + std::abs(m[2][2] * halfSize.z));
+        aabb.minPoint = newCenter - newDir;
+        aabb.maxPoint = newCenter + newDir;
+    }
+
    AABB AABB::FromCenterAndSize(const J3ML::Geometry::Vector3 &center, const J3ML::Geometry::Vector3 &size) {
        Vector3 halfSize =  size * 0.5f;
-        return {center - halfSize, center + halfSize};
+        return AABB{center - halfSize, center + halfSize};
    }

    float AABB::MinX() const { return minPoint.x; }
@@ -178,18 +209,23 @@ namespace J3ML::Geometry {

    void AABB::SetFromCenterAndSize(const Vector3& center, const Vector3& size)
    {
-
+        Vector3 halfSize = 0.5f * size;
+        minPoint = center - halfSize;
+        maxPoint = center + halfSize;
    }


    void AABB::SetFrom(const OBB& obb)
    {
-
+        Vector3 halfSize = Vector3::Abs(obb.axis[0] * obb.r[0]) + Vector3::Abs(obb.axis[1]*obb.r[1]) + Vector3::Abs(obb.axis[2]*obb.r[2]);
+        SetFromCenterAndSize(obb.pos, 2.f*halfSize);
    }

    void AABB::SetFrom(const Sphere& s)
    {
-
+        Vector3 d = Vector3(s.Radius, s.Radius, s.Radius);
+        minPoint = s.Position - d;
+        maxPoint = s.Position + d;
    }

    void AABB::SetFrom(const Vector3 *pointArray, int numPoints) {
@@ -201,8 +237,12 @@ namespace J3ML::Geometry {
            Enclose(pointArray[i]);
    }

-    Vector3 AABB::GetRandomPointInside() const {
+    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() {
@@ -234,4 +274,349 @@ namespace J3ML::Geometry {
        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;
+
+    }
+
 }
--- a/src/J3ML/Geometry/AABB2D.cpp
+++ b/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;
+    }
+}
--- a/src/J3ML/Geometry/Capsule.cpp
+++ b/src/J3ML/Geometry/Capsule.cpp
@@ -1,7 +1,15 @@
 #include <J3ML/Geometry/Capsule.h>
+#include <J3ML/Geometry/AABB.h>

 namespace J3ML::Geometry
 {

    Capsule::Capsule() : l() {}
+
+    AABB Capsule::MinimalEnclosingAABB() const
+    {
+        Vector3 d = Vector3(r, r, r);
+        AABB aabb(Vector3::Min(l.A, l.B) - d, Vector3::Max(l.A, l.B) + d);
+        return aabb;
+    }
 }
--- a/src/J3ML/Geometry/OBB.cpp
+++ b/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());
+    }
+
+
+}
--- a/src/J3ML/Geometry/Polygon.cpp
+++ b/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 {

-}
--- a/src/J3ML/Geometry/Polyhedron.cpp
+++ b/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];
+    }
+
+}

-}
--- a/src/J3ML/Geometry/Ray.cpp
+++ b/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();
+    }
+}
+

-}
--- a/src/J3ML/Geometry/Triangle.cpp
+++ b/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);
+    }
+}
--- a/src/J3ML/J3ML.cpp
+++ b/src/J3ML/J3ML.cpp
@@ -24,6 +24,10 @@ namespace J3ML

    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) {}
--- a/src/J3ML/LinearAlgebra/Matrix2x2.cpp
+++ b/src/J3ML/LinearAlgebra/Matrix2x2.cpp
@@ -18,4 +18,8 @@ namespace J3ML::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];
+    }
 }
--- a/src/J3ML/LinearAlgebra/Matrix3x3.cpp
+++ b/src/J3ML/LinearAlgebra/Matrix3x3.cpp
@@ -362,5 +362,69 @@ namespace J3ML::LinearAlgebra {
        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);
+    }
+
 }

--- a/src/J3ML/LinearAlgebra/Matrix4x4.cpp
+++ b/src/J3ML/LinearAlgebra/Matrix4x4.cpp
@@ -2,7 +2,10 @@
 #include <J3ML/LinearAlgebra/Vector4.h>

 namespace J3ML::LinearAlgebra {
-    const Matrix4x4 Matrix4x4::Zero = Matrix4x4(0);
+
+
+
+    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);

@@ -570,4 +573,65 @@ namespace J3ML::LinearAlgebra {

        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);
+    }
+
 }
--- a/src/J3ML/LinearAlgebra/Vector2.cpp
+++ b/src/J3ML/LinearAlgebra/Vector2.cpp
@@ -161,6 +161,7 @@ namespace J3ML::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; }

@@ -324,4 +325,12 @@ namespace J3ML::LinearAlgebra {
        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();
+    }
 }
--- a/src/J3ML/LinearAlgebra/Vector3.cpp
+++ b/src/J3ML/LinearAlgebra/Vector3.cpp
@@ -103,6 +103,8 @@ namespace J3ML::LinearAlgebra {
        return this->IsWithinMarginOfError(rhs) == false;
    }

+
+
    Vector3 Vector3::Min(const Vector3& min) const
    {
        return {
@@ -353,5 +355,89 @@ namespace J3ML::LinearAlgebra {
        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))
+        };
+    }
+

 }
--- a/src/J3ML/LinearAlgebra/Vector4.cpp
+++ b/src/J3ML/LinearAlgebra/Vector4.cpp
@@ -150,6 +150,20 @@ Vector4 Vector4::operator-(const Vector4& rhs) const
        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
Author	SHA1	Message	Date
josh	de108630b6	Unfinished Work	2024-03-23 16:20:57 -04:00
josh	f6abe5c430	Implement Vector4::Equals	2024-03-21 20:43:16 -04:00
josh	4085a1700c	Implement Vector3::Equals	2024-03-21 20:43:07 -04:00
josh	06bb959e3f	Implement Matrix3x3::IsRowOrthogonal IsColOrthogonal	2024-03-21 18:37:43 -04:00
josh	802c321115	Implement Missing things (More To Come) (Broken build)	2024-03-21 15:24:50 -04:00
josh	d1529f05b0	Implement AABB::GetRandomPoint methods	2024-03-21 14:00:31 -04:00
josh	dc41dcf520	Implement Vector3::MinElement	2024-03-20 15:36:27 -04:00
josh	f4c6337f12	Implement AABB::Intersects(Triangle) SAT algorithm	2024-03-20 00:24:16 -04:00
josh	212c1d3bc4	Implement AABB::Intersects(Triangle)	2024-03-19 18:42:41 -04:00
josh	d60c71373b	Implement Vector3 += -= *= /=	2024-03-19 14:26:26 -04:00
josh	4cb497be29	Template Forward Declaration Fix	2024-03-19 14:20:32 -04:00
josh	cd58676ece	Implement more OBB methods	2024-03-19 14:20:12 -04:00
josh	9f60f296c6	Massive Refactor	2024-03-15 15:31:14 -04:00
josh	e8ed68f3c7	Implement(ing) Ray class	2024-03-07 00:40:12 -05:00
josh	44b8bb8172	Migrate AABB2D implementation to it's cpp file	2024-03-05 01:05:25 -05:00
josh	4aaf430f68	Implement Several Methods	2024-02-29 02:17:06 -05:00
josh	232bfebbef	Implement Matrix4x4::Matrix4x4 from float pointer	2024-02-27 02:56:09 -05:00