Move to implementation file

Implement Mat4x4 Inverse() (Yikes!!!)
Implement Mat4x4 Translate, Transform, FromTranslation
2024-01-31 20:06:35 -05:00 · 2024-01-31 20:05:31 -05:00 · 2024-01-31 18:34:15 -05:00 · 2024-01-30 21:35:55 -05:00 · 2024-01-30 21:35:41 -05:00 · 2024-01-30 21:30:13 -05:00
46 changed files with 1534 additions and 188 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -30,7 +30,34 @@ include_directories("include")

 add_library(J3ML SHARED ${J3ML_SRC}
        src/J3ML/LinearAlgebra/AxisAngle.cpp
-        include/J3ML/LinearAlgebra/Vector.h)
+        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)
 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
@@ -13,90 +13,20 @@ namespace Geometry {
        Vector2 B;
    };

-    class Rectangle; //AABB2D;
+    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);
-
-
-    // A 3D axis-aligned bounding box
-    // This data structure can be used to represent coarse bounds of objects, in situations where detailed triangle-level
-    // computations can be avoided. In physics systems, bounding boxes are used as an efficient early-out test for geometry
-    // intersection queries.
-    // the 'Axis-aligned' part in the name means that the local axes of this bounding box are restricted to align with the
-    // axes of the world space coordinate system. This makes computation involving AABB's very fast, since AABB's cannot
-    // be arbitrarily oriented in the space with respect to each other.
-    // If you need to represent a box in 3D space with arbitrary orientation, see the class OBB. */
-    class AABB;
-    class Capsule;
-    class Line;
-    class LineSegment
-    {
-        Vector3 A;
-        Vector3 B;
-    };
-    class Ray
-    {
-        Vector3 Origin;
-        Vector3 Direction;
-    };
-
-    class OBB;
-    class Plane
-    {
-    public:
-        Vector3 Position;
-        Vector3 Normal;
-        float distance = 0.f;
-
-    };
-    class Frustum {
-    public:
-        Plane TopFace;
-        Plane BottomFace;
-        Plane RightFace;
-        Plane LeftFace;
-        Plane FarFace;
-        Plane NearFace;
-    };
-
-    class Camera {
-    public:
-        Vector3 Position;
-        Vector3 Front;
-        Vector3 Right;
-        Vector3 Up;
-    };
-
-    static 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;
-    }
-
-    class Polygon;
-    class Polyhedron;
-    class QuadTree;
-    class OctTree;
-    class Sphere;
-    class Triangle;
-    class TriangleMesh;
 }
