Implement QuadTree 2D

CMakeLists.txt

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

include/J3ML/Geometry.h

@@ -13,90 +13,16 @@ namespace Geometry {
         Vector2 B;
     };
 
-    class Rectangle; //AABB2D;
+    class Rectangle;
     class OBB2D;
     class Line2D;
     class Ray2D;
     class Triangle2D;
     class Polygon2D;
 
+
     struct IntersectionResult2D {};
 
     bool Intersects2D(LineSegment2D seg, Rectangle rect);
     IntersectionResult2D GetIntersection2D(LineSegment2D seg, Rectangle rect);
-
-
-    // A 3D axis-aligned bounding box
-    // This data structure can be used to represent coarse bounds of objects, in situations where detailed triangle-level
-    // computations can be avoided. In physics systems, bounding boxes are used as an efficient early-out test for geometry
-    // intersection queries.
-    // the 'Axis-aligned' part in the name means that the local axes of this bounding box are restricted to align with the
-    // axes of the world space coordinate system. This makes computation involving AABB's very fast, since AABB's cannot
-    // be arbitrarily oriented in the space with respect to each other.
-    // If you need to represent a box in 3D space with arbitrary orientation, see the class OBB. */
-    class AABB;
-    class Capsule;
-    class Line;
-    class LineSegment
-    {
-        Vector3 A;
-        Vector3 B;
-    };
-    class Ray
-    {
-        Vector3 Origin;
-        Vector3 Direction;
-    };
-
-    class OBB;
-    class Plane
-    {
-    public:
-        Vector3 Position;
-        Vector3 Normal;
-        float distance = 0.f;
-
-    };
-    class Frustum {
-    public:
-        Plane TopFace;
-        Plane BottomFace;
-        Plane RightFace;
-        Plane LeftFace;
-        Plane FarFace;
-        Plane NearFace;
-    };
-
-    class Camera {
-    public:
-        Vector3 Position;
-        Vector3 Front;
-        Vector3 Right;
-        Vector3 Up;
-    };
-
-    static Frustum CreateFrustumFromCamera(const Camera& cam, float aspect, float fovY, float zNear, float zFar)
-    {
-        Frustum frustum;
-        const float halfVSide = zFar * tanf(fovY * 0.5f);
-        const float halfHSide = halfVSide * aspect;
-
-        const Vector3 frontMultFar = cam.Front * zFar;
-
-        frustum.NearFace  = Plane{cam.Position + cam.Front * zNear, cam.Front};
-        frustum.FarFace   = Plane{cam.Position + frontMultFar, -cam.Front};
-        frustum.RightFace = Plane{cam.Position, Vector3::Cross(frontMultFar - cam.Right * halfHSide, cam.Up)};
-        frustum.LeftFace  = Plane{cam.Position, Vector3::Cross(cam.Up, frontMultFar+cam.Right*halfHSide)};
-        frustum.TopFace   = Plane{cam.Position, Vector3::Cross(cam.Right, frontMultFar - cam.Up * halfVSide)};
-        frustum.BottomFace   = Plane{cam.Position, Vector3::Cross(frontMultFar + cam.Up * halfVSide, cam.Right)};
-        return frustum;
-    }
-
-    class Polygon;
-    class Polyhedron;
-    class QuadTree;
-    class OctTree;
-    class Sphere;
-    class Triangle;
-    class TriangleMesh;
 }

include/J3ML/Geometry/AABB.h

