Add triangle SAT intersection

include/J3ML/Geometry/Common.h

@@ -27,5 +27,13 @@ namespace J3ML::Geometry
 // Methods required by Geometry types
 namespace J3ML::Geometry
 {
+    // Represents a segment along an axis, with the axis as a unit
+    struct Interval {
+        float min;
+        float max;
 
+        bool Intersects(const Interval& rhs) const;
+
+        bool operator==(const Interval& rhs) const = default;
+    };
 }

include/J3ML/Geometry/Triangle.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "J3ML/LinearAlgebra/Vector3.h"
 #include <J3ML/Geometry/Common.h>
 #include <J3ML/LinearAlgebra.h>
 #include <cfloat>
@@ -13,11 +14,18 @@ namespace J3ML::Geometry
         Vector3 V1;
         Vector3 V2;
     public:
-
         float DistanceSq(const Vector3 &point) const;
 
+        /// Returns a new triangle, translated with a direction vector
+        Triangle Translated(const Vector3& translation) const;
+        /// Returns a new triangle, scaled from 3D factors
+        Triangle Scaled(const Vector3& scaled) const;
+
         bool Intersects(const AABB& aabb) const;
         bool Intersects(const Capsule& capsule) const;
+        bool Intersects(const Triangle& rhs) const;
+        friend bool Intersects(const Triangle& lhs, const Triangle &rhs);
+
         AABB BoundingAABB() const;
 
         /// Tests if the given object is fully contained inside this triangle.
@@ -29,6 +37,8 @@ namespace J3ML::Geometry
         bool Contains(const LineSegment& lineSeg, float triangleThickness = 1e-3f) const;
         bool Contains(const Triangle& triangle, float triangleThickness = 1e-3f) const;
 
+        /// Project the triangle onto an axis, and returns the min and max value with the axis as a unit
+        Interval ProjectionInterval(const Vector3& axis) const;
         void ProjectToAxis(const Vector3 &axis, float &dMin, float &dMax) const;
 
         /// Quickly returns an arbitrary point inside this Triangle. Used in GJK intersection test.
@@ -108,6 +118,8 @@ namespace J3ML::Geometry
 
         Plane PlaneCW() const;
 
+        Vector3 FaceNormal() const;
+
         Vector3 Vertex(int i) const;
 
         LineSegment Edge(int i) const;

src/J3ML/Geometry/Common.cpp

@@ -0,0 +1,10 @@
+#include <J3ML/Geometry/Common.h>
+
+namespace J3ML::Geometry {
+
+    bool Interval::Intersects(const Interval& rhs) const {
+        return *this == rhs || this->min > rhs.max != this->max >= rhs.min;
+    }
+
+}
+

src/J3ML/Geometry/Triangle.cpp

@@ -5,9 +5,42 @@
 #include <J3ML/Geometry/Line.h>
 #include <J3ML/Geometry/Capsule.h>
 
