From 48300150600e05d618d06670f97761b793603ffa Mon Sep 17 00:00:00 2001 From: Mishura Date: Wed, 8 May 2024 09:23:20 -0400 Subject: [PATCH] Add triangle SAT intersection --- include/J3ML/Geometry/Common.h | 8 +++ include/J3ML/Geometry/Triangle.h | 14 +++- src/J3ML/Geometry/Common.cpp | 10 +++ src/J3ML/Geometry/Triangle.cpp | 114 ++++++++++++++++++++++++++++++- tests/Geometry/Geometry.cpp | 51 ++++++++++++++ tests/Geometry/TriangleTests.cpp | 89 ++++++++++++++++++++++++ 6 files changed, 284 insertions(+), 2 deletions(-) create mode 100644 src/J3ML/Geometry/Common.cpp create mode 100644 tests/Geometry/Geometry.cpp create mode 100644 tests/Geometry/TriangleTests.cpp diff --git a/include/J3ML/Geometry/Common.h b/include/J3ML/Geometry/Common.h index 53508a2..e687d8e 100644 --- a/include/J3ML/Geometry/Common.h +++ b/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; + }; } \ No newline at end of file diff --git a/include/J3ML/Geometry/Triangle.h b/include/J3ML/Geometry/Triangle.h index ae61052..83007f0 100644 --- a/include/J3ML/Geometry/Triangle.h +++ b/include/J3ML/Geometry/Triangle.h @@ -1,5 +1,6 @@ #pragma once +#include "J3ML/LinearAlgebra/Vector3.h" #include #include #include @@ -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; diff --git a/src/J3ML/Geometry/Common.cpp b/src/J3ML/Geometry/Common.cpp new file mode 100644 index 0000000..1a37fc3 --- /dev/null +++ b/src/J3ML/Geometry/Common.cpp @@ -0,0 +1,10 @@ +#include + +namespace J3ML::Geometry { + + bool Interval::Intersects(const Interval& rhs) const { + return *this == rhs || this->min > rhs.max != this->max >= rhs.min; + } + +} + diff --git a/src/J3ML/Geometry/Triangle.cpp b/src/J3ML/Geometry/Triangle.cpp index 91a775a..51bbc0f 100644 --- a/src/J3ML/Geometry/Triangle.cpp +++ b/src/J3ML/Geometry/Triangle.cpp @@ -5,9 +5,42 @@ #include #include - 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; diff --git a/tests/Geometry/Geometry.cpp b/tests/Geometry/Geometry.cpp new file mode 100644 index 0000000..b226412 --- /dev/null +++ b/tests/Geometry/Geometry.cpp @@ -0,0 +1,51 @@ +#include +#include + +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); +} + diff --git a/tests/Geometry/TriangleTests.cpp b/tests/Geometry/TriangleTests.cpp new file mode 100644 index 0000000..0858d91 --- /dev/null +++ b/tests/Geometry/TriangleTests.cpp @@ -0,0 +1,89 @@ +#include +#include + +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); +} +