Implement Mat4x4::Determinant

CMakeLists.txt

@@ -54,7 +54,10 @@ add_library(J3ML SHARED ${J3ML_SRC}
         src/J3ML/Geometry/TriangleMesh.cpp
         src/J3ML/Geometry/QuadTree.cpp
         src/J3ML/Geometry/LineSegment.cpp
-        include/J3ML/Geometry/AABB2D.h)
+        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})

include/J3ML/Geometry.h

@@ -14,6 +14,10 @@ namespace Geometry {
     };
 
     class Rectangle;
+    class AABB;
+    class OBB;
+    class Capsule;
+    class Frustum;
     class OBB2D;
     class Line2D;
     class Ray2D;

include/J3ML/Geometry/AABB.h

@@ -1,15 +1,31 @@
 #pragma once
 
 #include <J3ML/LinearAlgebra/Vector3.h>
-#include "Plane.h"
-#include "Sphere.h"
-#include "OBB.h"
-#include "LineSegment.h"
 
-using namespace LinearAlgebra;
+#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
@@ -21,21 +37,75 @@ namespace Geometry
 
     class AABB
     {
-        static AABB FromCenterAndSize(const Vector3 FromSize);
-        float MinX();
-        // Returns the smallest sphere that contains this AABB.
-        // This function computes the minimal volume sphere that contains all the points inside this AABB
-        Sphere MinimalEnclosingSphere() const;
-        // Returns the largest sphere that can fit inside this 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 GetCentroid() const;
+        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 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;
+        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);
@@ -45,7 +115,6 @@ namespace Geometry
         Vector3 FaceNormal(int faceIndex) const;
         Plane FacePlane(int faceIndex);
         static AABB MinimalEnclosingAABB(const Vector3* pointArray, int numPoints);
-        Vector3 GetSize();
         Vector3 GetVolume();
         float GetVolumeCubed();
         float GetSurfaceArea();

include/J3ML/Geometry/AABB2D.h

@@ -12,6 +12,7 @@ namespace Geometry
 
         Vector2 minPoint;
         Vector2 maxPoint;
+        AABB2D() {}
         AABB2D(const Vector2& min, const Vector2& max):
         minPoint(min), maxPoint(max)
         {}
@@ -33,7 +34,7 @@ namespace Geometry
             maxPoint = Vector2::Max(maxPoint, point);
         }
 
-        bool Intersects(const AABB2D &rhs) const
+        bool Intersects(const AABB2D& rhs) const
         {
             return maxPoint.x >= rhs.minPoint.x &&
                    maxPoint.y >= rhs.minPoint.y &&
@@ -41,13 +42,13 @@ namespace Geometry
                    rhs.maxPoint.y >= minPoint.y;
         }
 
-        bool Contains(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 vec2d &pt) const
+        bool Contains(const Vector2& pt) const
         {
             return pt.x >= minPoint.x && pt.y >= minPoint.y
                    && pt.x <= maxPoint.x && pt.y <= maxPoint.y;
@@ -79,9 +80,27 @@ namespace Geometry
             return minPoint + normalizedPos.Mul(maxPoint - minPoint);
         }
 
-        Vector2 ToNormalizedLocalSpace(const vec2d &pt) const
+        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;
+        }
+
     };
 }

include/J3ML/Geometry/Capsule.h

@@ -13,10 +13,10 @@ namespace Geometry
         // Specifies the radius of this capsule
         float r;
 
-        Capsule() {}
+        Capsule();
         Capsule(const LineSegment& endPoints, float radius);
         Capsule(const Vector3& bottomPt, const Vector3& topPt, float radius);
-        bool IsDegenerate()const;
+        bool IsDegenerate() const;
         float Height() const;
         float Diameter() const;
         Vector3 Bottom() const;

include/J3ML/Geometry/Frustum.h

@@ -4,6 +4,7 @@
 #pragma once
 
 #include "Plane.h"
+#include <J3ML/LinearAlgebra/CoordinateFrame.h>
 
 namespace Geometry
 {
@@ -32,24 +33,8 @@ namespace Geometry
         Plane LeftFace;
         Plane FarFace;
         Plane NearFace;
-        static Frustum CreateFrustumFromCamera(const Camera& cam, float aspect, float fovY, float zNear, float zFar);
-
+        static Frustum CreateFrustumFromCamera(const CoordinateFrame& cam, float aspect, float fovY, float zNear, float zFar);
     };
 