--- a/include/J3ML/Geometry/AABB.h
+++ b/include/J3ML/Geometry/AABB.h
@@ -0,0 +1,153 @@
+#pragma once
+
+#include <J3ML/LinearAlgebra/Vector3.h>
+
+#include <J3ML/LinearAlgebra.h>
+#include <J3ML/Geometry.h>
+
+#include <J3ML/Geometry/Plane.h>
+#include <J3ML/Geometry/Sphere.h>
+#include <J3ML/Geometry/OBB.h>
+#include <J3ML/Geometry/LineSegment.h>
+#include <J3ML/Geometry/Triangle.h>
+#include <J3ML/Geometry/Polygon.h>
+#include <J3ML/Geometry/Frustum.h>
+#include <J3ML/Geometry/Capsule.h>
+#include <J3ML/Geometry/Ray.h>
+#include <J3ML/Geometry/TriangleMesh.h>
+#include <J3ML/Geometry/Polyhedron.h>
+
+
+// TODO: Fix circular include between AABB and OBB
+
+
+
+namespace Geometry
+{
+
+    using namespace LinearAlgebra;
+    // A 3D axis-aligned bounding box
+    // This data structure can be used to represent coarse bounds of objects, in situations where detailed triangle-level
+    // computations can be avoided. In physics systems, bounding boxes are used as an efficient early-out test for geometry
+    // intersection queries.
+    // the 'Axis-aligned' part in the name means that the local axes of this bounding box are restricted to align with the
+    // axes of the world space coordinate system. This makes computation involving AABB's very fast, since AABB's cannot
+    // be arbitrarily oriented in the space with respect to each other.
+    // If you need to represent a box in 3D space with arbitrary orientation, see the class OBB. */
+
+    class AABB
+    {
+    public:
+        Vector3 minPoint;
+        Vector3 maxPoint;
+
+        static int NumFaces() { return 6; }
+        static int NumEdges() { return 12;}
+        static int NumVertices() { return 8;}
+
+        static AABB FromCenterAndSize(const Vector3& center, const Vector3& size)
+        {
+            Vector3 halfSize =  size * 0.5f;
+            return {center - halfSize, center + halfSize};
+        }
+        float MinX() const { return minPoint.x; }
+        float MinY() const { return minPoint.y; }
+        float MinZ() const { return minPoint.z; }
+
+        float MaxX() const { return maxPoint.x; }
+        float MaxY() const { return maxPoint.y; }
+        float MaxZ() const { return maxPoint.z; }
+
+
+        /// 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
+        {
+            return Sphere(Centroid(), Size().Length()*0.5f);
+        }
+
+        Vector3 HalfSize() const {
+            return this->Size()/2.f;
+        }
+
+// 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 halfSize = HalfSize();
+            return Sphere(Centroid(), std::min(halfSize.x, std::min(halfSize.y, halfSize.z)));
+        }
+        bool IsFinite() const
+        {
+            return minPoint.IsFinite() && maxPoint.IsFinite();
+        }
+        Vector3 Centroid() const
+        {
+            return (minPoint+maxPoint) * 0.5f;
+        }
+        Vector3 Size() const
+        {
+            return this->maxPoint - this->minPoint;
+        }
+        // Quickly returns an arbitrary point inside this AABB
+        Vector3 AnyPointFast() const;
+
+        Vector3 PointInside(float x, float y, float z) const
+        {
+            Vector3 d = maxPoint - minPoint;
+            return minPoint + d.Mul({x, y, z});
+        }
+        // Returns an edge of this AABB
+        LineSegment Edge(int edgeIndex) const
+        {
+            switch(edgeIndex)
+            {
+                default:
+                case 0: return LineSegment(minPoint, {minPoint.x, minPoint.y, maxPoint.z});
+            }
+        }
+        Vector3 CornerPoint(int cornerIndex);
+        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 GetVolume();
+        float GetVolumeCubed();
+        float GetSurfaceArea();
+        Vector3 GetRandomPointInside();
+        Vector3 GetRandomPointOnSurface();
+        Vector3 GetRandomPointOnEdge();
+        Vector3 GetRandomCornerPoint();
+        AABB Translated(const Vector3& offset) const;
+        AABB TransformAABB(const Matrix3x3& transform);
+        AABB TransformAABB(const Matrix4x4& transform);
+        AABB TransformAABB(const Quaternion& transform);
+        OBB Transform(const Matrix3x3& transform);
+        OBB Transform(const Matrix4x4& transform);
+        OBB Transform(const Quaternion& transform);
+        bool Contains(const Vector3& point) const;
+        bool Contains(const LineSegment& lineSegment) const;
+        bool Contains(const AABB& aabb) const;
+        bool Contains(const OBB& obb) const;
+        bool Contains(const Sphere& sphere) const;
+        bool Contains(const Triangle& triange) const;
+        bool Contains(const Polygon& polygon) const;
+        bool Contains(const Frustum& frustum) const;
+        bool Contains(const Polyhedron& polyhedron) const;
+        bool Contains(const Capsule& capsule) const;
+        // Tests whether this AABB and the given object intersect.
+        bool Intersects(const Ray& ray, float dNear, float dFar) const;
+        bool Intersects(const Capsule& capsule) const;
+        bool Intersects(const Triangle& triangle) const;
+        bool Intersects(const Polygon& polygon) const;
+        bool Intersects(const Frustum& frustum) const;
+        bool Intersects(const Polyhedron& polyhedron) const;
+        TriangleMesh Triangulate(int numFacesX, int numFacesY, int numFacesZ, bool ccwIsFrontFacing) const;
+        AABB Intersection(const AABB& rhs) const;
+        bool IntersectLineAABB(const Vector3& linePos, const Vector3& lineDir, float tNear, float tFar) const;
+    };
+}
--- a/include/J3ML/Geometry/AABB2D.h
+++ b/include/J3ML/Geometry/AABB2D.h
@@ -0,0 +1,106 @@
+#pragma once
+
+#include <J3ML/LinearAlgebra/Vector2.h>
+
+namespace Geometry
+{
+    using LinearAlgebra::Vector2;
+    // CaveGame AABB
+    class AABB2D
+    {
+    public:
+
+        Vector2 minPoint;
+        Vector2 maxPoint;
+        AABB2D() {}
+        AABB2D(const Vector2& min, const Vector2& max):
+        minPoint(min), maxPoint(max)
+        {}
+
+        float Width() const { return maxPoint.x - minPoint.x;  }
+        float Height() const { return maxPoint.y - minPoint.y; }
+
+        float DistanceSq(const Vector2& pt) const
+        {
+            Vector2 cp = pt.Clamp(minPoint, maxPoint);
+            return cp.DistanceSq(pt);
+        }
+
+        void SetNegativeInfinity();
+
+        void Enclose(const Vector2& point)
+        {
+            minPoint = Vector2::Min(minPoint, point);
+            maxPoint = Vector2::Max(maxPoint, point);
+        }
+
+        bool Intersects(const AABB2D& rhs) const
+        {
+            return maxPoint.x >= rhs.minPoint.x &&
+                   maxPoint.y >= rhs.minPoint.y &&
+                   rhs.maxPoint.x >= minPoint.x &&
+                   rhs.maxPoint.y >= minPoint.y;
+        }
+
+        bool Contains(const AABB2D& rhs) const
+        {
+            return rhs.minPoint.x >= minPoint.x && rhs.minPoint.y >= minPoint.y
+                   && rhs.maxPoint.x <= maxPoint.x && rhs.maxPoint.y <= maxPoint.y;
+        }
+
+        bool Contains(const Vector2& pt) const
+        {
+            return pt.x >= minPoint.x && pt.y >= minPoint.y
+                   && pt.x <= maxPoint.x && pt.y <= maxPoint.y;
+        }
+
+        bool Contains(int x, int y) const
+        {
+            return x >= minPoint.x && y >= minPoint.y
+                   && x <= maxPoint.x && y <= maxPoint.y;
+        }
+
+        bool IsDegenerate() const
+        {
+            return minPoint.x >= maxPoint.x || minPoint.y >= maxPoint.y;
+        }
+
+        bool HasNegativeVolume() const
+        {
+            return maxPoint.x < minPoint.x || maxPoint.y < minPoint.y;
+        }
+
+        bool IsFinite() const
+        {
+            return minPoint.IsFinite() && maxPoint.IsFinite() && minPoint.MinElement() > -1e5f && maxPoint.MaxElement() < 1e5f;
+        }
+
+        Vector2 PosInside(const Vector2 &normalizedPos) const
+        {
+            return minPoint + normalizedPos.Mul(maxPoint - minPoint);
+        }
+
+        Vector2 ToNormalizedLocalSpace(const Vector2 &pt) const
+        {
+            return (pt - minPoint).Div(maxPoint - minPoint);
+        }
+
+
+        AABB2D operator+(const Vector2& pt) const
+        {
+            AABB2D a;
+            a.minPoint = minPoint + pt;
+            a.maxPoint = maxPoint + pt;
+            return a;
+        }
+
+        AABB2D operator-(const Vector2& pt) const
+        {
+            AABB2D a;
+            a.minPoint = minPoint - pt;
+            a.maxPoint = maxPoint - pt;
+            return a;
+        }
+
+    };
+}
--- a/include/J3ML/Geometry/Capsule.h
+++ b/include/J3ML/Geometry/Capsule.h
@@ -0,0 +1,27 @@
+#pragma once
+
+#include "LineSegment.h"
+#include <J3ML/LinearAlgebra/Vector3.h>
+
+namespace Geometry
+{
+    using namespace LinearAlgebra;
+    class Capsule
+    {
+        // Specifies the two inner points of this capsule
+        LineSegment l;
+        // Specifies the radius of this capsule
+        float r;
+
+        Capsule();
+        Capsule(const LineSegment& endPoints, float radius);
+        Capsule(const Vector3& bottomPt, const Vector3& topPt, float radius);
+        bool IsDegenerate() const;
+        float Height() const;
+        float Diameter() const;
+        Vector3 Bottom() const;
+        Vector3 Center() const;
+        Vector3 Centroid() const;
+        Vector3 ExtremePoint(const Vector3& direction);
+    };
+}
--- a/include/J3ML/Geometry/Frustum.h
+++ b/include/J3ML/Geometry/Frustum.h
@@ -0,0 +1,40 @@
+//
+// Created by dawsh on 1/25/24.
+//
+#pragma once
+
+#include "Plane.h"
+#include <J3ML/LinearAlgebra/CoordinateFrame.h>
+
+namespace Geometry
+{
+
+    enum class FrustumType
+    {
+        Invalid,
+        /// Set the Frustum type to this value to define the orthographic projection formula. In orthographic projection,
+        /// 3D images are projected onto a 2D plane essentially by flattening the object along one direction (the plane normal).
+        /// The size of the projected images appear the same independent of their distance to the camera, and distant objects will
+        /// not appear smaller. The shape of the Frustum is identical to an oriented bounding box (OBB).
+        Orthographic,
+        /// Set the Frustum type to this value to use the perspective projection formula. With perspective projection, the 2D
+        /// image is formed by projecting 3D points towards a single point (the eye point/tip) of the Frustum, and computing the
+        /// point of intersection of the line of the projection and the near plane of the Frustum.
+        /// This corresponds to the optics in the real-world, and objects become smaller as they move to the distance.
+        /// The shape of the Frustum is a rectangular pyramid capped from the tip.
+        Perspective
+    };
+
+    class Frustum {
+    public:
+        Plane TopFace;
+        Plane BottomFace;
+        Plane RightFace;
+        Plane LeftFace;
+        Plane FarFace;
+        Plane NearFace;
+        static Frustum CreateFrustumFromCamera(const CoordinateFrame& cam, float aspect, float fovY, float zNear, float zFar);
+    };
+
+
+}
--- a/include/J3ML/Geometry/LineSegment.h
+++ b/include/J3ML/Geometry/LineSegment.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <J3ML/LinearAlgebra/Vector3.h>
+
+namespace Geometry
+{
+    using LinearAlgebra::Vector3;
+    class LineSegment
+    {
+    public:
+        LineSegment();
+        LineSegment(const Vector3& a, const Vector3& b);
+        Vector3 A;
+        Vector3 B;
+    };
+}
--- a/include/J3ML/Geometry/OBB.h
+++ b/include/J3ML/Geometry/OBB.h
@@ -0,0 +1,48 @@
+#pragma once
+
+#include <J3ML/Geometry.h>
+#include <J3ML/Geometry/AABB.h>
+#include <J3ML/Geometry/LineSegment.h>
+#include <J3ML/Geometry/Polyhedron.h>
+
+namespace Geometry {
+    class OBB
+    {
+    public:
+        // The center position of this OBB
+        Vector3 pos;
+        // Stores half-sizes to x, y, and z directions in the local space of this OBB.
+        Vector3 r;
+        // Specifies normalized direc tion vectors for the local axes
+        Vector3 axis[3];
+
+        OBB() {}
+        OBB(const Vector3& pos, const Vector3& radii, const Vector3& axis0, const Vector3& axis1, const Vector3& axis2);
+        OBB(const Geometry::AABB& aabb);
+        inline static int NumFaces() { return 6;  }
+        inline static int NumEdges() { return 12; }
+        inline static int NumVertices() { return 8; }
+
+        Polyhedron ToPolyhedron() const;
+
+        Geometry::AABB MinimalEnclosingAABB() const;
+
+        Sphere MinimalEnclosingSphere() const;
+        Sphere MaximalContainedSphere() const;
+        Vector3 Size() const;
+        Vector3 HalfSize() const;
+        Vector3 Diagonal() const;
+        Vector3 HalfDiagonal() const;
+        bool IsFinite() const;
+        bool IsDegenerate() const;
+        Vector3 CenterPoint() const;
+        Vector3 Centroid() const;
+
+        Vector3 AnyPointFast() const;
+
+        float Volume();
+        float SurfaceArea();
+        Geometry::LineSegment Edge(int edgeIndex) const;
+        Vector3 CornerPoint(int cornerIndex) const;
+    };
+}
--- a/include/J3ML/Geometry/Plane.h
+++ b/include/J3ML/Geometry/Plane.h
@@ -0,0 +1,13 @@
+#pragma once
+#include <J3ML/LinearAlgebra/Vector3.h>
+
+using namespace LinearAlgebra;
+
+class Plane
+{
+public:
+    Vector3 Position;
+    Vector3 Normal;
+    float distance = 0.f;
+
+};
--- a/include/J3ML/Geometry/Polygon.h
+++ b/include/J3ML/Geometry/Polygon.h
@@ -0,0 +1,7 @@
+#pragma once
+
+namespace Geometry {
+    class Polygon {
+
+    };
+}
--- a/include/J3ML/Geometry/Polyhedron.h
+++ b/include/J3ML/Geometry/Polyhedron.h
@@ -0,0 +1,8 @@
+#pragma once
+
+namespace Geometry
+{
+    class Polyhedron {
+
+    };
+}
--- a/include/J3ML/Geometry/QuadTree.h
+++ b/include/J3ML/Geometry/QuadTree.h
@@ -0,0 +1,369 @@
+#pragma once
+
+#include <vector>
+#include <cstdint>
+#include <J3ML/LinearAlgebra/Vector2.h>
+#include "AABB2D.h"
+
+namespace Geometry {
+
+
+    using LinearAlgebra::Vector2;
+
+    template<typename T>
+    class QuadTree {
+        /// A fixed split rule for all QuadTrees: A QuadTree leaf node is only ever split if the leaf contains at least this many objects.
+        /// Leaves containing fewer than this many objects are always kept as leaves until the object count is exceeded.
+        constexpr static const int minQuadTreeNodeObjectCount = 16;
+
+        /// A fixed split limit rule for all QuadTrees: If the QuadTree node side length is smaller than this, the node will
+        /// never be split again into smaller subnodes. This provides a hard limit safety net for infinite/extra long recursion
+        /// in case multiple identical overlapping objects are placed into the tree.
+        constexpr static const float minQuadTreeQuadrantSize = 0.05f;
+
+    public:
+        struct Node {
+            Node *parent;
+            uint32_t childIndex;
+            std::vector<T> objects;
+
+            Vector2 center;
+            Vector2 radius;
+
+            bool IsLeaf() const { return childIndex == 0xFFFFFFFF; }
+
+            uint32_t TopLeftChildIndex() const { return childIndex; }
+
+            uint32_t TopRightChildIndex() const { return childIndex + 1; }
+
+            uint32_t BottomLeftChildIndex() const { return childIndex + 2; }
+
+            uint32_t BottomRightChildIndex() const { return childIndex + 3; }
+
+            /// This assumes that the QuadTree contains unique objects and never duplicates
+            void Remove(const T &object) {
+                for (size_t i = 0; i < objects.size(); ++i) {
+                    if (objects[i] == object) {
+                        AssociateQuadTreeNode(object,
+                                              0); // Mark in the object that it has been removed from the QuadTree.
+                        std::swap(objects[i], objects.back());
+                        objects.pop_back();
+                        return;
+                    }
+                }
+            }
+
+            AABB2D ComputeAABB() {}
+
+            float DistanceSq(const Vector2 &point) const {
+                Vector2 centered = point - center;
+                Vector2 closestPoint = centered.Clamp(-radius, radius);
+                return closestPoint.DistanceSq(centered);
+            }
+
+        };
+
+        // Helper struct used when traversing through the tree
+        struct TraversalStackItem {
+            Node *node;
+        };
+
+        QuadTree() :
+                rootNodeIndex(-1),
+                boundingAABB(Vector2(0, 0), Vector2(0, 0)) {
+            // TODO: currently storing persistent raw pointers to this array outside the array
+            nodes.reserve(200000);
+        }
+
+        /// Removes all nodes and objects in this tree and reintializes the tree to a single root node.
+        void Clear(const Vector2 &minXY = Vector2(-1, -1), const Vector2 &maxXY = Vector2(1, 1));
+
+        /// Places the given object onto the proper (leaf) node of the tree. After placing, if the leaf split rule is
+        /// satisfied, subdivides the leaf node into 4 subquadrants and reassings the objects to the new leaves.
+        void Add(const T &object);
+
+        /// Removes the given object from this tree.
+        /// To call this function, you must define a function QuadTree<T>::Node *GetQuadTreeNode(const T& object)
+        /// which returns the node of this quadtree where the object resides in.
+        void Remove(const T &object);
+
+        /// @return The bounding rectangle for the whole tree.
+        /// @note This bounding rectangle does not tightly bound the objects themselves, only the root node of the tree.
+        AABB2D BoundingAABB() const { return boundingAABB; }
+
+        /// @return The topmost node in the tree.
+        Node *Root();
+
+        const Node *Root() const;
+
+        /// Returns the total number of nodes (all nodes, i.e. inner nodes + leaves) in the tree.
+        /// Runs in constant time.
+        int NumNodes() const;
+
+        /// Returns the total number of leaf nodes in the tree.
+        /// @warning Runs in time linear 'O(n)' to the number of nodes in the tree.
+        int NumLeaves() const;
+
+        /// Returns the total number of inner nodes in the tree.
+        /// @warning Runs in time linear 'O(n)' to the number of nodes in the tree.
+        int NumInnerNodes() const;
+
+        /// Returns the total number of objects stored in the tree.
+        /// @warning Runs in time linear 'O(n)' to the number of nodes in the tree.
+        int NumObjects() const;
+
+        /// Returns the maximum height of the whole tree (the path from the root to the farthest leaf node).
+        int TreeHeight() const;
+
+        /// Returns the height of the subtree rooted at 'node'.
+        int TreeHeight(const Node *node) const;
+
+        /// Performs an AABB intersection query in this Quadtreee, and calls the given callback function for each non-empty
+        /// node of the tree which intersects the given AABB.
+        /** @param aabb The axis-aligned bounding box to intersect this QuadTree with.
+            @param callback A function or a function object of prototype
+                bool callbackFunction(QuadTree<T> &tree, const AABB2D &queryAABB, QuadTree<T>::Node &node);
+            If the callback function returns true, the execution of the query is stopped and this function immediately
+            returns afterwards. If the callback function returns false, the execution of the query continues. */
+        template<typename Func>
+        inline void AABBQuery(const AABB2D &aabb, Func &callback);
+
+        /// Finds all object pairs inside the given AABB which have colliding AABBs. For each such pair, calls the
+        /// specified callback function.
+        template<typename Func>
+        inline void CollidingPairsQuery(const AABB2D &aabb, Func &callback);
+
+        /// Performs various consistency checks on the given node. Use only for debugging purposes.
+        void DebugSanityCheckNode(Node *n);
+
+    private:
+        void Add(const T &object, Node *n);
+
+        /// Allocates a sequential 4-tuple of QuadtreeNodes, contiguous in memory.
+        int AllocateNodeGroup(Node *parent);
+
+        void SplitLeaf(Node *leaf);
+
+        std::vector<Node> nodes;
+        int rootNodeIndex;
+        AABB2D boundingAABB;
+
+        void GrowRootTopLeft();
+
+        void GrowRootTopRight();
+
+        void GrowRootBottomLeft();
+
+        void GrowRootBottomRight();
+
+        void GrowImpl(int quadrantForRoot);
+    };
+
+    // NOTE: Keep members defined here. Template-parameterized classes
+    // can't be split across header and implementation files
+    // but the presence of the implementation file is a requirement
+
+
+    template<typename T>
+    void QuadTree<T>::Clear(const Vector2 &minXY, const Vector2 &maxXY) {
+        nodes.clear();
+
+        boundingAABB.minPoint = minXY;
+        boundingAABB.maxPoint = maxXY;
+
+        rootNodeIndex = AllocateNodeGroup(0);
+
+        Node *root = Root();
+        root->center = (minXY + maxXY) * 0.5f;
+        root->radius = maxXY - root->center;
+    }
+
+    template<typename T>
+    void QuadTree<T>::Add(const T &object) {
+        Node *n = Root();
+        AABB2D objectAABB = GetAABB2D(object);
+
+        // Ramen Noodle Bowls of nested if statements are generally bad practice
+        // Unfortunately, sometimes the problem domain makes this unavoidable
+
+        if (objectAABB.minPoint.x >= boundingAABB.minPoint.x) {
+            // object fits left.
+            if (objectAABB.maxPoint.x <= boundingAABB.maxPoint.x) {
+                // object fits left and right.
+                if (objectAABB.minPoint.y >= boundingAABB.minPoint.y) {
+                    // Object fits left, right, and top.
+                    if (objectAABB.maxPoint.y <= boundingAABB.maxPoint.y) {
+                        // Object fits the whole root AABB. Can safely add into the existing tree size.
+                        AddObject(object, n);
+                        return;
+                    } else {
+                        // Object fits left, right, and top, but not bottom.
+                        GrowRootBottomRight(); // Could grow bottom-left as well, wouldn't matter here.
+                    }
+                } else {
+                    // Object fits left and right, but not to top.
+                    GrowRootTopRight(); // Could grow top-left as well, wouldn't matter here.
+                }
+            } else {
+                // Object fits left, but not to right. We must grow right. Check whether to grow top or bottom;
+                if (objectAABB.minPoint.y < boundingAABB.minPoint.y)
+                    GrowRootTopRight();
+                else
+                    GrowRootBottomRight();
+            }
+        } else {
+            // We must grow left. Check whether to grow top or bottom.
+            if (objectAABB.minPoint.y < boundingAABB.minPoint.y)
+                GrowRootTopLeft();
+            else
+                GrowRootBottomLeft();
+        }
+        // Now that we have grown the tree root node, try adding again.
+        Add(object);
+    }
+
+    template<typename T>
+    void QuadTree<T>::Remove(const T &object) {
+        Node *n = GetQuadTreeNode(object);
+        if (n) {
+            n->Remove(object);
+        }
+    }
+
+    template<typename T>
+    void QuadTree<T>::Add(const T &object, Node *n) {
+        for (;;) {
+            // Traverse the QuadTree to decide which quad to place this object on.
+            float left = n->center.x - MinX(object); // If left > 0.f, then the object overlaps with the left quadrant.
+            float right = MaxX(object) - n->center.x; // If right > 0.f, then the object overlaps with the right quadrant.
+
+            float top = n->center.y - MinY(object); // If top > 0.f, then the object overlaps with the top quadrant
+            float bottom =
+                    MaxY(object) - n->center.y; // If bottom > 0.f, then the object overlaps with the bottom quadrant
+            float leftAndRight = std::min(left, right); // If > 0.f, then the object straddles left-right halves.
+            float topAndBottom = std::min(top, bottom); // If > 0.f, then the object straddles top-bottom halves.
+            float straddledEitherOne = std::max(leftAndRight,
+                                                topAndBottom); // If > 0.f, thne the object is in two or more quadrants.
+
+            // Note: It can happen that !left && !right, or !top && !bottom.
+            // but the if()s are setup below so that right/bottom is taken if no left/top, so that is ok.
+
+            // We must put the object onto this node if
+            // a) the object straddled the parent->child split lines.
+            // b) this object is a leaf
+            if (straddledEitherOne > 0.f) {
+                n->objects.push_back(object);
+                AssociateQuadTreeNode(object, n);
+                return;
+            }
+            if (n->IsLeaf()) {
+                n->objects.push_back(object);
+                AssociateQuadTreeNode(object, n);
+
+                if ((int) n->objects.size() > minQuadTreeNodeObjectCount &&
+                    std::min(n->radius.x, n->radius.y) >= minQuadTreeQuadrantSize) {
+                    SplitLeaf(n);
+                }
+
+                return;
+            }
+            if (left > 0.f) {
+                if (top > 0.f) {
+                    n = &nodes[n->TopLeftChildIndex()];
+                } else {
+                    n = &nodes[n->BottomLeftChildIndex()];
+                }
+            } else {
+                if (top > 0.f) {
+                    n = &nodes[n->TopRightChildIndex()];
+                } else {
+                    n = &nodes[n->BottomRightChildIndex()];
+                }
+            }
+        }
+    }
+
+    template<typename T>
+    typename QuadTree<T>::Node *QuadTree<T>::Root() {
+        return nodes.empty() ? 0 : &nodes[rootNodeIndex];
+    }
+
+    template<typename T>
+    const typename QuadTree<T>::Node *QuadTree<T>::Root() const {
+        return nodes.empty() ? 0 : &nodes[rootNodeIndex];
+    }
+
+    template <typename T>
+    int QuadTree<T>::AllocateNodeGroup(Node* parent) {
+        size_t oldCap = nodes.capacity();
+
+        int index = (int)nodes.size();
+        Node n;
+        n.parent = parent;
+        n.childIndex = 0xFFFFFFFF;
+        if (parent) {
+            n.radius = parent->radius * 0.5f;
+            n.center = parent->center - n.radius;
+        }
+
+        nodes.push_back(n);
+        if (parent)
+            n.center.x = parent->center.x + n.radius.x;
+        nodes.push_back(n);
+
+        if (parent) {
+            n.center.x = parent->center.x - n.radius.x;
+            n.center.y = parent->center.y + n.radius.y;
+        }
+
+        nodes.push_back(n);
+        if (parent)
+            n.center.x = parent->center.x + n.radius.x;
+        nodes.push_back(n);
+
+        return index;
+    }
+
+    template <typename T>
+    void QuadTree<T>::SplitLeaf(Node *leaf) {
+        leaf->childIndex = AllocateNodeGroup(leaf);
+
+        size_t i = 0;
+        while (i < leaf->objects.size()) {
+            const T& object = leaf->objects[i];
+
+            // Traverse the QuadTree to decide which quad to place this object into
+            float left = leaf->center.x - MinX(object);
+            float right = MaxX(object) - leaf->center;
+            float top = leaf->center.y - MinY(object);
+            float bottom = MaxY(object) - leaf->center.y;
+            float leftAndRight = std::min(left, right);
+            float topAndBottom = std::min(top, bottom);
+
+            float straddledEitherOne = std::max(leftAndRight, topAndBottom);
+
+            if (straddledEitherOne > 0.f) {
+                ++i;
+                continue;
+            }
+
+            if (left > 0.f) {
+                if (top > 0.f) {
+                    Add(object, &nodes[leaf->TopLeftChildIndex()]);
+                } else {
+                    Add(object, &nodes[leaf->BottomLeftChildIndex()]);
+                }
+            } else {
+                if (top > 0.f) {
+                    Add(object, &nodes[leaf->TopRightChildIndex()]);
+                } else {
+                    Add(object, &nodes[leaf->BottomRightChildIndex()]);
+                }
+            }
+
+            // Remove the object we added to a child from this node.
+            leaf->objects[i] = leaf->objects.back();
+            leaf->objects.pop_back();
+        }
+    }
+}
--- a/include/J3ML/Geometry/Ray.h
+++ b/include/J3ML/Geometry/Ray.h
@@ -0,0 +1,17 @@
+//
+// Created by dawsh on 1/25/24.
+//
+
+#pragma once
+
+#include <J3ML/LinearAlgebra/Vector3.h>
+
+namespace Geometry
+{
+    using LinearAlgebra::Vector3;
+    class Ray
+    {
+        Vector3 Origin;
+        Vector3 Direction;
+    };
+}
--- a/include/J3ML/Geometry/Sphere.h
+++ b/include/J3ML/Geometry/Sphere.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include "J3ML/Geometry.h"
+
+namespace Geometry
+{
+    class Sphere
+    {
+    public:
+        Sphere(const Vector3& pos, float radius)
+        {
+
+        }
+    };
+}
--- a/include/J3ML/Geometry/Triangle.h
+++ b/include/J3ML/Geometry/Triangle.h
@@ -0,0 +1,9 @@
+#pragma once
+
+namespace Geometry
+{
+    class Triangle
+    {
+
+    };
+}
--- a/include/J3ML/Geometry/Triangle2D.h
+++ b/include/J3ML/Geometry/Triangle2D.h
@@ -0,0 +1,8 @@
+//
+// Created by dawsh on 1/25/24.
+//
+
+#ifndef J3ML_TRIANGLE2D_H
+#define J3ML_TRIANGLE2D_H
+
+#endif //J3ML_TRIANGLE2D_H
--- a/include/J3ML/Geometry/TriangleMesh.h
+++ b/include/J3ML/Geometry/TriangleMesh.h
@@ -0,0 +1,9 @@
+#pragma once
+
+namespace Geometry
+{
+    class TriangleMesh
+    {
+
+    };
+}
--- a/include/J3ML/J3ML.h
+++ b/include/J3ML/J3ML.h
@@ -1,8 +1,17 @@
 //
 // Created by josh on 12/25/2023.
 //
