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