Implement Vector4 operator=

CMakeLists.txt

@@ -4,13 +4,10 @@ PROJECT(J3ML
         LANGUAGES CXX
 )
 
-
 if (PROJECT_SOURCE_DIR STREQUAL PROJECT_BINARY_DIR)
     message(FATAL_ERROR "In-source builds are not allowed")
 endif()
 
-
-
 set(CMAKE_CXX_STANDARD 20)
 #set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 if (WIN32)
@@ -32,7 +29,35 @@ file(GLOB_RECURSE J3ML_SRC     "src/J3ML/*.c" "src/J3ML/*.cpp")
 include_directories("include")
 
 add_library(J3ML SHARED ${J3ML_SRC}
-        src/J3ML/LinearAlgebra/AxisAngle.cpp)
+        src/J3ML/LinearAlgebra/AxisAngle.cpp
+        include/J3ML/LinearAlgebra/Vector.h
+        include/J3ML/Geometry/Plane.h
+        include/J3ML/Geometry/AABB.h
+        include/J3ML/Geometry/Frustum.h
+        include/J3ML/Geometry/OBB.h
+        include/J3ML/Geometry/Capsule.h
+        include/J3ML/Geometry/Sphere.h
+        include/J3ML/Geometry/Ray.h
+        include/J3ML/Geometry/QuadTree.h
+        include/J3ML/Geometry/LineSegment.h
+        include/J3ML/Geometry/TriangleMesh.h
+        include/J3ML/Geometry/Polygon.h
+        include/J3ML/Geometry/Triangle.h
+        include/J3ML/Geometry/Triangle2D.h
+        src/J3ML/Geometry/AABB.cpp
+        src/J3ML/Geometry/Plane.cpp
+        src/J3ML/Geometry/Sphere.cpp
+        src/J3ML/Geometry/Frustum.cpp
+        src/J3ML/Geometry/OBB.cpp
+        src/J3ML/Geometry/Ray.cpp
+        src/J3ML/Geometry/Capsule.cpp
+        src/J3ML/Geometry/TriangleMesh.cpp
+        src/J3ML/Geometry/QuadTree.cpp
+        src/J3ML/Geometry/LineSegment.cpp
+        include/J3ML/Geometry/AABB2D.h
+        src/J3ML/Geometry/Polygon.cpp
+        include/J3ML/Geometry/Polyhedron.h
+        src/J3ML/Geometry/Polyhedron.cpp)
 set_target_properties(J3ML PROPERTIES LINKER_LANGUAGE CXX)
 
 install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME})

include/J3ML/Geometry.h

@@ -1,66 +1,32 @@
+#include <J3ML/LinearAlgebra/Vector2.h>
 #include <J3ML/LinearAlgebra/Vector3.h>
 
 #pragma once
 
 namespace Geometry {
-    using Point2D = LinearAlgebra::Vector2;
+    using Vector2 = LinearAlgebra::Vector2;
+    using Vector3 = LinearAlgebra::Vector3;
 
     class LineSegment2D
     {
-        Point2D A;
-        Point2D B;
+        Vector2 A;
+        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 {
 
-    };
+    struct IntersectionResult2D {};
 
     bool Intersects2D(LineSegment2D seg, Rectangle rect);
     IntersectionResult2D GetIntersection2D(LineSegment2D seg, Rectangle rect);
-
-
-
-    
-
-    using Point3D = LinearAlgebra::Vector3;
-
-    // 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
-    {
-        Point3D A;
-        Point3D B;
-    };
-    class Ray
-    {
-        Point3D Origin;
-        Point3D Direction;
-    };
-
-    class OBB;
-    class Frustum;
-    class Plane;
-    class Polygon;
-    class Polyhedron;
-    class QuadTree;
-    class OctTree;
-    class Sphere;
-    class Triangle;
-    class TriangleMesh;
 }

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

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

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

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

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

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

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

include/J3ML/Geometry/Polygon.h

@@ -0,0 +1,7 @@
+#pragma once
+
+namespace Geometry {
+    class Polygon {
+
+    };
+}

include/J3ML/Geometry/Polyhedron.h

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

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

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

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

include/J3ML/Geometry/Triangle.h

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

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

include/J3ML/Geometry/TriangleMesh.h

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

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

include/J3ML/LinearAlgebra/AxisAngle.h

@@ -13,6 +13,12 @@ namespace LinearAlgebra
         float angle;
     public:
         AxisAngle();
-        AxisAngle(const Vector3& axis, float angle);
+
+        AxisAngle(const Vector3 &axis, float angle);
+
+        EulerAngle ToEulerAngleXYZ() const;
+
+        Quaternion ToQuaternion() const;
+        static AxisAngle FromEulerAngleXYZ(const EulerAngle&);
     };
 }

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/EulerAngle.h

@@ -11,8 +11,13 @@ public:
     EulerAngle();
     EulerAngle(float pitch, float yaw, float roll);
     EulerAngle(const Vector3& vec) : pitch(vec.x), yaw(vec.y), roll(vec.z) {}
-    static EulerAngle FromRadians(float radians);
-    static EulerAngle FromDegrees(float degrees);
+
+    AxisAngle ToAxisAngle() const;
+
+
+    explicit EulerAngle(const Quaternion& orientation);
+    explicit EulerAngle(const AxisAngle& orientation);
+
     /// TODO: Implement separate upper and lower bounds
     /// Preserves internal value of euler angles, normalizes and clamps the output.
     /// This does not solve gimbal lock!!!

include/J3ML/LinearAlgebra/Matrix3x3.h

@@ -43,12 +43,34 @@ 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;
 
-        /// Creates a new M3x3 that rotates about the given axis by the given angle
-        static Matrix3x3 RotateAxisAngle(const Vector3& rhs);
+        void SetRotatePart(const Vector3& a, float angle);
 
-        static Matrix3x3 RotateFromTo(const Vector3& source, const Vector3& direction);
+        /// Creates a new M3x3 that rotates about the given axis by the given angle
+        static Matrix3x3 RotateAxisAngle(const Vector3& axis, float angleRadians);
+
+        static Matrix3x3 RotateFromTo(const Vector3& source, const Vector3& direction)
+        {
+
+        }
+
+        void SetRow(int i, const Vector3 &vector3);
+        void SetColumn(int i, const Vector3& vector);
+        void SetAt(int x, int y, float value);
+
+        void Orthonormalize(int c0, int c1, int c2)
+        {
+            Vector3 v0 = GetColumn(c0);
+            Vector3 v1 = GetColumn(c1);
+            Vector3 v2 = GetColumn(c2);
+            Vector3::Orthonormalize(v0, v1, v2);
+            SetColumn(c0, v0);
+            SetColumn(c1, v1);
+            SetColumn(c2, v2);
+        }
 
         static Matrix3x3 LookAt(const Vector3& forward, const Vector3& target, const Vector3& localUp, const Vector3& worldUp);
 