+#include <cstdint>

-#ifndef J3ML_J3ML_H
-#define J3ML_J3ML_H
+namespace J3ML
+{
+    using u8 = uint8_t;
+    using u16 = uint16_t;
+    using u32 = uint32_t;
+    using u64 = uint64_t;

-#endif //J3ML_J3ML_H
+    using s8 = int8_t;
+    using s16 = int16_t;
+    using s32 = int32_t;
+    using s64 = int64_t;
+}
--- a/include/J3ML/LinearAlgebra/CoordinateFrame.h
+++ b/include/J3ML/LinearAlgebra/CoordinateFrame.h
@@ -1,20 +1,17 @@
 #pragma once

 #include <J3ML/LinearAlgebra.h>
+#include <J3ML/LinearAlgebra/Vector3.h>

 namespace 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;
    };
-
-
 }
--- a/include/J3ML/LinearAlgebra/Matrix3x3.h
+++ b/include/J3ML/LinearAlgebra/Matrix3x3.h
@@ -44,6 +44,7 @@ namespace LinearAlgebra {
        Vector3 GetRow(int index) const;
        Vector3 GetColumn(int index) const;

+        float &At(int row, int col);
        float At(int x, int y) const;

        void SetRotatePart(const Vector3& a, float angle);
@@ -58,6 +59,7 @@ namespace LinearAlgebra {

        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)
        {
@@ -85,14 +87,20 @@ 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 Matrix3x3& scale);
-        static Matrix3x3 FromRS(const Matrix3x3 &rotate, const Matrix3x3& scale);
+        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);
+        }


        /// 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 Matrix3x3& scale);
