Implement Missing things (More To Come) (Broken build)

include/J3ML/Geometry/AABB.h

@@ -5,6 +5,7 @@
 #include <J3ML/Geometry/Shape.h>
 #include "J3ML/Algorithm/RNG.h"
 
+
 namespace J3ML::Geometry
 {
     using namespace J3ML::LinearAlgebra;
@@ -92,11 +93,12 @@ namespace J3ML::Geometry
         float GetSurfaceArea() const;
         Vector3 GetClosestPoint(const Vector3& point) const;
 
+        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 Matrix3x3& transform) const;
         OBB Transform(const Matrix4x4& transform);
         OBB Transform(const Quaternion& transform);
         bool Contains(const Vector3& point) const;
@@ -146,4 +148,6 @@ namespace J3ML::Geometry
 
         bool TestAxis(const Vector3& axis, const Vector3& v0, const Vector3& v1, const Vector3& v2) const;
     };
+
+
 }

include/J3ML/Geometry/OBB.h

@@ -86,5 +86,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/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>
@@ -125,8 +125,19 @@ 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;
+
+        void IsColOrthogonal();
 
     protected:
         float elems[3][3];

include/J3ML/LinearAlgebra/Matrix4x4.h

@@ -248,6 +248,8 @@ namespace J3ML::LinearAlgebra {
         Matrix4x4 &operator = (const Quaternion& rhs);
         Matrix4x4 &operator = (const Matrix4x4& rhs) = default;
 
+        void IsColOrthogonal();
+
     protected:
         float elems[4][4];
 

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;

src/J3ML/Geometry/AABB.cpp

@@ -15,6 +15,24 @@
 
 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.MulPos(centerPoint);
+
+        // The following is equal to taking the absolute value of the whole matrix m.
+        Vector3 newDir = DIR_VEC(Abs(m[0][0] * halfSize.x) + Abs(m[0][1] * halfSize.y) + Abs(m[0][2] * halfSize.z),
+                                 Abs(m[1][0] * halfSize.x) + Abs(m[1][1] * halfSize.y) + Abs(m[1][2] * halfSize.z),
+                                 Abs(m[2][0] * halfSize.x) + Abs(m[2][1] * halfSize.y) + 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};
@@ -432,4 +450,39 @@ namespace J3ML::Geometry {
         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;
+    }
+
 }

src/J3ML/Geometry/OBB.cpp

@@ -312,7 +312,46 @@ 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] = DIR_VEC(m.Col(0));
+        obb.axis[1] = DIR_VEC(m.Col(1));
+        obb.axis[2] = DIR_VEC(m.Col(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]);
+
+    }
+
+    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.IsColOrthogonal());
+
+        OBBSetFrom(*this, aabb, transform);
+    }
+
+    void OBB::SetFrom(const AABB& aabb, const Quaternion &transform) {
+
+        OBBSetFrom(*this, aabb, Matrix3x3(transform));
+    }
 
 
 }

src/J3ML/LinearAlgebra/Matrix3x3.cpp

@@ -377,5 +377,29 @@ 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;
+    }
+
+    void Matrix3x3::IsColOrthogonal() {
+
+    }
+
 }
 

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -593,4 +593,8 @@ namespace J3ML::LinearAlgebra {
         At(3, 2) = data[14];
         At(3, 3) = data[15];
     }
+
+    void Matrix4x4::IsColOrthogonal() {
+
+    }
 }