Skip to content

Commit 2bd2a2a

Browse files
Sploder12Sploder12
committed
distance GJK
it works! I hope.
1 parent f3fd80c commit 2bd2a2a

4 files changed

Lines changed: 242 additions & 2 deletions

File tree

‎src/include/sndx/collision/gjk.hpp‎

Lines changed: 174 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,51 @@ namespace sndx::collision {
5353
size = std::min(uint8_t(size + 1u), uint8_t(points.size()));
5454
}
5555

56+
auto pointClosest() const {
57+
return points[0].out;
58+
}
59+
60+
auto lineClosest() const {
61+
auto ab = points[1].out - points[0].out;
62+
auto abSqr = glm::dot(ab, ab);
63+
if (abSqr < 0.0000001f) { // degenerate case
64+
return points[0].out;
65+
}
66+
67+
auto t = glm::dot(-points[0].out, ab) / abSqr;
68+
return points[0].out + glm::clamp(t, 0.0f, 1.0f) * points[1].out;
69+
}
70+
71+
auto triangleClosest() const {
72+
// degenerate cases
73+
if (points[0].out == points[1].out) {
74+
SimplexGJK tmp{};
75+
tmp.push_front(points[0]);
76+
tmp.push_front(points[2]);
77+
return tmp.lineClosest();
78+
}
79+
else if (points[0].out == points[2].out) {
80+
return lineClosest();
81+
}
82+
else if (points[1].out == points[2].out) {
83+
return lineClosest();
84+
}
85+
86+
sndx::collision::Tri<Vec> tri{ points[0].out, points[1].out, points[2].out };
87+
return tri.closestPoint(glm::vec3{ 0.0f });
88+
}
89+
90+
[[nodiscard]]
91+
Vec gjkClosest() const {
92+
switch (size) {
93+
case 1: return pointClosest();
94+
case 2: return lineClosest();
95+
case 3: return triangleClosest();
96+
default:
97+
throw std::logic_error("GJK had weird number of points in simplex");
98+
}
99+
}
100+
56101
bool lineOrigin(Vec& newDirection) {
57102
auto ab = points[1].out - points[0].out;
58103
auto ao = -points[0].out;
@@ -203,6 +248,135 @@ namespace sndx::collision {
203248
return std::nullopt;
204249
}
205250

251+
struct ResDistGJK {
252+
bool hit = false; // hit IS NOT TO BE TRUSTED
253+
glm::vec3 a{}, b{};
254+
};
255+
256+
// if the two shapes ARE colliding this has a high chance of failure.
257+
// use gjk to ensure no collision before using this.
258+
template <class SFnA, class SFnB> [[nodiscard]]
259+
ResDistGJK gjkDist(const SFnA& supportA, const SFnB& supportB) {
260+
auto support = detail::gjkMinkowski(supportA, supportB, glm::vec3(1.0, 0.0, 0.0));
261+
262+
// force a triangle so we don't have to deal with lines or points later
263+
SimplexGJK simplex{};
264+
simplex.push_front(support);
265+
simplex.push_front(detail::gjkMinkowski(supportA, supportB, -support.out));
266+
267+
auto dir = -simplex.gjkClosest();
268+
if (glm::length(dir) <= 0.000001f) { // we hit the origin!
269+
return ResDistGJK{ true };
270+
}
271+
simplex.push_front(detail::gjkMinkowski(supportA, supportB, dir));
272+
273+
size_t iterations = 0;
274+
while (true) {
275+
dir = -simplex.gjkClosest();
276+
auto dirMag = glm::length(dir);
277+
if (dirMag <= 0.000001f) { // we hit the origin!
278+
return ResDistGJK{ true };
279+
}
280+
281+
support = detail::gjkMinkowski(supportA, supportB, dir);
282+
283+
auto alignment = glm::dot(support.out, dir);
284+
auto old = glm::dot(simplex.points[0].out, dir);
285+
286+
// check making progress
287+
if (alignment - old < 0.000001f) {
288+
break;
289+
}
290+
291+
auto newAB = simplex;
292+
newAB.points[2] = support;
293+
294+
auto newAC = simplex;
295+
newAC.points[1] = support;
296+
297+
auto newBC = simplex;
298+
newBC.points[0] = support;
299+
300+
auto pAB = newAB.gjkClosest();
301+
auto pAC = newAC.gjkClosest();
302+
auto pBC = newBC.gjkClosest();
303+
304+
auto abLen = glm::length(pAB);
305+
auto acLen = glm::length(pAC);
306+
auto bcLen = glm::length(pBC);
307+
308+
if (abLen < acLen) {
309+
if (abLen < bcLen) {
310+
simplex = newAB;
311+
dir = -pAB;
312+
}
313+
else {
314+
simplex = newBC;
315+
dir = -pBC;
316+
}
317+
}
318+
else {
319+
if (acLen < bcLen) {
320+
simplex = newAC;
321+
dir = -pAC;
322+
}
323+
else {
324+
simplex = newBC;
325+
dir = -pBC;
326+
}
327+
}
328+
329+
++iterations;
330+
}
331+
332+
ResDistGJK out{ false };
333+
334+
// degenerate cases
335+
if (simplex.points[0].out == simplex.points[1].out) { // A == B
336+
if (simplex.points[0].out == simplex.points[2].out) { // point
337+
out.a = simplex.points[0].a;
338+
out.b = simplex.points[0].b;
339+
return out;
340+
}
341+
342+
// line AC
343+
auto ac = simplex.points[2].out - simplex.points[0].out;
344+
auto t = glm::dot(-simplex.points[0].out, ac) / glm::dot(ac, ac);
345+
t = glm::clamp(t, 0.0f, 1.0f);
346+
out.a = simplex.points[0].a + t * simplex.points[2].a;
347+
out.b = simplex.points[0].b + t * simplex.points[2].b;
348+
return out;
349+
}
350+
else if (simplex.points[0].out == simplex.points[2].out) { // A == C
351+
// line BC
352+
auto bc = simplex.points[2].out - simplex.points[1].out;
353+
auto t = glm::dot(-simplex.points[1].out, bc) / glm::dot(bc, bc);
354+
t = glm::clamp(t, 0.0f, 1.0f);
355+
out.a = simplex.points[1].a + t * simplex.points[2].a;
356+
out.b = simplex.points[1].b + t * simplex.points[2].b;
357+
return out;
358+
}
359+
else if (simplex.points[1].out == simplex.points[2].out) { // B == C
360+
// line AB
361+
auto ab = simplex.points[1].out - simplex.points[0].out;
362+
auto t = glm::dot(-simplex.points[0].out, ab) / glm::dot(ab, ab);
363+
t = glm::clamp(t, 0.0f, 1.0f);
364+
out.a = simplex.points[0].a + t * simplex.points[1].a;
365+
out.b = simplex.points[0].b + t * simplex.points[1].b;
366+
return out;
367+
}
368+
369+
sndx::collision::Tri3D tri{ simplex.points[0].out, simplex.points[1].out, simplex.points[2].out };
370+
sndx::collision::Tri3D triA{ simplex.points[0].a, simplex.points[1].a, simplex.points[2].a };
371+
sndx::collision::Tri3D triB{ simplex.points[0].b, simplex.points[1].b, simplex.points[2].b };
372+
373+
auto uvw = tri.uvw(glm::vec3{ 0.0f });
374+
out.a = triA.fromUVW(uvw);
375+
out.b = triB.fromUVW(uvw);
376+
377+
return out;
378+
}
379+
206380
struct EpaResult {
207381
glm::vec3 normal;
208382
float depth;

‎src/include/sndx/collision/rect.hpp‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -195,6 +195,11 @@ namespace sndx::collision {
195195
return this->contains(Rect{ other });
196196
}
197197

198+
[[nodiscard]]
199+
constexpr Vec closestPoint(const Vec& point) const noexcept {
200+
return glm::clamp(point, getP1(), getP2());
201+
}
202+
198203
// get the signed distance from a point
199204
// adapted from https://iquilezles.org/articles/distfunctions/
200205
[[nodiscard]]
@@ -278,6 +283,20 @@ namespace sndx::collision {
278283
out.rect = this;
279284
return out;
280285
}
286+
287+
[[nodiscard]]
288+
constexpr RaycastResult spherecast(const Vec& from, const Vec& dir, Precision radius, bool cull = false) const noexcept {
289+
Rect expanded{ getP1() - Vec{radius}, getP2() + Vec{radius} };
290+
auto res = expanded.raycast(from, dir, cull);
291+
292+
if (res.hit()) {
293+
auto spherePos = from + dir * res.distance();
294+
auto closest = closestPoint(spherePos);
295+
res.setNormal(glm::normalize(spherePos - closest));
296+
}
297+
298+
return res;
299+
}
281300
};
282301

283302
template <size_t n, typename InternalT = float, glm::qualifier Qualifier = glm::qualifier::defaultp>

‎src/include/sndx/collision/triangle.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -24,15 +24,15 @@ namespace sndx::collision {
2424
constexpr Tri(const Vec& p1, const Vec& p2, const Vec& p3) noexcept :
2525
m_p1(p1), m_p2(p2), m_p3(p3) {}
2626

27-
constexpr const Tri& translate(const Vec& vec) const noexcept {
27+
constexpr Tri& translate(const Vec& vec) noexcept {
2828
m_p1 += vec;
2929
m_p2 += vec;
3030
m_p3 += vec;
3131

3232
return *this;
3333
}
3434

35-
constexpr const Tri& flipNormal() const noexcept {
35+
constexpr Tri& flipNormal() noexcept {
3636
std::swap(m_p2, m_p3);
3737
return *this;
3838
}

‎src/tests/collision/gjk.cpp‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -97,6 +97,53 @@ TEST(GJK, circleAndTriangleCollide) {
9797
EXPECT_TRUE(result);
9898
}
9999

100+
// because EXPECT_FLOAT_EQ is WAYYYYY too strict.
101+
bool floatEq(float a, float b) {
102+
return std::abs(a - b) <= 0.000001f;
103+
}
104+
105+
TEST(Dist_GJK, simpleBoxesGiveDist) {
106+
Rect3D boxA{ glm::vec3{-0.5f}, glm::vec3{0.1f} };
107+
Rect3D boxB{ glm::vec3{0.11f}, glm::vec3{0.5f} };
108+
109+
auto result = gjkDist(getSupportFn(boxA), getSupportFn(boxB));
110+
EXPECT_FALSE(result.hit);
111+
EXPECT_TRUE(boxA.contains(result.a));
112+
EXPECT_TRUE(boxB.contains(result.b));
113+
114+
auto dist = glm::distance(result.a, result.b);
115+
EXPECT_TRUE(floatEq(dist, 0.01732f));
116+
117+
result = gjkDist(getSupportFn(boxB), getSupportFn(boxA));
118+
EXPECT_FALSE(result.hit);
119+
EXPECT_TRUE(boxB.contains(result.a));
120+
EXPECT_TRUE(boxA.contains(result.b));
121+
122+
dist = glm::distance(result.a, result.b);
123+
EXPECT_TRUE(floatEq(dist, 0.01732f));
124+
}
125+
126+
TEST(Dist_GJK, axisAlignedSpheresGiveDist) {
127+
Circle3D circleA{ glm::vec3{-1.0f, 0.0f, 0.0f}, 1.0f };
128+
Circle3D circleB{ glm::vec3{-3.0f, 0.0f, 0.0f}, 0.5f };
129+
130+
auto result = gjkDist(getSupportFn(circleA), getSupportFn(circleB));
131+
EXPECT_FALSE(result.hit);
132+
EXPECT_TRUE(circleA.contains(result.a));
133+
EXPECT_TRUE(circleB.contains(result.b));
134+
135+
auto dist = glm::distance(result.a, result.b);
136+
EXPECT_TRUE(floatEq(dist, 0.5f));
137+
138+
result = gjkDist(getSupportFn(circleB), getSupportFn(circleA));
139+
EXPECT_FALSE(result.hit);
140+
EXPECT_TRUE(circleB.contains(result.a));
141+
EXPECT_TRUE(circleA.contains(result.b));
142+
143+
dist = glm::distance(result.a, result.b);
144+
EXPECT_TRUE(floatEq(dist, 0.5f));
145+
}
146+
100147
/*
101148
TEST(EPA, simpleBoxesGiveCorrectDirection) {
102149
Rect3D boxA{ glm::vec3{-1.0f}, glm::vec3{0.0f} };

0 commit comments

Comments
 (0)