PhysicsBasedAnimationToolkit 0.0.10
Cross-platform C++20 library of algorithms and data structures commonly used in computer graphics research on physically-based simulation.
Loading...
Searching...
No Matches
PointTriangleCcd.h
Go to the documentation of this file.
1
10
11#ifndef PBAT_GEOMETRY_POINTTRIANGLECCD_H
12#define PBAT_GEOMETRY_POINTTRIANGLECCD_H
13
14#include "IntersectionQueries.h"
15#include "pbat/HostDevice.h"
16#include "pbat/math/linalg/mini/Concepts.h"
17#include "pbat/math/linalg/mini/Matrix.h"
18#include "pbat/math/linalg/mini/Reductions.h"
20
21#include <array>
22#include <cmath>
23
24namespace pbat::geometry {
25
26namespace detail {
27
58template <
59 math::linalg::mini::CReadableVectorizedMatrix TXT,
60 math::linalg::mini::CReadableVectorizedMatrix TAT,
61 math::linalg::mini::CReadableVectorizedMatrix TBT,
62 math::linalg::mini::CReadableVectorizedMatrix TCT,
63 math::linalg::mini::CReadableVectorizedMatrix TX,
64 math::linalg::mini::CReadableVectorizedMatrix TA,
65 math::linalg::mini::CReadableVectorizedMatrix TB,
66 math::linalg::mini::CReadableVectorizedMatrix TC,
67 class TScalar = typename TX::ScalarType>
68PBAT_HOST_DEVICE std::array<TScalar, 4> PointTriangleCcdUnivariatePolynomial(
69 TXT const& XT,
70 TAT const& AT,
71 TBT const& BT,
72 TCT const& CT,
73 TX const& X,
74 TA const& A,
75 TB const& B,
76 TC const& C);
77
78} // namespace detail
79
115template <
116 math::linalg::mini::CReadableVectorizedMatrix TXT,
117 math::linalg::mini::CReadableVectorizedMatrix TAT,
118 math::linalg::mini::CReadableVectorizedMatrix TBT,
119 math::linalg::mini::CReadableVectorizedMatrix TCT,
120 math::linalg::mini::CReadableVectorizedMatrix TX,
121 math::linalg::mini::CReadableVectorizedMatrix TA,
122 math::linalg::mini::CReadableVectorizedMatrix TB,
123 math::linalg::mini::CReadableVectorizedMatrix TC,
124 class TScalar = typename TXT::ScalarType>
125PBAT_HOST_DEVICE auto PointTriangleCcd(
126 TXT const& XT,
127 TAT const& AT,
128 TBT const& BT,
129 TCT const& CT,
130 TX const& X,
131 TA const& A,
132 TB const& B,
133 TC const& C) -> math::linalg::mini::SVector<TScalar, 4>
134{
135 auto constexpr kDims = TXT::kRows;
136 // 1. Form co-planarity polynomial
137 std::array<TScalar, 4> const coeffs =
138 detail::PointTriangleCcdUnivariatePolynomial(XT, AT, BT, CT, X, A, B, C);
139 // 2. Filter roots
140 using namespace pbat::math::linalg::mini;
141 SVector<TScalar, 4> r;
142 bool const bIntersectionFound = math::polynomial::ForEachRoot<3>(
143 [&](TScalar t) {
144 // 3. Compute barycentric coordinates of intersection point at earliest root
145 auto uvw = r.template Slice<3, 1>(1, 0);
146 SVector<TScalar, kDims> x = XT + t * (X - XT);
147 SVector<TScalar, kDims> a = AT + t * (A - AT);
148 SVector<TScalar, kDims> b = BT + t * (B - BT);
149 SVector<TScalar, kDims> c = CT + t * (C - CT);
151 bool const bIsInsideTriangle = All((uvw >= TScalar(0)) and (uvw <= TScalar(1)));
152 // Point and triangle intersect at t, if X(t) is inside the triangle (A(t), B(t), C(t))
153 r[0] = bIsInsideTriangle * t + (not bIsInsideTriangle) * r[0];
154 // Exit as soon as an intersection is found, since we are traversing roots from earliest
155 // to latest time of impact
156 return bIsInsideTriangle;
157 },
158 coeffs,
159 TScalar(0),
160 TScalar(1));
161 // Compute return value
162 r[0] = bIntersectionFound * r[0] + (not bIntersectionFound) * TScalar(-1);
163 return r;
164}
165
166namespace detail {
167
168template <
177 class TScalar>
178PBAT_HOST_DEVICE std::array<TScalar, 4> PointTriangleCcdUnivariatePolynomial(
179 TXT const& XT,
180 TAT const& AT,
181 TBT const& BT,
182 TCT const& CT,
183 TX const& X,
184 TA const& A,
185 TB const& B,
186 TC const& C)
187{
188 std::array<TScalar, 4> sigma;
189 TScalar const z0 = AT[0] * BT[1];
190 TScalar const z1 = z0 * CT[2];
191 TScalar const z2 = AT[0] * BT[2];
192 TScalar const z3 = z2 * XT[1];
193 TScalar const z4 = CT[1] * XT[2];
194 TScalar const z5 = z4 * AT[0];
195 TScalar const z6 = AT[1] * BT[0];
196 TScalar const z7 = z6 * XT[2];
197 TScalar const z8 = AT[1] * BT[2];
198 TScalar const z9 = z8 * CT[0];
199 TScalar const z10 = CT[2] * XT[0];
200 TScalar const z11 = z10 * AT[1];
201 TScalar const z12 = AT[2] * BT[0];
202 TScalar const z13 = z12 * CT[1];
203 TScalar const z14 = AT[2] * BT[1];
204 TScalar const z15 = z14 * XT[0];
205 TScalar const z16 = CT[0] * XT[1];
206 TScalar const z17 = z16 * AT[2];
207 TScalar const z18 = CT[2] * XT[1];
208 TScalar const z19 = z18 * BT[0];
209 TScalar const z20 = CT[0] * XT[2];
210 TScalar const z21 = z20 * BT[1];
211 TScalar const z22 = CT[1] * XT[0];
212 TScalar const z23 = z22 * BT[2];
213 TScalar const z24 = z0 * XT[2];
214 TScalar const z25 = z2 * CT[1];
215 TScalar const z26 = z18 * AT[0];
216 TScalar const z27 = z6 * CT[2];
217 TScalar const z28 = z8 * XT[0];
218 TScalar const z29 = z20 * AT[1];
219 TScalar const z30 = z12 * XT[1];
220 TScalar const z31 = z14 * CT[0];
221 TScalar const z32 = z22 * AT[2];
222 TScalar const z33 = z4 * BT[0];
223 TScalar const z34 = z10 * BT[1];
224 TScalar const z35 = z16 * BT[2];
225 TScalar const z36 = 3 * z1;
226 TScalar const z37 = 3 * z3;
227 TScalar const z38 = 3 * z5;
228 TScalar const z39 = 3 * z7;
229 TScalar const z40 = 3 * z9;
230 TScalar const z41 = 3 * z11;
231 TScalar const z42 = 3 * z13;
232 TScalar const z43 = 3 * z15;
233 TScalar const z44 = 3 * z17;
234 TScalar const z45 = 3 * z19;
235 TScalar const z46 = 3 * z21;
236 TScalar const z47 = 3 * z23;
237 TScalar const z48 = 3 * z24;
238 TScalar const z49 = 3 * z25;
239 TScalar const z50 = 3 * z26;
240 TScalar const z51 = 3 * z27;
241 TScalar const z52 = 3 * z28;
242 TScalar const z53 = 3 * z29;
243 TScalar const z54 = 3 * z30;
244 TScalar const z55 = 3 * z31;
245 TScalar const z56 = 3 * z32;
246 TScalar const z57 = 3 * z33;
247 TScalar const z58 = 3 * z34;
248 TScalar const z59 = 3 * z35;
249 TScalar const z60 = A[0] * BT[1];
250 TScalar const z61 = A[0] * BT[2];
251 TScalar const z62 = A[1] * BT[0];
252 TScalar const z63 = A[1] * BT[2];
253 TScalar const z64 = A[2] * BT[0];
254 TScalar const z65 = A[2] * BT[1];
255 TScalar const z66 = AT[0] * B[1];
256 TScalar const z67 = AT[0] * B[2];
257 TScalar const z68 = C[1] * XT[2];
258 TScalar const z69 = CT[1] * X[2];
259 TScalar const z70 = AT[1] * B[0];
260 TScalar const z71 = AT[1] * B[2];
261 TScalar const z72 = C[2] * XT[0];
262 TScalar const z73 = CT[2] * X[0];
263 TScalar const z74 = AT[2] * B[0];
264 TScalar const z75 = AT[2] * B[1];
265 TScalar const z76 = C[0] * XT[1];
266 TScalar const z77 = CT[0] * X[1];
267 TScalar const z78 = C[2] * XT[1];
268 TScalar const z79 = CT[2] * X[1];
269 TScalar const z80 = C[0] * XT[2];
270 TScalar const z81 = CT[0] * X[2];
271 TScalar const z82 = C[1] * XT[0];
272 TScalar const z83 = CT[1] * X[0];
273 TScalar const z84 =
274 z0 * C[2] + z10 * A[1] + z12 * C[1] + z14 * X[0] + z16 * A[2] + z18 * B[0] + z2 * X[1] +
275 z20 * B[1] + z22 * B[2] + z4 * A[0] + z6 * X[2] + z60 * CT[2] + z61 * XT[1] + z62 * XT[2] +
276 z63 * CT[0] + z64 * CT[1] + z65 * XT[0] + z66 * CT[2] + z67 * XT[1] + z68 * AT[0] +
277 z69 * AT[0] + z70 * XT[2] + z71 * CT[0] + z72 * AT[1] + z73 * AT[1] + z74 * CT[1] +
278 z75 * XT[0] + z76 * AT[2] + z77 * AT[2] + z78 * BT[0] + z79 * BT[0] + z8 * C[0] +
279 z80 * BT[1] + z81 * BT[1] + z82 * BT[2] + z83 * BT[2] - A[0] * BT[1] * XT[2] -
280 A[0] * BT[2] * CT[1] - A[0] * CT[2] * XT[1] - A[1] * BT[0] * CT[2] - A[1] * BT[2] * XT[0] -
281 A[1] * CT[0] * XT[2] - A[2] * BT[0] * XT[1] - A[2] * BT[1] * CT[0] - A[2] * CT[1] * XT[0] -
282 AT[0] * B[1] * XT[2] - AT[0] * B[2] * CT[1] - AT[0] * BT[1] * X[2] - AT[0] * BT[2] * C[1] -
283 AT[0] * C[2] * XT[1] - AT[0] * CT[2] * X[1] - AT[1] * B[0] * CT[2] - AT[1] * B[2] * XT[0] -
284 AT[1] * BT[0] * C[2] - AT[1] * BT[2] * X[0] - AT[1] * C[0] * XT[2] - AT[1] * CT[0] * X[2] -
285 AT[2] * B[0] * XT[1] - AT[2] * B[1] * CT[0] - AT[2] * BT[0] * X[1] - AT[2] * BT[1] * C[0] -
286 AT[2] * C[1] * XT[0] - AT[2] * CT[1] * X[0] - B[0] * CT[1] * XT[2] - B[1] * CT[2] * XT[0] -
287 B[2] * CT[0] * XT[1] - BT[0] * C[1] * XT[2] - BT[0] * CT[1] * X[2] - BT[1] * C[2] * XT[0] -
288 BT[1] * CT[2] * X[0] - BT[2] * C[0] * XT[1] - BT[2] * CT[0] * X[1];
289 TScalar const z85 = A[0] * B[1];
290 TScalar const z86 = z85 * CT[2];
291 TScalar const z87 = A[0] * B[2];
292 TScalar const z88 = z87 * XT[1];
293 TScalar const z89 = z60 * C[2];
294 TScalar const z90 = z61 * X[1];
295 TScalar const z91 = z68 * A[0];
296 TScalar const z92 = z69 * A[0];
297 TScalar const z93 = A[1] * B[0];
298 TScalar const z94 = z93 * XT[2];
299 TScalar const z95 = A[1] * B[2];
300 TScalar const z96 = z95 * CT[0];
301 TScalar const z97 = z62 * X[2];
302 TScalar const z98 = z63 * C[0];
303 TScalar const z99 = z72 * A[1];
304 TScalar const z100 = z73 * A[1];
305 TScalar const z101 = A[2] * B[0];
306 TScalar const z102 = z101 * CT[1];
307 TScalar const z103 = A[2] * B[1];
308 TScalar const z104 = z103 * XT[0];
309 TScalar const z105 = z64 * C[1];
310 TScalar const z106 = z65 * X[0];
311 TScalar const z107 = z76 * A[2];
312 TScalar const z108 = z77 * A[2];
313 TScalar const z109 = z66 * C[2];
314 TScalar const z110 = z67 * X[1];
315 TScalar const z111 = C[1] * X[2];
316 TScalar const z112 = z111 * AT[0];
317 TScalar const z113 = z70 * X[2];
318 TScalar const z114 = z71 * C[0];
319 TScalar const z115 = C[2] * X[0];
320 TScalar const z116 = z115 * AT[1];
321 TScalar const z117 = z74 * C[1];
322 TScalar const z118 = z75 * X[0];
323 TScalar const z119 = C[0] * X[1];
324 TScalar const z120 = z119 * AT[2];
325 TScalar const z121 = z78 * B[0];
326 TScalar const z122 = z79 * B[0];
327 TScalar const z123 = z80 * B[1];
328 TScalar const z124 = z81 * B[1];
329 TScalar const z125 = z82 * B[2];
330 TScalar const z126 = z83 * B[2];
331 TScalar const z127 = C[2] * X[1];
332 TScalar const z128 = z127 * BT[0];
333 TScalar const z129 = C[0] * X[2];
334 TScalar const z130 = z129 * BT[1];
335 TScalar const z131 = C[1] * X[0];
336 TScalar const z132 = z131 * BT[2];
337 TScalar const z133 = z85 * XT[2];
338 TScalar const z134 = z87 * CT[1];
339 TScalar const z135 = z60 * X[2];
340 TScalar const z136 = z61 * C[1];
341 TScalar const z137 = z78 * A[0];
342 TScalar const z138 = z79 * A[0];
343 TScalar const z139 = z93 * CT[2];
344 TScalar const z140 = z95 * XT[0];
345 TScalar const z141 = z62 * C[2];
346 TScalar const z142 = z63 * X[0];
347 TScalar const z143 = z80 * A[1];
348 TScalar const z144 = z81 * A[1];
349 TScalar const z145 = z101 * XT[1];
350 TScalar const z146 = z103 * CT[0];
351 TScalar const z147 = z64 * X[1];
352 TScalar const z148 = z65 * C[0];
353 TScalar const z149 = z82 * A[2];
354 TScalar const z150 = z83 * A[2];
355 TScalar const z151 = z66 * X[2];
356 TScalar const z152 = z67 * C[1];
357 TScalar const z153 = z127 * AT[0];
358 TScalar const z154 = z70 * C[2];
359 TScalar const z155 = z71 * X[0];
360 TScalar const z156 = z129 * AT[1];
361 TScalar const z157 = z74 * X[1];
362 TScalar const z158 = z75 * C[0];
363 TScalar const z159 = z131 * AT[2];
364 TScalar const z160 = z68 * B[0];
365 TScalar const z161 = z69 * B[0];
366 TScalar const z162 = z72 * B[1];
367 TScalar const z163 = z73 * B[1];
368 TScalar const z164 = z76 * B[2];
369 TScalar const z165 = z77 * B[2];
370 TScalar const z166 = z111 * BT[0];
371 TScalar const z167 = z115 * BT[1];
372 TScalar const z168 = z119 * BT[2];
373 sigma[0] = -z1 - z11 - z13 - z15 - z17 - z19 - z21 - z23 + z24 + z25 + z26 + z27 + z28 + z29 -
374 z3 + z30 + z31 + z32 + z33 + z34 + z35 - z5 - z7 - z9;
375 sigma[1] = z36 + z37 + z38 + z39 + z40 + z41 + z42 + z43 + z44 + z45 + z46 + z47 - z48 - z49 -
376 z50 - z51 - z52 - z53 - z54 - z55 - z56 - z57 - z58 - z59 - z84;
377 sigma[2] =
378 -2 * z0 * X[2] - 2 * z10 * B[1] - z100 - z102 - z104 - z105 - z106 - z107 - z108 - z109 -
379 z110 - z112 - z113 - z114 - z116 - z117 - z118 - 2 * z12 * X[1] - z120 - z121 - z122 -
380 z123 - z124 - z125 - z126 - z128 - z130 - z132 + z133 + z134 + z135 + z136 + z137 + z138 +
381 z139 - 2 * z14 * C[0] + z140 + z141 + z142 + z143 + z144 + z145 + z146 + z147 + z148 +
382 z149 + z150 + z151 + z152 + z153 + z154 + z155 + z156 + z157 + z158 + z159 -
383 2 * z16 * B[2] + z160 + z161 + z162 + z163 + z164 + z165 + z166 + z167 + z168 -
384 2 * z18 * A[0] - 2 * z2 * C[1] - 2 * z20 * A[1] - 2 * z22 * A[2] - z36 - z37 - z38 - z39 -
385 2 * z4 * B[0] - z40 - z41 - z42 - z43 - z44 - z45 - z46 - z47 + z48 + z49 + z50 + z51 +
386 z52 + z53 + z54 + z55 + z56 + z57 + z58 + z59 - 2 * z6 * C[2] - 2 * z60 * XT[2] -
387 2 * z61 * CT[1] - 2 * z62 * CT[2] - 2 * z63 * XT[0] - 2 * z64 * XT[1] - 2 * z65 * CT[0] -
388 2 * z66 * XT[2] - 2 * z67 * CT[1] - 2 * z68 * BT[0] - 2 * z69 * BT[0] - 2 * z70 * CT[2] -
389 2 * z71 * XT[0] - 2 * z72 * BT[1] - 2 * z73 * BT[1] - 2 * z74 * XT[1] - 2 * z75 * CT[0] -
390 2 * z76 * BT[2] - 2 * z77 * BT[2] - 2 * z78 * AT[0] - 2 * z79 * AT[0] - 2 * z8 * X[0] -
391 2 * z80 * AT[1] - 2 * z81 * AT[1] - 2 * z82 * AT[2] - 2 * z83 * AT[2] - z86 - z88 - z89 -
392 z90 - z91 - z92 - z94 - z96 - z97 - z98 - z99 + 2 * A[0] * BT[1] * CT[2] +
393 2 * A[0] * BT[2] * XT[1] + 2 * A[0] * CT[1] * XT[2] + 2 * A[1] * BT[0] * XT[2] +
394 2 * A[1] * BT[2] * CT[0] + 2 * A[1] * CT[2] * XT[0] + 2 * A[2] * BT[0] * CT[1] +
395 2 * A[2] * BT[1] * XT[0] + 2 * A[2] * CT[0] * XT[1] + 2 * AT[0] * B[1] * CT[2] +
396 2 * AT[0] * B[2] * XT[1] + 2 * AT[0] * BT[1] * C[2] + 2 * AT[0] * BT[2] * X[1] +
397 2 * AT[0] * C[1] * XT[2] + 2 * AT[0] * CT[1] * X[2] + 2 * AT[1] * B[0] * XT[2] +
398 2 * AT[1] * B[2] * CT[0] + 2 * AT[1] * BT[0] * X[2] + 2 * AT[1] * BT[2] * C[0] +
399 2 * AT[1] * C[2] * XT[0] + 2 * AT[1] * CT[2] * X[0] + 2 * AT[2] * B[0] * CT[1] +
400 2 * AT[2] * B[1] * XT[0] + 2 * AT[2] * BT[0] * C[1] + 2 * AT[2] * BT[1] * X[0] +
401 2 * AT[2] * C[0] * XT[1] + 2 * AT[2] * CT[0] * X[1] + 2 * B[0] * CT[2] * XT[1] +
402 2 * B[1] * CT[0] * XT[2] + 2 * B[2] * CT[1] * XT[0] + 2 * BT[0] * C[2] * XT[1] +
403 2 * BT[0] * CT[2] * X[1] + 2 * BT[1] * C[0] * XT[2] + 2 * BT[1] * CT[0] * X[2] +
404 2 * BT[2] * C[1] * XT[0] + 2 * BT[2] * CT[1] * X[0];
405 sigma[3] = z1 + z100 - z101 * C[1] + z102 - z103 * X[0] + z104 + z105 + z106 + z107 + z108 +
406 z109 + z11 + z110 - z111 * A[0] + z112 + z113 + z114 - z115 * A[1] + z116 + z117 +
407 z118 - z119 * A[2] + z120 + z121 + z122 + z123 + z124 + z125 + z126 - z127 * B[0] +
408 z128 - z129 * B[1] + z13 + z130 - z131 * B[2] + z132 - z133 - z134 - z135 - z136 -
409 z137 - z138 - z139 - z140 - z141 - z142 - z143 - z144 - z145 - z146 - z147 - z148 -
410 z149 + z15 - z150 - z151 - z152 - z153 - z154 - z155 - z156 - z157 - z158 - z159 -
411 z160 - z161 - z162 - z163 - z164 - z165 - z166 - z167 - z168 + z17 + z19 + z21 +
412 z23 - z24 - z25 - z26 - z27 - z28 - z29 + z3 - z30 - z31 - z32 - z33 - z34 - z35 +
413 z5 + z7 - z84 - z85 * C[2] + z86 - z87 * X[1] + z88 + z89 + z9 + z90 + z91 + z92 -
414 z93 * X[2] + z94 - z95 * C[0] + z96 + z97 + z98 + z99 + A[0] * B[1] * X[2] +
415 A[0] * B[2] * C[1] + A[0] * C[2] * X[1] + A[1] * B[0] * C[2] + A[1] * B[2] * X[0] +
416 A[1] * C[0] * X[2] + A[2] * B[0] * X[1] + A[2] * B[1] * C[0] + A[2] * C[1] * X[0] +
417 B[0] * C[1] * X[2] + B[1] * C[2] * X[0] + B[2] * C[0] * X[1];
418 return sigma;
419}
420
421} // namespace detail
422
423} // namespace pbat::geometry
424
425#endif // PBAT_GEOMETRY_POINTTRIANGLECCD_H
This file contains functions to answer intersection queries.
PBAT_HOST_DEVICE std::array< TScalar, 4 > PointTriangleCcdUnivariatePolynomial(TXT const &XT, TAT const &AT, TBT const &BT, TCT const &CT, TX const &X, TA const &A, TB const &B, TC const &C)
Computes the univariate linearly swept point-triangle co-planarity polynomial.
Definition PointTriangleCcd.h:178
Root-finders for polynomial of arbitrary degree.
PBAT_HOST_DEVICE auto TriangleBarycentricCoordinates(TMatrixAP const &AP, TMatrixAB const &AB, TMatrixAC const &AC) -> mini::SMatrix< typename TMatrixAP::ScalarType, 3, 1 >
Computes the barycentric coordinates of point P with respect to the triangle spanned by vertices A,...
Definition IntersectionQueries.h:45
Geometric queries, quantities and data structures.
Definition AabbKdTreeHierarchy.h:23
PBAT_HOST_DEVICE auto PointTriangleCcd(TXT const &XT, TAT const &AT, TBT const &BT, TCT const &CT, TX const &X, TA const &A, TB const &B, TC const &C) -> math::linalg::mini::SVector< TScalar, 4 >
Computes the time of impact and barycentric coordinates of the intersection point between a point a...
Definition PointTriangleCcd.h:125
Mini linear algebra related functionality.
Definition Assign.h:12
bool ForEachRoot(FOnRoot &&fOnRoot, std::array< TScalar, N+1 > const &coeffs, TScalar min=std::numeric_limits< TScalar >::lowest(), TScalar max=std::numeric_limits< TScalar >::max())
Computes the each real root of a degree N polynomial in the range [min,max] in increasing order.
Definition Roots.h:3153