+        static Matrix3x3 Scale(const Vector3& scale);

        /// Returns the main diagonal.
        Vector3 Diagonal() const;
@@ -125,6 +133,11 @@ namespace LinearAlgebra {
        Vector2 Transform(const Vector2& rhs) const;
        Vector3 Transform(const Vector3& rhs) const;

+        Vector3 operator[](int row) const
+        {
+            return Vector3{elems[row][0], elems[row][1], elems[row][2]};
+        }
+
        Vector3 operator * (const Vector3& rhs) const;
        Matrix3x3 operator * (const Matrix3x3& rhs) const;

--- a/include/J3ML/LinearAlgebra/Matrix4x4.h
+++ b/include/J3ML/LinearAlgebra/Matrix4x4.h
@@ -12,7 +12,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.
@@ -23,29 +23,46 @@ namespace LinearAlgebra {
        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&);
-
+        /// Constructs a new float4x4 by explicitly specifying all the matrix elements.
+        /// The elements are specified in row-major format, i.e. the first row first followed by the second and third row.
+        /// E.g. The element _10 denotes the scalar at second (index 1) row, first (index 0) column.
        Matrix4x4(float m00, float m01, float m02, float m03,
            float m10, float m11, float m12, float m13,
            float m20, float m21, float m22, float m23,
            float m30, float m31, float m32, float m33);
+        /// Constructs the matrix by explicitly specifying the four column vectors.
+        /** @param col0 The first column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
+            direction of the local X axis.
+            @param col1 The second column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
+            direction of the local Y axis.
+            @param col2 The third column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
+            direction of the local Z axis.
+            @param col3 The fourth column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
+            position of the local space pivot. */
        Matrix4x4(const Vector4& r1, const Vector4& r2, const Vector4& r3, const Vector4& r4);


        explicit Matrix4x4(const Quaternion& orientation);

+        /// Constructs this float4x4 from the given quaternion and translation.
+        /// Logically, the translation occurs after the rotation has been performed.
+        Matrix4x4(const Quaternion& orientation, const Vector3 &translation);

        Vector3 GetTranslatePart() const;
        Matrix3x3 GetRotatePart() const
@@ -59,6 +76,8 @@ namespace LinearAlgebra {
        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);
@@ -66,14 +85,18 @@ namespace LinearAlgebra {
        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
-        {
-            return elems[x][y];
-        }
+        float &At(int row, int col);
+        float At(int x, int y) const;
+
+        /// 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;
        Vector4 WorldX() const;
@@ -84,18 +107,27 @@ namespace LinearAlgebra {
        // If the determinant is nonzero, this matrix is invertible.
        float Determinant() const;

-        Matrix4x4 Inverse() 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);
@@ -112,14 +144,34 @@ namespace LinearAlgebra {
        Vector4 GetRow() const;
        Vector4 GetColumn() const;

-        Vector4 operator[](int row)
-        {
-            return Vector4{elems[row][0], elems[row][1], elems[row][2], elems[row][3]};
-        }
+        Vector4 operator[](int row);
+
+        Matrix4x4 operator-() const;
+        Matrix4x4 operator +(const Matrix4x4& rhs) const;
+        Matrix4x4 operator - (const Matrix4x4& rhs) const;
+
+        Matrix4x4 operator *(float scalar) const;
+        Matrix4x4 operator /(float scalar) const;
+
+
+
+        Vector2 operator * (const Vector2& rhs) const { return this->Transform(rhs);}
+        Vector3 operator * (const Vector3& rhs) const { return this->Transform(rhs);}
+        Vector4 operator * (const Vector4& rhs) const { return this->Transform(rhs);}
+
+        Matrix4x4 operator * (const Matrix3x3 &rhs) const;
+
+        Matrix4x4 operator +() const;
+
+        Matrix4x4 operator * (const Matrix4x4& rhs) const;
+
+        Matrix4x4 &operator = (const Matrix3x3& rhs);
+        Matrix4x4 &operator = (const Quaternion& rhs);
+        Matrix4x4 &operator = (const Matrix4x4& rhs) = default;

    protected:
        float elems[4][4];

-        void SetMatrixRotatePart(Matrix4x4 &m, const Quaternion &q);
+
    };
 }
--- a/include/J3ML/LinearAlgebra/Quaternion.h
+++ b/include/J3ML/LinearAlgebra/Quaternion.h
@@ -8,37 +8,64 @@

 namespace LinearAlgebra
 {
-    class Quaternion : public Vector4
-    {
+    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) {
+            SetFromAxisAngle(rotationAxis, rotationAngleBetween);
+        }
+
+        Quaternion(const Vector4 &rotationAxis, float rotationAngleBetween) {
+            SetFromAxisAngle(rotationAxis, 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;
+        }
+
+
        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
@@ -57,7 +84,6 @@ namespace LinearAlgebra
        // TODO: Document (But do not override!) certain math functions that are the same for Vec4
        // TODO: Double Check which operations need to be overriden for correct behavior!

-
        // Multiplies two quaternions together.
        // The product q1 * q2 returns a quaternion that concatenates the two orientation rotations.
        // The rotation q2 is applied first before q1.
@@ -71,12 +97,12 @@ namespace LinearAlgebra

        // Divides a quaternion by another. Divison "a / b" results in a quaternion that rotates the orientation b to coincide with orientation of
        Quaternion operator / (const Quaternion& rhs) const;
-        Quaternion operator +(const Quaternion& rhs) const;
-        Quaternion operator +() const;
-        Quaternion operator -() const;
+        Quaternion operator + (const Quaternion& rhs) const;
+        Quaternion operator + () const;
+        Quaternion operator - () const;
        float Dot(const Quaternion &quaternion) const;

-        float Angle() const;
+        float Angle() const { return std::acos(w) * 2.f;}

        float AngleBetween(const Quaternion& target) const;

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

--- a/include/J3ML/LinearAlgebra/Vector2.h
+++ b/include/J3ML/LinearAlgebra/Vector2.h
@@ -1,14 +1,18 @@
 #pragma once
+#include <J3ML/J3ML.h>
 #include <J3ML/LinearAlgebra.h>
 #include <cstddef>

 namespace LinearAlgebra {
-    // A 2D (x, y) ordered pair.
+    using namespace J3ML;
+
+
+    /// A 2D (x, y) ordered pair.
    class Vector2 {
    public:
-        // Default Constructor - Initializes values to zero
+        /// Default Constructor - Initializes values to zero
        Vector2();
-        // Constructs a new Vector2 with the value (X, Y)
+        /// Constructs a new Vector2 with the value (X, Y)
        Vector2(float X, float Y);
        Vector2(const Vector2& rhs);   // Copy Constructor
        //Vector2(Vector2&&) = default;   // Move Constructor
@@ -44,73 +48,95 @@ namespace LinearAlgebra {
        Vector2 Clamp(const Vector2& min, const Vector2& max) const;
        static Vector2 Clamp(const Vector2& min, const Vector2& middle, const Vector2& max);

-        // Returns the magnitude between the two vectors.
+        /// Returns the magnitude between the two vectors.
        float Distance(const Vector2& to) const;
        static float Distance(const Vector2& from, const Vector2& to);

+        float DistanceSq(const Vector2& to) const;
+        static float DistanceSq(const Vector2& from, const Vector2& to);
+
+        float MinElement() const;
+
+        float MaxElement() const;
+
        float Length() const;
        static float Length(const Vector2& of);

        float LengthSquared() const;
        static float LengthSquared(const Vector2& of);

-        // Returns the length of the vector, which is sqrt(x^2 + y^2)
+        /// Returns the length of the vector, which is sqrt(x^2 + y^2)
        float Magnitude() const;
        static float Magnitude(const Vector2& 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.
+
+
+
+
+
+        bool IsFinite() const;
+        static bool IsFinite(const Vector2& v);
+
+        /// Returns a float value equal to the magnitudes of the two vectors multiplied together and then multiplied by the cosine of the angle between them.
+        /// For normalized vectors, dot returns 1 if they point in exactly the same direction,
+        /// -1 if they point in completely opposite directions, and 0 if the vectors are perpendicular.
        float Dot(const Vector2& rhs) const;
        static float Dot(const Vector2& lhs, const Vector2& rhs);

-        // Projects one vector onto another and returns the result. (IDK)
+        /// Projects one vector onto another and returns the result. (IDK)
        Vector2 Project(const Vector2& rhs) const;
-        // @see Project
+        /// @see Project
        static Vector2 Project(const Vector2& lhs, const Vector2& rhs);

-        // Returns a copy of this vector, resized to have a magnitude of 1, while preserving "direction"
+        /// Returns a copy of this vector, resized to have a magnitude of 1, while preserving "direction"
        Vector2 Normalize() const;
        static Vector2 Normalize(const Vector2& of);

-        // Linearly interpolates between two points.
-        // Interpolates between the points and b by the interpolant t.
-        // The parameter is (TODO: SHOULD BE!) clamped to the range[0, 1].
-        // This is most commonly used to find a point some fraction of the wy along a line between two endpoints (eg. to move an object gradually between those points).
+        /// Linearly interpolates between two points.
+        /// Interpolates between the points and b by the interpolant t.
+        /// The parameter is (TODO: SHOULD BE!) clamped to the range[0, 1].
+        /// This is most commonly used to find a point some fraction of the wy along a line between two endpoints (eg. to move an object gradually between those points).
        Vector2 Lerp(const Vector2& rhs, float alpha) const;
-        // @see Lerp
+        /// @see Lerp
        static Vector2 Lerp(const Vector2& lhs, const Vector2& rhs, float alpha);
-        // Note: Input vectors MUST be normalized first!
+        /// Note: Input vectors MUST be normalized first!
        float AngleBetween(const Vector2& rhs) const;
        static float AngleBetween(const Vector2& lhs, const Vector2& rhs);

-        // Adds two vectors.
+        /// Adds two vectors.
        Vector2 operator +(const Vector2& rhs) const;
        Vector2 Add(const Vector2& rhs) const;
        static Vector2 Add(const Vector2& lhs, const Vector2& rhs);

-        // Subtracts two vectors.
+        /// Subtracts two vectors.
        Vector2 operator -(const Vector2& rhs) const;
        Vector2 Sub(const Vector2& rhs) const;
        static Vector2 Sub(const Vector2& lhs, const Vector2& rhs);

-        // Multiplies this vector by a scalar value.
+        /// Multiplies this vector by a scalar value.
        Vector2 operator *(float rhs) const;
        Vector2 Mul(float scalar) const;
        static Vector2 Mul(const Vector2& lhs, float rhs);

-        // 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.
+        Vector2 Mul(const Vector2& v) const;
+
+        /// Divides this vector by a scalar.
        Vector2 operator /(float rhs) const;
        Vector2 Div(float scalar) const;
        static Vector2 Div(const Vector2& lhs, float rhs);

-        // 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
+        Vector2 Div(const Vector2& v) const;
+
+        /// Unary operator +
        Vector2 operator +() const; // TODO: Implement
        Vector2 operator -() const;
-        // Assigns a vector to another
-
-
-
+        /// 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*=(float scalar);
--- a/include/J3ML/LinearAlgebra/Vector3.h
+++ b/include/J3ML/LinearAlgebra/Vector3.h
@@ -38,8 +38,8 @@ public:
    }


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


    static void Orthonormalize(Vector3& a, Vector3& b, Vector3& c)
@@ -78,6 +78,11 @@ public:
    bool operator == (const Vector3& rhs) const;
    bool operator != (const Vector3& rhs) const;

+    bool IsFinite() const
+    {
+        return std::isfinite(x) && std::isfinite(y) && std::isfinite(z);
+    }
+
    Vector3 Min(const Vector3& min) const;
    static Vector3 Min(const Vector3& lhs, const Vector3& rhs);

@@ -87,7 +92,7 @@ public:
    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);

@@ -97,33 +102,33 @@ public:
    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,24 +141,38 @@ 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
+    Vector2 Div(const Vector2& v) const;
+
+    /// Unary + operator
    Vector3 operator+() const; // TODO: Implement
-    // Unary - operator (Negation)
+    /// Unary - operator (Negation)
    Vector3 operator-() const;
 public:
     float x = 0;
--- a/main.cpp
+++ b/main.cpp
@@ -1,5 +1,6 @@
 #include <iostream>
 #include <J3ML/Geometry.h>
+#include <J3ML/J3ML.h>

 int main(int argc, char** argv)
 {
--- a/src/J3ML/Geometry/AABB.cpp
+++ b/src/J3ML/Geometry/AABB.cpp
@@ -0,0 +1,5 @@
+#include <J3ML/Geometry/AABB.h>
+
+namespace Geometry {
+
+}
--- a/src/J3ML/Geometry/Capsule.cpp
+++ b/src/J3ML/Geometry/Capsule.cpp
@@ -0,0 +1,7 @@
+#include <J3ML/Geometry/Capsule.h>
+
+namespace Geometry
+{
+
+    Capsule::Capsule() : l() {}
+}
--- a/src/J3ML/Geometry/Frustum.cpp
+++ b/src/J3ML/Geometry/Frustum.cpp
@@ -0,0 +1,20 @@
+#include <J3ML/Geometry/Frustum.h>
+
+namespace 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;
+    }
+}
--- a/src/J3ML/Geometry/LineSegment.cpp
+++ b/src/J3ML/Geometry/LineSegment.cpp
@@ -0,0 +1,11 @@
+#include <J3ML/Geometry/LineSegment.h>
+
+namespace Geometry {
+
+    LineSegment::LineSegment(const Vector3 &a, const Vector3 &b) : A(a), B(b)
+    {
+
+    }
+
+    LineSegment::LineSegment() {}
+}
--- a/src/J3ML/Geometry/OBB.cpp
+++ b/src/J3ML/Geometry/OBB.cpp
@@ -0,0 +1,3 @@
+//
+// Created by dawsh on 1/25/24.
+//
--- a/src/J3ML/Geometry/Plane.cpp
+++ b/src/J3ML/Geometry/Plane.cpp
@@ -0,0 +1 @@
+#include <J3ML/Geometry/Plane.h>
--- a/src/J3ML/Geometry/Polygon.cpp
+++ b/src/J3ML/Geometry/Polygon.cpp
@@ -0,0 +1,5 @@
+#include <J3ML/Geometry/Polygon.h>
+
+namespace Geometry {
+
+}
--- a/src/J3ML/Geometry/Polyhedron.cpp
+++ b/src/J3ML/Geometry/Polyhedron.cpp
@@ -0,0 +1,6 @@
+#include <J3ML/Geometry/Polyhedron.h>
+
+namespace Geometry
+{
+
+}
--- a/src/J3ML/Geometry/QuadTree.cpp
+++ b/src/J3ML/Geometry/QuadTree.cpp
@@ -0,0 +1,6 @@
+#include <J3ML/Geometry/QuadTree.h>
+
+namespace Geometry
+{
+
+}
--- a/src/J3ML/Geometry/Ray.cpp
+++ b/src/J3ML/Geometry/Ray.cpp
@@ -0,0 +1,6 @@
+#include <J3ML/Geometry/Ray.h>
+
+namespace Geometry
+{
+
+}
--- a/src/J3ML/Geometry/Sphere.cpp
+++ b/src/J3ML/Geometry/Sphere.cpp
@@ -0,0 +1 @@
+#include <J3ML/Geometry/Sphere.h>
--- a/src/J3ML/Geometry/TriangleMesh.cpp
+++ b/src/J3ML/Geometry/TriangleMesh.cpp
@@ -0,0 +1 @@
+#include <J3ML/Geometry/TriangleMesh.h>
--- a/src/J3ML/LinearAlgebra/Matrix3x3.cpp
+++ b/src/J3ML/LinearAlgebra/Matrix3x3.cpp
@@ -37,9 +37,14 @@ namespace LinearAlgebra {
        return {x, y, z};
    }

+
    float Matrix3x3::At(int x, int y) const {
        return this->elems[x][y];
+    }

+    void Matrix3x3::SetAt(int x, int y, float value)
+    {
+        this->elems[x][y] = value;
    }

    Vector3 Matrix3x3::operator*(const Vector3 &rhs) const {
@@ -291,5 +296,9 @@ namespace LinearAlgebra {
        };
    }

+    float &Matrix3x3::At(int row, int col) {
+        return elems[row][col];
+    }
+
 }

--- a/src/J3ML/LinearAlgebra/Matrix4x4.cpp
+++ b/src/J3ML/LinearAlgebra/Matrix4x4.cpp
@@ -90,11 +90,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 +103,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 +173,140 @@ 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;
+    }
 }
--- a/src/J3ML/LinearAlgebra/Quaternion.cpp
+++ b/src/J3ML/LinearAlgebra/Quaternion.cpp
@@ -1,6 +1,7 @@
 #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 {
@@ -60,16 +61,14 @@ namespace LinearAlgebra {
    Quaternion::Quaternion(float X, float Y, float Z, float W) : Vector4(X,Y,Z,W) {}

    // TODO: implement
-    float Quaternion::Dot(const Quaternion &quaternion) const {}
+    float Quaternion::Dot(const Quaternion &rhs) const {
+        return x * rhs.x + y * rhs.y + z * rhs.z + w * rhs.w;
+    }

    Quaternion::Quaternion(Vector4 vector4) {

    }

-    float Quaternion::Angle() const {
-        return std::acos(w) * 2.f;
-    }
-
    Quaternion Quaternion::Normalize() const {
        float length = Length();
        if (length < 1e-4f)
@@ -165,4 +164,12 @@ 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};
+    }
 }