@@ -0,0 +1,84 @@
+#pragma once
+
+#include <J3ML/LinearAlgebra/Vector3.h>
+#include "Plane.h"
+#include "Sphere.h"
+#include "OBB.h"
+#include "LineSegment.h"
+
+using namespace LinearAlgebra;
+
+namespace Geometry
+{
+    // 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
+    {
+        static AABB FromCenterAndSize(const Vector3 FromSize);
+        float MinX();
+        // Returns the smallest sphere that contains this AABB.
+        // This function computes the minimal volume sphere that contains all the points inside this AABB
+        Sphere MinimalEnclosingSphere() const;
+        // Returns the largest sphere that can fit inside this AABB
+        // This function computes the largest sphere that can fit inside this AABB.
+        Sphere MaximalContainedSphere() const;
+        Vector3 GetCentroid() const;
+        // Quickly returns an arbitrary point inside this AABB
+        Vector3 AnyPointFast() const;
+
+        Vector3 PointInside(float x, float y, float z) const;
+        // Returns an edge of this AABB
+        LineSegment Edge(int edgeIndex) const;
+        Vector3 CornerPoint(int cornerIndex);
+        Vector3 ExtremePoint(const Vector3& direction) const;
+        Vector3 ExtremePoint(const Vector3& direction, float projectionDistance);
+        Vector3 PointOnEdge(int edgeIndex, float u) const;
+        Vector3 FaceCenterPoint(int faceIndex) const;
+        Vector3 FacePoint(int faceIndex, float u, float v) const;
+        Vector3 FaceNormal(int faceIndex) const;
+        Plane FacePlane(int faceIndex);
+        static AABB MinimalEnclosingAABB(const Vector3* pointArray, int numPoints);
+        Vector3 GetSize();
+        Vector3 GetVolume();
+        float GetVolumeCubed();
+        float GetSurfaceArea();
+        Vector3 GetRandomPointInside();
+        Vector3 GetRandomPointOnSurface();
+        Vector3 GetRandomPointOnEdge();
+        Vector3 GetRandomCornerPoint();
+        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,87 @@
+#pragma once
+
+#include <J3ML/LinearAlgebra/Vector2.h>
+
+namespace Geometry
+{
+    using LinearAlgebra::Vector2;
+    // CaveGame AABB
+    class AABB2D
+    {
+    public:
+
+        Vector2 minPoint;
+        Vector2 maxPoint;
+        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 vec2d &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 vec2d &pt) const
+        {
+            return (pt - minPoint).Div(maxPoint - minPoint);
+        }
+    };
+}

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,55 @@
+//
+// Created by dawsh on 1/25/24.
+//
+#pragma once
+
+#include "Plane.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 Camera& cam, float aspect, float fovY, float zNear, float zFar);
+
+    };
+
+    Frustum Frustum::CreateFrustumFromCamera(const Camera &cam, float aspect, float fovY, float zNear, float zFar) {
+        Frustum frustum;
+        const float halfVSide = zFar * tanf(fovY * 0.5f);
+        const float halfHSide = halfVSide * aspect;
+
+        const Vector3 frontMultFar = cam.Front * zFar;
+
+        frustum.NearFace  = Plane{cam.Position + cam.Front * zNear, cam.Front};
+        frustum.FarFace   = Plane{cam.Position + frontMultFar, -cam.Front};
+        frustum.RightFace = Plane{cam.Position, Vector3::Cross(frontMultFar - cam.Right * halfHSide, cam.Up)};
+        frustum.LeftFace  = Plane{cam.Position, Vector3::Cross(cam.Up, frontMultFar+cam.Right*halfHSide)};
+        frustum.TopFace   = Plane{cam.Position, Vector3::Cross(cam.Right, frontMultFar - cam.Up * halfVSide)};
+        frustum.BottomFace   = Plane{cam.Position, Vector3::Cross(frontMultFar + cam.Up * halfVSide, cam.Right)};
+        return frustum;
+    }
+
+}

include/J3ML/Geometry/LineSegment.h

@@ -0,0 +1,12 @@
+#pragma once
+
+#include <J3ML/LinearAlgebra/Vector3.h>
+
+namespace Geometry
+{
+    class LineSegment
+    {
+        Vector3 A;
+        Vector3 B;
+    };
+}

include/J3ML/Geometry/OBB.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#include "AABB.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 AABB& aabb);
+        inline static int NumFaces() { return 6;  }
+        inline static int NumEdges() { return 12; }
+        inline static int NumVertices() { return 8; }
+
+        Polyhedron ToPolyhedron() const;
+
+        AABB MinimalEnclosingAABB() const;
+
+        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();
+        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,8 @@
+//
+// Created by dawsh on 1/25/24.
+//
+
+#ifndef J3ML_POLYGON_H
+#define J3ML_POLYGON_H
+
+#endif //J3ML_POLYGON_H

