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