--- a/src/J3ML/LinearAlgebra/Transform2D.cpp
+++ b/src/J3ML/LinearAlgebra/Transform2D.cpp
@@ -2,4 +2,29 @@

 namespace LinearAlgebra {

+    const Transform2D Transform2D::Identity = Transform2D({0, 0}, {1, 1}, {0,0}, {0,0}, 0);
+    const Transform2D Transform2D::FlipX    = Transform2D({0, 0}, {-1, 1}, {0,0}, {0,0}, 0);
+    const Transform2D Transform2D::FlipY    = Transform2D({0, 0}, {1, -1}, {0,0}, {0,0}, 0);
+
+
+    Vector2 Transform2D::Transform(const Vector2 &input) const {
+        return transformation.Transform(input);
+    }
+
+    Transform2D::Transform2D(const Matrix3x3 &transform) : transformation(transform) { }
+
+    Transform2D::Transform2D(const Vector2& pos, const Vector2& scale, const Vector2& origin, const Vector2& skew, float rotation) {
+        transformation = Matrix3x3(pos.x, pos.y, rotation, scale.x, scale.y, origin.x, origin.y, skew.x, skew.y);
+    }
+
+    Transform2D Transform2D::Translate(float x, float y) const {
+        auto copy = Matrix3x3(transformation);
+        copy.SetAt(0, 0, transformation.At(0, 0) + x);
+        copy.SetAt(0, 1, transformation.At(0, 1) + y);
+        return Transform2D(copy);
+    }
+
+    Transform2D Transform2D::Translate(const LinearAlgebra::Vector2 &input) const {
+        return Translate(input.x, input.y);
+    }
 }
