Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
ClosestPoint.h
Go to the documentation of this file.
1// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
2// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
3// SPDX-License-Identifier: MIT
4
5#pragma once
6
8
9// Turn off fused multiply add instruction because it makes the equations of the form a * b - c * d inaccurate below
10JPH_PRECISE_MATH_ON
11
13namespace ClosestPoint
14{
17 inline void GetBaryCentricCoordinates(Vec3Arg inA, Vec3Arg inB, float &outU, float &outV)
18 {
19 Vec3 ab = inB - inA;
20 float denominator = ab.LengthSq();
21 if (denominator < Square(FLT_EPSILON))
22 {
23 // Degenerate line segment, fallback to points
24 if (inA.LengthSq() < inB.LengthSq())
25 {
26 // A closest
27 outU = 1.0f;
28 outV = 0.0f;
29 }
30 else
31 {
32 // B closest
33 outU = 0.0f;
34 outV = 1.0f;
35 }
36 }
37 else
38 {
39 outV = -inA.Dot(ab) / denominator;
40 outU = 1.0f - outV;
41 }
42 }
43
46 inline void GetBaryCentricCoordinates(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, float &outU, float &outV, float &outW)
47 {
48 // Taken from: Real-Time Collision Detection - Christer Ericson (Section: Barycentric Coordinates)
49 // With p = 0
50 // Adjusted to always include the shortest edge of the triangle in the calculation to improve numerical accuracy
51
52 // First calculate the three edges
53 Vec3 v0 = inB - inA;
54 Vec3 v1 = inC - inA;
55 Vec3 v2 = inC - inB;
56
57 // Make sure that the shortest edge is included in the calculation to keep the products a * b - c * d as small as possible to preserve accuracy
58 float d00 = v0.Dot(v0);
59 float d11 = v1.Dot(v1);
60 float d22 = v2.Dot(v2);
61 if (d00 <= d22)
62 {
63 // Use v0 and v1 to calculate barycentric coordinates
64 float d01 = v0.Dot(v1);
65
66 float denominator = d00 * d11 - d01 * d01;
67 if (abs(denominator) < FLT_EPSILON)
68 {
69 // Degenerate triangle, return coordinates along longest edge
70 if (d00 > d11)
71 {
72 GetBaryCentricCoordinates(inA, inB, outU, outV);
73 outW = 0.0f;
74 }
75 else
76 {
77 GetBaryCentricCoordinates(inA, inC, outU, outW);
78 outV = 0.0f;
79 }
80 }
81 else
82 {
83 float a0 = inA.Dot(v0);
84 float a1 = inA.Dot(v1);
85 outV = (d01 * a1 - d11 * a0) / denominator;
86 outW = (d01 * a0 - d00 * a1) / denominator;
87 outU = 1.0f - outV - outW;
88 }
89 }
90 else
91 {
92 // Use v1 and v2 to calculate barycentric coordinates
93 float d12 = v1.Dot(v2);
94
95 float denominator = d11 * d22 - d12 * d12;
96 if (abs(denominator) < FLT_EPSILON)
97 {
98 // Degenerate triangle, return coordinates along longest edge
99 if (d11 > d22)
100 {
101 GetBaryCentricCoordinates(inA, inC, outU, outW);
102 outV = 0.0f;
103 }
104 else
105 {
106 GetBaryCentricCoordinates(inB, inC, outV, outW);
107 outU = 0.0f;
108 }
109 }
110 else
111 {
112 float c1 = inC.Dot(v1);
113 float c2 = inC.Dot(v2);
114 outU = (d22 * c1 - d12 * c2) / denominator;
115 outV = (d11 * c2 - d12 * c1) / denominator;
116 outW = 1.0f - outU - outV;
117 }
118 }
119 }
120
124 {
125 float u, v;
126 GetBaryCentricCoordinates(inA, inB, u, v);
127 if (v <= 0.0f)
128 {
129 // inA is closest point
130 outSet = 0b0001;
131 return inA;
132 }
133 else if (u <= 0.0f)
134 {
135 // inB is closest point
136 outSet = 0b0010;
137 return inB;
138 }
139 else
140 {
141 // Closest point lies on line inA inB
142 outSet = 0b0011;
143 return u * inA + v * inB;
144 }
145 }
146
150 template <bool MustIncludeC = false>
152 {
153 // Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Triangle to Point)
154 // With p = 0
155
156 // The most accurate normal is calculated by using the two shortest edges
157 // See: https://box2d.org/posts/2014/01/troublesome-triangle/
158 // The difference in normals is most pronounced when one edge is much smaller than the others (in which case the other 2 must have roughly the same length).
159 // Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal.
160 // We first check which of the edges is shorter and if bc is shorter than ac then we swap a with c to a is always on the shortest edge
161 UVec4 swap_ac;
162 {
163 Vec3 ac = inC - inA;
164 Vec3 bc = inC - inB;
165 swap_ac = Vec4::sLess(bc.DotV4(bc), ac.DotV4(ac));
166 }
167 Vec3 a = Vec3::sSelect(inA, inC, swap_ac);
168 Vec3 c = Vec3::sSelect(inC, inA, swap_ac);
169
170 // Calculate normal
171 Vec3 ab = inB - a;
172 Vec3 ac = c - a;
173 Vec3 n = ab.Cross(ac);
174 float n_len_sq = n.LengthSq();
175
176 // Check degenerate
177 if (n_len_sq < 1.0e-11f) // Square(FLT_EPSILON) was too small and caused numerical problems, see test case TestCollideParallelTriangleVsCapsule
178 {
179 // Degenerate, fallback to vertices and edges
180
181 // Start with vertex C being the closest
182 uint32 closest_set = 0b0100;
183 Vec3 closest_point = inC;
184 float best_dist_sq = inC.LengthSq();
185
186 // If the closest point must include C then A or B cannot be closest
187 if constexpr (!MustIncludeC)
188 {
189 // Try vertex A
190 float a_len_sq = inA.LengthSq();
191 if (a_len_sq < best_dist_sq)
192 {
193 closest_set = 0b0001;
194 closest_point = inA;
195 best_dist_sq = a_len_sq;
196 }
197
198 // Try vertex B
199 float b_len_sq = inB.LengthSq();
200 if (b_len_sq < best_dist_sq)
201 {
202 closest_set = 0b0010;
203 closest_point = inB;
204 best_dist_sq = b_len_sq;
205 }
206
207 // Edge AB
208 float ab_len_sq = ab.LengthSq();
209 if (ab_len_sq > Square(FLT_EPSILON))
210 {
211 float v = Clamp(-a.Dot(ab) / ab_len_sq, 0.0f, 1.0f);
212 Vec3 q = a + v * ab;
213 float dist_sq = q.LengthSq();
214 if (dist_sq < best_dist_sq)
215 {
216 closest_set = swap_ac.GetX()? 0b0110 : 0b0011;
217 closest_point = q;
218 best_dist_sq = dist_sq;
219 }
220 }
221 }
222
223 // Edge AC
224 float ac_len_sq = ac.LengthSq();
225 if (ac_len_sq > Square(FLT_EPSILON))
226 {
227 float v = Clamp(-a.Dot(ac) / ac_len_sq, 0.0f, 1.0f);
228 Vec3 q = a + v * ac;
229 float dist_sq = q.LengthSq();
230 if (dist_sq < best_dist_sq)
231 {
232 closest_set = 0b0101;
233 closest_point = q;
234 best_dist_sq = dist_sq;
235 }
236 }
237
238 // Edge BC
239 Vec3 bc = c - inB;
240 float bc_len_sq = bc.LengthSq();
241 if (bc_len_sq > Square(FLT_EPSILON))
242 {
243 float v = Clamp(-inB.Dot(bc) / bc_len_sq, 0.0f, 1.0f);
244 Vec3 q = inB + v * bc;
245 float dist_sq = q.LengthSq();
246 if (dist_sq < best_dist_sq)
247 {
248 closest_set = swap_ac.GetX()? 0b0011 : 0b0110;
249 closest_point = q;
250 best_dist_sq = dist_sq;
251 }
252 }
253
254 outSet = closest_set;
255 return closest_point;
256 }
257
258 // Check if P in vertex region outside A
259 Vec3 ap = -a;
260 float d1 = ab.Dot(ap);
261 float d2 = ac.Dot(ap);
262 if (d1 <= 0.0f && d2 <= 0.0f)
263 {
264 outSet = swap_ac.GetX()? 0b0100 : 0b0001;
265 return a; // barycentric coordinates (1,0,0)
266 }
267
268 // Check if P in vertex region outside B
269 Vec3 bp = -inB;
270 float d3 = ab.Dot(bp);
271 float d4 = ac.Dot(bp);
272 if (d3 >= 0.0f && d4 <= d3)
273 {
274 outSet = 0b0010;
275 return inB; // barycentric coordinates (0,1,0)
276 }
277
278 // Check if P in edge region of AB, if so return projection of P onto AB
279 if (d1 * d4 <= d3 * d2 && d1 >= 0.0f && d3 <= 0.0f)
280 {
281 float v = d1 / (d1 - d3);
282 outSet = swap_ac.GetX()? 0b0110 : 0b0011;
283 return a + v * ab; // barycentric coordinates (1-v,v,0)
284 }
285
286 // Check if P in vertex region outside C
287 Vec3 cp = -c;
288 float d5 = ab.Dot(cp);
289 float d6 = ac.Dot(cp);
290 if (d6 >= 0.0f && d5 <= d6)
291 {
292 outSet = swap_ac.GetX()? 0b0001 : 0b0100;
293 return c; // barycentric coordinates (0,0,1)
294 }
295
296 // Check if P in edge region of AC, if so return projection of P onto AC
297 if (d5 * d2 <= d1 * d6 && d2 >= 0.0f && d6 <= 0.0f)
298 {
299 float w = d2 / (d2 - d6);
300 outSet = 0b0101;
301 return a + w * ac; // barycentric coordinates (1-w,0,w)
302 }
303
304 // Check if P in edge region of BC, if so return projection of P onto BC
305 float d4_d3 = d4 - d3;
306 float d5_d6 = d5 - d6;
307 if (d3 * d6 <= d5 * d4 && d4_d3 >= 0.0f && d5_d6 >= 0.0f)
308 {
309 float w = d4_d3 / (d4_d3 + d5_d6);
310 outSet = swap_ac.GetX()? 0b0011 : 0b0110;
311 return inB + w * (c - inB); // barycentric coordinates (0,1-w,w)
312 }
313
314 // P inside face region.
315 // Here we deviate from Christer Ericson's article to improve accuracy.
316 // Determine distance between triangle and origin: distance = (centroid - origin) . normal / |normal|
317 // Closest point to origin is then: distance . normal / |normal|
318 // Note that this way of calculating the closest point is much more accurate than first calculating barycentric coordinates
319 // and then calculating the closest point based on those coordinates.
320 outSet = 0b0111;
321 return n * (a + inB + c).Dot(n) / (3.0f * n_len_sq);
322 }
323
325 inline bool OriginOutsideOfPlane(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD)
326 {
327 // Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Tetrahedron to Point)
328 // With p = 0
329
330 // Test if point p and d lie on opposite sides of plane through abc
331 Vec3 n = (inB - inA).Cross(inC - inA);
332 float signp = inA.Dot(n); // [AP AB AC]
333 float signd = (inD - inA).Dot(n); // [AD AB AC]
334
335 // Points on opposite sides if expression signs are the same
336 // Note that we left out the minus sign in signp so we need to check > 0 instead of < 0 as in Christer's book
337 // We compare against a small negative value to allow for a little bit of slop in the calculations
338 return signp * signd > -FLT_EPSILON;
339 }
340
348 {
349 Vec3 ab = inB - inA;
350 Vec3 ac = inC - inA;
351 Vec3 ad = inD - inA;
352 Vec3 bd = inD - inB;
353 Vec3 bc = inC - inB;
354
355 Vec3 ab_cross_ac = ab.Cross(ac);
356 Vec3 ac_cross_ad = ac.Cross(ad);
357 Vec3 ad_cross_ab = ad.Cross(ab);
358 Vec3 bd_cross_bc = bd.Cross(bc);
359
360 // For each plane get the side on which the origin is
361 float signp0 = inA.Dot(ab_cross_ac); // ABC
362 float signp1 = inA.Dot(ac_cross_ad); // ACD
363 float signp2 = inA.Dot(ad_cross_ab); // ADB
364 float signp3 = inB.Dot(bd_cross_bc); // BDC
365 Vec4 signp(signp0, signp1, signp2, signp3);
366
367 // For each plane get the side that is outside (determined by the 4th point)
368 float signd0 = ad.Dot(ab_cross_ac); // D
369 float signd1 = ab.Dot(ac_cross_ad); // B
370 float signd2 = ac.Dot(ad_cross_ab); // C
371 float signd3 = -ab.Dot(bd_cross_bc); // A
372 Vec4 signd(signd0, signd1, signd2, signd3);
373
374 // The winding of all triangles has been chosen so that signd should have the
375 // same sign for all components. If this is not the case the tetrahedron
376 // is degenerate and we return that the origin is in front of all sides
377 int sign_bits = signd.GetSignBits();
378 switch (sign_bits)
379 {
380 case 0:
381 // All positive
382 return Vec4::sGreaterOrEqual(signp, Vec4::sReplicate(-FLT_EPSILON));
383
384 case 0xf:
385 // All negative
386 return Vec4::sLessOrEqual(signp, Vec4::sReplicate(FLT_EPSILON));
387
388 default:
389 // Mixed signs, degenerate tetrahedron
390 return UVec4::sReplicate(0xffffffff);
391 }
392 }
393
397 template <bool MustIncludeD = false>
399 {
400 // Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Tetrahedron to Point)
401 // With p = 0
402
403 // Start out assuming point inside all halfspaces, so closest to itself
404 uint32 closest_set = 0b1111;
405 Vec3 closest_point = Vec3::sZero();
406 float best_dist_sq = FLT_MAX;
407
408 // Determine for each of the faces of the tetrahedron if the origin is in front of the plane
409 UVec4 origin_out_of_planes = OriginOutsideOfTetrahedronPlanes(inA, inB, inC, inD);
410
411 // If point outside face abc then compute closest point on abc
412 if (origin_out_of_planes.GetX()) // OriginOutsideOfPlane(inA, inB, inC, inD)
413 {
414 if constexpr (MustIncludeD)
415 {
416 // If the closest point must include D then ABC cannot be closest but the closest point
417 // cannot be an interior point either so we return A as closest point
418 closest_set = 0b0001;
419 closest_point = inA;
420 }
421 else
422 {
423 // Test the face normally
424 closest_point = GetClosestPointOnTriangle<false>(inA, inB, inC, closest_set);
425 }
426 best_dist_sq = closest_point.LengthSq();
427 }
428
429 // Repeat test for face acd
430 if (origin_out_of_planes.GetY()) // OriginOutsideOfPlane(inA, inC, inD, inB)
431 {
432 uint32 set;
433 Vec3 q = GetClosestPointOnTriangle<MustIncludeD>(inA, inC, inD, set);
434 float dist_sq = q.LengthSq();
435 if (dist_sq < best_dist_sq)
436 {
437 best_dist_sq = dist_sq;
438 closest_point = q;
439 closest_set = (set & 0b0001) + ((set & 0b0110) << 1);
440 }
441 }
442
443 // Repeat test for face adb
444 if (origin_out_of_planes.GetZ()) // OriginOutsideOfPlane(inA, inD, inB, inC)
445 {
446 // Keep original vertex order, it doesn't matter if the triangle is facing inward or outward
447 // and it improves consistency for GJK which will always add a new vertex D and keep the closest
448 // feature from the previous iteration in ABC
449 uint32 set;
450 Vec3 q = GetClosestPointOnTriangle<MustIncludeD>(inA, inB, inD, set);
451 float dist_sq = q.LengthSq();
452 if (dist_sq < best_dist_sq)
453 {
454 best_dist_sq = dist_sq;
455 closest_point = q;
456 closest_set = (set & 0b0011) + ((set & 0b0100) << 1);
457 }
458 }
459
460 // Repeat test for face bdc
461 if (origin_out_of_planes.GetW()) // OriginOutsideOfPlane(inB, inD, inC, inA)
462 {
463 // Keep original vertex order, it doesn't matter if the triangle is facing inward or outward
464 // and it improves consistency for GJK which will always add a new vertex D and keep the closest
465 // feature from the previous iteration in ABC
466 uint32 set;
467 Vec3 q = GetClosestPointOnTriangle<MustIncludeD>(inB, inC, inD, set);
468 float dist_sq = q.LengthSq();
469 if (dist_sq < best_dist_sq)
470 {
471 closest_point = q;
472 closest_set = set << 1;
473 }
474 }
475
476 outSet = closest_set;
477 return closest_point;
478 }
479};
480
481JPH_PRECISE_MATH_OFF
482
uint32_t uint32
Definition Core.h:312
#define JPH_NAMESPACE_END
Definition Core.h:240
#define JPH_NAMESPACE_BEGIN
Definition Core.h:234
constexpr T Clamp(T inV, T inMin, T inMax)
Clamp a value between two values.
Definition Math.h:45
constexpr T Square(T inV)
Square a value.
Definition Math.h:52
Definition UVec4.h:12
JPH_INLINE uint32 GetZ() const
Definition UVec4.h:103
JPH_INLINE uint32 GetY() const
Definition UVec4.h:102
static JPH_INLINE UVec4 sReplicate(uint32 inV)
Replicate int inV across all components.
Definition UVec4.inl:56
JPH_INLINE uint32 GetW() const
Definition UVec4.h:104
JPH_INLINE uint32 GetX() const
Get individual components.
Definition UVec4.h:101
Definition Vec3.h:16
JPH_INLINE float Dot(Vec3Arg inV2) const
Dot product.
Definition Vec3.inl:637
JPH_INLINE Vec3 Cross(Vec3Arg inV2) const
Cross product.
Definition Vec3.inl:582
JPH_INLINE Vec4 DotV4(Vec3Arg inV2) const
Dot product, returns the dot product in X, Y, Z and W components.
Definition Vec3.inl:621
static JPH_INLINE Vec3 sSelect(Vec3Arg inV1, Vec3Arg inV2, UVec4Arg inControl)
Component wise select, returns inV1 when highest bit of inControl = 0 and inV2 when highest bit of in...
Definition Vec3.inl:269
JPH_INLINE float LengthSq() const
Squared length of vector.
Definition Vec3.inl:653
static JPH_INLINE Vec3 sZero()
Vector with all zeros.
Definition Vec3.inl:107
Definition Vec4.h:14
static JPH_INLINE UVec4 sLessOrEqual(Vec4Arg inV1, Vec4Arg inV2)
Less than or equal (component wise)
Definition Vec4.inl:194
static JPH_INLINE UVec4 sLess(Vec4Arg inV1, Vec4Arg inV2)
Less than (component wise)
Definition Vec4.inl:180
static JPH_INLINE UVec4 sGreaterOrEqual(Vec4Arg inV1, Vec4Arg inV2)
Greater than or equal (component wise)
Definition Vec4.inl:222
JPH_INLINE int GetSignBits() const
Store if X is negative in bit 0, Y in bit 1, Z in bit 2 and W in bit 3.
Definition Vec4.inl:746
static JPH_INLINE Vec4 sReplicate(float inV)
Replicate inV across all components.
Definition Vec4.inl:74
Helper utils to find the closest point to a line segment, triangle or tetrahedron.
Definition ClosestPoint.h:14
Vec3 GetClosestPointOnTriangle(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, uint32 &outSet)
Definition ClosestPoint.h:151
void GetBaryCentricCoordinates(Vec3Arg inA, Vec3Arg inB, float &outU, float &outV)
Definition ClosestPoint.h:17
UVec4 OriginOutsideOfTetrahedronPlanes(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD)
Definition ClosestPoint.h:347
Vec3 GetClosestPointOnTetrahedron(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD, uint32 &outSet)
Definition ClosestPoint.h:398
bool OriginOutsideOfPlane(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD)
Check if the origin is outside the plane of triangle (inA, inB, inC). inD specifies the front side of...
Definition ClosestPoint.h:325
Vec3 GetClosestPointOnLine(Vec3Arg inA, Vec3Arg inB, uint32 &outSet)
Definition ClosestPoint.h:123