-    Frustum Frustum::CreateFrustumFromCamera(const Camera &cam, float aspect, float fovY, float zNear, float zFar) {
-        Frustum frustum;
-        const float halfVSide = zFar * tanf(fovY * 0.5f);
-        const float halfHSide = halfVSide * aspect;
-
-        const Vector3 frontMultFar = cam.Front * zFar;
-
-        frustum.NearFace  = Plane{cam.Position + cam.Front * zNear, cam.Front};
-        frustum.FarFace   = Plane{cam.Position + frontMultFar, -cam.Front};
-        frustum.RightFace = Plane{cam.Position, Vector3::Cross(frontMultFar - cam.Right * halfHSide, cam.Up)};
-        frustum.LeftFace  = Plane{cam.Position, Vector3::Cross(cam.Up, frontMultFar+cam.Right*halfHSide)};
-        frustum.TopFace   = Plane{cam.Position, Vector3::Cross(cam.Right, frontMultFar - cam.Up * halfVSide)};
-        frustum.BottomFace   = Plane{cam.Position, Vector3::Cross(frontMultFar + cam.Up * halfVSide, cam.Right)};
-        return frustum;
-    }
 
 }

include/J3ML/Geometry/LineSegment.h

@@ -4,8 +4,12 @@
 
 namespace Geometry
 {
+    using LinearAlgebra::Vector3;
     class LineSegment
     {
+    public:
+        LineSegment();
+        LineSegment(const Vector3& a, const Vector3& b);
         Vector3 A;
         Vector3 B;
     };

include/J3ML/Geometry/OBB.h

@@ -1,6 +1,9 @@
 #pragma once
 
-#include "AABB.h"
+#include <J3ML/Geometry.h>
+#include <J3ML/Geometry/AABB.h>
+#include <J3ML/Geometry/LineSegment.h>
+#include <J3ML/Geometry/Polyhedron.h>
 
 namespace Geometry {
     class OBB
@@ -15,14 +18,14 @@ namespace Geometry {
 
         OBB() {}
         OBB(const Vector3& pos, const Vector3& radii, const Vector3& axis0, const Vector3& axis1, const Vector3& axis2);
-        OBB(const AABB& aabb);
+        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;
 
-        AABB MinimalEnclosingAABB() const;
+        Geometry::AABB MinimalEnclosingAABB() const;
 
         Sphere MinimalEnclosingSphere() const;
         Sphere MaximalContainedSphere() const;
@@ -39,7 +42,7 @@ namespace Geometry {
 
         float Volume();
         float SurfaceArea();
-        LineSegment Edge(int edgeIndex) const;
+        Geometry::LineSegment Edge(int edgeIndex) const;
         Vector3 CornerPoint(int cornerIndex) const;
     };
 }

include/J3ML/Geometry/Polygon.h

@@ -1,8 +1,7 @@
-//
-// Created by dawsh on 1/25/24.
-//
+#pragma once
 
-#ifndef J3ML_POLYGON_H
-#define J3ML_POLYGON_H
+namespace Geometry {
+    class Polygon {
 
-#endif //J3ML_POLYGON_H
+    };
+}

include/J3ML/Geometry/Polyhedron.h

@@ -0,0 +1,8 @@
+#pragma once
+
+namespace Geometry
+{
+    class Polyhedron {
+
+    };
+}

include/J3ML/Geometry/QuadTree.h

@@ -5,15 +5,24 @@
 #include <J3ML/LinearAlgebra/Vector2.h>
 #include "AABB2D.h"
 
-namespace Geometry
-{
+namespace Geometry {
+
+
     using LinearAlgebra::Vector2;
-    template <typename T>
-    class QuadTree
-    {
+
+    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
-        {
+        struct Node {
             Node *parent;
             uint32_t childIndex;
             std::vector<T> objects;
@@ -21,19 +30,22 @@ namespace Geometry
             Vector2 center;
             Vector2 radius;
 
-            bool IsLeaf() const { return childIndex == 0xFFFFFFFF;}
+            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;}
+            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){
+            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.
+                        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;
@@ -41,11 +53,9 @@ namespace Geometry
                 }
             }
 
-            AABB2D ComputeAABB()
-            {}
+            AABB2D ComputeAABB() {}
 
-            float DistanceSq(const Vector2& point) const
-            {
+            float DistanceSq(const Vector2 &point) const {
                 Vector2 centered = point - center;
                 Vector2 closestPoint = centered.Clamp(-radius, radius);
                 return closestPoint.DistanceSq(centered);
@@ -54,30 +64,28 @@ namespace Geometry
         };
 
         // Helper struct used when traversing through the tree
-        struct TraversalStackItem
-        {
+        struct TraversalStackItem {
             Node *node;
         };
 
-        QuadTree():
-        rootNodeIndex(-1),
-        boundingAABB(Vector2(0,0), Vector2(0,0))
-        {
+        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));
+        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);
+        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);
+        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.
@@ -85,6 +93,7 @@ namespace Geometry
 
         /// @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.
@@ -123,6 +132,7 @@ namespace Geometry
         /// 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);
 
@@ -139,8 +149,11 @@ namespace Geometry
         AABB2D boundingAABB;
 
         void GrowRootTopLeft();
+
         void GrowRootTopRight();
+
         void GrowRootBottomLeft();
+
         void GrowRootBottomRight();
 
         void GrowImpl(int quadrantForRoot);
@@ -151,9 +164,8 @@ namespace Geometry
     // but the presence of the implementation file is a requirement
 
 
-    template <typename T>
-    void QuadTree<T>::Clear(const Vector2& minXY, const Vector2& maxXY)
-    {
+    template<typename T>
+    void QuadTree<T>::Clear(const Vector2 &minXY, const Vector2 &maxXY) {
         nodes.clear();
 
         boundingAABB.minPoint = minXY;
@@ -165,4 +177,193 @@ namespace Geometry
         root->center = (minXY + maxXY) * 0.5f;
         root->radius = maxXY - root->center;
     }
+
+    template<typename T>
+    void QuadTree<T>::Add(const T &object) {
+        Node *n = Root();
+        AABB2D objectAABB = GetAABB2D(object);
+
+        // Ramen Noodle Bowls of nested if statements are generally bad practice
+        // Unfortunately, sometimes the problem domain makes this unavoidable
+
+        if (objectAABB.minPoint.x >= boundingAABB.minPoint.x) {
+            // object fits left.
+            if (objectAABB.maxPoint.x <= boundingAABB.maxPoint.x) {
+                // object fits left and right.
+                if (objectAABB.minPoint.y >= boundingAABB.minPoint.y) {
+                    // Object fits left, right, and top.
+                    if (objectAABB.maxPoint.y <= boundingAABB.maxPoint.y) {
+                        // Object fits the whole root AABB. Can safely add into the existing tree size.
+                        AddObject(object, n);
+                        return;
+                    } else {
+                        // Object fits left, right, and top, but not bottom.
+                        GrowRootBottomRight(); // Could grow bottom-left as well, wouldn't matter here.
+                    }
+                } else {
+                    // Object fits left and right, but not to top.
+                    GrowRootTopRight(); // Could grow top-left as well, wouldn't matter here.
+                }
+            } else {
+                // Object fits left, but not to right. We must grow right. Check whether to grow top or bottom;
+                if (objectAABB.minPoint.y < boundingAABB.minPoint.y)
+                    GrowRootTopRight();
+                else
+                    GrowRootBottomRight();
+            }
+        } else {
+            // We must grow left. Check whether to grow top or bottom.
+            if (objectAABB.minPoint.y < boundingAABB.minPoint.y)
+                GrowRootTopLeft();
+            else
+                GrowRootBottomLeft();
+        }
+        // Now that we have grown the tree root node, try adding again.
+        Add(object);
+    }
+
+    template<typename T>
+    void QuadTree<T>::Remove(const T &object) {
+        Node *n = GetQuadTreeNode(object);
+        if (n) {
+            n->Remove(object);
+        }
+    }
+
+    template<typename T>
+    void QuadTree<T>::Add(const T &object, Node *n) {
+        for (;;) {
+            // Traverse the QuadTree to decide which quad to place this object on.
+            float left = n->center.x - MinX(object); // If left > 0.f, then the object overlaps with the left quadrant.
+            float right = MaxX(object) - n->center.x; // If right > 0.f, then the object overlaps with the right quadrant.
+
+            float top = n->center.y - MinY(object); // If top > 0.f, then the object overlaps with the top quadrant
+            float bottom =
+                    MaxY(object) - n->center.y; // If bottom > 0.f, then the object overlaps with the bottom quadrant
+            float leftAndRight = std::min(left, right); // If > 0.f, then the object straddles left-right halves.
+            float topAndBottom = std::min(top, bottom); // If > 0.f, then the object straddles top-bottom halves.
+            float straddledEitherOne = std::max(leftAndRight,
+                                                topAndBottom); // If > 0.f, thne the object is in two or more quadrants.
+
+            // Note: It can happen that !left && !right, or !top && !bottom.
+            // but the if()s are setup below so that right/bottom is taken if no left/top, so that is ok.
+
+            // We must put the object onto this node if
+            // a) the object straddled the parent->child split lines.
+            // b) this object is a leaf
+            if (straddledEitherOne > 0.f) {
+                n->objects.push_back(object);
+                AssociateQuadTreeNode(object, n);
+                return;
+            }
+            if (n->IsLeaf()) {
+                n->objects.push_back(object);
+                AssociateQuadTreeNode(object, n);
+
+                if ((int) n->objects.size() > minQuadTreeNodeObjectCount &&
+                    std::min(n->radius.x, n->radius.y) >= minQuadTreeQuadrantSize) {
+                    SplitLeaf(n);
+                }
+
+                return;
+            }
+            if (left > 0.f) {
+                if (top > 0.f) {
+                    n = &nodes[n->TopLeftChildIndex()];
+                } else {
+                    n = &nodes[n->BottomLeftChildIndex()];
+                }
+            } else {
+                if (top > 0.f) {
+                    n = &nodes[n->TopRightChildIndex()];
+                } else {
+                    n = &nodes[n->BottomRightChildIndex()];
+                }
+            }
+        }
+    }
+
+    template<typename T>
+    typename QuadTree<T>::Node *QuadTree<T>::Root() {
+        return nodes.empty() ? 0 : &nodes[rootNodeIndex];
+    }
+
+    template<typename T>
+    const typename QuadTree<T>::Node *QuadTree<T>::Root() const {
+        return nodes.empty() ? 0 : &nodes[rootNodeIndex];
+    }
+
+    template <typename T>
+    int QuadTree<T>::AllocateNodeGroup(Node* parent) {
+        size_t oldCap = nodes.capacity();
+
+        int index = (int)nodes.size();
+        Node n;
+        n.parent = parent;
+        n.childIndex = 0xFFFFFFFF;
+        if (parent) {
+            n.radius = parent->radius * 0.5f;
+            n.center = parent->center - n.radius;
+        }
+
+        nodes.push_back(n);
+        if (parent)
+            n.center.x = parent->center.x + n.radius.x;
+        nodes.push_back(n);
+
+        if (parent) {
+            n.center.x = parent->center.x - n.radius.x;
+            n.center.y = parent->center.y + n.radius.y;
+        }
+
+        nodes.push_back(n);
+        if (parent)
+            n.center.x = parent->center.x + n.radius.x;
+        nodes.push_back(n);
+
+        return index;
+    }
+
+    template <typename T>
+    void QuadTree<T>::SplitLeaf(Node *leaf) {
+        leaf->childIndex = AllocateNodeGroup(leaf);
+
+        size_t i = 0;
+        while (i < leaf->objects.size()) {
+            const T& object = leaf->objects[i];
+
+            // Traverse the QuadTree to decide which quad to place this object into
+            float left = leaf->center.x - MinX(object);
+            float right = MaxX(object) - leaf->center;
+            float top = leaf->center.y - MinY(object);
+            float bottom = MaxY(object) - leaf->center.y;
+            float leftAndRight = std::min(left, right);
+            float topAndBottom = std::min(top, bottom);
+
+            float straddledEitherOne = std::max(leftAndRight, topAndBottom);
+
+            if (straddledEitherOne > 0.f) {
+                ++i;
+                continue;
+            }
+
+            if (left > 0.f) {
+                if (top > 0.f) {
+                    Add(object, &nodes[leaf->TopLeftChildIndex()]);
+                } else {
+                    Add(object, &nodes[leaf->BottomLeftChildIndex()]);
+                }
+            } else {
+                if (top > 0.f) {
+                    Add(object, &nodes[leaf->TopRightChildIndex()]);
+                } else {
+                    Add(object, &nodes[leaf->BottomRightChildIndex()]);
+                }
+            }
+
+            // Remove the object we added to a child from this node.
+            leaf->objects[i] = leaf->objects.back();
+            leaf->objects.pop_back();
+        }
+    }
 }

include/J3ML/Geometry/Ray.h

@@ -8,6 +8,7 @@
 
 namespace Geometry
 {
+    using LinearAlgebra::Vector3;
     class Ray
     {
         Vector3 Origin;

include/J3ML/Geometry/Sphere.h

@@ -1,9 +1,15 @@
 #pragma once
 
+#include "J3ML/Geometry.h"
+
 namespace Geometry
 {
     class Sphere
     {
+    public:
+        Sphere(const Vector3& pos, float radius)
+        {
 
+        }
     };
 }

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

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);
@@ -132,7 +133,11 @@ namespace LinearAlgebra {
         Vector2 Transform(const Vector2& rhs) const;
         Vector3 Transform(const Vector3& rhs) const;
 
-        Vector2 operator * (const Vector2& 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;
 

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,42 +23,54 @@ 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
-        {
-            return Matrix3x3 {
-                At(0, 0), At(0, 1), At(0, 2),
-                At(1, 0), At(1, 1), At(1, 2),
-                At(2, 0), At(2, 1), At(2, 2)
-            };
-        }
+        Matrix3x3 GetRotatePart() const;
         void SetTranslatePart(float translateX, float translateY, float translateZ);
         void SetTranslatePart(const Vector3& offset);
         void SetRotatePart(const Quaternion& q);
+        void Set3x3Part(const Matrix3x3& r);
+
 
 
         void SetRow(int row, const Vector3& rowVector, float m_r3);
@@ -66,36 +78,54 @@ 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;
         Vector4 WorldY() const;
         Vector4 WorldZ() const;
 
+        float Determinant3x3() const;
         /// Computes the determinant of this matrix.
         // If the determinant is nonzero, this matrix is invertible.
         float Determinant() const;
 
-        Matrix4x4 Inverse() const
-        {
+        #define SKIPNUM(val, skip) (val >= skip ? (val+1) : val)
 
-        }
+        float Minor(int i, int j) const;
+
+        Matrix4x4 Inverse() const;
 
         Matrix4x4 Transpose() const;
 
+        Vector2 Transform(float tx, float ty) const;
         Vector2 Transform(const Vector2& rhs) const;
+
+
+        Vector3 Transform(float tx, float ty, float tz) const;
         Vector3 Transform(const Vector3& rhs) const;
+
+        Vector4 Transform(float tx, float ty, float tz, float tw) const;
         Vector4 Transform(const Vector4& rhs) const;
 
 
+        Matrix4x4 Translate(const Vector3& rhs) const;
+        static Matrix4x4 FromTranslation(const Vector3& rhs);
+
+
+
+
         static Matrix4x4 D3DOrthoProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
         static Matrix4x4 D3DOrthoProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
         static Matrix4x4 D3DPerspProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
@@ -112,14 +142,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);
+
     };
 }

include/J3ML/LinearAlgebra/Quaternion.h

@@ -8,51 +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
-        {
+        Vector3 GetAxis() const {
             float rcpSinAngle = 1 - (std::sqrt(1 - w * w));
 
             return Vector3(x, y, z) * rcpSinAngle;
         }
 
-        float GetAngle() const
-        {
+        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
@@ -89,7 +102,7 @@ namespace LinearAlgebra
         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;
 

include/J3ML/LinearAlgebra/Vector2.h

@@ -1,8 +1,12 @@
 #pragma once
+#include <J3ML/J3ML.h>
 #include <J3ML/LinearAlgebra.h>
 #include <cstddef>
 
 namespace LinearAlgebra {
+    using namespace J3ML;
+
+
     /// A 2D (x, y) ordered pair.
     class Vector2 {
     public:
@@ -65,6 +69,11 @@ namespace LinearAlgebra {
         float Magnitude() const;
         static float Magnitude(const Vector2& of);
 
+
+
+
+
+
         bool IsFinite() const;
         static bool IsFinite(const Vector2& v);
 
@@ -112,16 +121,18 @@ namespace LinearAlgebra {
         /// Multiplies this vector by a vector, element-wise
         /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
         ///	 but this function is provided here for syntactical convenience.
-        Vector2 Mul(const Vector2& v) const
-        {
-            return {this->x*v.x, this->y*v.y};
-        }
+        Vector2 Mul(const Vector2& v) const;
 
         /// Divides this vector by a scalar.
         Vector2 operator /(float rhs) const;
         Vector2 Div(float scalar) const;
         static Vector2 Div(const Vector2& lhs, float rhs);
 
+        /// Divides this vector by a vector, element-wise
+        /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
+        /// but this function is provided here for syntactical convenience
+        Vector2 Div(const Vector2& v) const;
+
         /// Unary operator +
         Vector2 operator +() const; // TODO: Implement
         Vector2 operator -() const;

include/J3ML/LinearAlgebra/Vector3.h

@@ -38,7 +38,7 @@ public:
     }
 
 
-    //Returns the DirectionVector for a given angle.
+    /// Returns the DirectionVector for a given angle.
     static Vector3 Direction(const Vector3 &rhs) ;
 
 
@@ -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;

src/J3ML/Geometry/Capsule.cpp

@@ -3,4 +3,5 @@
 namespace Geometry
 {
 
+    Capsule::Capsule() : l() {}
 }

src/J3ML/Geometry/Frustum.cpp

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

src/J3ML/Geometry/LineSegment.cpp

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

src/J3ML/Geometry/Polygon.cpp

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

src/J3ML/Geometry/Polyhedron.cpp

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

src/J3ML/LinearAlgebra/Matrix3x3.cpp

@@ -296,5 +296,9 @@ namespace LinearAlgebra {
         };
     }
 
+    float &Matrix3x3::At(int row, int col) {
+        return elems[row][col];
+    }
+
 }
 

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,188 @@ namespace LinearAlgebra {
         float p30 = 0;       float p31 = 0;       float p32 = 0.f;         float p33 = 1.f;
     }
 
+    float Matrix4x4::At(int x, int y) const {
+        return elems[x][y];
+    }
 
+    Matrix4x4 Matrix4x4::operator*(const Matrix4x4 &rhs) const {
+
+        float r00 = At(0, 0) * rhs.At(0, 0) + At(0, 1) * rhs.At(1, 0) + At(0, 2) * rhs.At(2, 0) + At(0, 3) * rhs.At(3, 0);
+        float r01 = At(0, 0) * rhs.At(0, 1) + At(0, 1) * rhs.At(1, 1) + At(0, 2) * rhs.At(2, 1) + At(0, 3) * rhs.At(3, 1);
+        float r02 = At(0, 0) * rhs.At(0, 2) + At(0, 1) * rhs.At(1, 2) + At(0, 2) * rhs.At(2, 2) + At(0, 3) * rhs.At(3, 2);
+        float r03 = At(0, 0) * rhs.At(0, 3) + At(0, 1) * rhs.At(1, 3) + At(0, 2) * rhs.At(2, 3) + At(0, 3) * rhs.At(3, 3);
+
+        float r10 = At(1, 0) * rhs.At(0, 0) + At(1, 1) * rhs.At(1, 0) + At(1, 2) * rhs.At(2, 0) + At(1, 3) * rhs.At(3, 0);
+        float r11 = At(1, 0) * rhs.At(0, 1) + At(1, 1) * rhs.At(1, 1) + At(1, 2) * rhs.At(2, 1) + At(1, 3) * rhs.At(3, 1);
+        float r12 = At(1, 0) * rhs.At(0, 2) + At(1, 1) * rhs.At(1, 2) + At(1, 2) * rhs.At(2, 2) + At(1, 3) * rhs.At(3, 2);
+        float r13 = At(1, 0) * rhs.At(0, 3) + At(1, 1) * rhs.At(1, 3) + At(1, 2) * rhs.At(2, 3) + At(1, 3) * rhs.At(3, 3);
+
+        float r20 = At(2, 0) * rhs.At(0, 0) + At(2, 1) * rhs.At(1, 0) + At(2, 2) * rhs.At(2, 0) + At(2, 3) * rhs.At(3, 0);
+        float r21 = At(2, 0) * rhs.At(0, 1) + At(2, 1) * rhs.At(1, 1) + At(2, 2) * rhs.At(2, 1) + At(2, 3) * rhs.At(3, 1);
+        float r22 = At(2, 0) * rhs.At(0, 2) + At(2, 1) * rhs.At(1, 2) + At(2, 2) * rhs.At(2, 2) + At(2, 3) * rhs.At(3, 2);
+        float r23 = At(2, 0) * rhs.At(0, 3) + At(2, 1) * rhs.At(1, 3) + At(2, 2) * rhs.At(2, 3) + At(2, 3) * rhs.At(3, 3);
+
+        float r30 = At(3, 0) * rhs.At(0, 0) + At(3, 1) * rhs.At(1, 0) + At(3, 2) * rhs.At(2, 0) + At(3, 3) * rhs.At(3, 0);
+        float r31 = At(3, 0) * rhs.At(0, 1) + At(3, 1) * rhs.At(1, 1) + At(3, 2) * rhs.At(2, 1) + At(3, 3) * rhs.At(3, 1);
+        float r32 = At(3, 0) * rhs.At(0, 2) + At(3, 1) * rhs.At(1, 2) + At(3, 2) * rhs.At(2, 2) + At(3, 3) * rhs.At(3, 2);
+        float r33 = At(3, 0) * rhs.At(0, 3) + At(3, 1) * rhs.At(1, 3) + At(3, 2) * rhs.At(2, 3) + At(3, 3) * rhs.At(3, 3);
+        return {r00,r01,r02,r03, r10, r11, r12, r13, r20,r21,r22,r23, r30,r31,r32,r33};
+    }
+
+    Vector4 Matrix4x4::operator[](int row) {
+        return Vector4{elems[row][0], elems[row][1], elems[row][2], elems[row][3]};
+    }
+
+    Matrix4x4 Matrix4x4::operator*(const Matrix3x3 &rhs) const {
+        float r00 = At(0, 0) * rhs.At(0, 0) + At(0, 1) * rhs.At(1, 0) + At(0, 2) * rhs.At(2, 0);
+        float r01 = At(0, 0) * rhs.At(0, 1) + At(0, 1) * rhs.At(1, 1) + At(0, 2) * rhs.At(2, 1);
+        float r02 = At(0, 0) * rhs.At(0, 2) + At(0, 1) * rhs.At(1, 2) + At(0, 2) * rhs.At(2, 2);
+        float r03 = At(0, 3);
+
+        float r10 = At(1, 0) * rhs.At(0, 0) + At(1, 1) * rhs.At(1, 0) + At(1, 2) * rhs.At(2, 0);
+        float r11 = At(1, 0) * rhs.At(0, 1) + At(1, 1) * rhs.At(1, 1) + At(1, 2) * rhs.At(2, 1);
+        float r12 = At(1, 0) * rhs.At(0, 2) + At(1, 1) * rhs.At(1, 2) + At(1, 2) * rhs.At(2, 2);
+        float r13 = At(1, 3);
+
+        float r20 = At(2, 0) * rhs.At(0, 0) + At(2, 1) * rhs.At(1, 0) + At(2, 2) * rhs.At(2, 0);
+        float r21 = At(2, 0) * rhs.At(0, 1) + At(2, 1) * rhs.At(1, 1) + At(2, 2) * rhs.At(2, 1);
+        float r22 = At(2, 0) * rhs.At(0, 2) + At(2, 1) * rhs.At(1, 2) + At(2, 2) * rhs.At(2, 2);
+        float r23 = At(2, 3);
+
+        float r30 = At(3, 0) * rhs.At(0, 0) + At(3, 1) * rhs.At(1, 0) + At(3, 2) * rhs.At(2, 0);
+        float r31 = At(3, 0) * rhs.At(0, 1) + At(3, 1) * rhs.At(1, 1) + At(3, 2) * rhs.At(2, 1);
+        float r32 = At(3, 0) * rhs.At(0, 2) + At(3, 1) * rhs.At(1, 2) + At(3, 2) * rhs.At(2, 2);
+        float r33 = At(3, 3);
+
+        return {r00,r01,r02,r03, r10, r11, r12, r13, r20,r21,r22,r23, r30,r31,r32,r33};
+    }
+
+    Matrix4x4 Matrix4x4::operator+() const { return *this; }
+
+    Matrix4x4 Matrix4x4::FromTranslation(const Vector3 &rhs) {
+        return Matrix4x4(1.f, 0, 0, rhs.x,
+                         0, 1.f, 0, rhs.y,
+                         0, 0, 1.f, rhs.z,
+                         0, 0, 0,     1.f);
+    }
+
+    Matrix4x4 Matrix4x4::Translate(const Vector3 &rhs) const {
+        return *this * FromTranslation(rhs);
+    }
+
+    Vector3 Matrix4x4::Transform(const Vector3 &rhs) const {
+        return Transform(rhs.x, rhs.y, rhs.z);
+    }
+
+    Vector3 Matrix4x4::Transform(float tx, float ty, float tz) const {
+        return Vector3(At(0, 0) * tx + At(0, 1) * ty + At(0, 2) * tz + At(0, 3),
+                       At(1, 0) * tx + At(1, 1) * ty + At(1, 2) * tz + At(1, 3),
+                       At(2, 0) * tx + At(2, 1) * ty + At(2, 2) * tz + At(2, 3));
+    }
+
+    Vector2 Matrix4x4::Transform(float tx, float ty) const {
+        return Vector2(At(0, 0) * tx + At(0, 1) * ty + At(0, 2) + At(0, 3),
+                       At(1, 0) * tx + At(1, 1) * ty + At(1, 2) + At(1, 3));
+    }
+
+    Vector2 Matrix4x4::Transform(const Vector2 &rhs) const {
+        return Transform(rhs.x, rhs.y);
+    }
+
+    Matrix4x4 &Matrix4x4::operator=(const Matrix3x3 &rhs) {
+        Set3x3Part(rhs);
+        SetTranslatePart(0,0,0);
+        SetRow(3, 0, 0, 0, 1);
+        return *this;
+    }
+
+    Matrix4x4 &Matrix4x4::operator=(const Quaternion &rhs) {
+        *this = rhs.ToMatrix4x4();
+        return *this;
+    }
+
+    float &Matrix4x4::At(int row, int col) {
+        return elems[row][col];
+    }
+
+    Matrix4x4 Matrix4x4::Inverse() const {
+        // Compute the inverse directly using Cramer's rule
+        // Warning: This method is numerically very unstable!
+        float d = Determinant();
+
+        d = 1.f / d;
+
+        float a11 = At(0, 0);float a12 = At(0, 1);float a13 = At(0, 2);float a14 = At(0, 3);
+        float a21 = At(1, 0);float a22 = At(1, 1);float a23 = At(1, 2);float a24 = At(1, 3);
+        float a31 = At(2, 0);float a32 = At(2, 1);float a33 = At(2, 2);float a34 = At(2, 3);
+        float a41 = At(3, 0);float a42 = At(3, 1);float a43 = At(3, 2);float a44 = At(3, 3);
+
+        Matrix4x4 i = {
+                d * (a22*a33*a44 + a23*a34*a42 + a24*a32*a43 - a22*a34*a43 - a23*a32*a44 - a24*a33*a42),
+                d * (a12*a34*a43 + a13*a32*a44 + a14*a33*a42 - a12*a33*a44 - a13*a34*a42 - a14*a32*a43),
+                d * (a12*a23*a44 + a13*a24*a42 + a14*a22*a43 - a12*a24*a43 - a13*a22*a44 - a14*a23*a42),
+                d * (a12*a24*a33 + a13*a22*a34 + a14*a23*a32 - a12*a23*a34 - a13*a24*a32 - a14*a22*a33),
+                d * (a21*a34*a43 + a23*a31*a44 + a24*a33*a41 - a21*a33*a44 - a23*a34*a41 - a24*a31*a43),
+                d * (a11*a33*a44 + a13*a34*a41 + a14*a31*a43 - a11*a34*a43 - a13*a31*a44 - a14*a33*a41),
+                d * (a11*a24*a43 + a13*a21*a44 + a14*a23*a41 - a11*a23*a44 - a13*a24*a41 - a14*a21*a43),
+                d * (a11*a23*a34 + a13*a24*a31 + a14*a21*a33 - a11*a24*a33 - a13*a21*a34 - a14*a23*a31),
+                d * (a21*a32*a44 + a22*a34*a41 + a24*a31*a42 - a21*a34*a42 - a22*a31*a44 - a24*a32*a41),
+                d * (a11*a34*a42 + a12*a31*a44 + a14*a32*a41 - a11*a32*a44 - a12*a34*a41 - a14*a31*a42),
+                d * (a11*a22*a44 + a12*a24*a41 + a14*a21*a42 - a11*a24*a42 - a12*a21*a44 - a14*a22*a41),
+                d * (a11*a24*a32 + a12*a21*a34 + a14*a22*a31 - a11*a22*a34 - a12*a24*a31 - a14*a21*a32),
+                d * (a21*a33*a42 + a22*a31*a43 + a23*a32*a41 - a21*a32*a43 - a22*a33*a41 - a23*a31*a42),
+                d * (a11*a32*a43 + a12*a33*a41 + a13*a31*a42 - a11*a33*a42 - a12*a31*a43 - a13*a32*a41),
+                d * (a11*a23*a42 + a12*a21*a43 + a13*a22*a41 - a11*a22*a43 - a12*a23*a41 - a13*a21*a42),
+                d * (a11*a22*a33 + a12*a23*a31 + a13*a21*a32 - a11*a23*a32 - a12*a21*a33 - a13*a22*a31)
+        };
+        return i;
+    }
+
+    float Matrix4x4::Minor(int i, int j) const {
+        int r0 = SKIPNUM(0, i);
+        int r1 = SKIPNUM(1, i);
+        int r2 = SKIPNUM(2, i);
+        int c0 = SKIPNUM(0, j);
+        int c1 = SKIPNUM(1, j);
+        int c2 = SKIPNUM(2, j);
+
+        float a = At(r0, c0);
+        float b = At(r0, c1);
+        float c = At(r0, c2);
+        float d = At(r1, c0);
+        float e = At(r1, c1);
+        float f = At(r1, c2);
+        float g = At(r2, c0);
+        float h = At(r2, c1);
+        float k = At(r2, c2);
+
+        return a*e*k + b*f*g + c*d*h - a*f*h - b*d*k - c*e*g;
+    }
+
+    float Matrix4x4::Determinant() const {
+        return At(0, 0) * Minor(0,0) - At(0, 1) * Minor(0,1) + At(0, 2) * Minor(0,2) - At(0, 3) * Minor(0,3);
+    }
+
+    float Matrix4x4::Determinant3x3() const {
+
+        const float a = elems[0][0];
+        const float b = elems[0][1];
+        const float c = elems[0][2];
+        const float d = elems[1][0];
+        const float e = elems[1][1];
+        const float f = elems[1][2];
+        const float g = elems[2][0];
+        const float h = elems[2][1];
+        const float i = elems[2][2];
+
+        return a*e*i + b*f*g + c*d*h - a*f*h - b*d*i - c*e*g;
+    }
+
+    Matrix3x3 Matrix4x4::GetRotatePart() const {
+        return Matrix3x3 {
+                At(0, 0), At(0, 1), At(0, 2),
+                At(1, 0), At(1, 1), At(1, 2),
+                At(2, 0), At(2, 1), At(2, 2)
+        };
+    }
 }

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 {
@@ -163,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};
+    }
 }

src/J3ML/LinearAlgebra/Transform2D.cpp

@@ -8,7 +8,7 @@ namespace LinearAlgebra {
 
 
     Vector2 Transform2D::Transform(const Vector2 &input) const {
-        return transformation * input;
+        return transformation.Transform(input);
     }
 
     Transform2D::Transform2D(const Matrix3x3 &transform) : transformation(transform) { }

src/J3ML/LinearAlgebra/Vector2.cpp

@@ -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;
@@ -243,5 +242,15 @@ namespace LinearAlgebra {
         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};
+    }
 }

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)
     {}