@@ -62,14 +84,14 @@ namespace LinearAlgebra {
         // Transforming a vector v using this matrix computes the vector
         // v' == M * v == R*S*v == (R * (S * v)) which means the scale operation
         // is applied to the vector first, followed by rotation, and finally translation
-        static Matrix3x3 FromRS(const Quaternion& rotate, const Matrix3x3& scale);
-        static Matrix3x3 FromRS(const Matrix3x3 &rotate, const Matrix3x3& scale);
+        static Matrix3x3 FromRS(const Quaternion& rotate, const Vector3& scale);
+        static Matrix3x3 FromRS(const Matrix3x3 &rotate, const Vector3& scale);
 
 
         /// Creates a new transformation matrix that scales by the given factors.
         // This matrix scales with respect to origin.
         static Matrix3x3 Scale(float sx, float sy, float sz);
-        static Matrix3x3 Scale(const Matrix3x3& scale);
+        static Matrix3x3 Scale(const Vector3& scale);
 
         /// Returns the main diagonal.
         Vector3 Diagonal() const;
@@ -102,6 +124,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;
 

include/J3ML/LinearAlgebra/Matrix4x4.h

@@ -1,32 +1,242 @@
 #pragma once
 
 #include <J3ML/LinearAlgebra.h>
+#include <J3ML/LinearAlgebra/Quaternion.h>
 
 namespace LinearAlgebra {
+
     /// A 4-by-4 matrix for affine transformations and perspective projections of 3D geometry.
-/* This matrix can represent the most generic form of transformations for 3D objects,
- * including perspective projections, which a 4-by-3 cannot store,
- * and translations, which a 3-by-3 cannot represent.
- * The elements of this matrix are
- *  m_00, m_01, m_02, m_03
- *  m_10, m_11, m_12, m_13
- *  m_20, m_21, m_22, m_23,
- *  m_30, m_31, m_32, m_33
- *
- *  The element m_yx is the value on the row y and column x.
- *  You can access m_yx using the double-bracket notation m[y][x]
- */
+    /* This matrix can represent the most generic form of transformations for 3D objects,
+     * including perspective projections, which a 4-by-3 cannot store,
+     * and translations, which a 3-by-3 cannot represent.
+     * The elements of this matrix are
+     *  m_00, m_01, m_02, m_03
+     *  m_10, m_11, m_12, m_13
+     *  m_20, m_21, m_22, m_23,
+     *  m_30, m_31, m_32, m_33
+     *
+     *  The element m_yx is the value on the row y and column x.
+     *  You can access m_yx using the double-bracket notation m[y][x]
+     */
     class Matrix4x4 {
     public:
+        // TODO: Implement assertions to ensure matrix bounds are not violated!
+        enum { Rows = 4 };
+        enum { Cols = 4 };
+
+        /// A constant matrix that has zeroes in all its entries
         static const Matrix4x4 Zero;
+        /// A constant matrix that is the identity.
         static const Matrix4x4 Identity;
 
-        Vector3 GetTranslationComponent() const;
-        Matrix3x3 GetRotationComponent() const;
+        /// 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);
+
+        /// Creates a LookAt matrix from a look-at direction vector.
+        /** A LookAt matrix is a rotation matrix that orients an object to face towards a specified target direction.
+            @param localForward Specifies the forward direction in the local space of the object. This is the direction
+                the model is facing at in its own local/object space, often +X (1,0,0), +Y (0,1,0) or +Z (0,0,1). The
+                vector to pass in here depends on the conventions you or your modeling software is using, and it is best
+                pick one convention for all your objects, and be consistent.
+                This input parameter must be a normalized vector.
+            @param targetDirection Specifies the desired world space direction the object should look at. This function
+                will compute a rotation matrix which will rotate the localForward vector to orient towards this targetDirection
+                vector. This input parameter must be a normalized vector.
+            @param localUp Specifies the up direction in the local space of the object. This is the up direction the model
+                was authored in, often +Y (0,1,0) or +Z (0,0,1). The vector to pass in here depends on the conventions you
+                or your modeling software is using, and it is best to pick one convention for all your objects, and be
+                consistent. This input parameter must be a normalized vector. This vector must be perpendicular to the
+                vector localForward, i.e. localForward.Dot(localUp) == 0.
+            @param worldUp Specifies the global up direction of the scene in world space. Simply rotating one vector to
+                coincide with another (localForward->targetDirection) would cause the up direction of the resulting
+                orientation to drift (e.g. the model could be looking at its target its head slanted sideways). To keep
+                the up direction straight, this function orients the localUp direction of the model to point towards the
+                specified worldUp direction (as closely as possible). The worldUp and targetDirection vectors cannot be
+                collinear, but they do not need to be perpendicular either.
+            @return A matrix that maps the given local space forward direction vector to point towards the given target
+                direction, and the given local up direction towards the given target world up direction. The returned
+                matrix M is orthonormal with a determinant of +1. For the matrix M it holds that
+                M * localForward = targetDirection, and M * localUp lies in the plane spanned by the vectors targetDirection
+                and worldUp.
+            @note The position of (the translation performed by) the resulting matrix will be set to (0,0,0), i.e. the object
+                will be placed to origin. Call SetTranslatePart() on the resulting matrix to set the position of the model.
+            @see RotateFromTo(). */
+        static Matrix4x4 LookAt(const Vector3& localFwd, const Vector3& targetDir, const Vector3& localUp, const Vector3& worldUp);
+
+        /// Returns the translation part.
+        /** The translation part is stored in the fourth column of this matrix.
+            This is equivalent to decomposing this matrix in the form M = T * M', i.e. this translation is applied last,
+            after applying rotation and scale. If this matrix represents a local->world space transformation for an object,
+            then this gives the world space position of the object.
+            @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0 0 0 1]). */
+        Vector3 GetTranslatePart() const;
+        /// Returns the top-left 3x3 part of this matrix. This stores the rotation part of this matrix (if this matrix represents a rotation).
+        Matrix3x3 GetRotatePart() const;
+        void SetTranslatePart(float translateX, float translateY, float translateZ);
+        void SetTranslatePart(const Vector3& offset);
+        void SetRotatePart(const Quaternion& q);
+        void Set3x3Part(const Matrix3x3& r);
+
+        void SetRow(int row, const Vector3& rowVector, float m_r3);
+        void SetRow(int row, const Vector4& rowVector);
+        void SetRow(int row, float m_r0, float m_r1, float m_r2, float m_r3);
+
+        Vector4 GetRow(int index) const;
+        Vector4 GetColumn(int index) const;
+        Vector3 GetRow3(int index) const;
+        Vector3 GetColumn3(int index) const;
+
+        float &At(int row, int col);
+        float At(int x, int y) const;
+
+        template <typename T>
+        void Swap(T &a, T &b)
+        {
+            T temp = std::move(a);
+            a = std::move(b);
+            b = std::move(temp);
+        }
+
+
+        void SwapColumns(int col1, int col2);
+
+        /// Swaps two rows.
+        void SwapRows(int row1, int row2);
+        /// Swapsthe xyz-parts of two rows element-by-element
+        void SwapRows3(int row1, int row2);
+
+        void ScaleRow(int row, float scalar);
+        void ScaleRow3(int row, float scalar);
+        void ScaleColumn(int col, float scalar);
+        void ScaleColumn3(int col, float scalar);
+        /// Algorithm from Eric Lengyel's Mathematics for 3D Game Programming & Computer Graphics, 2nd Ed.
+        void Pivot();
+
+        /// Tests if this matrix does not contain any NaNs or infs.
+        /** @return Returns true if the entries of this float4x4 are all finite, and do not contain NaN or infs. */
+        bool IsFinite() const;
+
+        /// Tests if this matrix has an inverse.
+        /** @return Returns true if this matrix can be inverted, up to the given epsilon. */
+        bool IsInvertible(float epsilon = 1e-3f) const;
+
+        Vector4 Diagonal() const;
+        Vector3 Diagonal3() const;
+        /// Returns the local +X axis in world space.
+        /// This is the same as transforming the vector (1,0,0) by this matrix.
+        Vector3 WorldX() const;
+        /// Returns the local +Y axis in world space.
+        /// This is the same as transforming the vector (0,1,0) by this matrix.
+        Vector3 WorldY() const;
+        /// Returns the local +Z axis in world space.
+        /// This is the same as transforming the vector (0,0,1) by this matrix.
+        Vector3 WorldZ() const;
+
+        /// Accesses this structure as a float array.
+        /// @return A pointer to the upper-left element. The data is contiguous in memory.
+        /// ptr[0] gives the element [0][0], ptr[1] is [0][1], ptr[2] is [0][2].
+        /// ptr[4] == [1][0], ptr[5] == [1][1], ..., and finally, ptr[15] == [3][3].
+        float *ptr() { return &elems[0][0]; }
+        const float *ptr() const { return &elems[0][0]; }
+
+        float Determinant3x3() const;
+        /// Computes the determinant of this matrix.
+        // If the determinant is nonzero, this matrix is invertible.
+        float Determinant() const;
+
+        #define SKIPNUM(val, skip) (val >= skip ? (val+1) : val)
+
+        float Minor(int i, int j) const;
+
+        Matrix4x4 Inverse() const;
+
+        Matrix4x4 Transpose() const;
+
+        Vector2 Transform(float tx, float ty) const;
+        Vector2 Transform(const Vector2& rhs) const;
+
+
+        Vector3 Transform(float tx, float ty, float tz) const;
+        Vector3 Transform(const Vector3& rhs) const;
+
+        Vector4 Transform(float tx, float ty, float tz, float tw) const;
+        Vector4 Transform(const Vector4& rhs) const;
+
+
+        Matrix4x4 Translate(const Vector3& rhs) const;
+        static Matrix4x4 FromTranslation(const Vector3& rhs);
+
+
+        static Matrix4x4 D3DOrthoProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+        static Matrix4x4 D3DOrthoProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+        static Matrix4x4 D3DPerspProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+        static Matrix4x4 D3DPerspProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+
+        static Matrix4x4 OpenGLOrthoProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+        static Matrix4x4 OpenGLOrthoProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+        static Matrix4x4 OpenGLPerspProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+        static Matrix4x4 OpenGLPerspProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
+
+        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;
+        Vector3 operator * (const Vector3& rhs) const;
+        Vector4 operator * (const Vector4& rhs) const;
+
+        Matrix4x4 operator * (const Matrix3x3 &rhs) const;
+
+        Matrix4x4 operator +() const;
+
+        Matrix4x4 operator * (const Matrix4x4& rhs) const;
+
+        Matrix4x4 &operator = (const Matrix3x3& rhs);
+        Matrix4x4 &operator = (const Quaternion& rhs);
+        Matrix4x4 &operator = (const Matrix4x4& rhs) = default;
 
-        Vector4 GetRow() const;
-        Vector4 GetColumn() const;
     protected:
         float elems[4][4];
+
+
     };
 }

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;
 

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

include/J3ML/LinearAlgebra/Vector.h

@@ -0,0 +1,13 @@
+#pragma once
+
+
+template <typename T, int Dims>
+class templated_vector
+{
+
+};
+
+
+using v2f = templated_vector<float, 2>;
+using v3f = templated_vector<float, 3>;
+using v4f = templated_vector<float, 4>;

include/J3ML/LinearAlgebra/Vector2.h

@@ -1,14 +1,20 @@
 #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 +50,99 @@ 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 operator *(const Vector2& rhs) const
+        {
+
+        }
+        Vector2 Mul(const Vector2& v) const;
+
+        /// Divides this vector by a scalar.
         Vector2 operator /(float rhs) const;
         Vector2 Div(float scalar) const;
         static Vector2 Div(const Vector2& lhs, float rhs);
 
-        // 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);
@@ -121,4 +153,9 @@ namespace LinearAlgebra {
         float y = 0;
 
     };
+
+    static Vector2 operator*(float lhs, const Vector2 &rhs)
+    {
+        return rhs * lhs;
+    }
 }

