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