--- a/src/J3ML/LinearAlgebra/Vector2.cpp
+++ b/src/J3ML/LinearAlgebra/Vector2.cpp
@@ -22,7 +22,6 @@ namespace LinearAlgebra {
        if (index == 1) return y;
        return 0;
    }
-
    bool Vector2::IsWithinMarginOfError(const Vector2& rhs, float margin) const
    {
        return this->Distance(rhs) <= margin;
@@ -231,5 +230,27 @@ namespace LinearAlgebra {
        return *this / scalar;
    }

+    bool Vector2::IsFinite(const Vector2 &v) {
+        return v.IsFinite();
+    }

+    float Vector2::MinElement() const {
+        return std::min(x, y);
+    }
+
+    float Vector2::MaxElement() const {
+        return std::max(x, y);
+    }
+
+    Vector2 Vector2::Mul(const Vector2 &v) const {
+        return {this->x*v.x, this->y*v.y};
+    }
+
+    bool Vector2::IsFinite() const {
+        return std::isfinite(x) && std::isfinite(y);
+    }
+
+    Vector2 Vector2::Div(const Vector2 &v) const {
+        return {this->x/v.x, this->y/v.y};
+    }
 }
--- a/src/J3ML/LinearAlgebra/Vector3.cpp
+++ b/src/J3ML/LinearAlgebra/Vector3.cpp
@@ -301,7 +301,7 @@ namespace LinearAlgebra {
        return lhs.AngleBetween(rhs);
    }

