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