-
 namespace J3ML::Geometry
 {
+    Interval Triangle::ProjectionInterval(const Vector3& axis) const {
+        // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter4/generic_sat.html
+        float min = axis.Dot(V0);
+        float max = min;
+
+        float value = axis.Dot(V1);
+        if (value < min)
+            min = value;
+        if (value > max)
+            max = value;
+
+        value = axis.Dot(V2);
+        if (value < min)
+            min = value;
+        if (value > max)
+            max = value;
+        return Interval{min, max};
+    }
+
+    Triangle Triangle::Translated(const Vector3& translation) const {
+        return {
+            V0 + translation,
+            V1 + translation,
+            V2 + translation
+        };
+    }
+
+    Triangle Triangle::Scaled(const Vector3& scale) const {
+        return {
+            {V0.x * scale.x, V0.y * scale.y, V0.z * scale.y},
+            {V1.x * scale.x, V1.y * scale.y, V1.z * scale.y},
+            {V2.x * scale.x, V2.y * scale.y, V2.z * scale.y},
+        };
+    }
 
     LineSegment Triangle::Edge(int i) const
     {
@@ -37,6 +70,13 @@ namespace J3ML::Geometry
             return Vector3::NaN;
     }
 
+    Vector3 Triangle::FaceNormal() const {
+        Vector3 edge1 = V1 - V0;
+        Vector3 edge2 = V2 - V1;
+
+        return edge1.Cross(edge2);
+    }
+
     Plane Triangle::PlaneCCW() const
     {
         return Plane(V0, V1, V2);
@@ -351,6 +391,78 @@ namespace J3ML::Geometry
         return capsule.Intersects(*this);
     }
 
+    namespace {
+        bool HaveSeparatingAxis(const Triangle& t1, const Triangle& t2, const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) {
+            // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter4/robust_sat.html
+            Vector3 ab = (a - b);
+            Vector3 axis = ab.Cross(c - d);
+
+            if (axis.IsZero()) {
+                // Axis is zero, they are parallel, try to find the vector orthogonal to both
+                Vector3 n = ab.Cross(c - a);
+
+                if (n.IsZero()) {
+                    // AB and AC are parallel, this means they are both on the same axis, just pick one
+                    axis = ab;
+                } else {
+                    // Parallel but not on the same axis, get the vector that is normal to both edges
+                    axis = ab.Cross(n);
+                }
+            }
+
+            return !t1.ProjectionInterval(axis).Intersects(t2.ProjectionInterval(axis));
+        }
+    }
+
+    bool Intersects(const Triangle& lhs, const Triangle& rhs) {
+        // Triangle v Triangle intersection check using SAT
+        // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter4/generic_sat.html
+        // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter4/triangle-triangle.html
+        // Reminder, we only need to find *one* axis to disprove that they collide, the corrolary is that we need to check ALL axes to prove that they collide
+        
+        Vector3 lEdges[3] = {
+            lhs.V1 - lhs.V0,
+            lhs.V2 - lhs.V1,
+            lhs.V0 - lhs.V2,
+        };
+
+        // First, use lhs's face normal as the separating axis
+        if (HaveSeparatingAxis(lhs, rhs, lhs.V1, lhs.V0, lhs.V2, lhs.V1)) {
+            return false;
+        }
+
+        Vector3 rEdges[3] = {
+            rhs.V1 - rhs.V0,
+            rhs.V2 - rhs.V1,
+            rhs.V0 - rhs.V2,
+        };
+
+        // Second, use rhs's face normal as the separating axis
+        if (HaveSeparatingAxis(lhs, rhs, rhs.V1, rhs.V0, rhs.V2, rhs.V1)) {
+            return false;
+        }
+
+        // Third, check the normals of each edge against each other
+        if (HaveSeparatingAxis(lhs, rhs, lhs.V1, lhs.V0, rhs.V1, rhs.V0) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V1, lhs.V0, rhs.V2, rhs.V1) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V1, lhs.V0, rhs.V0, rhs.V2) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V2, lhs.V1, rhs.V1, rhs.V0) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V2, lhs.V1, rhs.V2, rhs.V1) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V2, lhs.V1, rhs.V0, rhs.V2) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V0, lhs.V2, rhs.V1, rhs.V0) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V0, lhs.V2, rhs.V2, rhs.V1) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V0, lhs.V2, rhs.V0, rhs.V2)) {
+            return false;
+        }
+
+        // No axis is separating, we can safely conclude they intersect
+        return true;
+    }
+
+    bool Triangle::Intersects(const Triangle& rhs) const {
+        return Geometry::Intersects(*this, rhs);
+    }
+
     Vector3 Triangle::ExtremePoint(const Vector3 &direction) const {
         Vector3 mostExtreme = Vector3::NaN;
         float mostExtremeDist = -FLT_MAX;

tests/Geometry/Geometry.cpp

@@ -0,0 +1,51 @@
+#include <gtest/gtest.h>
+#include <J3ML/Geometry/Common.h>
+
+using J3ML::Geometry::Interval;
+
+TEST(CommonGeometry, Interval_Intersect) {
+    // <- a ->
+    //             <- b ->
+    EXPECT_EQ((Interval{0, 1}.Intersects({2, 3})), false);
+
+    //             <- a ->
+    // <- b ->
+    EXPECT_EQ((Interval{2, 3}.Intersects({0, 1})), false);
+
+    // <- a ->
+    //    <- b ->
+    EXPECT_EQ((Interval{2, 4}.Intersects({3, 5})), true);
+
+    //    <- a ->
+    // <- b ->
+    EXPECT_EQ((Interval{2, 4}.Intersects({1, 3})), true);
+
+    // <- a ->
+    //      <- b ->
+    EXPECT_EQ((Interval{2, 3}.Intersects({3, 5})), true);
+
+    //      <- a ->
+    // <- b ->
+    EXPECT_EQ((Interval{3, 5}.Intersects({2, 3})), true);
+
+    // <- a ->
+    // <- b     ->
+    EXPECT_EQ((Interval{2, 3}.Intersects({2, 5})), true);\
+
+    // <- a ->
+    // <- b ->
+    EXPECT_EQ((Interval{2, 3}.Intersects({2, 3})), true);
+
+    // . a
+    // . b
+    EXPECT_EQ((Interval{2, 2}.Intersects({2, 2})), true);
+
+    // <-    a    ->
+    //    <- b ->
+    EXPECT_EQ((Interval{2, 5}.Intersects({3, 4})), true);
+
+    //    <- a ->
+    // <-    b    ->
+    EXPECT_EQ((Interval{3, 4}.Intersects({2, 5})), true);
+}
+

tests/Geometry/TriangleTests.cpp

@@ -0,0 +1,89 @@
+#include <gtest/gtest.h>
+#include <J3ML/Geometry/Triangle.h>
+
+using J3ML::Geometry::Interval;
+using J3ML::Geometry::Triangle;
+
+TEST(TriangleTests, FaceNormal)
+{
+    Triangle t{
+        Vector3{-1, -1, -1},
+        Vector3{0,   1,  0},
+        Vector3{1,  -1,  1}
+    };
+
+    EXPECT_EQ(t.FaceNormal(), (Vector3{4, 0, -4}));
+}
+
+TEST(TriangleTests, IntersectTriangle)
+{
+    Triangle xyTriangle{
+        {0.0f, 0.0f, 0.0f},
+        {1.0f, 1.0f, 0.0f},
+        {2.0f, 0.0f, 0.0f}
+    };
+
+    // Triangle collides with itself
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle), true);
+    // Translate 1 towards x -- should collide
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(1.0f, 0.0f, 0.0f))), true);
+    // Translate 2 towards x -- should collide exactly on V1
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(2.0f, 0.0f, 0.0f))), true);
+    // Translate 2 towards negative x -- should collide exactly on V0
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(-2.0f, 0.0f, 0.0f))), true);
+    // Translate 3 towards x -- should not collide
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(3.0f, 0.0f, 0.0f))), false);
+    // Translate 3 towards negative x -- should not collide
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(-3.0f, 0.0f, 0.0f))), false);
+    // Translate 1 towards z -- should not collide
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(0.0f, 0.0f, 1.0f))), false);
+    // Triangle collides with contained smaller triangle
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Scaled(Vector3(0.5f, 0.5f, 0.5f)).Translated(Vector3(0.25f, 0.25f, 0.0f))), true);
+
+    Triangle zxTriangle {
+        {0.0f, 0.0f, 0.0f},
+        {1.0f, 0.0f, 1.0f},
+        {0.0f, 0.0f, 2.0f}
+    };
+
+    // Should collide exactly on V0
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle), true);
+    // Should collide across xyTriangle's edge and  zxTriangle's face
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(0.0f, 0.0f, -1.0))), true);
+    // Should collide exactly on V1
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(0.0f, 0.0f, -2.0))), true);
+    // xyTriangle's face should be poked by zxTriangle's V0
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(1.0f, 1.0f, 0.0f))), true);
+    // xyTriangle's face should be cut by zxTriangle
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(1.0f, 1.0f, -0.5f))), true);
+    // Should not collide
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(1.0f, 1.0f, 1.0f))), false);
+    // Should not collide
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(0.0f, 0.0f, -3.0f))), false);
+
+    Triangle yxTriangle{
+        {0.0f, 0.0f, 0.0f},
+        {1.0f, 1.0f, 0.0f},
+        {0.0f, 2.0f, 0.0f}
+    };
+
+    // Should collide on V0-V1 edge
+    EXPECT_EQ(Intersects(yxTriangle, yxTriangle), true);
+    // Should not collide
+    EXPECT_EQ(Intersects(xyTriangle, yxTriangle.Translated(Vector3(0.0f, 1.0f, 0.0f))), false);
+    // Should not collide
+    EXPECT_EQ(Intersects(yxTriangle, yxTriangle.Translated(Vector3(0.0f, 0.0f, 1.0f))), false);
+
+    Triangle zyInvertedTriangle{
+        {0.0f, 1.0f, -1.0f},
+        {0.0f, 0.0f, 0.0f},
+        {0.0f, 1.0f, 1.0f}
+    };
+    // Should collide exactly on V1
+    EXPECT_EQ(Intersects(xyTriangle, zyInvertedTriangle), true);
+    // Should not collide
+    EXPECT_EQ(Intersects(xyTriangle, zyInvertedTriangle.Translated(Vector3(0.0f, 1.0f, 0.0f))), false);
+    // Should not collide
+    EXPECT_EQ(Intersects(xyTriangle, zyInvertedTriangle.Translated(Vector3(0.25f, 0.75f, 0.0f))), false);
+}
+