-    Vector3 Vector3::Direction(const Vector3 &rhs) const {
+    Vector3 Vector3::Direction(const Vector3 &rhs) {
        float x = (cos(Math::Radians(rhs.y)) * cos(Math::Radians(rhs.x)));
        float y = -sin(Math::Radians(rhs.x));
        float z = (sin(Math::Radians(rhs.y)) * cos(Math::Radians(rhs.x)));
--- a/src/J3ML/LinearAlgebra/Vector4.cpp
+++ b/src/J3ML/LinearAlgebra/Vector4.cpp
@@ -9,6 +9,8 @@

 namespace 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)
    {}
--- a/tests/LinearAlgebra/Vector4Tests.cpp
+++ b/tests/LinearAlgebra/Vector4Tests.cpp
@@ -1,3 +1,106 @@
-//
-// Created by josh on 12/26/2023.
-//
+#include <gtest/gtest.h>
+#include <J3ML/LinearAlgebra/Vector4.h>
+
+using Vector4 = LinearAlgebra::Vector4;
+
+
+void EXPECT_V4_EQ(const Vector4& lhs, const Vector4& rhs)
+{
+
+}
+
+TEST(Vector4Test, V4_Constructor_Default)
+{
+    EXPECT_V4_EQ(Vector4(), Vector4::Zero);
+}
+
+TEST(Vector4Test, V4_Constructor_XYZW)
+{
+    Vector4 Input {0, 1, 0, 1};
+    EXPECT_FLOAT_EQ(Input.x, 0);
+    EXPECT_FLOAT_EQ(Input.y, 1);
+    EXPECT_FLOAT_EQ(Input.z, 0);
+    EXPECT_FLOAT_EQ(Input.w, 1);
+}
+
+TEST(Vector4Test, V4_Addition_Op) {
+    Vector4 A {1, 1, 1, 1};
+    Vector4 B {2, 2, 2, 2};
+
+    Vector4 ExpectedResult {3, 3, 3, 3};
+
+    EXPECT_V4_EQ(A + B, ExpectedResult);
+}
+
+TEST(Vector4Test, V4_Addition_Method)
+{
+
+}
+
+TEST(Vector4Test, V4_Addition_Static)
+{
+
+}
+
+TEST(Vector4Test, V4_Subtract_Op)
+{
+
+}
+
+TEST(Vector4Test, V4_Subtract_Method)
+{
+
+}
+
+TEST(Vector4Test, V4_Subtract_Static)
+{
+
+}
+
+TEST(Vector4Test, V4_Scalar_Mult_Op)
+{
+
+}
+
+TEST(Vector4Test, V4_Scalar_Mult_Method)
+{
+
+}
+
+TEST(Vector4Test, V4_Scalar_Mult_Static)
+{
+
+}
+
+TEST(Vector4Test, V4_Scalar_Div_Op)
+{
+
+}
+
+TEST(Vector4Test, V4_Scalar_Div_Method)
+{
+
+}
+
+TEST(Vector4Test, V4_Scalar_Div_Static)
+{
+
+}
+
+TEST(Vector4Test, V4_Sizeof)
+{
+
+}
+
+TEST(Vector4Test, V4_NaN)
+{
+
+}
+
+TEST(Vector4Tests, V4_Min) {}
+TEST(Vector4Tests, V4_Max) {}
+TEST(Vector4Tests, V4_Clamp) {}
+TEST(Vector4Tests, V4_DotProduct) {}
+TEST(Vector4Tests, V4_CrossProduct) {}
+TEST(Vector4Tests, V4_Project) {}
+TEST(Vector4Test, V4_Normalize) {}
Author	SHA1	Message	Date
josh	19b5630deb	Move to implementation file	2024-01-31 20:06:35 -05:00
josh	5080305965	Implement Mat4x4 Inverse() (Yikes!!!)	2024-01-31 20:05:31 -05:00
josh	40e69d5c4f	Implement Mat4x4 Translate, Transform, FromTranslation	2024-01-31 18:34:15 -05:00
josh	132b8a0a66	Implement more methods	2024-01-30 21:35:55 -05:00
josh	0c20e9bb21	Implement constant Vector4s	2024-01-30 21:35:41 -05:00
josh	710a41cbb1	Implement Mat4x4	2024-01-30 21:30:13 -05:00
josh	b76c5683db	Remove Vector2 *	2024-01-30 21:29:42 -05:00
josh	7278d783dc	Fix Circular depends	2024-01-30 21:29:19 -05:00
josh	ef297e525c	Implement CreateFrustumFromCoordinateFrame()	2024-01-30 21:29:07 -05:00
josh	239c90f75b	Implement CreateFrustumFromCoordinateFrame()	2024-01-30 21:29:01 -05:00
josh	09d0391c85	Fix member public	2024-01-30 21:28:42 -05:00
josh	83021229d5	Fix circular depends	2024-01-30 21:28:21 -05:00
josh	21ceca62dc	Adding more Mat4x4 functionality	2024-01-30 13:13:09 -05:00
josh	32577f79b8	Header implementations	2024-01-29 14:50:00 -05:00
josh	e76a0954d3	Implement Vector-to-Vector Multiplication + division	2024-01-29 14:43:51 -05:00
josh	47b25c695f	Partial Implement QuadTree	2024-01-29 14:43:26 -05:00
josh	6c7d63e467	Implement AABB2D	2024-01-29 03:16:50 -05:00
josh	16c8dd1998	Implement QuadTree 2D	2024-01-26 15:55:51 -05:00
josh	79f6b2f154	Implement Vector2 functions	2024-01-26 15:55:42 -05:00
josh	08974413ae	Implement AABB2D	2024-01-26 15:53:25 -05:00
josh	256fe730cd	Implement Geometric Class Definitions (round 2)	2024-01-26 00:14:28 -05:00
josh	4152b0d2aa	Implement Geometric Class Definitions	2024-01-25 19:09:48 -05:00
josh	56077b7c86	Implement AABB	2024-01-25 19:09:36 -05:00
josh	5cd5a44963	Implement Geometric Types	2024-01-25 14:01:19 -05:00
josh	d012af1214	Implement Transform2D	2024-01-23 21:46:07 -05:00
josh	f04e08201d	Merge remote-tracking branch 'origin/main'	2024-01-23 04:14:34 -05:00
josh	f7a7ec38d7	Implement Transform2D.h	2024-01-23 04:14:24 -05:00
Redacted	a64f129bf7	Make it static	2024-01-15 16:09:44 -05:00