11#include <boost/multiprecision/cpp_bin_float.hpp>
29inline T det3(std::span<const T, 9> A)
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;
41template <
typename Vec>
42inline Vec::value_type dot3(
const Vec& a,
const Vec& b)
44 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
53std::vector<T> nearest_simplex(std::span<const T> s)
55 assert(s.size() % 3 == 0);
56 const std::size_t s_rows = s.size() / 3;
58 SPDLOG_DEBUG(
"GJK: nearest_simplex({})", s_rows);
66 std::span<const T, 3> s0 = s.template subspan<0, 3>();
67 std::span<const T, 3> s1 = s.template subspan<3, 3>();
69 T lm = dot3(s0, s0) - dot3(s0, s1);
72 SPDLOG_DEBUG(
"GJK: line point A");
75 T mu = dot3(s1, s1) - dot3(s1, s0);
78 SPDLOG_DEBUG(
"GJK: line point B");
82 SPDLOG_DEBUG(
"GJK line: AB");
83 T f1 = 1.0 / (lm + mu);
84 return {mu * f1, lm * f1};
90 auto a = s.template subspan<0, 3>();
91 auto b = s.template subspan<3, 3>();
92 auto c = s.template subspan<6, 3>();
99 if (d1 < 0.0 and d2 < 0.0)
101 SPDLOG_DEBUG(
"GJK: Point A");
102 return {1.0, 0.0, 0.0};
109 if (d3 < 0.0 and d4 < 0.0)
111 SPDLOG_DEBUG(
"GJK: Point B");
112 return {0.0, 1.0, 0.0};
118 if (d5 < 0.0 and d6 < 0.0)
120 SPDLOG_DEBUG(
"GJK: Point C");
121 return {0.0, 0.0, 1.0};
124 T vc = d4 * d1 - d1 * d3 + d3 * d2;
125 if (vc < 0.0 and d1 > 0.0 and d3 > 0.0)
127 SPDLOG_DEBUG(
"GJK: edge AB");
128 T f1 = 1.0 / (d1 + d3);
131 return {mu, lm, 0.0};
133 T vb = d1 * d5 - d5 * d2 + d2 * d6;
134 if (vb < 0.0 and d2 > 0.0 and d5 > 0.0)
136 SPDLOG_DEBUG(
"GJK: edge AC");
137 T f1 = 1.0 / (d2 + d5);
140 return {mu, 0.0, lm};
142 T va = d3 * d6 - d6 * d4 + d4 * d5;
143 if (va < 0.0 and d4 > 0.0 and d6 > 0.0)
145 SPDLOG_DEBUG(
"GJK: edge BC");
146 T f1 = 1.0 / (d4 + d6);
149 return {0.0, mu, lm};
152 SPDLOG_DEBUG(
"GJK: triangle ABC");
153 T f1 = 1.0 / (va + vb + vc);
154 return {va * f1, vb * f1, vc * f1};
160 std::vector<T> rv = {0.0, 0.0, 0.0, 0.0};
163 for (
int i = 0; i < 4; ++i)
166 std::span<const T, 3> si(s.begin() + i * 3, 3);
167 T sii = dot3(si, si);
169 for (
int j = 0; j < 4; ++j)
171 std::span<const T, 3> sj(s.begin() + j * 3, 3);
173 d[i][j] = (sii - dot3(si, sj));
174 SPDLOG_DEBUG(
"d[{}][{}] = {}", i, j,
static_cast<double>(d[i][j]));
186 SPDLOG_DEBUG(
"Check for edges");
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)
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];
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)
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];
219 std::span<const T, 9> Mspan(M.begin(), M.size());
220 std::copy(s.begin(), s.begin() + 9, M.begin());
222 std::copy(s.begin() + 9, s.begin() + 12, M.begin() + 6);
224 std::copy(s.begin() + 6, s.begin() + 9, M.begin() + 3);
226 std::copy(s.begin() + 3, s.begin() + 6, M.begin() + 0);
228 T wsum = w[0] + w[1] + w[2] + w[3];
238 if (w[0] < 0.0 and v[2][0] > 0.0 and v[4][0] > 0.0 and v[5][0] > 0.0)
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};
244 if (w[1] < 0.0 and v[1][0] > 0.0 and v[3][0] > 0.0 and v[5][1] > 0.0)
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};
250 if (w[2] < 0.0 and v[0][0] > 0.0 and v[3][1] > 0 and v[4][1] > 0.0)
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};
256 if (w[3] < 0.0 and v[0][1] > 0.0 and v[1][1] > 0.0 and v[2][1] > 0.0)
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};
263 return {w[3] / wsum, w[2] / wsum, w[1] / wsum, w[0] / wsum};
266 throw std::runtime_error(
"Number of rows defining simplex not supported.");
275std::array<T, 3> support(std::span<const T> bd, std::array<T, 3> v)
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)
281 T q = bd[3 * m] * v[0] + bd[3 * m + 1] * v[1] + bd[3 * m + 2] * v[2];
289 return {bd[3 * i], bd[3 * i + 1], bd[3 * i + 2]};
306template <std::floating_point T,
307 typename U = boost::multiprecision::cpp_bin_float_double_extended>
309 std::span<const T> q0)
311 assert(p0.size() % 3 == 0);
312 assert(q0.size() % 3 == 0);
315 std::vector<U> p(p0.begin(), p0.end());
316 std::vector<U> q(q0.begin(), q0.end());
318 constexpr int maxk = 15;
321 const U eps = 1.0e4 * std::numeric_limits<T>::epsilon();
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]};
329 for (k = 0; k < maxk; ++k)
333 = impl_gjk::support(std::span<const U>(p), {-v[0], -v[1], -v[2]});
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]};
339 assert(s.size() % 3 == 0);
341 for (m = 0; m < s.size() / 3; ++m)
343 auto it = std::next(s.begin(), 3 * m);
344 if (std::equal(it, std::next(it, 3), w.begin(), w.end()))
348 if (m != s.size() / 3)
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)
357 SPDLOG_DEBUG(
"GJK: vw={}/{}",
static_cast<double>(vw),
358 static_cast<double>(eps));
361 s.insert(s.end(), w.begin(), w.end());
364 std::vector<U> lmn = impl_gjk::nearest_simplex<U>(s);
369 for (std::size_t i = 0; i < lmn.size(); ++i)
371 std::span<const U> sc(std::next(s.begin(), 3 * i), 3);
374 v[0] += lmn[i] * sc[0];
375 v[1] += lmn[i] * sc[1];
376 v[2] += lmn[i] * sc[2];
378 std::copy(sc.begin(), sc.end(), std::next(s.begin(), 3 * j));
382 SPDLOG_DEBUG(
"new s size={}", 3 * j);
385 const U vn = impl_gjk::dot3(v, v);
392 throw std::runtime_error(
"GJK error - max iteration limit reached");
394 return {
static_cast<T
>(v[0]),
static_cast<T
>(v[1]),
static_cast<T
>(v[2])};
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