Unfinished Work

CMakeLists.txt

@@ -29,7 +29,8 @@ file(GLOB_RECURSE J3ML_SRC     "src/J3ML/*.c" "src/J3ML/*.cpp")
 include_directories("include")
 
 add_library(J3ML SHARED ${J3ML_SRC}
-        include/J3ML/Geometry/Common.h)
+        include/J3ML/Geometry/Common.h
+        src/J3ML/Geometry/Triangle.cpp)
 set_target_properties(J3ML PROPERTIES LINKER_LANGUAGE CXX)
 
 install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME})

include/J3ML/Geometry/AABB.h

@@ -3,12 +3,15 @@
 #include <J3ML/LinearAlgebra.h>
 #include <J3ML/Geometry/Common.h>
 #include <J3ML/Geometry/Shape.h>
+#include "J3ML/Algorithm/RNG.h"
 
-// TODO: Fix circular include between AABB and OBB
 
 namespace J3ML::Geometry
 {
+
+
     using namespace J3ML::LinearAlgebra;
+    using J3ML::Algorithm::RNG;
     // A 3D axis-aligned bounding box
     // This data structure can be used to represent coarse bounds of objects, in situations where detailed triangle-level
     // computations can be avoided. In physics systems, bounding boxes are used as an efficient early-out test for geometry
@@ -91,23 +94,22 @@ namespace J3ML::Geometry
         float GetVolume() const;
         float GetSurfaceArea() const;
         Vector3 GetClosestPoint(const Vector3& point) const;
-        Vector3 GetRandomPointInside();
-        Vector3 GetRandomPointOnSurface();
-        Vector3 GetRandomPointOnEdge();
-        Vector3 GetRandomCornerPoint();
+
+        void Translate(const Vector3& offset);
         AABB Translated(const Vector3& offset) const;
         AABB TransformAABB(const Matrix3x3& transform);
         AABB TransformAABB(const Matrix4x4& transform);
         AABB TransformAABB(const Quaternion& transform);
-        OBB Transform(const Matrix3x3& transform);
-        OBB Transform(const Matrix4x4& transform);
-        OBB Transform(const Quaternion& transform);
+        OBB Transform(const Matrix3x3& transform) const;
+        OBB Transform(const Matrix4x4& transform) const;
+        OBB Transform(const Quaternion& transform) const;
         bool Contains(const Vector3& point) const;
+        bool Contains(const Vector3& aabbMinPoint, const Vector3& aabbMaxPoint) const;
         bool Contains(const LineSegment& lineSegment) const;
         bool Contains(const AABB& aabb) const;
         bool Contains(const OBB& obb) const;
         bool Contains(const Sphere& sphere) const;
-        bool Contains(const Triangle& triange) const;
+        bool Contains(const Triangle& triangle) const;
         bool Contains(const Polygon& polygon) const;
         bool Contains(const Frustum& frustum) const;
         bool Contains(const Polyhedron& polyhedron) const;
@@ -132,7 +134,10 @@ namespace J3ML::Geometry
 
         void SetFrom(const Sphere &s);
 
-        Vector3 GetRandomPointInside() const;
+        Vector3 GetRandomPointInside(RNG& rng) const;
+        Vector3 GetRandomPointOnSurface(RNG& rng) const;
+        Vector3 GetRandomPointOnEdge(RNG& rng) const;
+        Vector3 GetRandomCornerPoint(RNG& rng) const;
 
         void SetNegativeInfinity();
 
@@ -146,4 +151,6 @@ namespace J3ML::Geometry
 
         bool TestAxis(const Vector3& axis, const Vector3& v0, const Vector3& v1, const Vector3& v2) const;
     };
+
+
 }

include/J3ML/Geometry/Capsule.h

@@ -3,12 +3,14 @@
 #include "LineSegment.h"
 #include "Shape.h"
 #include <J3ML/LinearAlgebra/Vector3.h>
+#include <J3ML/Geometry/Common.h>
 
 namespace J3ML::Geometry
 {
     using namespace LinearAlgebra;
     class Capsule : public Shape
     {
+    public:
         // Specifies the two inner points of this capsule
         LineSegment l;
         // Specifies the radius of this capsule
@@ -24,5 +26,6 @@ namespace J3ML::Geometry
         Vector3 Center() const;
         Vector3 Centroid() const;
         Vector3 ExtremePoint(const Vector3& direction);
+        AABB MinimalEnclosingAABB() const;
     };
 }

include/J3ML/Geometry/Frustum.h

@@ -3,9 +3,10 @@
 //
 #pragma once
 
+#include <J3ML/Geometry/Common.h>
 #include "Plane.h"
 #include "Shape.h"
-#include <J3ML/LinearAlgebra/CoordinateFrame.h>
+#include <J3ML/LinearAlgebra.h>
 
 namespace J3ML::Geometry
 {
@@ -37,6 +38,7 @@ namespace J3ML::Geometry
         Plane FarFace;
         Plane NearFace;
         static Frustum CreateFrustumFromCamera(const CoordinateFrame& cam, float aspect, float fovY, float zNear, float zFar);
+        AABB MinimalEnclosingAABB() const;
     };
 
 

