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
IntersectionQueries.h
Go to the documentation of this file.
1
10
11#ifndef PBAT_GEOMETRY_INTERSECTIONQUERIES_H
12#define PBAT_GEOMETRY_INTERSECTIONQUERIES_H
13
14#include "pbat/Aliases.h"
15#include "pbat/HostDevice.h"
16#include "pbat/math/linalg/mini/Mini.h"
17
18#include <array>
19#include <cmath>
20#include <optional>
21
22namespace pbat {
23namespace geometry {
27namespace IntersectionQueries {
28namespace mini = math::linalg::mini;
29
43template <mini::CMatrix TMatrixAP, mini::CMatrix TMatrixAB, mini::CMatrix TMatrixAC>
44PBAT_HOST_DEVICE auto
45TriangleBarycentricCoordinates(
46 TMatrixAP const& AP,
47 TMatrixAB const& AB,
48 TMatrixAC const& AC)
49 -> mini::SMatrix<typename TMatrixAP::ScalarType, 3, 1>
50{
51 using ScalarType = typename TMatrixAP::ScalarType;
52 using namespace mini;
53 ScalarType const d00 = Dot(AB, AB);
54 ScalarType const d01 = Dot(AB, AC);
55 ScalarType const d11 = Dot(AC, AC);
56 ScalarType const d20 = Dot(AP, AB);
57 ScalarType const d21 = Dot(AP, AC);
58 ScalarType const denom = d00 * d11 - d01 * d01;
59 ScalarType const v = (d11 * d20 - d01 * d21) / denom;
60 ScalarType const w = (d00 * d21 - d01 * d20) / denom;
61 ScalarType const u = ScalarType(1) - v - w;
62 SMatrix<ScalarType, 3, 1> uvw{};
63 uvw(0, 0) = u;
64 uvw(1, 0) = v;
65 uvw(2, 0) = w;
66 return uvw;
67};
68
84template <
85 mini::CMatrix TMatrixP,
86 mini::CMatrix TMatrixA,
87 mini::CMatrix TMatrixB,
88 mini::CMatrix TMatrixC>
89PBAT_HOST_DEVICE auto TriangleBarycentricCoordinates(
90 TMatrixP const& P,
91 TMatrixA const& A,
92 TMatrixB const& B,
93 TMatrixC const& C) -> mini::SVector<typename TMatrixP::ScalarType, 3>
94{
95 using ScalarType = typename TMatrixP::ScalarType;
96 auto constexpr kRows = TMatrixP::kRows;
97 mini::SVector<ScalarType, kRows> const AP = P - A;
98 mini::SVector<ScalarType, kRows> const AB = B - A;
99 mini::SVector<ScalarType, kRows> const AC = C - A;
100 return TriangleBarycentricCoordinates(AP, AB, AC);
101};
102
115template <
116 mini::CMatrix TMatrixL1,
117 mini::CMatrix TMatrixU1,
118 mini::CMatrix TMatrixL2,
119 mini::CMatrix TMatrixU2>
120PBAT_HOST_DEVICE auto AxisAlignedBoundingBoxes(
121 TMatrixL1 const& L1,
122 TMatrixU1 const& U1,
123 TMatrixL2 const& L2,
124 TMatrixU2 const& U2) -> mini::SMatrix<typename TMatrixL1::ScalarType, TMatrixL1::Rows, 2>;
125
137template <mini::CMatrix TMatrixP, mini::CMatrix TMatrixQ, mini::CMatrix TMatrixC>
138PBAT_HOST_DEVICE auto LineSegmentSphere(
139 TMatrixP const& P,
140 TMatrixQ const& Q,
141 TMatrixC const& C,
142 typename TMatrixC::ScalarType R)
143 -> std::optional<mini::SVector<typename TMatrixP::ScalarType, TMatrixP::kRows>>;
144
161template <
162 mini::CMatrix TMatrixP,
163 mini::CMatrix TMatrixQ,
164 mini::CMatrix TMatrixA,
165 mini::CMatrix TMatrixB,
166 mini::CMatrix TMatrixC>
167PBAT_HOST_DEVICE auto LineSegmentPlane3D(
168 TMatrixP const& P,
169 TMatrixQ const& Q,
170 TMatrixA const& A,
171 TMatrixB const& B,
172 TMatrixC const& C)
173 -> std::optional<mini::SVector<typename TMatrixP::ScalarType, TMatrixP::kRows>>;
174
188template <mini::CMatrix TMatrixP, mini::CMatrix TMatrixQ, mini::CMatrix TMatrixN>
189PBAT_HOST_DEVICE auto LineSegmentPlane3D(
190 TMatrixP const& P,
191 TMatrixQ const& Q,
192 TMatrixN const& n,
193 typename TMatrixN::ScalarType d)
194 -> std::optional<mini::SVector<typename TMatrixP::ScalarType, TMatrixP::kRows>>;
195
212template <
213 mini::CMatrix TMatrixP,
214 mini::CMatrix TMatrixQ,
215 mini::CMatrix TMatrixA,
216 mini::CMatrix TMatrixB,
217 mini::CMatrix TMatrixC>
218PBAT_HOST_DEVICE auto UvwLineTriangle3D(
219 TMatrixP const& P,
220 TMatrixQ const& Q,
221 TMatrixA const& A,
222 TMatrixB const& B,
223 TMatrixC const& C) -> std::optional<mini::SVector<typename TMatrixP::ScalarType, 3>>;
224
243template <
244 mini::CMatrix TMatrixP,
245 mini::CMatrix TMatrixQ,
246 mini::CMatrix TMatrixA,
247 mini::CMatrix TMatrixB,
248 mini::CMatrix TMatrixC>
249PBAT_HOST_DEVICE auto UvwLineSegmentTriangle3D(
250 TMatrixP const& P,
251 TMatrixQ const& Q,
252 TMatrixA const& A,
253 TMatrixB const& B,
254 TMatrixC const& C) -> std::optional<mini::SVector<typename TMatrixP::ScalarType, 4>>;
255
275template <
276 mini::CMatrix TMatrixA1,
277 mini::CMatrix TMatrixB1,
278 mini::CMatrix TMatrixC1,
279 mini::CMatrix TMatrixA2,
280 mini::CMatrix TMatrixB2,
281 mini::CMatrix TMatrixC2>
282PBAT_HOST_DEVICE auto UvwTriangles3D(
283 TMatrixA1 const& A1,
284 TMatrixB1 const& B1,
285 TMatrixC1 const& C1,
286 TMatrixA2 const& A2,
287 TMatrixB2 const& B2,
288 TMatrixC2 const& C2)
289 -> std::array<std::optional<mini::SVector<typename TMatrixA1::ScalarType, 3>>, 6u>;
290
291template <
292 mini::CMatrix TMatrixL1,
293 mini::CMatrix TMatrixU1,
294 mini::CMatrix TMatrixL2,
295 mini::CMatrix TMatrixU2>
296PBAT_HOST_DEVICE auto AxisAlignedBoundingBoxes(
297 TMatrixL1 const& L1,
298 TMatrixU1 const& U1,
299 TMatrixL2 const& L2,
300 TMatrixU2 const& U2) -> mini::SMatrix<typename TMatrixL1::ScalarType, TMatrixL1::Rows, 2>
301{
302 using ScalarType = typename TMatrixL1::ScalarType;
303 mini::SMatrix<ScalarType, TMatrixL1::Rows, 2> LU;
304 LU.Col(0) = Max(L1, L2);
305 LU.Col(1) = Min(U1, U2);
306 return LU;
307}
308
309template <mini::CMatrix TMatrixP, mini::CMatrix TMatrixQ, mini::CMatrix TMatrixC>
310PBAT_HOST_DEVICE auto LineSegmentSphere(
311 TMatrixP const& P,
312 TMatrixQ const& Q,
313 TMatrixC const& C,
314 typename TMatrixC::ScalarType R)
315 -> std::optional<mini::SVector<typename TMatrixP::ScalarType, TMatrixP::kRows>>
316{
317 using ScalarType = typename TMatrixP::ScalarType;
318 auto constexpr kRows = TMatrixP::kRows;
319 mini::SVector<ScalarType, kRows> const PQ = (Q - P);
320 ScalarType const len = Norm(PQ);
321 mini::SVector<ScalarType, kRows> const d = PQ / len;
322 mini::SVector<ScalarType, kRows> const m = P - C;
323 ScalarType const b = Dot(m, d);
324 ScalarType const c = Dot(m, m) - (R * R);
325 // Exit if r's origin outside s (c > 0) and r pointing away from s (b > 0)
326 if (c > ScalarType(0) and b > ScalarType(0))
327 return {};
328 ScalarType const discr = b * b - c;
329 // A negative discriminant corresponds to ray missing sphere
330 if (discr < ScalarType(0))
331 return {};
332 // Ray now found to intersect sphere, compute smallest t value of intersection
333 using namespace std;
334 ScalarType t = -b - sqrt(discr);
335 // If t is negative, ray started inside sphere so clamp t to zero
336 if (t < ScalarType(0))
337 t = ScalarType(0);
338 // If the intersection point lies beyond the segment PQ, then return nullopt
339 if (t > len)
340 return {};
341
342 mini::SVector<ScalarType, kRows> I = P + t * d;
343 return I;
344}
345
346template <
347 mini::CMatrix TMatrixP,
348 mini::CMatrix TMatrixQ,
349 mini::CMatrix TMatrixA,
350 mini::CMatrix TMatrixB,
351 mini::CMatrix TMatrixC>
352PBAT_HOST_DEVICE auto LineSegmentPlane3D(
353 TMatrixP const& P,
354 TMatrixQ const& Q,
355 TMatrixA const& A,
356 TMatrixB const& B,
357 TMatrixC const& C)
358 -> std::optional<mini::SVector<typename TMatrixP::ScalarType, TMatrixP::kRows>>
359{
360 using ScalarType = typename TMatrixP::ScalarType;
361 auto constexpr kRows = TMatrixP::kRows;
362 auto constexpr kDims = 3;
363 static_assert(kRows == kDims, "This overlap test is specialized for 3D");
364 // Intersect segment ab against plane of triangle def. If intersecting,
365 // return t value and position q of intersection
366 mini::SVector<ScalarType, kRows> const n = Cross(B - A, C - A);
367 ScalarType const d = Dot(n, A);
368 return LineSegmentPlane3D(P, Q, n, d);
369}
370
371template <mini::CMatrix TMatrixP, mini::CMatrix TMatrixQ, mini::CMatrix TMatrixN>
372PBAT_HOST_DEVICE auto LineSegmentPlane3D(
373 TMatrixP const& P,
374 TMatrixQ const& Q,
375 TMatrixN const& n,
376 typename TMatrixN::ScalarType d)
377 -> std::optional<mini::SVector<typename TMatrixP::ScalarType, TMatrixP::kRows>>
378{
379 using ScalarType = typename TMatrixP::ScalarType;
380 auto constexpr kRows = TMatrixP::kRows;
381 // Compute the t value for the directed line ab intersecting the plane
382 mini::SVector<ScalarType, kRows> const PQ = Q - P;
383 ScalarType const t = (d - Dot(n, P)) / Dot(n, PQ);
384 // If t in [0..1] compute and return intersection point
385 if (t >= ScalarType(0) and t <= ScalarType(1))
386 {
387 auto I = P + t * PQ;
388 return I;
389 }
390 // Else no intersection
391 return {};
392}
393
394template <
395 mini::CMatrix TMatrixP,
396 mini::CMatrix TMatrixQ,
397 mini::CMatrix TMatrixA,
398 mini::CMatrix TMatrixB,
399 mini::CMatrix TMatrixC>
400PBAT_HOST_DEVICE auto UvwLineTriangle3D(
401 TMatrixP const& P,
402 TMatrixQ const& Q,
403 TMatrixA const& A,
404 TMatrixB const& B,
405 TMatrixC const& C) -> std::optional<mini::SVector<typename TMatrixP::ScalarType, 3>>
406{
407 using ScalarType = typename TMatrixP::ScalarType;
408 auto constexpr kRows = TMatrixP::kRows;
409 auto constexpr kDims = 3;
410 static_assert(kRows == kDims, "This overlap test is specialized for 3D");
414 mini::SVector<ScalarType, kDims> const PQ = Q - P;
415 mini::SVector<ScalarType, kDims> const PA = A - P;
416 mini::SVector<ScalarType, kDims> const PB = B - P;
417 mini::SVector<ScalarType, kDims> const PC = C - P;
418 // Test if pq is inside the edges bc, ca and ab. Done by testing
419 // that the signed tetrahedral volumes, computed using ScalarType triple
420 // products, are all non-zero
421 mini::SVector<ScalarType, kDims> const m = Cross(PQ, PC);
422 mini::SVector<ScalarType, kDims> uvw = mini::Zeros<ScalarType, kDims, 1>();
423
424 ScalarType& u = uvw(0);
425 ScalarType& v = uvw(1);
426 ScalarType& w = uvw(2);
427 u = Dot(PB, m);
428 v = -Dot(PA, m);
429 using namespace std;
430 auto const SameSign = [](ScalarType a, ScalarType b) -> bool {
431 return signbit(a) == signbit(b);
432 };
433 if (not SameSign(u, v))
434 return {};
435 w = Dot(PQ, Cross(PB, PA));
436 if (not SameSign(u, w))
437 return {};
438
439 ScalarType constexpr eps = 1e-15;
440 ScalarType const uvwSum = u + v + w;
441 bool const bIsLineInPlaneOfTriangle = abs(uvwSum) < eps;
442 if (bIsLineInPlaneOfTriangle)
443 {
444 // WARNING:
445 // Technically, if the line is in the plane of the triangle, it is intersecting
446 // the triangle in a line segment, hence there are an infinite number of solutions.
447 // However, I don't want to spend too much compute power to return one of those
448 // solutions. If we ever need this feature in the future, we'll implement it at
449 // that point in time.
450 return {};
451 }
452
453 // Compute the barycentric coordinates (u, v, w) determining the
454 // intersection point r, r = u*a + v*b + w*c
455 ScalarType const denom = ScalarType(1) / (uvwSum);
456 u *= denom;
457 v *= denom;
458 w *= denom; // w = ScalarType(1) - u - v;
459 return uvw;
460}
461
462template <
463 mini::CMatrix TMatrixP,
464 mini::CMatrix TMatrixQ,
465 mini::CMatrix TMatrixA,
466 mini::CMatrix TMatrixB,
467 mini::CMatrix TMatrixC>
468PBAT_HOST_DEVICE auto UvwLineSegmentTriangle3D(
469 TMatrixP const& P,
470 TMatrixQ const& Q,
471 TMatrixA const& A,
472 TMatrixB const& B,
473 TMatrixC const& C) -> std::optional<mini::SVector<typename TMatrixP::ScalarType, 4>>
474{
475 using ScalarType = typename TMatrixP::ScalarType;
476 auto constexpr kRows = TMatrixP::kRows;
477 auto constexpr kDims = 3;
478 static_assert(kRows == kDims, "This overlap test is specialized for 3D");
479 mini::SVector<ScalarType, kDims> const AB = B - A;
480 mini::SVector<ScalarType, kDims> const AC = C - A;
481 mini::SVector<ScalarType, kDims> const PQ = Q - P;
482 mini::SVector<ScalarType, kDims> const n = Cross(AB, AC);
483 // Compute denominator d. If d == 0, segment is parallel to triangle, so exit early
484 ScalarType constexpr eps = static_cast<ScalarType>(1e-15);
485 ScalarType const d = Dot(PQ, n);
486 using namespace std;
487 bool const bIsSegmentParallelToTriangle = abs(d) < eps;
488 if (bIsSegmentParallelToTriangle)
489 return {};
490 // Compute intersection t value of pq with plane of triangle. A ray
491 // intersects iff 0 <= t. Segment intersects iff 0 <= t <= ScalarType(1).
492 ScalarType const t = Dot(n, A - P) / d;
493 if (t < ScalarType(0) or t > ScalarType(1))
494 return {};
495 // Compute barycentric coordinate components and test if within bounds
496 mini::SVector<ScalarType, kDims> const I = P + t * PQ;
497 mini::SVector<ScalarType, 4> uvwt;
498 auto uvw = uvwt.template Slice<3, 1>(1, 0);
499 uvw = TriangleBarycentricCoordinates(I, A, B, C);
500 bool const bIsInsideTriangle = All((uvw >= ScalarType(0)) and (uvw <= ScalarType(1)));
501 if (not bIsInsideTriangle)
502 return {};
503 uvwt(0) = t;
504 return uvwt;
505}
506
507template <
508 mini::CMatrix TMatrixA1,
509 mini::CMatrix TMatrixB1,
510 mini::CMatrix TMatrixC1,
511 mini::CMatrix TMatrixA2,
512 mini::CMatrix TMatrixB2,
513 mini::CMatrix TMatrixC2>
514PBAT_HOST_DEVICE auto UvwTriangles3D(
515 TMatrixA1 const& A1,
516 TMatrixB1 const& B1,
517 TMatrixC1 const& C1,
518 TMatrixA2 const& A2,
519 TMatrixB2 const& B2,
520 TMatrixC2 const& C2)
521 -> std::array<std::optional<mini::SVector<typename TMatrixA1::ScalarType, 3>>, 6u>
522{
523 using ScalarType = typename TMatrixA1::ScalarType;
524 auto constexpr kRows = TMatrixA1::kRows;
525 auto constexpr kDims = 3;
526 static_assert(kRows == kDims, "This overlap test is specialized for 3D");
527
528 using namespace std;
529 mini::SVector<ScalarType, kDims> const n1 = Normalized(Cross(B1 - A1, C1 - A1));
530 mini::SVector<ScalarType, kDims> const n2 = Normalized(Cross(B2 - A2, C2 - A2));
531 ScalarType constexpr eps = 1e-15;
532 bool const bAreTrianglesCoplanar = (ScalarType(1) - abs(Dot(n1, n2))) < eps;
533 if (bAreTrianglesCoplanar)
534 {
535 // NOTE: If triangles are coplanar and vertex of one triangle is in the plane of the other
536 // triangle, then they are intersecting. We do not handle this case for now.
537 return {};
538 }
539 // NOTE: Maybe handle degenerate triangles? Or maybe we should require the user to check
540 // that.
541 // - For a colinear triangle, we can perform line segment against triangle test.
542 // - For 2 colinear triangles, we can perform line segment against line segment test.
543 // - For a triangle collapsed to a point, we perform point in triangle test.
544 // - For 2 triangles collapsed to a point, we check if both points are numerically the same.
545
546 // Test 3 edges of each triangle against the other triangle
547 std::array<std::optional<mini::SVector<ScalarType, 3>>, 6u> intersections;
548 auto uvwt = UvwLineSegmentTriangle3D(A1, B1, A2, B2, C2);
549 #if defined(CUDART_VERSION)
550 #pragma nv_diag_suppress 174
551 #endif
552 if (uvwt)
553 intersections[0] = uvwt->template Slice<3, 1>(1, 0);
554 uvwt = UvwLineSegmentTriangle3D(B1, C1, A2, B2, C2);
555 if (uvwt)
556 intersections[1] = uvwt->template Slice<3, 1>(1, 0);
557 uvwt = UvwLineSegmentTriangle3D(C1, A1, A2, B2, C2);
558 if (uvwt)
559 intersections[2] = uvwt->template Slice<3, 1>(1, 0);
560 uvwt = UvwLineSegmentTriangle3D(A2, B2, A1, B1, C1);
561 if (uvwt)
562 intersections[3] = uvwt->template Slice<3, 1>(1, 0);
563 uvwt = UvwLineSegmentTriangle3D(B2, C2, A1, B1, C1);
564 if (uvwt)
565 intersections[4] = uvwt->template Slice<3, 1>(1, 0);
566 uvwt = UvwLineSegmentTriangle3D(C2, A2, A1, B1, C1);
567 if (uvwt)
568 intersections[5] = uvwt->template Slice<3, 1>(1, 0);
569 #if defined(CUDART_VERSION)
570 #pragma nv_diag_default 174
571 #endif
572 return intersections;
573}
574} // namespace IntersectionQueries
575} // namespace geometry
576} // namespace pbat
577
578#endif // PBAT_GEOMETRY_INTERSECTIONQUERIES_H
Mini.h
This file includes all the mini linear algebra headers.
pbat::geometry::IntersectionQueries
This namespace contains functions to answer intersection queries.
Definition IntersectionQueries.h:27
pbat::geometry::IntersectionQueries::LineSegmentPlane3D
PBAT_HOST_DEVICE auto LineSegmentPlane3D(TMatrixP const &P, TMatrixQ const &Q, TMatrixA const &A, TMatrixB const &B, TMatrixC const &C) -> std::optional< mini::SVector< typename TMatrixP::ScalarType, TMatrixP::kRows > >
Computes the intersection point, if any, between a line including points P,Q and the plane spanned by...
Definition IntersectionQueries.h:352
pbat::geometry::IntersectionQueries::UvwLineSegmentTriangle3D
PBAT_HOST_DEVICE auto UvwLineSegmentTriangle3D(TMatrixP const &P, TMatrixQ const &Q, TMatrixA const &A, TMatrixB const &B, TMatrixC const &C) -> std::optional< mini::SVector< typename TMatrixP::ScalarType, 4 > >
Computes the intersection point, if any, between a line segment delimited by points P,...
Definition IntersectionQueries.h:468
pbat::geometry::IntersectionQueries::LineSegmentSphere
PBAT_HOST_DEVICE auto LineSegmentSphere(TMatrixP const &P, TMatrixQ const &Q, TMatrixC const &C, typename TMatrixC::ScalarType R) -> std::optional< mini::SVector< typename TMatrixP::ScalarType, TMatrixP::kRows > >
Computes the intersection point, if any, between a line segment PQ and a sphere (C,...
Definition IntersectionQueries.h:310
pbat::geometry::IntersectionQueries::UvwLineTriangle3D
PBAT_HOST_DEVICE auto UvwLineTriangle3D(TMatrixP const &P, TMatrixQ const &Q, TMatrixA const &A, TMatrixB const &B, TMatrixC const &C) -> std::optional< mini::SVector< typename TMatrixP::ScalarType, 3 > >
Computes the intersection point, if any, between a line including points P,Q and a triangle ABC,...
Definition IntersectionQueries.h:400
pbat::geometry::IntersectionQueries::AxisAlignedBoundingBoxes
PBAT_HOST_DEVICE auto AxisAlignedBoundingBoxes(TMatrixL1 const &L1, TMatrixU1 const &U1, TMatrixL2 const &L2, TMatrixU2 const &U2) -> mini::SMatrix< typename TMatrixL1::ScalarType, TMatrixL1::Rows, 2 >
Computes the intersection volume between 2 axis aligned bounding boxes.
Definition IntersectionQueries.h:296
pbat::geometry::IntersectionQueries::UvwTriangles3D
PBAT_HOST_DEVICE auto UvwTriangles3D(TMatrixA1 const &A1, TMatrixB1 const &B1, TMatrixC1 const &C1, TMatrixA2 const &A2, TMatrixB2 const &B2, TMatrixC2 const &C2) -> std::array< std::optional< mini::SVector< typename TMatrixA1::ScalarType, 3 > >, 6u >
Computes intersection points between 3 edges (as line segments) of triangle A1B1C1 and triangle A2B2C...
Definition IntersectionQueries.h:514
pbat::geometry::IntersectionQueries::TriangleBarycentricCoordinates
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
pbat::geometry
Geometric queries, quantities and data structures.
Definition AabbKdTreeHierarchy.h:23
pbat::math::linalg::mini
Mini linear algebra related functionality.
Definition Assign.h:12
pbat
The main namespace of the library.
Definition Aliases.h:15