DOLFINx 0.10.0.0
DOLFINx C++ interface
Loading...
Searching...
No Matches
gjk.h
1// Copyright (C) 2020 Chris Richardson
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 <concepts>
13#include <limits>
14#include <numeric>
15#include <span>
16#include <utility>
17#include <vector>
18
19namespace dolfinx::geometry
20{
21
22namespace impl_gjk
23{
24
28template <typename T>
29inline T det3(std::span<const T, 9> A)
30{
31 T w0 = A[3 + 1] * A[2 * 3 + 2] - A[3 + 2] * A[3 * 2 + 1];
32 T w1 = A[3] * A[3 * 2 + 2] - A[3 + 2] * A[3 * 2];
33 T w2 = A[3] * A[3 * 2 + 1] - A[3 + 1] * A[3 * 2];
34 return A[0] * w0 - A[1] * w1 + A[2] * w2;
35}
36
41template <typename Vec>
42inline Vec::value_type dot3(const Vec& a, const Vec& b)
43{
44 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
45}
46
52template <typename T>
53std::vector<T> nearest_simplex(std::span<const T> s)
54{
55 assert(s.size() % 3 == 0);
56 const std::size_t s_rows = s.size() / 3;
57
58 SPDLOG_DEBUG("GJK: nearest_simplex({})", s_rows);
59
60 switch (s_rows)
61 {
62 case 2:
63 {
64 // Simplex is an interval. Point may lie on the interval, or on either end.
65 // Compute lm = dot(s0, ds / |ds|)
66 std::span<const T, 3> s0 = s.template subspan<0, 3>();
67 std::span<const T, 3> s1 = s.template subspan<3, 3>();
68
69 T lm = dot3(s0, s0) - dot3(s0, s1);
70 if (lm < 0.0)
71 {
72 SPDLOG_DEBUG("GJK: line point A");
73 return {1.0, 0.0};
74 }
75 T mu = dot3(s1, s1) - dot3(s1, s0);
76 if (mu < 0.0)
77 {
78 SPDLOG_DEBUG("GJK: line point B");
79 return {0.0, 1.0};
80 }
81
82 SPDLOG_DEBUG("GJK line: AB");
83 T f1 = 1.0 / (lm + mu);
84 return {mu * f1, lm * f1};
85 }
86 case 3:
87 {
88 // Simplex is a triangle. Point may lie in one of 7 regions (outside near a
89 // vertex, outside near an edge, or on the interior)
90 auto a = s.template subspan<0, 3>();
91 auto b = s.template subspan<3, 3>();
92 auto c = s.template subspan<6, 3>();
93
94 T aa = dot3(a, a);
95 T ab = dot3(a, b);
96 T ac = dot3(a, c);
97 T d1 = aa - ab;
98 T d2 = aa - ac;
99 if (d1 < 0.0 and d2 < 0.0)
100 {
101 SPDLOG_DEBUG("GJK: Point A");
102 return {1.0, 0.0, 0.0};
103 }
104
105 T bb = dot3(b, b);
106 T bc = dot3(b, c);
107 T d3 = bb - ab;
108 T d4 = bb - bc;
109 if (d3 < 0.0 and d4 < 0.0)
110 {
111 SPDLOG_DEBUG("GJK: Point B");
112 return {0.0, 1.0, 0.0};
113 }
114
115 T cc = dot3(c, c);
116 T d5 = cc - ac;
117 T d6 = cc - bc;
118 if (d5 < 0.0 and d6 < 0.0)
119 {
120 SPDLOG_DEBUG("GJK: Point C");
121 return {0.0, 0.0, 1.0};
122 }
123
124 T vc = d4 * d1 - d1 * d3 + d3 * d2;
125 if (vc < 0.0 and d1 > 0.0 and d3 > 0.0)
126 {
127 SPDLOG_DEBUG("GJK: edge AB");
128 T f1 = 1.0 / (d1 + d3);
129 T lm = d1 * f1;
130 T mu = d3 * f1;
131 return {mu, lm, 0.0};
132 }
133 T vb = d1 * d5 - d5 * d2 + d2 * d6;
134 if (vb < 0.0 and d2 > 0.0 and d5 > 0.0)
135 {
136 SPDLOG_DEBUG("GJK: edge AC");
137 T f1 = 1.0 / (d2 + d5);
138 T lm = d2 * f1;
139 T mu = d5 * f1;
140 return {mu, 0.0, lm};
141 }
142 T va = d3 * d6 - d6 * d4 + d4 * d5;
143 if (va < 0.0 and d4 > 0.0 and d6 > 0.0)
144 {
145 SPDLOG_DEBUG("GJK: edge BC");
146 T f1 = 1.0 / (d4 + d6);
147 T lm = d4 * f1;
148 T mu = d6 * f1;
149 return {0.0, mu, lm};
150 }
151
152 SPDLOG_DEBUG("GJK: triangle ABC");
153 T f1 = 1.0 / (va + vb + vc);
154 return {va * f1, vb * f1, vc * f1};
155 }
156 case 4:
157 {
158 // Most complex case, where simplex is a tetrahedron, with 15 possible
159 // outcomes (4 vertices, 6 edges, 4 facets and the interior).
160 std::vector<T> rv = {0.0, 0.0, 0.0, 0.0};
161
162 T d[4][4];
163 for (int i = 0; i < 4; ++i)
164 // Compute dot products at each vertex
165 {
166 std::span<const T, 3> si(s.begin() + i * 3, 3);
167 T sii = dot3(si, si);
168 bool out = true;
169 for (int j = 0; j < 4; ++j)
170 {
171 std::span<const T, 3> sj(s.begin() + j * 3, 3);
172 if (i != j)
173 d[i][j] = (sii - dot3(si, sj));
174 SPDLOG_DEBUG("d[{}][{}] = {}", i, j, static_cast<double>(d[i][j]));
175 if (d[i][j] > 0.0)
176 out = false;
177 }
178 if (out)
179 {
180 // Return if a vertex is closest
181 rv[i] = 1.0;
182 return rv;
183 }
184 }
185
186 SPDLOG_DEBUG("Check for edges");
187
188 // Check if an edge is closest
189 T v[6][2] = {{0.0}};
190 int edges[6][2] = {{2, 3}, {1, 3}, {1, 2}, {0, 3}, {0, 2}, {0, 1}};
191 for (int i = 0; i < 6; ++i)
192 {
193 // Four vertices of the tetrahedron, j0 and j1 at the ends of the current
194 // edge and j2 and j3 on the opposing edge.
195 int j0 = edges[i][0];
196 int j1 = edges[i][1];
197 int j2 = edges[5 - i][0];
198 int j3 = edges[5 - i][1];
199 v[i][0] = d[j1][j2] * d[j0][j1] - d[j0][j1] * d[j1][j0]
200 + d[j1][j0] * d[j0][j2];
201 v[i][1] = d[j1][j3] * d[j0][j1] - d[j0][j1] * d[j1][j0]
202 + d[j1][j0] * d[j0][j3];
203
204 SPDLOG_DEBUG("v[{}] = {},{}", i, (double)v[i][0], (double)v[i][1]);
205 if (v[i][0] <= 0.0 and v[i][1] <= 0.0 and d[j0][j1] >= 0.0
206 and d[j1][j0] >= 0.0)
207 {
208 // On an edge
209 T f1 = 1.0 / (d[j0][j1] + d[j1][j0]);
210 rv[j0] = f1 * d[j1][j0];
211 rv[j1] = f1 * d[j0][j1];
212 return rv;
213 }
214 }
215
216 // Now check the facets of a tetrahedron
217 std::array<T, 4> w;
218 std::array<T, 9> M;
219 std::span<const T, 9> Mspan(M.begin(), M.size());
220 std::copy(s.begin(), s.begin() + 9, M.begin());
221 w[0] = -det3(Mspan);
222 std::copy(s.begin() + 9, s.begin() + 12, M.begin() + 6);
223 w[1] = det3(Mspan);
224 std::copy(s.begin() + 6, s.begin() + 9, M.begin() + 3);
225 w[2] = -det3(Mspan);
226 std::copy(s.begin() + 3, s.begin() + 6, M.begin() + 0);
227 w[3] = det3(Mspan);
228 T wsum = w[0] + w[1] + w[2] + w[3];
229 if (wsum < 0.0)
230 {
231 w[0] = -w[0];
232 w[1] = -w[1];
233 w[2] = -w[2];
234 w[3] = -w[3];
235 wsum = -wsum;
236 }
237
238 if (w[0] < 0.0 and v[2][0] > 0.0 and v[4][0] > 0.0 and v[5][0] > 0.0)
239 {
240 T f1 = 1.0 / (v[2][0] + v[4][0] + v[5][0]);
241 return {v[2][0] * f1, v[4][0] * f1, v[5][0] * f1, 0.0};
242 }
243
244 if (w[1] < 0.0 and v[1][0] > 0.0 and v[3][0] > 0.0 and v[5][1] > 0.0)
245 {
246 T f1 = 1.0 / (v[1][0] + v[3][0] + v[5][1]);
247 return {v[1][0] * f1, v[3][0] * f1, 0.0, v[5][1] * f1};
248 }
249
250 if (w[2] < 0.0 and v[0][0] > 0.0 and v[3][1] > 0 and v[4][1] > 0.0)
251 {
252 T f1 = 1.0 / (v[0][0] + v[3][1] + v[4][1]);
253 return {v[0][0] * f1, 0.0, v[3][1] * f1, v[4][1] * f1};
254 }
255
256 if (w[3] < 0.0 and v[0][1] > 0.0 and v[1][1] > 0.0 and v[2][1] > 0.0)
257 {
258 T f1 = 1.0 / (v[0][1] + v[1][1] + v[2][1]);
259 return {0.0, v[0][1] * f1, v[1][1] * f1, v[2][1] * f1};
260 }
261
262 // Point lies in interior of tetrahedron with these barycentric coordinates
263 return {w[3] / wsum, w[2] / wsum, w[1] / wsum, w[0] / wsum};
264 }
265 default:
266 throw std::runtime_error("Number of rows defining simplex not supported.");
267 }
268}
269
274template <typename T>
275std::array<T, 3> support(std::span<const T> bd, std::array<T, 3> v)
276{
277 int i = 0;
278 T qmax = bd[0] * v[0] + bd[1] * v[1] + bd[2] * v[2];
279 for (std::size_t m = 1; m < bd.size() / 3; ++m)
280 {
281 T q = bd[3 * m] * v[0] + bd[3 * m + 1] * v[1] + bd[3 * m + 2] * v[2];
282 if (q > qmax)
283 {
284 qmax = q;
285 i = m;
286 }
287 }
288
289 return {bd[3 * i], bd[3 * i + 1], bd[3 * i + 2]};
290}
291} // namespace impl_gjk
292
306template <std::floating_point T,
307 typename U = boost::multiprecision::cpp_bin_float_double_extended>
308std::array<T, 3> compute_distance_gjk(std::span<const T> p0,
309 std::span<const T> q0)
310{
311 assert(p0.size() % 3 == 0);
312 assert(q0.size() % 3 == 0);
313
314 // Copy from T to type U
315 std::vector<U> p(p0.begin(), p0.end());
316 std::vector<U> q(q0.begin(), q0.end());
317
318 constexpr int maxk = 15; // Maximum number of iterations of the GJK algorithm
319
320 // Tolerance
321 const U eps = 1.0e4 * std::numeric_limits<T>::epsilon();
322
323 // Initialise vector and simplex
324 std::array<U, 3> v = {p[0] - q[0], p[1] - q[1], p[2] - q[2]};
325 std::vector<U> s = {v[0], v[1], v[2]};
326
327 // Begin GJK iteration
328 int k;
329 for (k = 0; k < maxk; ++k)
330 {
331 // Support function
332 std::array w1
333 = impl_gjk::support(std::span<const U>(p), {-v[0], -v[1], -v[2]});
334 std::array w0
335 = impl_gjk::support(std::span<const U>(q), {v[0], v[1], v[2]});
336 const std::array<U, 3> w = {w1[0] - w0[0], w1[1] - w0[1], w1[2] - w0[2]};
337
338 // Break if any existing points are the same as w
339 assert(s.size() % 3 == 0);
340 std::size_t m;
341 for (m = 0; m < s.size() / 3; ++m)
342 {
343 auto it = std::next(s.begin(), 3 * m);
344 if (std::equal(it, std::next(it, 3), w.begin(), w.end()))
345 break;
346 }
347
348 if (m != s.size() / 3)
349 break;
350
351 // 1st exit condition (v - w).v = 0
352 const U vnorm2 = impl_gjk::dot3(v, v);
353 const U vw = vnorm2 - impl_gjk::dot3(v, w);
354 if (vw < (eps * vnorm2) or vw < eps)
355 break;
356
357 SPDLOG_DEBUG("GJK: vw={}/{}", static_cast<double>(vw),
358 static_cast<double>(eps));
359
360 // Add new vertex to simplex
361 s.insert(s.end(), w.begin(), w.end());
362
363 // Find nearest subset of simplex
364 std::vector<U> lmn = impl_gjk::nearest_simplex<U>(s);
365
366 // Recompute v and keep points with non-zero values in lmn
367 std::size_t j = 0;
368 v = {0.0, 0.0, 0.0};
369 for (std::size_t i = 0; i < lmn.size(); ++i)
370 {
371 std::span<const U> sc(std::next(s.begin(), 3 * i), 3);
372 if (lmn[i] > 0.0)
373 {
374 v[0] += lmn[i] * sc[0];
375 v[1] += lmn[i] * sc[1];
376 v[2] += lmn[i] * sc[2];
377 if (i > j)
378 std::copy(sc.begin(), sc.end(), std::next(s.begin(), 3 * j));
379 ++j;
380 }
381 }
382 SPDLOG_DEBUG("new s size={}", 3 * j);
383 s.resize(3 * j);
384
385 const U vn = impl_gjk::dot3(v, v);
386 // 2nd exit condition - intersecting or touching
387 if (vn < eps * eps)
388 break;
389 }
390
391 if (k == maxk)
392 throw std::runtime_error("GJK error - max iteration limit reached");
393
394 return {static_cast<T>(v[0]), static_cast<T>(v[1]), static_cast<T>(v[2])};
395}
396
397} // namespace dolfinx::geometry
Geometry data structures and algorithms.
Definition BoundingBoxTree.h:22
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:308