include/J3ML/LinearAlgebra/Vector3.h

@@ -30,6 +30,38 @@ public:
     static const Vector3 Backward;
     static const Vector3 NaN;
 
+    static void Orthonormalize(Vector3& a, Vector3& b)
+    {
+        a = a.Normalize();
+        b = b - b.ProjectToNorm(a);
+        b = b.Normalize();
+    }
+
+
+    /// Returns the DirectionVector for a given angle.
+    static Vector3 Direction(const Vector3 &rhs) ;
+
+
+    static void Orthonormalize(Vector3& a, Vector3& b, Vector3& c)
+    {
+        a = a.Normalize();
+        b = b - b.ProjectToNorm(a);
+        b = b.Normalize();
+        c = c - c.ProjectToNorm(a);
+        c = c - c.ProjectToNorm(b);
+        c = c.Normalize();
+    }
+
+    bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon)
+    {
+
+    }
+
+    Vector3 ProjectToNorm(const Vector3& direction)
+    {
+        return direction * this->Dot(direction);
+    }
+
     float GetX() const;
     float GetY() const;
     float GetZ() const;
@@ -46,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);
 
@@ -55,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);
 
@@ -65,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);
 
@@ -104,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;

include/J3ML/LinearAlgebra/Vector4.h

@@ -81,6 +81,8 @@ namespace LinearAlgebra {
 
         Vector4 operator+() const; // Unary + Operator
         Vector4 operator-() const; // Unary - Operator (Negation)
+
+
     public:
 #if MUTABLE
         float x;

main.cpp

@@ -1,5 +1,6 @@
 #include <iostream>
-
+#include <J3ML/Geometry.h>
+#include <J3ML/J3ML.h>
 
 int main(int argc, char** argv)
 {

src/J3ML/Geometry/AABB.cpp

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

src/J3ML/Geometry/Capsule.cpp

@@ -0,0 +1,7 @@
+#include <J3ML/Geometry/Capsule.h>
+
+namespace Geometry
+{
+
+    Capsule::Capsule() : l() {}
+}

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

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

src/J3ML/Geometry/OBB.cpp

@@ -0,0 +1,3 @@
+//
+// Created by dawsh on 1/25/24.
+//

src/J3ML/Geometry/Plane.cpp

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

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/Geometry/QuadTree.cpp

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

src/J3ML/Geometry/Ray.cpp

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

src/J3ML/Geometry/Sphere.cpp

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

src/J3ML/Geometry/TriangleMesh.cpp

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

src/J3ML/LinearAlgebra/AxisAngle.cpp

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

src/J3ML/LinearAlgebra/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 {
@@ -196,5 +201,128 @@ namespace LinearAlgebra {
         };
     }
 
+    void Matrix3x3::SetRotatePart(const Vector3 &a, float angle) {
+        float s = std::sin(angle);
+        float c = std::cos(angle);
+
+        const float c1 = 1.f - c;
+
+        elems[0][0] = c+c1*a.x*a.x;
+        elems[1][0] = c1*a.x*a.y+s*a.z;
+        elems[2][0] = c1*a.x*a.z-s*a.y;
+
+        elems[0][1] = c1*a.x*a.y-s*a.z;
+        elems[1][1] = c+c1*a.y*a.y;
+        elems[2][1] = c1*a.y*a.z+s*a.x;
+
+        elems[0][2] = c1*a.x*a.z+s*a.y;
+        elems[1][2] = c1*a.y*a.z-s*a.x;
+        elems[2][2] = c+c1*a.z*a.z;
+    }
+
+    Matrix3x3 Matrix3x3::RotateAxisAngle(const Vector3 &axis, float angleRadians) {
+        Matrix3x3 r;
+        r.SetRotatePart(axis, angleRadians);
+        return r;
+    }
+
+    void Matrix3x3::SetRow(int i, const Vector3 &vec) {
+        elems[i][0] = vec.x;
+        elems[i][1] = vec.y;
+        elems[i][2] = vec.z;
+    }
+
+    void Matrix3x3::SetColumn(int i, const Vector3& vec)
+    {
+        elems[0][i] = vec.x;
+        elems[1][i] = vec.y;
+        elems[2][i] = vec.z;
+    }
+
+    Matrix3x3
+    Matrix3x3::LookAt(const Vector3 &forward, const Vector3 &target, const Vector3 &localUp, const Vector3 &worldUp) {
+        // User must input proper normalized input direction vectors
+        // In the local space, the forward and up directions must be perpendicular to be well-formed.
+        // In the world space, the target direction and world up cannot be degenerate (co-linear)
+        // Generate the third basis vector in the local space;
+        Vector3 localRight = localUp.Cross(forward).Normalize();
+        // A. Now we have an orthonormal linear basis {localRight, localUp, forward} for the object local space.
+        // Generate the third basis vector for the world space
+        Vector3 worldRight = worldUp.Cross(target).Normalize();
+        // Since the input worldUp vector is not necessarily perpendicular to the target direction vector
+        // We need to compute the real world space up vector that the "head" of the object will point
+        // towards when the model is looking towards the desired target direction
+        Vector3 perpWorldUp = target.Cross(worldRight).Normalize();
+        // B. Now we have an orthonormal linear basis {worldRight, perpWorldUp, targetDirection } for the desired target orientation.
+        // We want to build a matrix M that performs the following mapping:
+        // 1. localRight must be mapped to worldRight.        (M * localRight = worldRight)
+        // 2. localUp must be mapped to perpWorldUp.          (M * localUp = perpWorldUp)
+        // 3. localForward must be mapped to targetDirection. (M * localForward = targetDirection)
+        // i.e. we want to map the basis A to basis B.
+
+        // This matrix M exists, and it is an orthonormal rotation matrix with a determinant of +1, because
+        // the bases A and B are orthonormal with the same handedness.
+
+        // Below, use the notation that (a,b,c) is a 3x3 matrix with a as its first column, b second, and c third.
+
+        // By algebraic manipulation, we can rewrite conditions 1, 2 and 3 in a matrix form:
+        //        M * (localRight, localUp, localForward) = (worldRight, perpWorldUp, targetDirection)
+        // or     M = (worldRight, perpWorldUp, targetDirection) * (localRight, localUp, localForward)^{-1}.
+        // or     M = m1 * m2, where
+
+        // m1 equals (worldRight, perpWorldUp, target):
+        Matrix3x3 m1(worldRight, perpWorldUp, target);
+
+        // and m2 equals (localRight, localUp, localForward)^{-1}:
+        Matrix3x3 m2;
+        m2.SetRow(0, localRight);
+        m2.SetRow(1, localUp);
+        m2.SetRow(2, forward);
+        // Above we used the shortcut that for an orthonormal matrix M, M^{-1} = M^T. So set the rows
+        // and not the columns to directly produce the transpose, i.e. the inverse of (localRight, localUp, localForward).
+
+        // Compute final M.
+        m2 = m1 * m2;
+
+        // And fix any numeric stability issues by re-orthonormalizing the result.
+        m2.Orthonormalize(0, 1, 2);
+        return m2;
+    }
+
+    Vector3 Matrix3x3::Diagonal() const {
+        return {elems[0][0],
+                elems[1][1],
+                elems[2][2]
+        };
+    }
+
+    float &Matrix3x3::At(int row, int col) {
+        return elems[row][col];
+    }
+
+    Vector3 Matrix3x3::WorldZ() const {
+        return GetColumn(2);
+    }
+
+    Vector3 Matrix3x3::WorldY() const {
+        return GetColumn(1);
+    }
+
+    Vector3 Matrix3x3::WorldX() const {
+        return GetColumn(0);
+    }
+
+    Matrix3x3 Matrix3x3::FromRS(const Quaternion &rotate, const Vector3 &scale) {
+        return Matrix3x3(rotate) * Matrix3x3::Scale(scale);
+    }
+
+    Matrix3x3 Matrix3x3::FromRS(const Matrix3x3 &rotate, const Vector3 &scale) {
+        return rotate * Matrix3x3::Scale(scale);
+    }
+
+    Matrix3x3 Matrix3x3::FromQuat(const Quaternion &orientation) {
+        return Matrix3x3(orientation);
+    }
+
 }
 

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -1,5 +1,517 @@
 #include <J3ML/LinearAlgebra/Matrix4x4.h>
+#include <J3ML/LinearAlgebra/Vector4.h>
 
 namespace LinearAlgebra {
+    const Matrix4x4 Matrix4x4::Zero = Matrix4x4(0);
+    const Matrix4x4 Matrix4x4::Identity = Matrix4x4({1,0,0,0}, {0,1,0,0}, {0,0,1,0}, {0,0,0,1});
+    const Matrix4x4 Matrix4x4::NaN = Matrix4x4(NAN);
 
+    Matrix4x4::Matrix4x4(const Vector4 &r1, const Vector4 &r2, const Vector4 &r3, const Vector4 &r4) {
+        this->elems[0][0] = r1.x;
+        this->elems[0][1] = r1.y;
+        this->elems[0][2] = r1.z;
+        this->elems[0][3] = r1.w;
+
+        this->elems[1][0] = r2.x;
+        this->elems[1][1] = r2.y;
+        this->elems[1][2] = r2.z;
+        this->elems[1][3] = r2.w;
+
+        this->elems[2][0] = r3.x;
+        this->elems[2][1] = r3.y;
+        this->elems[2][2] = r3.z;
+        this->elems[2][3] = r3.w;
+
+        this->elems[3][0] = r4.x;
+        this->elems[3][1] = r4.y;
+        this->elems[3][2] = r4.z;
+        this->elems[3][3] = r4.w;
+    }
+
+    Matrix4x4::Matrix4x4(float val) {
+        this->elems[0][0] = val;
+        this->elems[0][1] = val;
+        this->elems[0][2] = val;
+        this->elems[0][3] = val;
+
+        this->elems[1][0] = val;
+        this->elems[1][1] = val;
+        this->elems[1][2] = val;
+        this->elems[1][3] = val;
+
+        this->elems[2][0] = val;
+        this->elems[2][1] = val;
+        this->elems[2][2] = val;
+        this->elems[2][3] = val;
+
+        this->elems[3][0] = val;
+        this->elems[3][1] = val;
+        this->elems[3][2] = val;
+        this->elems[3][3] = val;
+    }
+
+    Matrix4x4::Matrix4x4(float m00, float m01, float m02, float m03, float m10, float m11, float m12, float m13,
+                         float m20, float m21, float m22, float m23, float m30, float m31, float m32, float m33) {
+        this->elems[0][0] = m00;
+        this->elems[0][1] = m01;
+        this->elems[0][2] = m02;
+        this->elems[0][3] = m03;
+
+        this->elems[1][0] = m10;
+        this->elems[1][1] = m11;
+        this->elems[1][2] = m12;
+        this->elems[1][3] = m13;
+
+        this->elems[2][0] = m20;
+        this->elems[2][1] = m21;
+        this->elems[2][2] = m22;
+        this->elems[2][3] = m23;
+
+        this->elems[3][0] = m30;
+        this->elems[3][1] = m31;
+        this->elems[3][2] = m32;
+        this->elems[3][3] = m33;
+
+    }
+
+    Matrix4x4::Matrix4x4(const Quaternion &orientation) {
+
+    }
+
+    void Matrix4x4::SetTranslatePart(float translateX, float translateY, float translateZ) {
+        elems[0][3] = translateX;
+        elems[1][3] = translateY;
+        elems[2][3] = translateZ;
+    }
+
+    void Matrix4x4::SetTranslatePart(const Vector3 &offset) {
+        elems[0][3] = offset.x;
+        elems[1][3] = offset.y;
+        elems[2][3] = offset.z;
+    }
+
+
+    void Matrix4x4::SetRotatePart(const Quaternion& q)
+    {
+        // See e.g. http://www.geometrictools.com/Documentation/LinearAlgebraicQuaternions.pdf .
+        const float x = q.x;
+        const float y = q.y;
+        const float z = q.z;
+        const float w = q.w;
+        elems[0][0] = 1 - 2*(y*y + z*z); elems[0][1] =     2*(x*y - z*w); elems[0][2] =     2*(x*z + y*w);
+        elems[1][0] =     2*(x*y + z*w); elems[1][1] = 1 - 2*(x*x + z*z); elems[1][2] =     2*(y*z - x*w);
+        elems[2][0] =     2*(x*z - y*w); elems[2][1] =     2*(y*z + x*w); elems[2][2] = 1 - 2*(x*x + y*y);
+    }
+
+    void Matrix4x4::Set3x3Part(const Matrix3x3& r)
+    {
+        At(0, 0) = r[0][0]; At(0, 1) = r[0][1]; At(0, 2) = r[0][2];
+        At(1, 0) = r[1][0]; At(1, 1) = r[1][1]; At(1, 2) = r[1][2];
+        At(2, 0) = r[2][0]; At(2, 1) = r[2][1]; At(2, 2) = r[2][2];
+    }
+
+
+
+    void Matrix4x4::SetRow(int row, const Vector3 &rowVector, float m_r3) {
+        SetRow(row, rowVector.x, rowVector.y, rowVector.z, m_r3);
+    }
+
+    void Matrix4x4::SetRow(int row, const Vector4 &rowVector) {
+        SetRow(row, rowVector.x, rowVector.y, rowVector.z, rowVector.w);
+    }
+
+    void Matrix4x4::SetRow(int row, float m_r0, float m_r1, float m_r2, float m_r3) {
+        elems[row][0] = m_r0;
+        elems[row][1] = m_r1;
+        elems[row][2] = m_r2;
+        elems[row][3] = m_r3;
+    }
+
+    Matrix4x4::Matrix4x4(const Quaternion &orientation, const Vector3 &translation) {
+        SetRotatePart(orientation);
+        SetTranslatePart(translation);
+        SetRow(3, 0, 0, 0, 1);
+    }
+
+    Matrix4x4 Matrix4x4::D3DOrthoProjLH(float n, float f, float h, float v) {
+        float p00 = 2.f / h; float p01 = 0;       float p02 = 0;           float p03 = 0.f;
+        float p10 = 0;       float p11 = 2.f / v; float p12 = 0;           float p13 = 0.f;
+        float p20 = 0;       float p21 = 0;       float p22 = 1.f / (f-n); float p23 = n / (n-f);
+        float p30 = 0;       float p31 = 0;       float p32 = 0.f;         float p33 = 1.f;
+
+        return {p00, p01, p02, p03, p10, p11, p12, p13, p20, p21, p22, p23, p30, p31, p32, p33};
+    }
+
+    /** This function generates an orthographic projection matrix that maps from
+	the Direct3D view space to the Direct3D normalized viewport space as follows:
+
+	In Direct3D view space, we assume that the camera is positioned at the origin (0,0,0).
+	The camera looks directly towards the positive Z axis (0,0,1).
+	The -X axis spans to the left of the screen, +X goes to the right.
+	-Y goes to the bottom of the screen, +Y goes to the top.
+
+	After the transformation, we're in the Direct3D normalized viewport space as follows:
+
+	(-1,-1,0) is the bottom-left corner of the viewport at the near plane.
+	(1,1,0) is the top-right corner of the viewport at the near plane.
+	(0,0,0) is the center point at the near plane.
+	Coordinates with z=1 are at the far plane.
+
+	Examples:
+		(0,0,n) maps to (0,0,0).
+		(0,0,f) maps to (0,0,1).
+		(-h/2, -v/2, n) maps to (-1, -1, 0).
+		(h/2, v/2, f) maps to (1, 1, 1).
+	*/
+    Matrix4x4 Matrix4x4::D3DOrthoProjRH(float n, float f, float h, float v)
+    {
+        // D3DOrthoProjLH and D3DOrthoProjRH differ from each other in that the third column is negated.
+        // This corresponds to LH = RH * In, where In is a diagonal matrix with elements [1 1 -1 1].
+        float p00 = 2.f / h; float p01 = 0;       float p02 = 0;           float p03 = 0.f;
+        float p10 = 0;       float p11 = 2.f / v; float p12 = 0;           float p13 = 0.f;
+        float p20 = 0;       float p21 = 0;       float p22 = 1.f / (n-f); float p23 = n / (n-f);
+        float p30 = 0;       float p31 = 0;       float p32 = 0.f;         float p33 = 1.f;
+    }
+
+    float Matrix4x4::At(int x, int y) const {
+        return elems[x][y];
+    }
+
+    Matrix4x4 Matrix4x4::operator*(const Matrix4x4 &rhs) const {
+
+        float r00 = At(0, 0) * rhs.At(0, 0) + At(0, 1) * rhs.At(1, 0) + At(0, 2) * rhs.At(2, 0) + At(0, 3) * rhs.At(3, 0);
+        float r01 = At(0, 0) * rhs.At(0, 1) + At(0, 1) * rhs.At(1, 1) + At(0, 2) * rhs.At(2, 1) + At(0, 3) * rhs.At(3, 1);
+        float r02 = At(0, 0) * rhs.At(0, 2) + At(0, 1) * rhs.At(1, 2) + At(0, 2) * rhs.At(2, 2) + At(0, 3) * rhs.At(3, 2);
+        float r03 = At(0, 0) * rhs.At(0, 3) + At(0, 1) * rhs.At(1, 3) + At(0, 2) * rhs.At(2, 3) + At(0, 3) * rhs.At(3, 3);
+
+        float r10 = At(1, 0) * rhs.At(0, 0) + At(1, 1) * rhs.At(1, 0) + At(1, 2) * rhs.At(2, 0) + At(1, 3) * rhs.At(3, 0);
+        float r11 = At(1, 0) * rhs.At(0, 1) + At(1, 1) * rhs.At(1, 1) + At(1, 2) * rhs.At(2, 1) + At(1, 3) * rhs.At(3, 1);
+        float r12 = At(1, 0) * rhs.At(0, 2) + At(1, 1) * rhs.At(1, 2) + At(1, 2) * rhs.At(2, 2) + At(1, 3) * rhs.At(3, 2);
+        float r13 = At(1, 0) * rhs.At(0, 3) + At(1, 1) * rhs.At(1, 3) + At(1, 2) * rhs.At(2, 3) + At(1, 3) * rhs.At(3, 3);
+
+        float r20 = At(2, 0) * rhs.At(0, 0) + At(2, 1) * rhs.At(1, 0) + At(2, 2) * rhs.At(2, 0) + At(2, 3) * rhs.At(3, 0);
+        float r21 = At(2, 0) * rhs.At(0, 1) + At(2, 1) * rhs.At(1, 1) + At(2, 2) * rhs.At(2, 1) + At(2, 3) * rhs.At(3, 1);
+        float r22 = At(2, 0) * rhs.At(0, 2) + At(2, 1) * rhs.At(1, 2) + At(2, 2) * rhs.At(2, 2) + At(2, 3) * rhs.At(3, 2);
+        float r23 = At(2, 0) * rhs.At(0, 3) + At(2, 1) * rhs.At(1, 3) + At(2, 2) * rhs.At(2, 3) + At(2, 3) * rhs.At(3, 3);
+
+        float r30 = At(3, 0) * rhs.At(0, 0) + At(3, 1) * rhs.At(1, 0) + At(3, 2) * rhs.At(2, 0) + At(3, 3) * rhs.At(3, 0);
+        float r31 = At(3, 0) * rhs.At(0, 1) + At(3, 1) * rhs.At(1, 1) + At(3, 2) * rhs.At(2, 1) + At(3, 3) * rhs.At(3, 1);
+        float r32 = At(3, 0) * rhs.At(0, 2) + At(3, 1) * rhs.At(1, 2) + At(3, 2) * rhs.At(2, 2) + At(3, 3) * rhs.At(3, 2);
+        float r33 = At(3, 0) * rhs.At(0, 3) + At(3, 1) * rhs.At(1, 3) + At(3, 2) * rhs.At(2, 3) + At(3, 3) * rhs.At(3, 3);
+        return {r00,r01,r02,r03, r10, r11, r12, r13, r20,r21,r22,r23, r30,r31,r32,r33};
+    }
+
+    Vector4 Matrix4x4::operator[](int row) {
+        return Vector4{elems[row][0], elems[row][1], elems[row][2], elems[row][3]};
+    }
+
+    Matrix4x4 Matrix4x4::operator*(const Matrix3x3 &rhs) const {
+        float r00 = At(0, 0) * rhs.At(0, 0) + At(0, 1) * rhs.At(1, 0) + At(0, 2) * rhs.At(2, 0);
+        float r01 = At(0, 0) * rhs.At(0, 1) + At(0, 1) * rhs.At(1, 1) + At(0, 2) * rhs.At(2, 1);
+        float r02 = At(0, 0) * rhs.At(0, 2) + At(0, 1) * rhs.At(1, 2) + At(0, 2) * rhs.At(2, 2);
+        float r03 = At(0, 3);
+
+        float r10 = At(1, 0) * rhs.At(0, 0) + At(1, 1) * rhs.At(1, 0) + At(1, 2) * rhs.At(2, 0);
+        float r11 = At(1, 0) * rhs.At(0, 1) + At(1, 1) * rhs.At(1, 1) + At(1, 2) * rhs.At(2, 1);
+        float r12 = At(1, 0) * rhs.At(0, 2) + At(1, 1) * rhs.At(1, 2) + At(1, 2) * rhs.At(2, 2);
+        float r13 = At(1, 3);
+
+        float r20 = At(2, 0) * rhs.At(0, 0) + At(2, 1) * rhs.At(1, 0) + At(2, 2) * rhs.At(2, 0);
+        float r21 = At(2, 0) * rhs.At(0, 1) + At(2, 1) * rhs.At(1, 1) + At(2, 2) * rhs.At(2, 1);
+        float r22 = At(2, 0) * rhs.At(0, 2) + At(2, 1) * rhs.At(1, 2) + At(2, 2) * rhs.At(2, 2);
+        float r23 = At(2, 3);
+
+        float r30 = At(3, 0) * rhs.At(0, 0) + At(3, 1) * rhs.At(1, 0) + At(3, 2) * rhs.At(2, 0);
+        float r31 = At(3, 0) * rhs.At(0, 1) + At(3, 1) * rhs.At(1, 1) + At(3, 2) * rhs.At(2, 1);
+        float r32 = At(3, 0) * rhs.At(0, 2) + At(3, 1) * rhs.At(1, 2) + At(3, 2) * rhs.At(2, 2);
+        float r33 = At(3, 3);
+
+        return {r00,r01,r02,r03, r10, r11, r12, r13, r20,r21,r22,r23, r30,r31,r32,r33};
+    }
+
+    Matrix4x4 Matrix4x4::operator+() const { return *this; }
+
+    Matrix4x4 Matrix4x4::FromTranslation(const Vector3 &rhs) {
+        return Matrix4x4(1.f, 0, 0, rhs.x,
+                         0, 1.f, 0, rhs.y,
+                         0, 0, 1.f, rhs.z,
+                         0, 0, 0,     1.f);
+    }
+
+    Matrix4x4 Matrix4x4::Translate(const Vector3 &rhs) const {
+        return *this * FromTranslation(rhs);
+    }
+
+    Vector3 Matrix4x4::Transform(const Vector3 &rhs) const {
+        return Transform(rhs.x, rhs.y, rhs.z);
+    }
+
+    Vector3 Matrix4x4::Transform(float tx, float ty, float tz) const {
+        return Vector3(At(0, 0) * tx + At(0, 1) * ty + At(0, 2) * tz + At(0, 3),
+                       At(1, 0) * tx + At(1, 1) * ty + At(1, 2) * tz + At(1, 3),
+                       At(2, 0) * tx + At(2, 1) * ty + At(2, 2) * tz + At(2, 3));
+    }
+
+    Vector2 Matrix4x4::Transform(float tx, float ty) const {
+        return Vector2(At(0, 0) * tx + At(0, 1) * ty + At(0, 2) + At(0, 3),
+                       At(1, 0) * tx + At(1, 1) * ty + At(1, 2) + At(1, 3));
+    }
+
+    Vector2 Matrix4x4::Transform(const Vector2 &rhs) const {
+        return Transform(rhs.x, rhs.y);
+    }
+
+    Matrix4x4 &Matrix4x4::operator=(const Matrix3x3 &rhs) {
+        Set3x3Part(rhs);
+        SetTranslatePart(0,0,0);
+        SetRow(3, 0, 0, 0, 1);
+        return *this;
+    }
+
+    Matrix4x4 &Matrix4x4::operator=(const Quaternion &rhs) {
+        *this = rhs.ToMatrix4x4();
+        return *this;
+    }
+
+    float &Matrix4x4::At(int row, int col) {
+        return elems[row][col];
+    }
+
+    Matrix4x4 Matrix4x4::Inverse() const {
+        // Compute the inverse directly using Cramer's rule
+        // Warning: This method is numerically very unstable!
+        float d = Determinant();
+
+        d = 1.f / d;
+
+        float a11 = At(0, 0);float a12 = At(0, 1);float a13 = At(0, 2);float a14 = At(0, 3);
+        float a21 = At(1, 0);float a22 = At(1, 1);float a23 = At(1, 2);float a24 = At(1, 3);
+        float a31 = At(2, 0);float a32 = At(2, 1);float a33 = At(2, 2);float a34 = At(2, 3);
+        float a41 = At(3, 0);float a42 = At(3, 1);float a43 = At(3, 2);float a44 = At(3, 3);
+
+        Matrix4x4 i = {
+                d * (a22*a33*a44 + a23*a34*a42 + a24*a32*a43 - a22*a34*a43 - a23*a32*a44 - a24*a33*a42),
+                d * (a12*a34*a43 + a13*a32*a44 + a14*a33*a42 - a12*a33*a44 - a13*a34*a42 - a14*a32*a43),
+                d * (a12*a23*a44 + a13*a24*a42 + a14*a22*a43 - a12*a24*a43 - a13*a22*a44 - a14*a23*a42),
+                d * (a12*a24*a33 + a13*a22*a34 + a14*a23*a32 - a12*a23*a34 - a13*a24*a32 - a14*a22*a33),
+                d * (a21*a34*a43 + a23*a31*a44 + a24*a33*a41 - a21*a33*a44 - a23*a34*a41 - a24*a31*a43),
+                d * (a11*a33*a44 + a13*a34*a41 + a14*a31*a43 - a11*a34*a43 - a13*a31*a44 - a14*a33*a41),
+                d * (a11*a24*a43 + a13*a21*a44 + a14*a23*a41 - a11*a23*a44 - a13*a24*a41 - a14*a21*a43),
+                d * (a11*a23*a34 + a13*a24*a31 + a14*a21*a33 - a11*a24*a33 - a13*a21*a34 - a14*a23*a31),
+                d * (a21*a32*a44 + a22*a34*a41 + a24*a31*a42 - a21*a34*a42 - a22*a31*a44 - a24*a32*a41),
+                d * (a11*a34*a42 + a12*a31*a44 + a14*a32*a41 - a11*a32*a44 - a12*a34*a41 - a14*a31*a42),
+                d * (a11*a22*a44 + a12*a24*a41 + a14*a21*a42 - a11*a24*a42 - a12*a21*a44 - a14*a22*a41),
+                d * (a11*a24*a32 + a12*a21*a34 + a14*a22*a31 - a11*a22*a34 - a12*a24*a31 - a14*a21*a32),
+                d * (a21*a33*a42 + a22*a31*a43 + a23*a32*a41 - a21*a32*a43 - a22*a33*a41 - a23*a31*a42),
+                d * (a11*a32*a43 + a12*a33*a41 + a13*a31*a42 - a11*a33*a42 - a12*a31*a43 - a13*a32*a41),
+                d * (a11*a23*a42 + a12*a21*a43 + a13*a22*a41 - a11*a22*a43 - a12*a23*a41 - a13*a21*a42),
+                d * (a11*a22*a33 + a12*a23*a31 + a13*a21*a32 - a11*a23*a32 - a12*a21*a33 - a13*a22*a31)
+        };
+        return i;
+    }
+
+    float Matrix4x4::Minor(int i, int j) const {
+        int r0 = SKIPNUM(0, i);
+        int r1 = SKIPNUM(1, i);
+        int r2 = SKIPNUM(2, i);
+        int c0 = SKIPNUM(0, j);
+        int c1 = SKIPNUM(1, j);
+        int c2 = SKIPNUM(2, j);
+
+        float a = At(r0, c0);
+        float b = At(r0, c1);
+        float c = At(r0, c2);
+        float d = At(r1, c0);
+        float e = At(r1, c1);
+        float f = At(r1, c2);
+        float g = At(r2, c0);
+        float h = At(r2, c1);
+        float k = At(r2, c2);
+
+        return a*e*k + b*f*g + c*d*h - a*f*h - b*d*k - c*e*g;
+    }
+
+    float Matrix4x4::Determinant() const {
+        return At(0, 0) * Minor(0,0) - At(0, 1) * Minor(0,1) + At(0, 2) * Minor(0,2) - At(0, 3) * Minor(0,3);
+    }
+
+    float Matrix4x4::Determinant3x3() const {
+
+        const float a = elems[0][0];
+        const float b = elems[0][1];
+        const float c = elems[0][2];
+        const float d = elems[1][0];
+        const float e = elems[1][1];
+        const float f = elems[1][2];
+        const float g = elems[2][0];
+        const float h = elems[2][1];
+        const float i = elems[2][2];
+
+        return a*e*i + b*f*g + c*d*h - a*f*h - b*d*i - c*e*g;
+    }
+
+    Matrix3x3 Matrix4x4::GetRotatePart() const {
+        return Matrix3x3 {
+                At(0, 0), At(0, 1), At(0, 2),
+                At(1, 0), At(1, 1), At(1, 2),
+                At(2, 0), At(2, 1), At(2, 2)
+        };
+    }
+
+    Matrix4x4 Matrix4x4::Transpose() const {
+        Matrix4x4 copy;
+        copy.elems[0][0] = elems[0][0]; copy.elems[0][1] = elems[1][0]; copy.elems[0][2] = elems[2][0]; copy.elems[0][3] = elems[3][0];
+        copy.elems[1][0] = elems[0][1]; copy.elems[1][1] = elems[1][1]; copy.elems[1][2] = elems[2][1]; copy.elems[1][3] = elems[3][1];
+        copy.elems[2][0] = elems[0][2]; copy.elems[2][1] = elems[1][2]; copy.elems[2][2] = elems[2][2]; copy.elems[2][3] = elems[3][2];
+        copy.elems[3][0] = elems[0][3]; copy.elems[3][1] = elems[1][3]; copy.elems[3][2] = elems[2][3]; copy.elems[3][3] = elems[3][3];
+        return copy;
+    }
+
+    Vector4 Matrix4x4::Diagonal() const {
+        return Vector4{At(0, 0), At(1,1), At(2,2), At(3,3)};
+    }
+
+    Vector3 Matrix4x4::Diagonal3() const {
+        return Vector3 { At(0, 0), At(1,1), At(2,2) };
+    }
+
+    Vector3 Matrix4x4::WorldX() const {
+        return GetColumn3(0);
+    }
+
+    Vector3 Matrix4x4::WorldY() const {
+        return GetColumn3(1);
+    }
+
+    Vector3 Matrix4x4::WorldZ() const {
+        return GetColumn3(2);
+    }
+
+    bool Matrix4x4::IsFinite() const {
+        for(int iy = 0; iy < Rows; ++iy)
+            for(int ix = 0; ix < Cols; ++ix)
+                if (!std::isfinite(elems[iy][ix]))
+                    return false;
+        return true;
+    }
+
+    Vector3 Matrix4x4::GetColumn3(int index) const {
+        return Vector3{At(0, index), At(1, index), At(2, index)};
+    }
+
+    Vector2 Matrix4x4::operator*(const Vector2 &rhs) const { return this->Transform(rhs);}
+
+    Vector3 Matrix4x4::operator*(const Vector3 &rhs) const { return this->Transform(rhs);}
+
+    Vector4 Matrix4x4::operator*(const Vector4 &rhs) const { return this->Transform(rhs);}
+
+    Vector4 Matrix4x4::Transform(float tx, float ty, float tz, float tw) const {
+        return Transform({tx, ty, tz, tw});
+    }
+
+    Vector4 Matrix4x4::Transform(const Vector4 &rhs) const {
+        return Vector4(At(0, 0) * rhs.x + At(0, 1) * rhs.y + At(0, 2) * rhs.z + At(0, 3) * rhs.w,
+                       At(1, 0) * rhs.x + At(1, 1) * rhs.y + At(1, 2) * rhs.z + At(1, 3) * rhs.w,
+                       At(2, 0) * rhs.x + At(2, 1) * rhs.y + At(2, 2) * rhs.z + At(2, 3) * rhs.w,
+                       At(3, 0) * rhs.x + At(3, 1) * rhs.y + At(3, 2) * rhs.z + At(3, 3) * rhs.w);
+    }
+
+    Vector3 Matrix4x4::GetTranslatePart() const {
+        return GetColumn3(3);
+    }
+
+    Matrix4x4
+    Matrix4x4::LookAt(const Vector3 &localFwd, const Vector3 &targetDir, const Vector3 &localUp, const Vector3 &worldUp) {
+        Matrix4x4 m;
+        m.Set3x3Part(Matrix3x3::LookAt(localFwd, targetDir, localUp, worldUp));
+        m.SetTranslatePart(0,0,0);
+        m.SetRow(3, 0,0,0,1);
+        return m;
+    }
+
+    Vector4 Matrix4x4::GetRow(int index) const {
+        return { At(index, 0), At(index, 1), At(index, 2), At(index, 3)};
+    }
+
+    Vector4 Matrix4x4::GetColumn(int index) const {
+        return { At(0, index), At(1, index), At(2, index), At(3, index)};
+    }
+
+    Vector3 Matrix4x4::GetRow3(int index) const {
+        return Vector3{ At(index, 0), At(index, 1), At(index, 2)};
+    }
+
+    void Matrix4x4::SwapColumns(int col1, int col2) {
+        Swap(At(0, col1), At(0, col2));
+        Swap(At(1, col1), At(1, col2));
+        Swap(At(2, col1), At(2, col2));
+        Swap(At(3, col1), At(3, col2));
+    }
+
+    void Matrix4x4::SwapRows(int row1, int row2) {
+        Swap(At(row1, 0), At(row2, 0));
+        Swap(At(row1, 1), At(row2, 1));
+        Swap(At(row1, 2), At(row2, 2));
+        Swap(At(row1, 3), At(row2, 3));
+    }
+
+    void Matrix4x4::SwapRows3(int row1, int row2) {
+        Swap(At(row1, 0), At(row2, 0));
+        Swap(At(row1, 1), At(row2, 1));
+        Swap(At(row1, 2), At(row2, 2));
+    }
+
+    void Matrix4x4::Pivot() {
+        int rowIndex = 0;
+
+        for(int col = 0; col < Cols; ++col)
+        {
+            int greatest = rowIndex;
+
+            // find the rowIndex k with k >= 1 for which Mkj has the largest absolute value.
+            for(int i = rowIndex; i < Rows; ++i)
+                if (std::abs(At(i, col)) > std::abs(At(greatest, col)))
+                    greatest = i;
+
+            if (std::abs(At(greatest, col)) != 0)
+            {
+                if (rowIndex != greatest)
+                    SwapRows(rowIndex, greatest); // the greatest now in rowIndex
+
+                ScaleRow(rowIndex, 1.f/At(rowIndex, col));
+
+                for(int r = 0; r < Rows; ++r)
+                    if (r != rowIndex)
+                        SetRow(r, GetRow(r) - GetRow(rowIndex) * At(r, col));
+
+                ++rowIndex;
+            }
+        }
+    }
+
+    void Matrix4x4::ScaleColumn3(int col, float scalar) {
+        At(0, col) *= scalar;
+        At(1, col) *= scalar;
+        At(2, col) *= scalar;
+    }
+
+    void Matrix4x4::ScaleColumn(int col, float scalar) {
+        At(0, col) *= scalar;
+        At(1, col) *= scalar;
+        At(2, col) *= scalar;
+        At(3, col) *= scalar;
+    }
+
+    void Matrix4x4::ScaleRow3(int row, float scalar) {
+        At(row, 0) *= scalar;
+        At(row, 1) *= scalar;
+        At(row, 2) *= scalar;
+    }
+
+    void Matrix4x4::ScaleRow(int row, float scalar) {
+        At(row, 0) *= scalar;
+        At(row, 1) *= scalar;
+        At(row, 2) *= scalar;
+        At(row, 3) *= scalar;
+    }
 }

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

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

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

src/J3ML/LinearAlgebra/Vector3.cpp

@@ -301,5 +301,12 @@ namespace LinearAlgebra {
         return lhs.AngleBetween(rhs);
     }
 
+    Vector3 Vector3::Direction(const Vector3 &rhs) {
+        float x = (cos(Math::Radians(rhs.y)) * cos(Math::Radians(rhs.x)));
+        float y = -sin(Math::Radians(rhs.x));
+        float z = (sin(Math::Radians(rhs.y)) * cos(Math::Radians(rhs.x)));
+        return {x, y, z};
+    }
+
 #pragma endregion
 }

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)
     {}
@@ -139,6 +141,15 @@ Vector4 Vector4::operator-(const Vector4& rhs) const
 
     }
 
+    Vector4 &Vector4::operator=(const Vector4 &rhs) {
+        x = rhs.x;
+        y = rhs.y;
+        z = rhs.z;
+        w = rhs.w;
+
+        return *this;
+    }
+
 
 }
 #pragma endregion

tests/LinearAlgebra/Vector3Tests.cpp

@@ -189,7 +189,7 @@ TEST(Vector3Test, V3_AngleBetween) {
     Vector3 B {.25f, .75f, .25f};
     A = A.Normalize();
     B = B.Normalize();
-    LinearAlgebra::Angle2D ExpectedResult {-0.697914, -2.35619};
+    LinearAlgebra::Angle2D ExpectedResult {-0.69791365, -2.3561945};
     std::cout << A.AngleBetween(B).x << ", " << A.AngleBetween(B).y << "";
     auto angle = A.AngleBetween(B);
     EXPECT_FLOAT_EQ(angle.x, ExpectedResult.x);

tests/LinearAlgebra/Vector4Tests.cpp

@@ -1,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) {}