include/J3ML/Geometry/OBB.h

@@ -47,6 +47,7 @@ namespace J3ML::Geometry {
         Vector3 HalfDiagonal() const;
         void Transform(const Matrix3x3& transform);
         void Transform(const Matrix4x4& transform);
+        void Transform(const Quaternion& transform);
         bool IsFinite() const;
         bool IsDegenerate() const;
         Vector3 CenterPoint() const;
@@ -86,5 +87,12 @@ namespace J3ML::Geometry {
         Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
 
         Vector3 ExtremePoint(const Vector3 &direction) const;
+
+
+        void SetFrom(const AABB &aabb, const Matrix3x3 &transform);
+
+        void SetFrom(const AABB &aabb, const Matrix4x4 &transform);
+
+        void SetFrom(const AABB &aabb, const Quaternion &transform);
     };
 }

include/J3ML/Geometry/Polygon.h

@@ -1,11 +1,27 @@
 #pragma once
 
+#include <J3ML/Geometry/Common.h>
+#include <vector>
 #include "Shape.h"
+#include "J3ML/LinearAlgebra.h"
 
 namespace J3ML::Geometry {
+
     class Polygon : public Shape
     {
     public:
+        std::vector<Vector3> vertices;
+        AABB MinimalEnclosingAABB() const;
+        int NumVertices() const
+        {
+            return (int)vertices.size();
+        }
+        Vector3 Vertex(int vertexIndex) const
+        {
+            assert(vertexIndex >= 0);
+            assert(vertexIndex < (int) vertices.size());
+            return vertices[vertexIndex];
+        }
     protected:
     private:
     };

include/J3ML/Geometry/Polyhedron.h

@@ -1,9 +1,11 @@
 #pragma once
 
+#include <J3ML/Geometry/Common.h>
 #include <J3ML/Geometry/Shape.h>
 #include <vector>
 #include <J3ML/LinearAlgebra/Vector3.h>
 
+
 namespace J3ML::Geometry
 {
     using namespace J3ML::LinearAlgebra;
@@ -30,6 +32,13 @@ namespace J3ML::Geometry
 
         // Specifies the vertices of this polyhedron.
         std::vector<Vector3> v;
+        std::vector<Face> f;
+
+        int NumVertices() const {return (int)v.size();}
+        int NumFaces() const { return (int)f.size();}
+        AABB MinimalEnclosingAABB() const;
+
+        Vector3 Vertex(int vertexIndex) const;
 
     protected:
     private:

include/J3ML/Geometry/Triangle.h

@@ -1,5 +1,8 @@
 #pragma once
 
+#include <J3ML/Geometry/Common.h>
+#include <J3ML/LinearAlgebra.h>
+
 namespace J3ML::Geometry
 {
     class Triangle
@@ -9,9 +12,9 @@ namespace J3ML::Geometry
         Vector3 V1;
         Vector3 V2;
 
-        bool Intersects(const AABB& aabb) const
-        {
-            return aabb.Intersects(*this);
-        }
+        bool Intersects(const AABB& aabb) const;
+        AABB BoundingAABB() const;
     };
+
+
 }

include/J3ML/J3ML.h

@@ -42,6 +42,7 @@ using namespace J3ML::SizedFloatTypes;
 namespace J3ML::Math
 {
 
+    bool EqualAbs(float a, float b, float epsilon = 1e-3f);
 
     // Coming soon: Units Namespace
     // For Dimensional Analysis

include/J3ML/LinearAlgebra/Matrix3x3.h

@@ -1,6 +1,6 @@
 #pragma once
 
-
+#include <J3ML/LinearAlgebra/Common.h>
 #include <J3ML/LinearAlgebra/Vector2.h>
 #include <J3ML/LinearAlgebra/Vector3.h>
 #include <J3ML/LinearAlgebra/Quaternion.h>
@@ -26,6 +26,7 @@ namespace J3ML::LinearAlgebra {
      *  vectors in the form M * v. This means that "Matrix3x3 M, M1, M2; M = M1 * M2;" gives a transformation M
      *  that applies M2 first, followed by M1 second
      */
+
     class Matrix3x3 {
     public:
         enum { Rows = 3 };
@@ -50,6 +51,10 @@ namespace J3ML::LinearAlgebra {
         Vector3 GetRow(int index) const;
         Vector3 GetColumn(int index) const;
 
+        Vector3 GetRow3(int index) const;
+
+        Vector3 GetColumn3(int index) const;
+
         float &At(int row, int col);
         float At(int x, int y) const;
 
@@ -125,8 +130,30 @@ namespace J3ML::LinearAlgebra {
 
         Vector3 operator[](int row) const;
 
+        Vector2 operator * (const Vector2& rhs) const;
         Vector3 operator * (const Vector3& rhs) const;
+        Vector4 operator * (const Vector4& rhs) const;
         Matrix3x3 operator * (const Matrix3x3& rhs) const;
+        Matrix4x4 operator * (const Matrix4x4& rhs) const;
+        Matrix3x3 Mul(const Matrix3x3& rhs) const;
+        Matrix4x4 Mul(const Matrix4x4& rhs) const;
+        Vector2 Mul(const Vector2& rhs) const;
+        Vector3 Mul(const Vector3& rhs) const;
+        Vector4 Mul(const Vector4& rhs) const;
+        Quaternion Mul(const Quaternion& rhs) const;
+
+        // Returns true if the column vectors of this matrix are all perpendicular to each other.
+        bool IsColOrthogonal(float epsilon = 1e-3f) const;
+        // Returns true if the row vectors of this matrix are all perpendicular to each other.
+        bool IsRowOrthogonal(float epsilon = 1e-3f) const;
+
+
+
+        bool HasUniformScale(float epsilon  = 1e-3f) const;
+
+        Vector3 ExtractScale() const {
+            return {GetColumn(0).Length(), GetColumn(1).Length(), GetColumn(2).Length()};
+        }
 
     protected:
         float elems[3][3];

include/J3ML/LinearAlgebra/Matrix4x4.h

@@ -232,12 +232,16 @@ namespace J3ML::LinearAlgebra {
         Matrix4x4 operator *(float scalar) const;
         Matrix4x4 operator /(float scalar) const;
 
-
+        Vector4 operator[](int row) const;
 
         Vector2 operator * (const Vector2& rhs) const;
         Vector3 operator * (const Vector3& rhs) const;
         Vector4 operator * (const Vector4& rhs) const;
 
+        Vector2 Mul(const Vector2& rhs) const;
+        Vector3 Mul(const Vector3& rhs) const;
+        Vector4 Mul(const Vector4& rhs) const;
+
         Matrix4x4 operator * (const Matrix3x3 &rhs) const;
 
         Matrix4x4 operator +() const;
@@ -248,6 +252,18 @@ namespace J3ML::LinearAlgebra {
         Matrix4x4 &operator = (const Quaternion& rhs);
         Matrix4x4 &operator = (const Matrix4x4& rhs) = default;
 
+
+        Vector3 ExtractScale() const;
+
+        bool HasUniformScale(float epsilon = 1e-3f) const;
+        bool IsColOrthogonal3(float epsilon = 1e-3f) const;
+        bool IsRowOrthogonal3(float epsilon = 1e-3f) const;
+
+        bool IsColOrthogonal(float epsilon = 1e-3f) const;
+        bool IsRowOrthogonal(float epsilon = 1e-3f) const;
+        /// Returns true if this matrix is seen to contain a "projective" part,
+        /// i.e. whether the last row of this matrix differs from [0 0 0 1]
+        bool ContainsProjection(float epsilon = 1e-3f) const;
     protected:
         float elems[4][4];
 

include/J3ML/LinearAlgebra/Vector3.h

@@ -38,7 +38,7 @@ public:
     static void Orthonormalize(Vector3& a, Vector3& b);
 
     Vector3 Abs() const;
-
+    static Vector3 Abs(const Vector3& rhs);
 
     /// Returns the DirectionVector for a given angle.
     static Vector3 Direction(const Vector3 &rhs) ;
@@ -71,9 +71,11 @@ public:
     static float MinElement(const Vector3& of);
 
     Vector3 Min(const Vector3& min) const;
+    static Vector3 Min(const Vector3& a, const Vector3& b, const Vector3& c);
     static Vector3 Min(const Vector3& lhs, const Vector3& rhs);
 
     Vector3 Max(const Vector3& max) const;
+    static Vector3 Max(const Vector3& a, const Vector3& b, const Vector3& c);
     static Vector3 Max(const Vector3& lhs, const Vector3& rhs);
 
     Vector3 Clamp(const Vector3& min, const Vector3& max) const;
@@ -151,7 +153,6 @@ public:
     Vector3 Div(float scalar) const;
     static Vector3 Div(const Vector3& lhs, float rhs);
 
-
     /// Divides this vector by a vector, element-wise
     /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
     /// but this function is provided here for syntactical convenience
@@ -162,6 +163,10 @@ public:
     /// Unary - operator (Negation)
     Vector3 operator-() const;
 
+    bool Equals(const Vector3& rhs, float epsilon = 1e-3f) const;
+    bool Equals(float _x, float _y, float _z, float epsilon = 1e-3f) const;
+
+
     Vector3 &operator =(const Vector3& rhs);
     Vector3& operator+=(const Vector3& rhs);
     Vector3& operator-=(const Vector3& rhs);

include/J3ML/LinearAlgebra/Vector4.h

@@ -20,6 +20,10 @@ namespace J3ML::LinearAlgebra {
         {
             return &x;
         }
+        Vector3 XYZ() const
+        {
+            return {x, y, z};
+        }
 
         float GetX() const;
         float GetY() const;
@@ -45,6 +49,9 @@ namespace J3ML::LinearAlgebra {
         bool operator==(const Vector4& rhs) const;
         bool operator!=(const Vector4& rhs) const;
 
+        bool Equals(const Vector4& rhs, float epsilon = 1e-3f) const;
+        bool Equals(float _x, float _y, float _z, float _w, float epsilon = 1e-3f) const;
+
         Vector4 Min(const Vector4& min) const;
         Vector4 Max(const Vector4& max) const;
         Vector4 Clamp(const Vector4& min, const Vector4& max) const;

src/J3ML/Geometry/AABB.cpp

@@ -11,9 +11,29 @@
 #include <J3ML/Geometry/Ray.h>
 #include <J3ML/Geometry/TriangleMesh.h>
 #include <J3ML/Geometry/Polyhedron.h>
+#include <J3ML/Algorithm/RNG.h>
 
 namespace J3ML::Geometry {
 
+    /// See Christer Ericson's Real-time Collision Detection, p. 87, or
+    /// James Arvo's "Transforming Axis-aligned Bounding Boxes" in Graphics Gems 1, pp. 548-550.
+    /// http://www.graphicsgems.org/
+    template<typename Matrix>
+    void AABBTransformAsAABB(AABB &aabb, Matrix &m)
+    {
+        const Vector3 centerPoint = (aabb.minPoint + aabb.maxPoint) * 0.5f;
+        const Vector3 halfSize = centerPoint - aabb.minPoint;
+        Vector3 newCenter = m.Mul(centerPoint);
+
+
+        // The following is equal to taking the absolute value of the whole matrix m.
+        Vector3 newDir = Vector3(std::abs(m[0][0] * halfSize.x) + std::abs(m[0][1] * halfSize.y) + std::abs(m[0][2] * halfSize.z),
+                                 std::abs(m[1][0] * halfSize.x) + std::abs(m[1][1] * halfSize.y) + std::abs(m[1][2] * halfSize.z),
+                                 std::abs(m[2][0] * halfSize.x) + std::abs(m[2][1] * halfSize.y) + std::abs(m[2][2] * halfSize.z));
+        aabb.minPoint = newCenter - newDir;
+        aabb.maxPoint = newCenter + newDir;
+    }
+
     AABB AABB::FromCenterAndSize(const J3ML::Geometry::Vector3 &center, const J3ML::Geometry::Vector3 &size) {
         Vector3 halfSize =  size * 0.5f;
         return AABB{center - halfSize, center + halfSize};
@@ -189,18 +209,23 @@ namespace J3ML::Geometry {
 
     void AABB::SetFromCenterAndSize(const Vector3& center, const Vector3& size)
     {
-
+        Vector3 halfSize = 0.5f * size;
+        minPoint = center - halfSize;
+        maxPoint = center + halfSize;
     }
 
 
     void AABB::SetFrom(const OBB& obb)
     {
-
+        Vector3 halfSize = Vector3::Abs(obb.axis[0] * obb.r[0]) + Vector3::Abs(obb.axis[1]*obb.r[1]) + Vector3::Abs(obb.axis[2]*obb.r[2]);
+        SetFromCenterAndSize(obb.pos, 2.f*halfSize);
     }
 
     void AABB::SetFrom(const Sphere& s)
     {
-
+        Vector3 d = Vector3(s.Radius, s.Radius, s.Radius);
+        minPoint = s.Position - d;
+        maxPoint = s.Position + d;
     }
 
     void AABB::SetFrom(const Vector3 *pointArray, int numPoints) {
@@ -212,8 +237,12 @@ namespace J3ML::Geometry {
             Enclose(pointArray[i]);
     }
 
-    Vector3 AABB::GetRandomPointInside() const {
+    Vector3 AABB::GetRandomPointInside(J3ML::Algorithm::RNG &rng) const {
+        float f1 = rng.Float();
+        float f2 = rng.Float();
+        float f3 = rng.Float();
 
+        return PointInside(f1, f2, f3);
     }
 
     void AABB::SetNegativeInfinity() {
@@ -329,6 +358,8 @@ namespace J3ML::Geometry {
             // Therefore the axis is separating and we can exit
             return false;
         }
+
+        return true;
     }
 
 
@@ -400,9 +431,192 @@ namespace J3ML::Geometry {
             return true;
 
         // Passed testing for all 13 separating axes that exist
-        return true;
+        return false;
     }
 
     Vector3 AABB::AnyPointFast() const { return minPoint;}
 
+    Vector3 AABB::GetRandomPointOnSurface(RNG &rng) const {
+        int i = rng.Int(0, 5);
+        float f1 = rng.Float();
+        float f2 = rng.Float();
+        return FacePoint(i, f1, f2);
+    }
+
+    Vector3 AABB::GetRandomPointOnEdge(RNG &rng) const {
+        int i = rng.Int(0, 11);
+        float f = rng.Float();
+        return PointOnEdge(i, f);
+    }
+
+    Vector3 AABB::GetRandomCornerPoint(RNG &rng) const {
+        return CornerPoint(rng.Int(0, 7));
+    }
+
+    void AABB::Translate(const Vector3 &offset) {
+        minPoint += offset;
+        maxPoint += offset;
+    }
+
+    AABB AABB::Translated(const Vector3 &offset) const {
+        return AABB(minPoint+offset, maxPoint+offset);
+    }
+
+    AABB AABB::TransformAABB(const Matrix3x3 &transform) {
+        // TODO: assert(transform.IsColOrthogonal());
+        // TODO: assert(transform.HasUniformScale());
+        AABBTransformAsAABB(*this, transform);
+    }
+
+    AABB AABB::TransformAABB(const Matrix4x4 &transform) {
+        // TODO: assert(transform.IsColOrthogonal());
+        // TODO: assert(transform.HasUniformScale());
+        // TODO: assert(transform.Row(3).Equals(0,0,0,1));
+        AABBTransformAsAABB(*this, transform);
+    }
+
+    AABB AABB::TransformAABB(const Quaternion &transform) {
+        Vector3 newCenter = transform.Transform(Centroid());
+        Vector3 newDir = Vector3::Abs((transform.Transform(Size())*0.5f));
+        minPoint = newCenter - newDir;
+        maxPoint = newCenter + newDir;
+    }
+
+    OBB AABB::Transform(const Matrix3x3 &transform) const {
+        OBB obb;
+        obb.SetFrom(*this, transform);
+        return obb;
+    }
+
+    bool AABB::Contains(const Vector3 &aabbMinPoint, const Vector3 &aabbMaxPoint) const {
+        return minPoint.x <= aabbMinPoint.x && maxPoint.x >= aabbMaxPoint.x &&
+               minPoint.y <= aabbMinPoint.y && maxPoint.y >= aabbMaxPoint.y &&
+               minPoint.z <= aabbMinPoint.z && maxPoint.z >= aabbMaxPoint.z;
+    }
+
+    bool AABB::Contains(const LineSegment &lineSegment) const {
+        return Contains(Vector3::Min(lineSegment.A, lineSegment.B), Vector3::Max(lineSegment.A, lineSegment.B));
+    }
+
+
+
+    bool AABB::Contains(const Vector3 &point) const {
+        return minPoint.x <= point.x && point.x <= maxPoint.x &&
+               minPoint.y <= point.y && point.y <= maxPoint.y &&
+               minPoint.z <= point.z && point.z <= maxPoint.z;
+    }
+
+    OBB AABB::Transform(const Matrix4x4 &transform) const {
+        OBB obb;
+        obb.SetFrom(*this, transform);
+        return obb;
+    }
+
+    OBB AABB::Transform(const Quaternion &transform) const {
+        OBB obb;
+        obb.SetFrom(*this, transform);
+        return obb;
+    }
+
+    bool AABB::Contains(const AABB &aabb) const {
+        return Contains(aabb.minPoint, aabb.maxPoint);
+    }
+
+    bool AABB::Contains(const OBB &obb) const {
+        return Contains(obb.MinimalEnclosingAABB());
+    }
+
+    bool AABB::Contains(const Sphere &sphere) const {
+        auto radiusVec = Vector3(sphere.Radius,sphere.Radius, sphere.Radius);
+        return Contains(sphere.Position - radiusVec, sphere.Position + radiusVec);
+    }
+
+    bool AABB::Contains(const Capsule &capsule) const {
+        return Contains(capsule.MinimalEnclosingAABB());
+    }
+
+    bool AABB::Contains(const Triangle &triangle) const {
+        return Contains(triangle.BoundingAABB());
+    }
+
+    bool AABB::Contains(const Polygon &polygon) const {
+        return Contains(polygon.MinimalEnclosingAABB());
+    }
+
+    bool AABB::Contains(const Frustum &frustum) const {
+        return Contains(frustum.MinimalEnclosingAABB());
+    }
+
+    bool AABB::Contains(const Polyhedron &polyhedron) const {
+        return Contains(polyhedron.MinimalEnclosingAABB());
+    }
+
+    bool AABB::IntersectLineAABB(const Vector3 &linePos, const Vector3 &lineDir, float tNear, float tFar) const {
+        //assert(lineDir.IsNormalized() && lineDir && lineDir.LengthSquared());
+        assert(tNear <= tFar && "");
+        // The user should have inputted values for tNear and tFar to specify the desired subrange [tNear, tFar] of the line
+        // for this intersection test.
+        // For a Line-AABB test, pass in
+        //    tNear = -FLOAT_INF;
+        //    tFar = FLOAT_INF;
+        // For a Ray-AABB test, pass in
+        //    tNear = 0.f;
+        //    tFar = FLOAT_INF;
+        // For a LineSegment-AABB test, pass in
+        //    tNear = 0.f;
+        //    tFar = LineSegment.Length();
+
+        // Test each cardinal plane (X, Y, and Z) in turn.
+        if (!Math::EqualAbs(lineDir.x, 0.f)) {
+            float recipDir = 1.f / lineDir.x;
+            float t1 = (minPoint.x - linePos.x) * recipDir;
+            float t2 = (maxPoint.x - linePos.x) * recipDir;
+
+            // tNear tracks distance to intersect (enter) the AABB
+            // tFar tracks the distance to exit the AABB
+            if (t1 < t2)
+                tNear = std::max(t1, tNear), tFar = std::min(t2, tFar);
+            else // swap t1 and t2;
+                tNear = std::max(t2, tNear), tFar = std::min(t1, tFar);
+
+            if (tNear > tFar)
+                return false; // Box is missed since we "exit" before entering it
+        }
+        else if (linePos.x < minPoint.x || linePos.x > maxPoint.x)
+            return false; // the ray can't possibly enter the box
+
+        if (!Math::EqualAbs(lineDir.y, 0.f)) // ray is parallel to plane in question
+        {
+            float recipDir = 1.f / lineDir.y;
+            float t1 = (minPoint.y - linePos.y) * recipDir;
+            float t2 = (maxPoint.y - linePos.y) * recipDir;
+
+            if (t1 < t2)
+                tNear = std::max(t1, tNear), tFar = std::min(t2, tFar);
+            else
+                tNear = std::max(t2, tNear), tFar = std::min(t1, tFar);
+
+            if (tNear > tFar)
+                return false;
+        }
+        else if (linePos.y < minPoint.y || linePos.y > maxPoint.y)
+            return false; // The ray can't possibly enter the box, abort.
+
+        if (!Math::EqualAbs(lineDir.z, 0.f)) // ray is parallel to plane in question
+        {
+            float recipDir = 1.f / lineDir.z;
+            float t1 = (minPoint.z - linePos.z) * recipDir;
+            float t2 = (maxPoint.z - linePos.z) * recipDir;
+
+            if (t1 < t2)
+                tNear = std::max(t1, tNear), tFar = std::min(t2, tFar);
+            else // Swap t1 and t2.
+                tNear = std::max(t2, tNear), tFar = std::min(t1, tFar);
+        } else if (linePos.z < minPoint.z || linePos.z > maxPoint.z)
+            return false;
+
+        return tNear <= tFar;
+
+    }
+
 }

src/J3ML/Geometry/Capsule.cpp

@@ -1,7 +1,15 @@
 #include <J3ML/Geometry/Capsule.h>
+#include <J3ML/Geometry/AABB.h>
 
 namespace J3ML::Geometry
 {
 
     Capsule::Capsule() : l() {}
+
+    AABB Capsule::MinimalEnclosingAABB() const
+    {
+        Vector3 d = Vector3(r, r, r);
+        AABB aabb(Vector3::Min(l.A, l.B) - d, Vector3::Max(l.A, l.B) + d);
+        return aabb;
+    }
 }

src/J3ML/Geometry/OBB.cpp

@@ -312,7 +312,72 @@ namespace J3ML::Geometry
         return 2.f * (size.x*size.y + size.x*size.z + size.y*size.z);
     }
 
+    template <typename Matrix>
+    void OBBSetFrom(OBB &obb, const AABB& aabb, const Matrix& m)
+    {
+        assert(m.IsColOrthogonal()); // We cannot convert transform an AABB to OBB if it gets sheared in the process.
+        assert(m.HasUniformScale()); // Nonuniform scale will produce shear as well
+        obb.pos = m.Mul(aabb.Centroid());
+        obb.r = aabb.HalfSize();
+        obb.axis[0] = Vector3(m.GetColumn3(0));
+        obb.axis[1] = Vector3(m.GetColumn3(1));
+        obb.axis[2] = Vector3(m.GetColumn3(2));
+        // If te matrix m contains scaling, propagate the scaling from the axis vectors to the half-length vectors,
+        // since we want to keep the axis vectors always normalized in our representations.
+        float matrixScale = obb.axis[0].LengthSquared();
+        matrixScale = std::sqrt(matrixScale);
+        obb.r *= matrixScale;
+        matrixScale = 1.f / matrixScale;
+        obb.axis[0] *= matrixScale;
+        obb.axis[1] *= matrixScale;
+        obb.axis[2] *= matrixScale;
 
+        Vector3::Orthonormalize(obb.axis[0], obb.axis[1], obb.axis[2]);
+
+    }
+
+    template <typename Matrix>
+    void OBBTransform(OBB& o, const Matrix& transform)
+    {
+        o.pos = transform.Mul(o.pos);
+        o.axis[0] = transform.Mul(o.r.x * o.axis[0]);
+        o.axis[1] = transform.Mul(o.r.y * o.axis[1]);
+        o.axis[2] = transform.Mul(o.r.z * o.axis[2]);
+        o.r.x = o.axis[0].Normalize().x;
+        o.r.y = o.axis[1].Normalize().y;
+        o.r.z = o.axis[2].Normalize().z;
+    }
+
+    void OBB::SetFrom(const AABB& aabb, const Matrix3x3 &transform) {
+        assert(transform.IsColOrthogonal());
+
+        OBBSetFrom(*this, aabb, transform);
+    }
+
+    void OBB::SetFrom(const AABB& aabb, const Matrix4x4 &transform) {
+        assert(transform.IsColOrthogonal3());
+
+        OBBSetFrom(*this, aabb, transform);
+    }
+
+    void OBB::SetFrom(const AABB& aabb, const Quaternion &transform) {
+
+        OBBSetFrom(*this, aabb, Matrix3x3(transform));
+    }
+
+    void OBB::Transform(const Matrix3x3 &transform) {
+        assert(transform.IsColOrthogonal());
+        OBBTransform(*this, transform);
+    }
+
+    void OBB::Transform(const Matrix4x4 &transform) {
+        assert(transform.IsColOrthogonal3());
+        OBBTransform(*this, transform);
+    }
+
+    void OBB::Transform(const Quaternion &transform) {
+        OBBTransform(*this, transform.ToMatrix3x3());
+    }
 
 
 }

src/J3ML/Geometry/Polygon.cpp

@@ -1,5 +1,14 @@
 #include <J3ML/Geometry/Polygon.h>
+#include <J3ML/Geometry/AABB.h>
+
+namespace J3ML::Geometry {
+    AABB Polygon::MinimalEnclosingAABB() const {
+        AABB aabb;
+        aabb.SetNegativeInfinity();
+        for(int i = 0; i < NumVertices(); ++i)
+            aabb.Enclose(Vertex(i));
+        return aabb;
+    }
+}
 
-namespace Geometry {
 
-}

src/J3ML/Geometry/Polyhedron.cpp

@@ -1,6 +1,19 @@
 #include <J3ML/Geometry/Polyhedron.h>
+#include <J3ML/Geometry/AABB.h>
 
-namespace Geometry
+namespace J3ML::Geometry
 {
+    AABB Polyhedron::MinimalEnclosingAABB() const {
+        AABB aabb;
+        aabb.SetNegativeInfinity();
+        for (int i = 0; i < NumVertices(); ++i)
+            aabb.Enclose(Vertex(i));
+        return aabb;
+    }
+
+    Vector3 Polyhedron::Vertex(int vertexIndex) const {
+        return v[vertexIndex];
+    }
+
+}
 
-}

src/J3ML/Geometry/Triangle.cpp

@@ -0,0 +1,17 @@
+#include <J3ML/Geometry/Triangle.h>
+#include <J3ML/LinearAlgebra.h>
+#include <J3ML/Geometry/AABB.h>
+
+namespace J3ML::Geometry
+{
+    AABB Triangle::BoundingAABB() const {
+        AABB aabb;
+        aabb.minPoint = Vector3::Min(V0, V1, V2);
+        aabb.maxPoint = Vector3::Max(V0, V1, V2);
+        return aabb;
+    }
+
+    bool Triangle::Intersects(const AABB &aabb) const {
+        return aabb.Intersects(*this);
+    }
+}

src/J3ML/J3ML.cpp

@@ -24,6 +24,10 @@ namespace J3ML
 
     float Math::Degrees(float radians) { return radians * (180.f/Pi); }
 
+    bool Math::EqualAbs(float a, float b, float epsilon) {
+        return std::abs(a - b) < epsilon;
+    }
+
     Math::Rotation::Rotation() : valueInRadians(0) {}
 
     Math::Rotation::Rotation(float value) : valueInRadians(value) {}

src/J3ML/LinearAlgebra/Matrix3x3.cpp

@@ -377,5 +377,54 @@ namespace J3ML::LinearAlgebra {
         At(2, 2) = data[10];
     }
 
+    Vector4 Matrix3x3::Mul(const Vector4 &rhs) const {
+        return {Mul(rhs.XYZ()), rhs.GetW()};
+    }
+
+    Vector3 Matrix3x3::Mul(const Vector3 &rhs) const {
+        return *this * rhs;
+    }
+
+    Vector2 Matrix3x3::Mul(const Vector2 &rhs) const {
+        return *this * rhs;
+    }
+
+    Matrix4x4 Matrix3x3::Mul(const Matrix4x4 &rhs) const {
+        return *this * rhs;
+    }
+
+    Matrix3x3 Matrix3x3::Mul(const Matrix3x3 &rhs) const {
+        return *this * rhs;
+    }
+
+    bool Matrix3x3::IsRowOrthogonal(float epsilon) const
+    {
+        return GetRow(0).IsPerpendicular(GetRow(1), epsilon)
+            && GetRow(0).IsPerpendicular(GetRow(2), epsilon)
+            && GetRow(1).IsPerpendicular(GetRow(2), epsilon);
+    }
+
+    bool Matrix3x3::IsColOrthogonal(float epsilon) const
+    {
+        return GetColumn(0).IsPerpendicular(GetColumn(1), epsilon)
+            && GetColumn(0).IsPerpendicular(GetColumn(2), epsilon)
+            && GetColumn(1).IsPerpendicular(GetColumn(2), epsilon);
+    }
+
+
+
+    bool Matrix3x3::HasUniformScale(float epsilon) const {
+        Vector3 scale = ExtractScale();
+        return Math::EqualAbs(scale.x, scale.y, epsilon) && Math::EqualAbs(scale.x, scale.z, epsilon);
+    }
+
+    Vector3 Matrix3x3::GetRow3(int index) const {
+        return GetRow(index);
+    }
+
+    Vector3 Matrix3x3::GetColumn3(int index) const {
+        return GetColumn(index);
+    }
+
 }
 

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -2,6 +2,9 @@
 #include <J3ML/LinearAlgebra/Vector4.h>
 
 namespace J3ML::LinearAlgebra {
+
+
+
     const Matrix4x4 Matrix4x4::Zero = Matrix4x4(0.f);
     const Matrix4x4 Matrix4x4::Identity = Matrix4x4({1,0,0,0}, {0,1,0,0}, {0,0,1,0}, {0,0,0,1});
     const Matrix4x4 Matrix4x4::NaN = Matrix4x4(NAN);
@@ -593,4 +596,42 @@ namespace J3ML::LinearAlgebra {
         At(3, 2) = data[14];
         At(3, 3) = data[15];
     }
+
+    bool Matrix4x4::ContainsProjection(float epsilon) const {
+        return GetRow(3).Equals(0.f, 0.f, 0.f, 1.f, epsilon) == false;
+    }
+
+    Vector4 Matrix4x4::Mul(const Vector4 &rhs) const {
+        return *this * rhs;
+    }
+
+    Vector3 Matrix4x4::Mul(const Vector3 &rhs) const {
+        return *this * rhs;
+    }
+
+    Vector2 Matrix4x4::Mul(const Vector2 &rhs) const {
+        return *this * rhs;
+    }
+
+    Vector4 Matrix4x4::operator[](int row) const {
+        return GetRow(row);
+    }
+
+    bool Matrix4x4::HasUniformScale(float epsilon) const {
+        Vector3 scale = ExtractScale();
+        return Math::EqualAbs(scale.x, scale.y, epsilon) && Math::EqualAbs(scale.x, scale.z, epsilon);
+    }
+
+    Vector3 Matrix4x4::ExtractScale() const {
+        return {GetColumn3(0).Length(), GetColumn3(1).Length(), GetColumn3(2).Length()};
+    }
+
+    bool Matrix4x4::IsColOrthogonal(float epsilon) const {
+        return IsColOrthogonal3(epsilon);
+    }
+
+    bool Matrix4x4::IsRowOrthogonal(float epsilon) const {
+        return IsRowOrthogonal3(epsilon);
+    }
+
 }

src/J3ML/LinearAlgebra/Vector3.cpp

@@ -103,6 +103,8 @@ namespace J3ML::LinearAlgebra {
         return this->IsWithinMarginOfError(rhs) == false;
     }
 
+
+
     Vector3 Vector3::Min(const Vector3& min) const
     {
         return {
@@ -405,5 +407,37 @@ namespace J3ML::LinearAlgebra {
         };
     }
 
+    Vector3 Vector3::Abs(const Vector3 &rhs) {
+        return rhs.Abs();
+    }
+
+    bool Vector3::Equals(const Vector3 &rhs, float epsilon) const {
+        return std::abs(x - rhs.x) < epsilon &&
+               std::abs(y - rhs.y) < epsilon &&
+               std::abs(z - rhs.z) < epsilon;
+    }
+
+    bool Vector3::Equals(float _x, float _y, float _z, float epsilon) const {
+        return std::abs(x - _x) < epsilon &&
+               std::abs(y - _y) < epsilon &&
+               std::abs(z - _z) < epsilon;
+    }
+
+    Vector3 Vector3::Min(const Vector3 &a, const Vector3 &b, const Vector3 &c) {
+        return {
+                std::min(a.x, std::min(b.x, c.x)),
+                std::min(a.y, std::min(b.y, c.y)),
+                std::min(a.z, std::min(b.z, c.z))
+        };
+    }
+
+    Vector3 Vector3::Max(const Vector3 &a, const Vector3 &b, const Vector3 &c) {
+        return {
+                std::max(a.x, std::max(b.x, c.x)),
+                std::max(a.y, std::max(b.y, c.y)),
+                std::max(a.z, std::max(b.z, c.z))
+        };
+    }
+
 
 }

src/J3ML/LinearAlgebra/Vector4.cpp

@@ -150,6 +150,20 @@ Vector4 Vector4::operator-(const Vector4& rhs) const
         return *this;
     }
 
+    bool Vector4::Equals(const Vector4 &rhs, float epsilon) const {
+        return std::abs(x - rhs.x) < epsilon &&
+               std::abs(y - rhs.y) < epsilon &&
+               std::abs(z - rhs.z) < epsilon &&
+               std::abs(w - rhs.w) < epsilon;
+    }
+
+    bool Vector4::Equals(float _x, float _y, float _z, float _w, float epsilon) const {
+        return std::abs(x - _x) < epsilon &&
+               std::abs(y - _y) < epsilon &&
+               std::abs(z - _z) < epsilon &&
+               std::abs(w - _w) < epsilon;
+    }
+
 
 }
 #pragma endregion