include/J3ML/Geometry/QuadTree.h

@@ -0,0 +1,168 @@
+#pragma once
+
+#include <vector>
+#include <cstdint>
+#include <J3ML/LinearAlgebra/Vector2.h>
+#include "AABB2D.h"
+
+namespace Geometry
+{
+    using LinearAlgebra::Vector2;
+    template <typename T>
+    class QuadTree
+    {
+    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;
+    }
+}

include/J3ML/Geometry/Ray.h

@@ -0,0 +1,16 @@
+//
+// Created by dawsh on 1/25/24.
+//
+
+#pragma once
+
+#include <J3ML/LinearAlgebra/Vector3.h>
+
+namespace Geometry
+{
+    class Ray
+    {
+        Vector3 Origin;
+        Vector3 Direction;
+    };
+}

include/J3ML/Geometry/Sphere.h

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

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

@@ -58,6 +58,7 @@ namespace LinearAlgebra {
 
         void SetRow(int i, const Vector3 &vector3);
         void SetColumn(int i, const Vector3& vector);
+        void SetAt(int x, int y, float value);
 
         void Orthonormalize(int c0, int c1, int c2)
         {
@@ -85,14 +86,20 @@ namespace LinearAlgebra {
         // Transforming a vector v using this matrix computes the vector
         // v' == M * v == R*S*v == (R * (S * v)) which means the scale operation
         // is applied to the vector first, followed by rotation, and finally translation
-        static Matrix3x3 FromRS(const Quaternion& rotate, const Matrix3x3& scale);
-        static Matrix3x3 FromRS(const Matrix3x3 &rotate, const Matrix3x3& scale);
+        static Matrix3x3 FromRS(const Quaternion& rotate, const Vector3& scale)
+        {
+            return Matrix3x3(rotate) * Matrix3x3::Scale(scale);
+        }
+        static Matrix3x3 FromRS(const Matrix3x3 &rotate, const Vector3& scale)
+        {
+            return rotate * Matrix3x3::Scale(scale);
+        }
 
 
         /// Creates a new transformation matrix that scales by the given factors.
         // This matrix scales with respect to origin.
         static Matrix3x3 Scale(float sx, float sy, float sz);
-        static Matrix3x3 Scale(const Matrix3x3& scale);
+        static Matrix3x3 Scale(const Vector3& scale);
 
         /// Returns the main diagonal.
         Vector3 Diagonal() const;
@@ -125,6 +132,7 @@ namespace LinearAlgebra {
         Vector2 Transform(const Vector2& rhs) const;
         Vector3 Transform(const Vector3& rhs) const;
 
+        Vector2 operator * (const Vector2& rhs) const;
         Vector3 operator * (const Vector3& rhs) const;
         Matrix3x3 operator * (const Matrix3x3& rhs) const;
 

include/J3ML/LinearAlgebra/Quaternion.h

@@ -37,6 +37,20 @@ namespace LinearAlgebra
         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;
 
         Vector3 Transform(const Vector3& vec) const;
@@ -57,7 +71,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,9 +84,9 @@ 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;

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

@@ -3,12 +3,12 @@
 #include <cstddef>
 
 namespace LinearAlgebra {
-    // A 2D (x, y) ordered pair.
+    /// 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 +44,88 @@ namespace LinearAlgebra {
         Vector2 Clamp(const Vector2& min, const Vector2& max) const;
         static Vector2 Clamp(const Vector2& min, const Vector2& middle, const Vector2& max);
 
-        // Returns the magnitude between the two vectors.
+        /// Returns the magnitude between the two vectors.
         float Distance(const Vector2& to) const;
         static float Distance(const Vector2& from, const Vector2& to);
 
+        float DistanceSq(const Vector2& to) const;
+        static float DistanceSq(const Vector2& from, const Vector2& to);
+
+        float MinElement() const;
+
+        float MaxElement() const;
+
         float Length() const;
         static float Length(const Vector2& of);
 
         float LengthSquared() const;
         static float LengthSquared(const Vector2& of);
 
-        // Returns the length of the vector, which is sqrt(x^2 + y^2)
+        /// Returns the length of the vector, which is sqrt(x^2 + y^2)
         float Magnitude() const;
         static float Magnitude(const Vector2& of);
 
-        // Returns a float value equal to the magnitudes of the two vectors multiplied together and then multiplied by the cosine of the angle between them.
-        // For normalized vectors, dot returns 1 if they point in exactly the same direction,
-        // -1 if they point in completely opposite directions, and 0 if the vectors are perpendicular.
+        bool IsFinite() const;
+        static bool IsFinite(const Vector2& v);
+
+        /// Returns a float value equal to the magnitudes of the two vectors multiplied together and then multiplied by the cosine of the angle between them.
+        /// For normalized vectors, dot returns 1 if they point in exactly the same direction,
+        /// -1 if they point in completely opposite directions, and 0 if the vectors are perpendicular.
         float Dot(const Vector2& rhs) const;
         static float Dot(const Vector2& lhs, const Vector2& rhs);
 
-        // Projects one vector onto another and returns the result. (IDK)
+        /// Projects one vector onto another and returns the result. (IDK)
         Vector2 Project(const Vector2& rhs) const;
-        // @see Project
+        /// @see Project
         static Vector2 Project(const Vector2& lhs, const Vector2& rhs);
 
-        // Returns a copy of this vector, resized to have a magnitude of 1, while preserving "direction"
+        /// Returns a copy of this vector, resized to have a magnitude of 1, while preserving "direction"
         Vector2 Normalize() const;
         static Vector2 Normalize(const Vector2& of);
 
-        // Linearly interpolates between two points.
-        // Interpolates between the points and b by the interpolant t.
-        // The parameter is (TODO: SHOULD BE!) clamped to the range[0, 1].
-        // This is most commonly used to find a point some fraction of the wy along a line between two endpoints (eg. to move an object gradually between those points).
+        /// Linearly interpolates between two points.
+        /// Interpolates between the points and b by the interpolant t.
+        /// The parameter is (TODO: SHOULD BE!) clamped to the range[0, 1].
+        /// This is most commonly used to find a point some fraction of the wy along a line between two endpoints (eg. to move an object gradually between those points).
         Vector2 Lerp(const Vector2& rhs, float alpha) const;
-        // @see Lerp
+        /// @see Lerp
         static Vector2 Lerp(const Vector2& lhs, const Vector2& rhs, float alpha);
-        // Note: Input vectors MUST be normalized first!
+        /// Note: Input vectors MUST be normalized first!
         float AngleBetween(const Vector2& rhs) const;
         static float AngleBetween(const Vector2& lhs, const Vector2& rhs);
 
-        // Adds two vectors.
+        /// Adds two vectors.
         Vector2 operator +(const Vector2& rhs) const;
         Vector2 Add(const Vector2& rhs) const;
         static Vector2 Add(const Vector2& lhs, const Vector2& rhs);
 
-        // Subtracts two vectors.
+        /// Subtracts two vectors.
         Vector2 operator -(const Vector2& rhs) const;
         Vector2 Sub(const Vector2& rhs) const;
         static Vector2 Sub(const Vector2& lhs, const Vector2& rhs);
 
-        // Multiplies this vector by a scalar value.
+        /// Multiplies this vector by a scalar value.
         Vector2 operator *(float rhs) const;
         Vector2 Mul(float scalar) const;
         static Vector2 Mul(const Vector2& lhs, float rhs);
 
-        // Divides this vector by a scalar.
+        /// Multiplies this vector by a vector, element-wise
+        /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
+        ///	 but this function is provided here for syntactical convenience.
+        Vector2 Mul(const Vector2& v) const
+        {
+            return {this->x*v.x, this->y*v.y};
+        }
+
+        /// 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 +
+        /// 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);

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

src/J3ML/Geometry/Frustum.cpp

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

src/J3ML/Geometry/LineSegment.cpp

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

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

src/J3ML/LinearAlgebra/Quaternion.cpp

@@ -60,16 +60,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)

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

@@ -231,5 +231,17 @@ 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);
+    }
+
 
 }

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