DOLFINx 0.12.0.0
DOLFINx C++
Loading...
Searching...
No Matches
gjk.h
1// Copyright (C) 2020-2026 Chris Richardson and Jørgen S. Dokken
2// Copyright (C) 2026 Garth N. Wells
3//
4// This file is part of DOLFINx (https://www.fenicsproject.org)
5//
6// SPDX-License-Identifier: LGPL-3.0-or-later
7
8#pragma once
9
10#include <algorithm>
11#include <array>
12#include <boost/multiprecision/cpp_bin_float.hpp>
13#include <cmath>
14#include <concepts>
15#include <dolfinx/common/local_range.h>
16#include <limits>
17#include <numeric>
18#include <span>
19#include <spdlog/spdlog.h>
20#include <stdexcept>
21#include <thread>
22#include <utility>
23#include <vector>
24
25namespace dolfinx::geometry
26{
27
28namespace impl_gjk
29{
30
37template <typename T>
38inline std::array<T, 4> det4(const std::array<T, 12>& s)
39{
40 std::span<const T, 3> s0(s.begin(), 3);
41 std::span<const T, 3> s1(s.begin() + 3, 3);
42 std::span<const T, 3> s2(s.begin() + 6, 3);
43 std::span<const T, 3> s3(s.begin() + 9, 3);
44
45 std::array<T, 4> w;
46 T c0 = s2[1] * s3[2] - s2[2] * s3[1];
47 T c1 = s2[0] * s3[2] - s2[2] * s3[0];
48 T c2 = s2[0] * s3[1] - s2[1] * s3[0];
49 w[2] = -s0[0] * c0 + s0[1] * c1 - s0[2] * c2;
50 w[3] = s1[0] * c0 - s1[1] * c1 + s1[2] * c2;
51
52 c0 = s0[1] * s1[2] - s0[2] * s1[1];
53 c1 = s0[0] * s1[2] - s0[2] * s1[0];
54 c2 = s0[0] * s1[1] - s0[1] * s1[0];
55 w[0] = -s2[0] * c0 + s2[1] * c1 - s2[2] * c2;
56 w[1] = s3[0] * c0 - s3[1] * c1 + s3[2] * c2;
57
58 return w;
59}
60
65template <typename Vec>
66inline Vec::value_type dot3(const Vec& a, const Vec& b)
67{
68 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
69}
70
79template <typename T, std::size_t simplex_size>
80void nearest_simplex(const std::array<T, 12>& s, std::array<T, 4>& coordinates)
81{
82
83 SPDLOG_DEBUG("GJK: nearest_simplex({})", simplex_size);
84
85 if constexpr (simplex_size == 2)
86 {
87 // Simplex is an interval. Point may lie on the interval, or on either end.
88 // Compute lm = dot(s0, ds / |ds|)
89 std::span<const T, 3> s0(s.data(), 3);
90 std::span<const T, 3> s1(s.data() + 3, 3);
91
92 T lm = dot3(s0, s0) - dot3(s0, s1);
93 if (lm < 0.0)
94 {
95 SPDLOG_DEBUG("GJK: line point A");
96
97 coordinates[0] = 1.0;
98 coordinates[1] = 0.0;
99 return;
100 }
101 T mu = dot3(s1, s1) - dot3(s1, s0);
102 if (mu < 0.0)
103 {
104 SPDLOG_DEBUG("GJK: line point B");
105 coordinates[0] = 0.0;
106 coordinates[1] = 1.0;
107 return;
108 }
109
110 SPDLOG_DEBUG("GJK line: AB");
111 T f1 = 1.0 / (lm + mu);
112 coordinates[0] = mu * f1;
113 coordinates[1] = lm * f1;
114 return;
115 }
116 else if constexpr (simplex_size == 3)
117 {
118 // Simplex is a triangle. Point may lie in one of 7 regions (outside near a
119 // vertex, outside near an edge, or on the interior)
120 std::span<const T, 3> a(s.data(), 3);
121 std::span<const T, 3> b(s.data() + 3, 3);
122 std::span<const T, 3> c(s.data() + 6, 3);
123
124 T aa = dot3(a, a);
125 T ab = dot3(a, b);
126 T ac = dot3(a, c);
127 T d1 = aa - ab;
128 T d2 = aa - ac;
129 if (d1 < 0.0 and d2 < 0.0)
130 {
131 SPDLOG_DEBUG("GJK: Point A");
132 coordinates[0] = 1.0;
133 coordinates[1] = 0.0;
134 coordinates[2] = 0.0;
135 return;
136 }
137
138 T bb = dot3(b, b);
139 T bc = dot3(b, c);
140 T d3 = bb - ab;
141 T d4 = bb - bc;
142 if (d3 < 0.0 and d4 < 0.0)
143 {
144 SPDLOG_DEBUG("GJK: Point B");
145 coordinates[0] = 0.0;
146 coordinates[1] = 1.0;
147 coordinates[2] = 0.0;
148 return;
149 }
150
151 T cc = dot3(c, c);
152 T d5 = cc - ac;
153 T d6 = cc - bc;
154 if (d5 < 0.0 and d6 < 0.0)
155 {
156 SPDLOG_DEBUG("GJK: Point C");
157 coordinates[0] = 0.0;
158 coordinates[1] = 0.0;
159 coordinates[2] = 1.0;
160 return;
161 }
162
163 T vc = d4 * d1 - d1 * d3 + d3 * d2;
164 if (vc < 0.0 and d1 > 0.0 and d3 > 0.0)
165 {
166 SPDLOG_DEBUG("GJK: edge AB");
167 T f1 = 1.0 / (d1 + d3);
168 T lm = d1 * f1;
169 T mu = d3 * f1;
170 coordinates[0] = mu;
171 coordinates[1] = lm;
172 coordinates[2] = 0.0;
173 return;
174 }
175 T vb = d1 * d5 - d5 * d2 + d2 * d6;
176 if (vb < 0.0 and d2 > 0.0 and d5 > 0.0)
177 {
178 SPDLOG_DEBUG("GJK: edge AC");
179 T f1 = 1.0 / (d2 + d5);
180 T lm = d2 * f1;
181 T mu = d5 * f1;
182 coordinates[0] = mu;
183 coordinates[1] = 0.0;
184 coordinates[2] = lm;
185 return;
186 }
187 T va = d3 * d6 - d6 * d4 + d4 * d5;
188 if (va < 0.0 and d4 > 0.0 and d6 > 0.0)
189 {
190 SPDLOG_DEBUG("GJK: edge BC");
191 T f1 = 1.0 / (d4 + d6);
192 T lm = d4 * f1;
193 T mu = d6 * f1;
194 coordinates[0] = 0.0;
195 coordinates[1] = mu;
196 coordinates[2] = lm;
197 return;
198 }
199
200 SPDLOG_DEBUG("GJK: triangle ABC");
201 T f1 = 1.0 / (va + vb + vc);
202 coordinates[0] = va * f1;
203 coordinates[1] = vb * f1;
204 coordinates[2] = vc * f1;
205 return;
206 }
207 else if constexpr (simplex_size == 4)
208 {
209 // Most complex case, where simplex is a tetrahedron, with 15 possible
210 // outcomes (4 vertices, 6 edges, 4 facets and the interior).
211 std::ranges::fill(coordinates, 0.0);
212
213 T d[4][4];
214 for (int i = 0; i < 4; ++i)
215 // Compute dot products at each vertex
216 {
217 std::span<const T, 3> si(s.begin() + i * 3, 3);
218 T sii = dot3(si, si);
219 bool out = true;
220 for (int j = 0; j < 4; ++j)
221 {
222 std::span<const T, 3> sj(s.begin() + j * 3, 3);
223 if (i != j)
224 d[i][j] = (sii - dot3(si, sj));
225 SPDLOG_DEBUG("d[{}][{}] = {}", i, j, static_cast<double>(d[i][j]));
226 if (d[i][j] > 0.0)
227 out = false;
228 }
229 if (out)
230 {
231 // Return if a vertex is closest
232 coordinates[i] = 1.0;
233 return;
234 }
235 }
236
237 SPDLOG_DEBUG("Check for edges");
238
239 // Check if an edge is closest
240 T v[6][2] = {{0.0}};
241 int edges[6][2] = {{2, 3}, {1, 3}, {1, 2}, {0, 3}, {0, 2}, {0, 1}};
242 for (int i = 0; i < 6; ++i)
243 {
244 // Four vertices of the tetrahedron, j0 and j1 at the ends of the current
245 // edge and j2 and j3 on the opposing edge.
246 int j0 = edges[i][0];
247 int j1 = edges[i][1];
248 int j2 = edges[5 - i][0];
249 int j3 = edges[5 - i][1];
250 v[i][0] = d[j1][j2] * d[j0][j1] - d[j0][j1] * d[j1][j0]
251 + d[j1][j0] * d[j0][j2];
252 v[i][1] = d[j1][j3] * d[j0][j1] - d[j0][j1] * d[j1][j0]
253 + d[j1][j0] * d[j0][j3];
254
255 SPDLOG_DEBUG("v[{}] = {},{}", i, (double)v[i][0], (double)v[i][1]);
256 if (v[i][0] <= 0.0 and v[i][1] <= 0.0 and d[j0][j1] >= 0.0
257 and d[j1][j0] >= 0.0)
258 {
259 // On an edge
260 T f1 = 1.0 / (d[j0][j1] + d[j1][j0]);
261 coordinates[j0] = f1 * d[j1][j0];
262 coordinates[j1] = f1 * d[j0][j1];
263 return;
264 }
265 }
266
267 // Now check the facets of a tetrahedron
268 std::array<T, 4> w = det4(s);
269 T wsum = w[0] + w[1] + w[2] + w[3];
270 if (wsum < 0.0)
271 {
272 w[0] = -w[0];
273 w[1] = -w[1];
274 w[2] = -w[2];
275 w[3] = -w[3];
276 wsum = -wsum;
277 }
278
279 if (w[0] < 0.0 and v[2][0] > 0.0 and v[4][0] > 0.0 and v[5][0] > 0.0)
280 {
281 T f1 = 1.0 / (v[2][0] + v[4][0] + v[5][0]);
282 coordinates[0] = v[2][0] * f1;
283 coordinates[1] = v[4][0] * f1;
284 coordinates[2] = v[5][0] * f1;
285 coordinates[3] = 0.0;
286 return;
287 }
288
289 if (w[1] < 0.0 and v[1][0] > 0.0 and v[3][0] > 0.0 and v[5][1] > 0.0)
290 {
291 T f1 = 1.0 / (v[1][0] + v[3][0] + v[5][1]);
292 coordinates[0] = v[1][0] * f1;
293 coordinates[1] = v[3][0] * f1;
294 coordinates[2] = 0.0;
295 coordinates[3] = v[5][1] * f1;
296 return;
297 }
298
299 if (w[2] < 0.0 and v[0][0] > 0.0 and v[3][1] > 0 and v[4][1] > 0.0)
300 {
301 T f1 = 1.0 / (v[0][0] + v[3][1] + v[4][1]);
302 coordinates[0] = v[0][0] * f1;
303 coordinates[1] = 0.0;
304 coordinates[2] = v[3][1] * f1;
305 coordinates[3] = v[4][1] * f1;
306 return;
307 }
308
309 if (w[3] < 0.0 and v[0][1] > 0.0 and v[1][1] > 0.0 and v[2][1] > 0.0)
310 {
311 T f1 = 1.0 / (v[0][1] + v[1][1] + v[2][1]);
312 coordinates[0] = 0.0;
313 coordinates[1] = v[0][1] * f1;
314 coordinates[2] = v[1][1] * f1;
315 coordinates[3] = v[2][1] * f1;
316 return;
317 }
318
319 // Point lies in interior of tetrahedron with these barycentric coordinates
320 coordinates[0] = w[3] / wsum;
321 coordinates[1] = w[2] / wsum;
322 coordinates[2] = w[1] / wsum;
323 coordinates[3] = w[0] / wsum;
324 return;
325 }
326 else
327 {
328 // Evaluated at compile-time instead of runtime!
329 static_assert(simplex_size >= 2 && simplex_size <= 4,
330 "Number of rows defining simplex not supported.");
331 }
332}
333
338template <typename T>
339inline int support(std::span<const T> bd, const std::array<T, 3>& v)
340{
341 int i = 0;
342 T qmax = bd[0] * v[0] + bd[1] * v[1] + bd[2] * v[2];
343 for (std::size_t m = 1; m < bd.size() / 3; ++m)
344 {
345 T q = bd[3 * m] * v[0] + bd[3 * m + 1] * v[1] + bd[3 * m + 2] * v[2];
346 if (q > qmax)
347 {
348 qmax = q;
349 i = m;
350 }
351 }
352
353 return i;
354}
355} // namespace impl_gjk
356
370template <std::floating_point T,
371 typename U = boost::multiprecision::cpp_bin_float_double_extended>
372std::array<T, 3> compute_distance_gjk(std::span<const T> p0,
373 std::span<const T> q0)
374{
375 assert(p0.size() % 3 == 0);
376 assert(q0.size() % 3 == 0);
377
378 constexpr int maxk = 15; // Maximum number of iterations of the GJK algorithm
379 const U eps = 1000 * std::numeric_limits<U>::epsilon();
380
381 // Initialize distance vector x_k
382 std::array<U, 3> x_k = {static_cast<U>(p0[0]) - static_cast<U>(q0[0]),
383 static_cast<U>(p0[1]) - static_cast<U>(q0[1]),
384 static_cast<U>(p0[2]) - static_cast<U>(q0[2])};
385 // Initialize simplex
386 std::array<U, 12> s = {0}; // Max simplex is a tetrahedron
387 s[0] = x_k[0];
388 s[1] = x_k[1];
389 s[2] = x_k[2];
390 std::array<U, 4> lmn = {0}; // Scratch memory for barycentric
391 // coordinates of closest point in simplex
392 std::size_t simplex_size = 1;
393 // Begin GJK iteration
394 int k;
395 for (k = 0; k < maxk; ++k)
396 {
397
398 // Compute the squared norm of current iterate to normalize support search
399 // in original precision
400 const U x_norm2 = impl_gjk::dot3(x_k, x_k);
401 std::array<U, 3> x_k_normalized = x_k;
402 if (x_norm2 > eps * eps)
403 {
404 // ADL lookup:
405 // If U is double/float use std::sqrt
406 // If U is a boost::multiprecision member use boost::multiprecision::sqrt
407 using std::sqrt;
408 U inv_norm = U(1.0) / sqrt(x_norm2);
409 x_k_normalized[0] *= inv_norm;
410 x_k_normalized[1] *= inv_norm;
411 x_k_normalized[2] *= inv_norm;
412 }
413 // Compute support point in original precision
414 std::array<T, 3> dir_p = {static_cast<T>(-x_k_normalized[0]),
415 static_cast<T>(-x_k_normalized[1]),
416 static_cast<T>(-x_k_normalized[2])};
417 std::array<T, 3> dir_q
418 = {static_cast<T>(x_k_normalized[0]), static_cast<T>(x_k_normalized[1]),
419 static_cast<T>(x_k_normalized[2])};
420 int ip = impl_gjk::support(p0, dir_p);
421 int iq = impl_gjk::support(q0, dir_q);
422
423 // Only cast the winning support points to U
424 std::array<U, 3> s_k
425 = {static_cast<U>(p0[ip * 3]) - static_cast<U>(q0[iq * 3]),
426 static_cast<U>(p0[ip * 3 + 1]) - static_cast<U>(q0[iq * 3 + 1]),
427 static_cast<U>(p0[ip * 3 + 2]) - static_cast<U>(q0[iq * 3 + 2])};
428
429 // Break if the newly found support point s_k is already in the simplex
430 std::size_t m;
431 for (m = 0; m < simplex_size; ++m)
432 {
433 auto it = std::next(s.begin(), 3 * m);
434 if (std::equal(it, std::next(it, 3), s_k.begin(), s_k.end()))
435 break;
436 }
437
438 if (m != simplex_size)
439 break;
440
441 // 1st exit condition: (x_k - s_k).x_k = 0
442 const U xs_diff = x_norm2 - impl_gjk::dot3(x_k, s_k);
443 if (xs_diff < (eps * x_norm2) or xs_diff < eps)
444 break;
445
446 SPDLOG_DEBUG("GJK: xs_diff={}/{}", static_cast<double>(xs_diff),
447 static_cast<double>(eps));
448
449 // Add new vertex to simplex
450 std::ranges::copy(s_k, s.begin() + 3 * simplex_size);
451 ++simplex_size;
452
453 // Find nearest subset of simplex
454 switch (simplex_size)
455 {
456 case 2:
457 impl_gjk::nearest_simplex<U, 2>(s, lmn);
458 break;
459 case 3:
460 impl_gjk::nearest_simplex<U, 3>(s, lmn);
461 break;
462 case 4:
463 impl_gjk::nearest_simplex<U, 4>(s, lmn);
464 break;
465 default:
466 throw std::runtime_error("Invalid simplex size");
467 }
468
469 // Recompute x_k and keep points with non-zero values in lmn
470 std::size_t j = 0;
471 x_k = {0.0, 0.0, 0.0};
472 for (std::size_t i = 0; i < simplex_size; ++i)
473 {
474 std::span<const U> sc(std::next(s.begin(), 3 * i), 3);
475 if (lmn[i] > 0.0)
476 {
477 x_k[0] += lmn[i] * sc[0];
478 x_k[1] += lmn[i] * sc[1];
479 x_k[2] += lmn[i] * sc[2];
480 if (i > j)
481 std::ranges::copy(sc, std::next(s.begin(), 3 * j));
482 ++j;
483 }
484 }
485 simplex_size = j;
486
487 // 2nd exit condition - strict monotonicity
488 const U x_next_norm2 = impl_gjk::dot3(x_k, x_k);
489 if (x_norm2 <= x_next_norm2)
490 break;
491
492 // 3rd exit condition - intersecting or touching
493 if (x_next_norm2 < eps * eps)
494 break;
495 }
496
497 if (k == maxk)
498 throw std::runtime_error("GJK error - max iteration limit reached");
499 return {static_cast<T>(x_k[0]), static_cast<T>(x_k[1]),
500 static_cast<T>(x_k[2])};
501}
502
520template <std::floating_point T,
521 typename U = boost::multiprecision::cpp_bin_float_double_extended>
522std::vector<T>
523compute_distances_gjk(const std::vector<std::span<const T>>& bodies,
524 std::span<const T> q, int num_threads)
525{
526 if (num_threads <= 0)
527 throw std::invalid_argument("num_threads must be >= 1.");
528
529 std::size_t total_size = bodies.size();
530 num_threads
531 = std::max<std::size_t>(1, std::min(num_threads, (int)total_size));
532
533 std::vector<T> results(total_size * 3);
534 auto compute_chunk
535 = [](std::vector<T>& chunk_results,
536 const std::vector<std::span<const T>>& chunk_bodies, std::size_t c0,
537 std::size_t c1, std::span<const T> q_ref)
538 {
539 for (std::size_t i = c0; i < c1; ++i)
540 {
541 // Using U explicitly as the internal precision type
542 std::array<T, 3> dist
543 = compute_distance_gjk<T, U>(chunk_bodies[i], q_ref);
544 chunk_results[3 * i + 0] = dist[0];
545 chunk_results[3 * i + 1] = dist[1];
546 chunk_results[3 * i + 2] = dist[2];
547 }
548 };
549
550 std::vector<std::jthread> threads;
551 for (int i = 1; i < num_threads; ++i)
552 {
553 auto [c0, c1] = common::local_range(i, total_size, num_threads);
554 threads.emplace_back(compute_chunk, std::ref(results), std::ref(bodies), c0,
555 c1, std::ref(q));
556 }
557 auto [c0, c1] = common::local_range(0, total_size, num_threads);
558 compute_chunk(std::ref(results), std::cref(bodies), c0, c1, q);
559
560 return results;
561}
562
563} // namespace dolfinx::geometry
dolfinx::common::local_range
constexpr std::array< std::int64_t, 2 > local_range(int index, std::int64_t N, int size)
Partition a global range [0, N - 1] across callers into non-overlapping sub-partitions of almost equa...
Definition local_range.h:26
dolfinx::geometry
Geometry data structures and algorithms.
Definition BoundingBoxTree.h:26
dolfinx::geometry::compute_distances_gjk
std::vector< T > compute_distances_gjk(const std::vector< std::span< const T > > &bodies, std::span< const T > q, int num_threads)
Compute the distance between a sequence of convex bodies p0, ..., pN and q, each defined by a set of ...
Definition gjk.h:523
dolfinx::geometry::compute_distance_gjk
std::array< T, 3 > compute_distance_gjk(std::span< const T > p0, std::span< const T > q0)
Compute the distance between two convex bodies p0 and q0, each defined by a set of points.
Definition gjk.h:372
Generated by doxygen 1.15.0