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
OverlapQueries.h
Go to the documentation of this file.
1
10
11#ifndef PBAT_GEOMETRY_OVERLAPQUERIES_H
12#define PBAT_GEOMETRY_OVERLAPQUERIES_H
13
14#include "ClosestPointQueries.h"
15#include "IntersectionQueries.h"
16#include "pbat/HostDevice.h"
17#include "pbat/common/ConstexprFor.h"
18#include "pbat/math/linalg/mini/Mini.h"
19
20#include <cmath>
21
22namespace pbat {
23namespace geometry {
27namespace OverlapQueries {
28
29namespace mini = math::linalg::mini;
30
41template <mini::CMatrix TMatrixP, mini::CMatrix TMatrixL, mini::CMatrix TMatrixU>
42PBAT_HOST_DEVICE bool
43PointAxisAlignedBoundingBox(TMatrixP const& P, TMatrixL const& L, TMatrixU const& U);
44
57template <
58 mini::CMatrix TMatrixP,
59 mini::CMatrix TMatrixA,
60 mini::CMatrix TMatrixB,
61 mini::CMatrix TMatrixC>
62PBAT_HOST_DEVICE bool
63PointTriangle(TMatrixP const& P, TMatrixA const& A, TMatrixB const& B, TMatrixC const& C);
64
79template <
80 mini::CMatrix TMatrixP,
81 mini::CMatrix TMatrixA,
82 mini::CMatrix TMatrixB,
83 mini::CMatrix TMatrixC,
84 mini::CMatrix TMatrixD>
85PBAT_HOST_DEVICE bool PointTetrahedron3D(
86 TMatrixP const& P,
87 TMatrixA const& A,
88 TMatrixB const& B,
89 TMatrixC const& C,
90 TMatrixD const& D);
91
102template <mini::CMatrix TMatrixC1, mini::CMatrix TMatrixC2>
103PBAT_HOST_DEVICE bool Spheres(
104 TMatrixC1 const& C1,
105 typename TMatrixC1::ScalarType R1,
106 TMatrixC2 const& C2,
107 typename TMatrixC2::ScalarType R2);
108
122template <
123 mini::CMatrix TMatrixL1,
124 mini::CMatrix TMatrixU1,
125 mini::CMatrix TMatrixL2,
126 mini::CMatrix TMatrixU2>
127PBAT_HOST_DEVICE bool AxisAlignedBoundingBoxes(
128 TMatrixL1 const& L1,
129 TMatrixU1 const& U1,
130 TMatrixL2 const& L2,
131 TMatrixU2 const& U2);
132
144template <mini::CMatrix TMatrixC, mini::CMatrix TMatrixL, mini::CMatrix TMatrixU>
145PBAT_HOST_DEVICE bool SphereAxisAlignedBoundingBox(
146 TMatrixC const& C,
147 typename TMatrixC::ScalarType R,
148 TMatrixL const& L,
149 TMatrixU const& U);
150
162template <mini::CMatrix TMatrixP, mini::CMatrix TMatrixQ, mini::CMatrix TMatrixC>
163PBAT_HOST_DEVICE bool LineSegmentSphere(
164 TMatrixP const& P,
165 TMatrixQ const& Q,
166 TMatrixC const& C,
167 typename TMatrixC::ScalarType R);
168
181template <
182 mini::CMatrix TMatrixP,
183 mini::CMatrix TMatrixQ,
184 mini::CMatrix TMatrixL,
185 mini::CMatrix TMatrixU>
186PBAT_HOST_DEVICE bool LineSegmentAxisAlignedBoundingBox(
187 TMatrixP const& P,
188 TMatrixQ const& Q,
189 TMatrixL const& L,
190 TMatrixU const& U);
191
206template <
207 mini::CMatrix TMatrixP,
208 mini::CMatrix TMatrixQ,
209 mini::CMatrix TMatrixA,
210 mini::CMatrix TMatrixB,
211 mini::CMatrix TMatrixC>
212PBAT_HOST_DEVICE bool LineSegmentTriangle3D(
213 TMatrixP const& P,
214 TMatrixQ const& Q,
215 TMatrixA const& A,
216 TMatrixB const& B,
217 TMatrixC const& C);
218
243template <
244 mini::CMatrix TMatrixP,
245 mini::CMatrix TMatrixQ,
246 mini::CMatrix TMatrixA1,
247 mini::CMatrix TMatrixB1,
248 mini::CMatrix TMatrixC1,
249 mini::CMatrix TMatrixA2,
250 mini::CMatrix TMatrixB2,
251 mini::CMatrix TMatrixC2>
252PBAT_HOST_DEVICE bool LineSegmentSweptTriangle3D(
253 TMatrixP const& P,
254 TMatrixQ const& Q,
255 TMatrixA1 const& A1,
256 TMatrixB1 const& B1,
257 TMatrixC1 const& C1,
258 TMatrixA2 const& A2,
259 TMatrixB2 const& B2,
260 TMatrixC2 const& C2);
261
274template <
275 mini::CMatrix TMatrixP,
276 mini::CMatrix TMatrixN,
277 mini::CMatrix TMatrixL,
278 mini::CMatrix TMatrixU>
279PBAT_HOST_DEVICE bool PlaneAxisAlignedBoundingBox(
280 TMatrixP const& P,
281 TMatrixN const& n,
282 TMatrixL const& L,
283 TMatrixU const& U);
284
299template <
300 mini::CMatrix TMatrixA,
301 mini::CMatrix TMatrixB,
302 mini::CMatrix TMatrixC,
303 mini::CMatrix TMatrixL,
304 mini::CMatrix TMatrixU>
305PBAT_HOST_DEVICE bool TriangleAxisAlignedBoundingBox(
306 TMatrixA const& A,
307 TMatrixB const& B,
308 TMatrixC const& C,
309 TMatrixL const& L,
310 TMatrixU const& U);
311
329template <
330 mini::CMatrix TMatrixA,
331 mini::CMatrix TMatrixB,
332 mini::CMatrix TMatrixC,
333 mini::CMatrix TMatrixD,
334 mini::CMatrix TMatrixL,
335 mini::CMatrix TMatrixU>
336PBAT_HOST_DEVICE bool TetrahedronAxisAlignedBoundingBox(
337 TMatrixA const& A,
338 TMatrixB const& B,
339 TMatrixC const& C,
340 TMatrixD const& D,
341 TMatrixL const& L,
342 TMatrixU const& U);
343
360template <
361 mini::CMatrix TMatrixA1,
362 mini::CMatrix TMatrixB1,
363 mini::CMatrix TMatrixC1,
364 mini::CMatrix TMatrixA2,
365 mini::CMatrix TMatrixB2,
366 mini::CMatrix TMatrixC2>
367PBAT_HOST_DEVICE bool Triangles2D(
368 TMatrixA1 const& A1,
369 TMatrixB1 const& B1,
370 TMatrixC1 const& C1,
371 TMatrixA2 const& A2,
372 TMatrixB2 const& B2,
373 TMatrixC2 const& C2);
374
391template <
392 mini::CMatrix TMatrixA1,
393 mini::CMatrix TMatrixB1,
394 mini::CMatrix TMatrixC1,
395 mini::CMatrix TMatrixA2,
396 mini::CMatrix TMatrixB2,
397 mini::CMatrix TMatrixC2>
398PBAT_HOST_DEVICE bool Triangles3D(
399 TMatrixA1 const& A1,
400 TMatrixB1 const& B1,
401 TMatrixC1 const& C1,
402 TMatrixA2 const& A2,
403 TMatrixB2 const& B2,
404 TMatrixC2 const& C2);
405
424template <
425 mini::CMatrix TMatrixA,
426 mini::CMatrix TMatrixB,
427 mini::CMatrix TMatrixC,
428 mini::CMatrix TMatrixI,
429 mini::CMatrix TMatrixJ,
430 mini::CMatrix TMatrixK,
431 mini::CMatrix TMatrixL>
432PBAT_HOST_DEVICE bool TriangleTetrahedron(
433 TMatrixA const& A,
434 TMatrixB const& B,
435 TMatrixC const& C,
436 TMatrixI const& I,
437 TMatrixJ const& J,
438 TMatrixK const& K,
439 TMatrixL const& L);
440
461template <
462 mini::CMatrix TMatrixA1,
463 mini::CMatrix TMatrixB1,
464 mini::CMatrix TMatrixC1,
465 mini::CMatrix TMatrixD1,
466 mini::CMatrix TMatrixA2,
467 mini::CMatrix TMatrixB2,
468 mini::CMatrix TMatrixC2,
469 mini::CMatrix TMatrixD2>
470PBAT_HOST_DEVICE bool Tetrahedra(
471 TMatrixA1 const& A1,
472 TMatrixB1 const& B1,
473 TMatrixC1 const& C1,
474 TMatrixD1 const& D1,
475 TMatrixA2 const& A2,
476 TMatrixB2 const& B2,
477 TMatrixC2 const& C2,
478 TMatrixD2 const& D2);
479
493template <
494 mini::CMatrix TMatrixA,
495 mini::CMatrix TMatrixB,
496 mini::CMatrix TMatrixC,
497 mini::CMatrix TMatrixSC>
498PBAT_HOST_DEVICE bool TriangleSphere(
499 TMatrixA const& A,
500 TMatrixB const& B,
501 TMatrixC const& C,
502 TMatrixSC const& SC,
503 typename TMatrixSC::ScalarType R);
504
520template <
521 mini::CMatrix TMatrixA,
522 mini::CMatrix TMatrixB,
523 mini::CMatrix TMatrixC,
524 mini::CMatrix TMatrixD,
525 mini::CMatrix TMatrixSC>
526PBAT_HOST_DEVICE bool TetrahedronSphere(
527 TMatrixA const& A,
528 TMatrixB const& B,
529 TMatrixC const& C,
530 TMatrixD const& D,
531 TMatrixSC const& SC,
532 typename TMatrixSC::ScalarType R);
533
534template <mini::CMatrix TMatrixP, mini::CMatrix TMatrixL, mini::CMatrix TMatrixU>
535PBAT_HOST_DEVICE bool
536PointAxisAlignedBoundingBox(TMatrixP const& P, TMatrixL const& L, TMatrixU const& U)
537{
538 bool bIsOutsideBox = Any((P < L) or (P > U));
539 return not bIsOutsideBox;
540}
541
542template <
543 mini::CMatrix TMatrixP,
544 mini::CMatrix TMatrixA,
545 mini::CMatrix TMatrixB,
546 mini::CMatrix TMatrixC>
547PBAT_HOST_DEVICE bool
548PointTriangle(TMatrixP const& P, TMatrixA const& A, TMatrixB const& B, TMatrixC const& C)
549{
550 auto uvw = IntersectionQueries::TriangleBarycentricCoordinates(P, A, B, C);
551 using ScalarType = typename TMatrixP::ScalarType;
552 bool bIsInsideTriangle = All((uvw >= ScalarType(0)) and (uvw <= ScalarType(1)));
553 return bIsInsideTriangle;
554}
555
556template <
557 mini::CMatrix TMatrixP,
558 mini::CMatrix TMatrixA,
559 mini::CMatrix TMatrixB,
560 mini::CMatrix TMatrixC,
561 mini::CMatrix TMatrixD>
562PBAT_HOST_DEVICE bool PointTetrahedron3D(
563 TMatrixP const& P,
564 TMatrixA const& A,
565 TMatrixB const& B,
566 TMatrixC const& C,
567 TMatrixD const& D)
568{
569 using ScalarType = typename TMatrixP::ScalarType;
570 auto constexpr kRows = TMatrixP::kRows;
571 auto constexpr kDims = 3;
572 static_assert(kRows == kDims, "This overlap test is specialized for 3D");
573
574 auto const PointOutsidePlane = [](auto const& p, auto const& a, auto const& b, auto const& c) {
575 ScalarType const d = Dot(p - a, Cross(b - a, c - a));
576 return d > ScalarType(0);
577 };
578 if (PointOutsidePlane(P, A, B, D))
579 return false;
580 if (PointOutsidePlane(P, B, C, D))
581 return false;
582 if (PointOutsidePlane(P, C, A, D))
583 return false;
584 if (PointOutsidePlane(P, A, C, B))
585 return false;
586 return true;
587}
588
589template <mini::CMatrix TMatrixC1, mini::CMatrix TMatrixC2>
590PBAT_HOST_DEVICE bool Spheres(
591 TMatrixC1 const& C1,
592 typename TMatrixC1::ScalarType R1,
593 TMatrixC2 const& C2,
594 typename TMatrixC2::ScalarType R2)
595{
596 using ScalarType = typename TMatrixC1::ScalarType;
597 ScalarType const upper = R1 + R2;
598 ScalarType const upper2 = upper * upper;
599 ScalarType const d2 = SquaredNorm(C1 - C2);
600 return d2 <= upper2;
601}
602
603template <
604 mini::CMatrix TMatrixL1,
605 mini::CMatrix TMatrixU1,
606 mini::CMatrix TMatrixL2,
607 mini::CMatrix TMatrixU2>
608PBAT_HOST_DEVICE bool AxisAlignedBoundingBoxes(
609 TMatrixL1 const& L1,
610 TMatrixU1 const& U1,
611 TMatrixL2 const& L2,
612 TMatrixU2 const& U2)
613{
614 bool bOverlap = All((L1 <= U2) and (L2 <= U1));
615 return bOverlap;
616}
617
618template <mini::CMatrix TMatrixC, mini::CMatrix TMatrixL, mini::CMatrix TMatrixU>
619PBAT_HOST_DEVICE bool SphereAxisAlignedBoundingBox(
620 TMatrixC const& C,
621 typename TMatrixC::ScalarType R,
622 TMatrixL const& L,
623 TMatrixU const& U)
624{
625 auto const Xaabb = ClosestPointQueries::PointOnAxisAlignedBoundingBox(C, L, U);
626 auto const d2 = SquaredNorm(C - Xaabb);
627 auto const r2 = R * R;
628 return d2 < r2;
629}
630
631template <mini::CMatrix TMatrixP, mini::CMatrix TMatrixQ, mini::CMatrix TMatrixC>
632PBAT_HOST_DEVICE bool LineSegmentSphere(
633 TMatrixP const& P,
634 TMatrixQ const& Q,
635 TMatrixC const& C,
636 typename TMatrixC::ScalarType R)
637{
638 using ScalarType = typename TMatrixC::ScalarType;
639 auto constexpr kRows = TMatrixP::kRows;
640 mini::SVector<ScalarType, kRows> const d = Q - P;
641 mini::SVector<ScalarType, kRows> const m = P - C;
642 ScalarType const b = Dot(m, d);
643 ScalarType const c = Dot(m, m) - R * R;
644 // Exit if r's origin outside s (c > 0) and r pointing away from s (b > 0)
645 if (c > ScalarType(0) and b > ScalarType(0))
646 return false;
647 ScalarType const discr = b * b - c;
648 // A negative discriminant corresponds to ray missing sphere
649 if (discr < ScalarType(0))
650 return false;
651 // Ray now found to intersect sphere, compute smallest t value of intersection
652 using namespace std;
653 ScalarType t = -b - sqrt(discr);
654 if (t > ScalarType(1))
655 return false;
656 return true;
657}
658
659template <
660 mini::CMatrix TMatrixP,
661 mini::CMatrix TMatrixQ,
662 mini::CMatrix TMatrixL,
663 mini::CMatrix TMatrixU>
664PBAT_HOST_DEVICE bool LineSegmentAxisAlignedBoundingBox(
665 TMatrixP const& P,
666 TMatrixQ const& Q,
667 TMatrixL const& L,
668 TMatrixU const& U)
669{
670 using ScalarType = typename TMatrixP::ScalarType;
671 auto constexpr kRows = TMatrixP::kRows;
672
673 mini::SVector<ScalarType, kRows> const c = ScalarType(0.5) * (L + U);
674 mini::SVector<ScalarType, kRows> const e = U - L;
675 mini::SVector<ScalarType, kRows> const d = Q - P;
676 mini::SVector<ScalarType, kRows> m = P + Q - L - U;
677 m = m - c; // Translate box and segment to origin
678
679 // Try world coordinate axes as separating axes
680 using namespace std;
681 auto constexpr kDims = c.Rows();
682 bool bAxesSeparating = Any(Abs(m) > (e + Abs(d)));
683 if (bAxesSeparating)
684 return false;
685 // Add in an epsilon term to counteract arithmetic errors when segment is
686 // (near) parallel to a coordinate axis (see text for detail)
687 common::ForRange<0, kDims>([&]<auto kDim>() {
688 ScalarType constexpr eps{1e-15};
689 ad(kDim) += eps;
690 auto i = (kDim + 1) % kDims;
691 auto j = (kDim + 2) % kDims;
692 // Try cross products of segment direction vector with coordinate axes
693 bAxesSeparating &= abs(m(i) * d(i) - m(i) * d(i)) > e(i) * ad(j) + e(j) * ad(i);
694 });
695 // No separating axis found; segment must be overlapping AABB
696 return not bAxesSeparating;
697}
698
699template <
700 mini::CMatrix TMatrixP,
701 mini::CMatrix TMatrixQ,
702 mini::CMatrix TMatrixA,
703 mini::CMatrix TMatrixB,
704 mini::CMatrix TMatrixC>
705PBAT_HOST_DEVICE bool LineSegmentTriangle3D(
706 TMatrixP const& P,
707 TMatrixQ const& Q,
708 TMatrixA const& A,
709 TMatrixB const& B,
710 TMatrixC const& C)
711{
712 return IntersectionQueries::UvwLineSegmentTriangle3D(P, Q, A, B, C).has_value();
713}
714
715template <
716 mini::CMatrix TMatrixP,
717 mini::CMatrix TMatrixQ,
718 mini::CMatrix TMatrixA1,
719 mini::CMatrix TMatrixB1,
720 mini::CMatrix TMatrixC1,
721 mini::CMatrix TMatrixA2,
722 mini::CMatrix TMatrixB2,
723 mini::CMatrix TMatrixC2>
724PBAT_HOST_DEVICE bool LineSegmentSweptTriangle3D(
725 TMatrixP const& P,
726 TMatrixQ const& Q,
727 TMatrixA1 const& A1,
728 TMatrixB1 const& B1,
729 TMatrixC1 const& C1,
730 TMatrixA2 const& A2,
731 TMatrixB2 const& B2,
732 TMatrixC2 const& C2)
733{
734 // We can construct the swept volume by intersecting the following half-planes:
735 // 1. Plane spanned by A1B1C1
736 // 2. Planed spanned by A2C2B2
737 // 3. Plane spanned by A1A2B2B1 -> 2 triangles A1A2B2 and A1B2B1
738 // 4. Plane spanned by B1B2C2C1 -> 2 triangles B1B2C2 and B1C2C1
739 // 5. Plane spanned by C1C2A2A1 -> 2 triangles C1C2A2 and C1A2A1
740
741 using ScalarType = typename TMatrixP::ScalarType;
742 static auto constexpr kDims = 3;
743
744 auto const fSignWrtPlane = [=](auto const& X, auto const& A, auto const& B, auto const& C) {
745 mini::SVector<ScalarType, kDims> const n = Cross(B - A, C - A);
746 auto const d = Dot(X - A, n);
747 return d;
748 };
749
750 // Check if triangles are coplanar
751 bool const bCoplanar = fSignWrtPlane(A1, A2, B2, C2) == ScalarType(0) and
752 fSignWrtPlane(B1, A2, B2, C2) == ScalarType(0) and
753 fSignWrtPlane(C1, A2, B2, C2) == ScalarType(0);
754 if (bCoplanar)
755 {
756 // WARNING:
757 // We are not handling this correctly... Ideally, we would form a polygon on the plane from
758 // the swept triangles, and intersect our line segment with that polygon. For now, let's
759 // just check for intersection between the line segment and both triangles.
760 return LineSegmentTriangle3D(P, Q, A1, B1, C1) or LineSegmentTriangle3D(P, Q, A2, C2, B2);
761 }
762
763 // Linearly swept triangle is a convex shape, check for PQ inside the volume.
764 // Because the triangle orientation can be inverted, we need to check for 2 cases:
765 // 1. All negative signs
766 // 2. All positive signs
767 // We do this by simply counting the number of negative signs and checking the count.
768 // clang-format off
769 mini::SVector<ScalarType, 10> sgn{};
770 sgn(0) = fSignWrtPlane(P, A1, B1, C1);
771 sgn(1) = fSignWrtPlane(P, A2, C2, B2);
772 sgn(2) = fSignWrtPlane(P, A1, A2, B2);
773 sgn(3) = fSignWrtPlane(P, B1, B2, C2);
774 sgn(4) = fSignWrtPlane(P, C1, C2, A2);
775 sgn(5) = fSignWrtPlane(Q, A1, B1, C1);
776 sgn(6) = fSignWrtPlane(Q, A2, C2, B2);
777 sgn(7) = fSignWrtPlane(Q, A1, A2, B2);
778 sgn(8) = fSignWrtPlane(Q, B1, B2, C2);
779 sgn(9) = fSignWrtPlane(Q, C1, C2, A2);
780 bool const bIsPqInside = All(sgn <= ScalarType(0)) or All(sgn >= ScalarType(0));
781 // clang-format on
782 if (bIsPqInside)
783 return true;
784
785 // Check for intersection of PQ with each triangle on the boundary of the swept volume.
786 // clang-format off
787 return LineSegmentTriangle3D(P, Q, A1, B1, C1) or
788 LineSegmentTriangle3D(P, Q, A2, C2, B2) or
789 LineSegmentTriangle3D(P, Q, A1, A2, B2) or
790 LineSegmentTriangle3D(P, Q, A1, B2, B1) or
791 LineSegmentTriangle3D(P, Q, B1, B2, C2) or
792 LineSegmentTriangle3D(P, Q, B1, C2, C1) or
793 LineSegmentTriangle3D(P, Q, C1, C2, A2) or
794 LineSegmentTriangle3D(P, Q, C1, A2, A1);
795 // clang-format on
796}
797
798template <
799 mini::CMatrix TMatrixP,
800 mini::CMatrix TMatrixN,
801 mini::CMatrix TMatrixL,
802 mini::CMatrix TMatrixU>
803PBAT_HOST_DEVICE bool PlaneAxisAlignedBoundingBox(
804 TMatrixP const& P,
805 TMatrixN const& n,
806 TMatrixL const& L,
807 TMatrixU const& U)
808{
809 using ScalarType = typename TMatrixP::ScalarType;
810 auto constexpr kRows = TMatrixP::kRows;
811 mini::SVector<ScalarType, kRows> const C = ScalarType(0.5) * (L + U); // Compute AABB center
812 mini::SVector<ScalarType, kRows> const e = U - C; // Compute positive extents
813 // Compute the projection interval radius of b onto L(t) = C + t * n
814 ScalarType const r = Dot(e, Abs(n));
815 // Compute distance of box center from plane
816 ScalarType const s = Dot(n, C - P);
817 // Intersection occurs when distance s falls within [-r,+r] interval
818 using namespace std;
819 return abs(s) <= r;
820}
821
822template <
823 mini::CMatrix TMatrixA,
824 mini::CMatrix TMatrixB,
825 mini::CMatrix TMatrixC,
826 mini::CMatrix TMatrixL,
827 mini::CMatrix TMatrixU>
828PBAT_HOST_DEVICE bool TriangleAxisAlignedBoundingBox(
829 TMatrixA const& A,
830 TMatrixB const& B,
831 TMatrixC const& C,
832 TMatrixL const& L,
833 TMatrixU const& U)
834{
838
839 using ScalarType = typename TMatrixA::ScalarType;
840 auto constexpr kRows = TMatrixL::kRows;
841 auto constexpr kDims = 3;
842 static_assert(kRows == kDims, "This overlap test is specialized for 3D");
843 // Transform triangle into reference space of AABB
844 mini::SVector<ScalarType, kDims> const O = ScalarType(0.5) * (L + U);
845 mini::SVector<ScalarType, kDims> const e = U - O;
846 mini::SVector<ScalarType, kDims> const AO = A - O;
847 mini::SVector<ScalarType, kDims> const BO = B - O;
848 mini::SVector<ScalarType, kDims> const CO = C - O;
849
850 /*
851 * Separating axis' to test are:
852 * - Perpendicular axis' of pairs of triangle edges and 3 perpendicular AABB edges
853 * - Face normals of AABB
854 * - Face normal of triangle
855 */
856
857 using namespace std;
858 auto const ProjectTriangle = [&](auto const& a) -> std::pair<ScalarType, ScalarType> {
859 mini::SVector<ScalarType, 3> const p{Dot(AO, a), Dot(BO, a), Dot(CO, a)};
860 return make_pair(Min(p), Max(p));
861 };
862 auto const ProjectAabb = [&](auto const& a) -> ScalarType {
863 return Dot(e, Abs(a));
864 };
865 auto const AreDisjoint = [](ScalarType ABCprojlow, ScalarType ABCprojup, ScalarType AABBproj) {
866 return (AABBproj < ABCprojlow) or (ABCprojup < -AABBproj);
867 };
868 auto const TestAxis = [&ProjectTriangle, &ProjectAabb, &AreDisjoint](auto const& axis) {
869 auto const [ABCmin, ABCmax] = ProjectTriangle(axis);
870 ScalarType const r = ProjectAabb(axis);
871 return AreDisjoint(ABCmin, ABCmax, r);
872 };
873
874 // ScalarType(1) Test edge pairs
875 auto const IsEdgePairIntersecting = [&TestAxis](auto const& a, auto const& b, auto dim) {
876 ScalarType constexpr eps = 1e-15;
877 auto const ab = b - a;
878 // Construct natural unit vector in axis dim
879 auto const u = mini::Unit<ScalarType, kDims>(dim);
880 mini::SVector<ScalarType, kDims> axis = Normalized(Cross(ab, u /* - zero*/));
881 bool bAxisIsZero = All(Abs(axis) <= eps);
882 if (not bAxisIsZero)
883 {
884 return TestAxis(axis);
885 }
886 else
887 {
888 // Edges ab and cd are numerically parallel
889 auto const n = Cross(ab, /*zero */ -a);
890 // Try a separating axis perpendicular to ab lying in the plane containing ab and cd
891 axis = Normalized(Cross(ab, n));
892 bAxisIsZero = All(Abs(axis) <= eps);
893 if (not bAxisIsZero)
894 return TestAxis(axis);
895 // ab and ac parallel too, so edges ab and cd are colinear and will not be a
896 // separating axis
897 }
898 return false;
899 };
906 for (auto dim = 0; dim < kDims; ++dim)
907 {
908 if (IsEdgePairIntersecting(AO, BO, dim))
909 return false;
910 if (IsEdgePairIntersecting(BO, CO, dim))
911 return false;
912 if (IsEdgePairIntersecting(CO, AO, dim))
913 return false;
914 }
915
916 // 2. Test AABB face normals
917 for (auto dim = 0; dim < kDims; ++dim)
918 {
919 ScalarType const ma = max({AO(dim), BO(dim), CO(dim)});
920 ScalarType const mi = min({AO(dim), BO(dim), CO(dim)});
921 if (ma < -e(dim) or mi > e(dim))
922 return false;
923 }
924
925 // 3. Test triangle face normal
926 mini::SVector<ScalarType, kDims> const n = Normalized(Cross(B - A, C - A));
927 return PlaneAxisAlignedBoundingBox(A, n, L, U);
928}
929
930template <
931 mini::CMatrix TMatrixA,
932 mini::CMatrix TMatrixB,
933 mini::CMatrix TMatrixC,
934 mini::CMatrix TMatrixD,
935 mini::CMatrix TMatrixL,
936 mini::CMatrix TMatrixU>
937PBAT_HOST_DEVICE bool TetrahedronAxisAlignedBoundingBox(
938 TMatrixA const& A,
939 TMatrixB const& B,
940 TMatrixC const& C,
941 TMatrixD const& D,
942 TMatrixL const& L,
943 TMatrixU const& U)
944{
945 using ScalarType = typename TMatrixA::ScalarType;
946 auto constexpr kRows = TMatrixL::kRows;
947 auto constexpr kDims = 3;
948 static_assert(kRows == kDims, "This overlap test is specialized for 3D");
949
950 // Transform tetrahedron into reference space of AABB
951 mini::SVector<ScalarType, kDims> const O = ScalarType(0.5) * (L + U);
952 mini::SVector<ScalarType, kDims> const e = U - O;
953 mini::SVector<ScalarType, kDims> const AO = A - O;
954 mini::SVector<ScalarType, kDims> const BO = B - O;
955 mini::SVector<ScalarType, kDims> const CO = C - O;
956 mini::SVector<ScalarType, kDims> const DO = D - O;
957
958 /*
959 * Separating axis' to test are:
960 * - Perpendicular axis' of pairs of 6 tetrahedron edges and 3 perpendicular AABB edges (18
961 * tests)
962 * - Face normals of AABB (3 tests)
963 * - Face normals of tetrahedron (4 tests)
964 */
965 using namespace std;
966 auto const ProjectTetrahedron = [&](auto const& a) -> std::pair<ScalarType, ScalarType> {
967 mini::SVector<ScalarType, 4> const p{Dot(AO, a), Dot(BO, a), Dot(CO, a), Dot(DO, a)};
968 return make_pair(Min(p), Max(p));
969 };
970 auto const ProjectAabb = [&](auto const& a) -> ScalarType {
971 return Dot(e, Abs(a));
972 };
973 auto const AreDisjoint = [](ScalarType low, ScalarType up, ScalarType r) {
974 return (up < -r) or (r < low);
975 };
976 auto const TestAxis = [&ProjectTetrahedron, &ProjectAabb, &AreDisjoint](auto const& axis) {
977 auto const [low, up] = ProjectTetrahedron(axis);
978 ScalarType const r = ProjectAabb(axis);
979 return AreDisjoint(low, up, r);
980 };
981
982 // ScalarType(1) Test edge pairs
983 auto const IsEdgePairIntersecting = [&TestAxis](auto const& a, auto const& b, auto dim) {
984 ScalarType constexpr eps = 1e-15;
985 auto const ab = b - a;
986 // Construct natural unit vector in axis dim
987 auto const u = mini::Unit<ScalarType, kDims>(dim);
988 mini::SVector<ScalarType, kDims> axis = Normalized(Cross(ab, u /* - zero*/));
989 bool bAxisIsZero = All(Abs(axis) <= eps);
990 if (not bAxisIsZero)
991 {
992 return TestAxis(axis);
993 }
994 else
995 {
996 // Edges ab and cd are numerically parallel
997 auto const n = Cross(ab, /*zero */ -a);
998 // Try a separating axis perpendicular to ab lying in the plane containing ab and cd
999 axis = Normalized(Cross(ab, n));
1000 bAxisIsZero = All(Abs(axis) <= eps);
1001 if (not bAxisIsZero)
1002 return TestAxis(axis);
1003 // ab and ac parallel too, so edges ab and cd are colinear and will not be a
1004 // separating axis
1005 }
1006 return false;
1007 };
1014 for (auto dim = 0; dim < kDims; ++dim)
1015 {
1016 // Edges of tetrahedron are: AB, BC, CA, AD, BD, CD
1017 if (IsEdgePairIntersecting(A, B, dim))
1018 return false;
1019
1020 if (IsEdgePairIntersecting(B, C, dim))
1021 return false;
1022
1023 if (IsEdgePairIntersecting(C, A, dim))
1024 return false;
1025
1026 if (IsEdgePairIntersecting(A, D, dim))
1027 return false;
1028
1029 if (IsEdgePairIntersecting(B, D, dim))
1030 return false;
1031
1032 if (IsEdgePairIntersecting(C, D, dim))
1033 return false;
1034 }
1035
1036 // 2. Test AABB face normals
1037 for (auto dim = 0; dim < kDims; ++dim)
1038 {
1039 ScalarType const ma = max({AO(dim), BO(dim), CO(dim), DO(dim)});
1040 ScalarType const mi = min({AO(dim), BO(dim), CO(dim), DO(dim)});
1041 if (ma < -e(dim) or mi > e(dim))
1042 return false;
1043 }
1044
1045 // 3. Test tetrahedron face normals
1046 // Tetrahedron faces are: ABD, BCD, CAD, ACB
1047 mini::SVector<ScalarType, kDims> n = Normalized(Cross(B - A, D - A));
1048 if (not PlaneAxisAlignedBoundingBox(A, n, L, U))
1049 return false;
1050 n = Normalized(Cross(C - B, D - B));
1051 if (not PlaneAxisAlignedBoundingBox(B, n, L, U))
1052 return false;
1053 n = Normalized(Cross(A - C, D - C));
1054 if (not PlaneAxisAlignedBoundingBox(C, n, L, U))
1055 return false;
1056 n = Normalized(Cross(C - A, B - A));
1057 return PlaneAxisAlignedBoundingBox(A, n, L, U);
1058}
1059
1060template <
1061 mini::CMatrix TMatrixA1,
1062 mini::CMatrix TMatrixB1,
1063 mini::CMatrix TMatrixC1,
1064 mini::CMatrix TMatrixA2,
1065 mini::CMatrix TMatrixB2,
1066 mini::CMatrix TMatrixC2>
1067PBAT_HOST_DEVICE bool Triangles2D(
1068 TMatrixA1 const& A1,
1069 TMatrixB1 const& B1,
1070 TMatrixC1 const& C1,
1071 TMatrixA2 const& A2,
1072 TMatrixB2 const& B2,
1073 TMatrixC2 const& C2)
1074{
1075 using ScalarType = typename TMatrixA1::ScalarType;
1076 auto constexpr kRows = TMatrixA1::kRows;
1077 auto constexpr kDims = 2;
1078 static_assert(kRows == kDims, "This overlap test is specialized for 2D");
1079
1080 using namespace std;
1081 // Separating axis' to test are all 6 triangle edges
1082 auto const ProjectTriangle = [&](auto const& a,
1083 auto const& b,
1084 auto const& c,
1085 auto const& axis) -> std::pair<ScalarType, ScalarType> {
1086 mini::SVector<ScalarType, 3> const p{Dot(a, axis), Dot(b, axis), Dot(c, axis)};
1087 return make_pair(Min(p), Max(p));
1088 };
1089 auto const AreDisjoint = [](ScalarType low1, ScalarType up1, ScalarType low2, ScalarType up2) {
1090 return (up1 < low2) or (up2 < low1);
1091 };
1092 auto const TestAxis = [&](auto const& axis) {
1093 auto const [low1, up1] = ProjectTriangle(A1, B1, C1, axis);
1094 auto const [low2, up2] = ProjectTriangle(A2, B2, C2, axis);
1095 return AreDisjoint(low1, up1, low2, up2);
1096 };
1097 auto const EdgeNormal = [](auto e) {
1098 return Normalized(mini::SVector<ScalarType, kDims>{-e(1), e(0)});
1099 };
1100 if (TestAxis(EdgeNormal(B1 - A1)))
1101 return false;
1102 if (TestAxis(EdgeNormal(C1 - B1)))
1103 return false;
1104 if (TestAxis(EdgeNormal(A1 - C1)))
1105 return false;
1106 if (TestAxis(EdgeNormal(B2 - A2)))
1107 return false;
1108 if (TestAxis(EdgeNormal(C2 - B2)))
1109 return false;
1110 if (TestAxis(EdgeNormal(A2 - C2)))
1111 return false;
1112 return true;
1113}
1114
1115template <
1116 mini::CMatrix TMatrixA1,
1117 mini::CMatrix TMatrixB1,
1118 mini::CMatrix TMatrixC1,
1119 mini::CMatrix TMatrixA2,
1120 mini::CMatrix TMatrixB2,
1121 mini::CMatrix TMatrixC2>
1122PBAT_HOST_DEVICE bool Triangles3D(
1123 TMatrixA1 const& A1,
1124 TMatrixB1 const& B1,
1125 TMatrixC1 const& C1,
1126 TMatrixA2 const& A2,
1127 TMatrixB2 const& B2,
1128 TMatrixC2 const& C2)
1129{
1130 auto const intersections = IntersectionQueries::UvwTriangles3D(A1, B1, C1, A2, B2, C2);
1131 for (auto const& intersection : intersections)
1132 if (intersection.has_value())
1133 return true;
1134 return false;
1135}
1136
1137template <
1138 mini::CMatrix TMatrixA,
1139 mini::CMatrix TMatrixB,
1140 mini::CMatrix TMatrixC,
1141 mini::CMatrix TMatrixI,
1142 mini::CMatrix TMatrixJ,
1143 mini::CMatrix TMatrixK,
1144 mini::CMatrix TMatrixL>
1145PBAT_HOST_DEVICE bool TriangleTetrahedron(
1146 TMatrixA const& A,
1147 TMatrixB const& B,
1148 TMatrixC const& C,
1149 TMatrixI const& I,
1150 TMatrixJ const& J,
1151 TMatrixK const& K,
1152 TMatrixL const& L)
1153{
1154 using ScalarType = typename TMatrixA::ScalarType;
1155 /*
1156 * Separating axis' to test are:
1157 * - Perpendicular axis' of pairs of 3 triangle edges and 6 tetrahedron edges (18
1158 * tests)
1159 * - Face normals of tetrahedron (4 tests)
1160 * - Face normal of triangle (1 test)
1161 */
1162 auto constexpr kRows = TMatrixA::kRows;
1163 auto constexpr kDims = 3;
1164 static_assert(kRows == kDims, "This overlap test is specialized for 3D");
1165
1166 using namespace std;
1167 // ScalarType(1) Test edge pairs
1168 auto const ProjectTriangle = [&](auto const& a) -> pair<ScalarType, ScalarType> {
1169 mini::SVector<ScalarType, 3> const p{Dot(A, a), Dot(B, a), Dot(C, a)};
1170 return make_pair(Min(p), Max(p));
1171 };
1172 auto const ProjectTetrahedron = [&](auto const& a) -> pair<ScalarType, ScalarType> {
1173 mini::SVector<ScalarType, 4> const p{Dot(I, a), Dot(J, a), Dot(K, a), Dot(L, a)};
1174 return make_pair(min({p(0), p(1), p(2), p(3)}), max({p(0), p(1), p(2), p(3)}));
1175 };
1176 auto const AreDisjoint = [](ScalarType low1, ScalarType up1, ScalarType low2, ScalarType up2) {
1177 return (up1 < low2) or (up2 < low1);
1178 };
1179 auto const TestAxis = [&ProjectTriangle, &ProjectTetrahedron, &AreDisjoint](auto const& a) {
1180 auto const [low1, up1] = ProjectTriangle(a);
1181 auto const [low2, up2] = ProjectTetrahedron(a);
1182 return AreDisjoint(low1, up1, low2, up2);
1183 };
1184 auto const IsEdgePairSeparating =
1185 [&TestAxis](auto const& a, auto const& b, auto const& c, auto const& d) {
1186 ScalarType constexpr eps = 1e-15;
1187 auto const ab = b - a;
1188 mini::SVector<ScalarType, kDims> axis = Normalized(Cross(ab, d - c));
1189 bool bAxisIsZero = All(Abs(axis) <= eps);
1190 if (not bAxisIsZero)
1191 {
1192 return TestAxis(axis);
1193 }
1194 else
1195 {
1196 // Edges ab and cd are numerically parallel
1197 auto const n = Cross(ab, c - a);
1198 // Try a separating axis perpendicular to ab lying in the plane containing ab and cd
1199 axis = Normalized(Cross(ab, n));
1200 bAxisIsZero = All(Abs(axis) <= eps);
1201 if (not bAxisIsZero)
1202 return TestAxis(axis);
1203 // ab and ac parallel too, so edges ab and cd are colinear and will not be a
1204 // separating axis
1205 }
1206 return false;
1207 };
1208
1209 // Tetrahedron edges are: IJ, JK, KI, IL, JL, KL
1210 // Triangle edges are: AB, BC, CA
1211 if (IsEdgePairSeparating(A, B, I, J))
1212 return false;
1213 if (IsEdgePairSeparating(B, C, I, J))
1214 return false;
1215 if (IsEdgePairSeparating(C, A, I, J))
1216 return false;
1217
1218 if (IsEdgePairSeparating(A, B, J, K))
1219 return false;
1220 if (IsEdgePairSeparating(B, C, J, K))
1221 return false;
1222 if (IsEdgePairSeparating(C, A, J, K))
1223 return false;
1224
1225 if (IsEdgePairSeparating(A, B, K, I))
1226 return false;
1227 if (IsEdgePairSeparating(B, C, K, I))
1228 return false;
1229 if (IsEdgePairSeparating(C, A, K, I))
1230 return false;
1231
1232 if (IsEdgePairSeparating(A, B, I, L))
1233 return false;
1234 if (IsEdgePairSeparating(B, C, I, L))
1235 return false;
1236 if (IsEdgePairSeparating(C, A, I, L))
1237 return false;
1238
1239 if (IsEdgePairSeparating(A, B, J, L))
1240 return false;
1241 if (IsEdgePairSeparating(B, C, J, L))
1242 return false;
1243 if (IsEdgePairSeparating(C, A, J, L))
1244 return false;
1245
1246 if (IsEdgePairSeparating(A, B, K, L))
1247 return false;
1248 if (IsEdgePairSeparating(B, C, K, L))
1249 return false;
1250 if (IsEdgePairSeparating(C, A, K, L))
1251 return false;
1252
1253 // 2. Test tetrahedron face normals
1254 mini::SVector<ScalarType, kDims> const IJ = J - I;
1255 mini::SVector<ScalarType, kDims> const JK = K - J;
1256 mini::SVector<ScalarType, kDims> const KI = I - K;
1257 mini::SVector<ScalarType, kDims> const IL = L - I;
1258 mini::SVector<ScalarType, kDims> const JL = L - J;
1259 mini::SVector<ScalarType, kDims> const KL = L - K;
1260 mini::SVector<ScalarType, kDims> n = Normalized(Cross(IJ, IL));
1261 if (TestAxis(n))
1262 return false;
1263 n = Normalized(Cross(JK, JL));
1264 if (TestAxis(n))
1265 return false;
1266 n = Normalized(Cross(KI, KL));
1267 if (TestAxis(n))
1268 return false;
1269 mini::SVector<ScalarType, kDims> const IK = K - I;
1270 n = Normalized(Cross(IK, IJ));
1271 if (TestAxis(n))
1272 return false;
1273
1274 // 3. Test triangle face normal
1275 n = Normalized(Cross(B - A, C - A));
1276 return not TestAxis(n);
1277}
1278
1279template <
1280 mini::CMatrix TMatrixA1,
1281 mini::CMatrix TMatrixB1,
1282 mini::CMatrix TMatrixC1,
1283 mini::CMatrix TMatrixD1,
1284 mini::CMatrix TMatrixA2,
1285 mini::CMatrix TMatrixB2,
1286 mini::CMatrix TMatrixC2,
1287 mini::CMatrix TMatrixD2>
1288PBAT_HOST_DEVICE bool Tetrahedra(
1289 TMatrixA1 const& A1,
1290 TMatrixB1 const& B1,
1291 TMatrixC1 const& C1,
1292 TMatrixD1 const& D1,
1293 TMatrixA2 const& A2,
1294 TMatrixB2 const& B2,
1295 TMatrixC2 const& C2,
1296 TMatrixD2 const& D2)
1297{
1298 using ScalarType = typename TMatrixA1::ScalarType;
1299 /*
1300 * Separating axis' to test are:
1301 * - Perpendicular axis' of pairs of 6 tetrahedron A1B1C1D1 edges and 6 tetrahedron A2B2C2D2
1302 * edges (36 tests)
1303 * - Face normals of tetrahedron (4+4=8 tests)
1304 */
1305 auto constexpr kRows = TMatrixA1::kRows;
1306 auto constexpr kDims = 3;
1307 static_assert(kRows == kDims, "This overlap test is specialized for 3D");
1308
1309 using namespace std;
1310 auto const ProjectTetrahedron1 = [&](auto const& a) -> pair<ScalarType, ScalarType> {
1311 mini::SVector<ScalarType, 4> const p{Dot(A1, a), Dot(B1, a), Dot(C1, a), Dot(D1, a)};
1312 return make_pair(Min(p), Max(p));
1313 };
1314 auto const ProjectTetrahedron2 = [&](auto const& a) -> pair<ScalarType, ScalarType> {
1315 mini::SVector<ScalarType, 4> const p{Dot(A2, a), Dot(B2, a), Dot(C2, a), Dot(D2, a)};
1316 return make_pair(Min(p), Max(p));
1317 };
1318 auto const AreDisjoint = [](ScalarType low1, ScalarType up1, ScalarType low2, ScalarType up2) {
1319 return (up1 < low2) or (up2 < low1);
1320 };
1321 auto const TestAxis =
1322 [&ProjectTetrahedron1, &ProjectTetrahedron2, &AreDisjoint](auto const& a) {
1323 auto const [low1, up1] = ProjectTetrahedron1(a);
1324 auto const [low2, up2] = ProjectTetrahedron2(a);
1325 return AreDisjoint(low1, up1, low2, up2);
1326 };
1327
1328 // ScalarType(1) Test edge pairs
1329 auto const IsEdgePairSeparating =
1330 [&TestAxis](auto const& a, auto const& b, auto const& c, auto const& d) {
1331 ScalarType constexpr eps = 1e-15;
1332 auto const ab = b - a;
1333 mini::SVector<ScalarType, kDims> axis = Normalized(Cross(ab, d - c));
1334 bool bAxisIsZero = All(Abs(axis) <= eps);
1335 if (not bAxisIsZero)
1336 {
1337 return TestAxis(axis);
1338 }
1339 else
1340 {
1341 // Edges ab and cd are numerically parallel
1342 auto const n = Cross(ab, c - a);
1343 // Try a separating axis perpendicular to ab lying in the plane containing ab and cd
1344 axis = Normalized(Cross(ab, n));
1345 bAxisIsZero = All(Abs(axis) <= eps);
1346 if (not bAxisIsZero)
1347 {
1348 return TestAxis(axis);
1349 }
1350 // ab and ac parallel too, so edges ab and cd are colinear and will not be a
1351 // separating axis
1352 }
1353 return false;
1354 };
1355
1356 // Tetrahedron 1 edges are: A1B1, B1C1, C1A1, A1D1, B1D1, C1D1
1357 // Tetrahedron 2 edges are: A2B2, B2C2, C2A2, A2D2, B2D2, C2D2
1358 if (IsEdgePairSeparating(A1, B1, A2, B2))
1359 return false;
1360 if (IsEdgePairSeparating(B1, C1, A2, B2))
1361 return false;
1362 if (IsEdgePairSeparating(C1, A1, A2, B2))
1363 return false;
1364 if (IsEdgePairSeparating(A1, D1, A2, B2))
1365 return false;
1366 if (IsEdgePairSeparating(B1, D1, A2, B2))
1367 return false;
1368 if (IsEdgePairSeparating(C1, D1, A2, B2))
1369 return false;
1370
1371 if (IsEdgePairSeparating(A1, B1, B2, C2))
1372 return false;
1373 if (IsEdgePairSeparating(B1, C1, B2, C2))
1374 return false;
1375 if (IsEdgePairSeparating(C1, A1, B2, C2))
1376 return false;
1377 if (IsEdgePairSeparating(A1, D1, B2, C2))
1378 return false;
1379 if (IsEdgePairSeparating(B1, D1, B2, C2))
1380 return false;
1381 if (IsEdgePairSeparating(C1, D1, B2, C2))
1382 return false;
1383
1384 if (IsEdgePairSeparating(A1, B1, C2, A2))
1385 return false;
1386 if (IsEdgePairSeparating(B1, C1, C2, A2))
1387 return false;
1388 if (IsEdgePairSeparating(C1, A1, C2, A2))
1389 return false;
1390 if (IsEdgePairSeparating(A1, D1, C2, A2))
1391 return false;
1392 if (IsEdgePairSeparating(B1, D1, C2, A2))
1393 return false;
1394 if (IsEdgePairSeparating(C1, D1, C2, A2))
1395 return false;
1396
1397 if (IsEdgePairSeparating(A1, B1, A2, D2))
1398 return false;
1399 if (IsEdgePairSeparating(B1, C1, A2, D2))
1400 return false;
1401 if (IsEdgePairSeparating(C1, A1, A2, D2))
1402 return false;
1403 if (IsEdgePairSeparating(A1, D1, A2, D2))
1404 return false;
1405 if (IsEdgePairSeparating(B1, D1, A2, D2))
1406 return false;
1407 if (IsEdgePairSeparating(C1, D1, A2, D2))
1408 return false;
1409
1410 if (IsEdgePairSeparating(A1, B1, B2, D2))
1411 return false;
1412 if (IsEdgePairSeparating(B1, C1, B2, D2))
1413 return false;
1414 if (IsEdgePairSeparating(C1, A1, B2, D2))
1415 return false;
1416 if (IsEdgePairSeparating(A1, D1, B2, D2))
1417 return false;
1418 if (IsEdgePairSeparating(B1, D1, B2, D2))
1419 return false;
1420 if (IsEdgePairSeparating(C1, D1, B2, D2))
1421 return false;
1422
1423 if (IsEdgePairSeparating(A1, B1, C2, D2))
1424 return false;
1425 if (IsEdgePairSeparating(B1, C1, C2, D2))
1426 return false;
1427 if (IsEdgePairSeparating(C1, A1, C2, D2))
1428 return false;
1429 if (IsEdgePairSeparating(A1, D1, C2, D2))
1430 return false;
1431 if (IsEdgePairSeparating(B1, D1, C2, D2))
1432 return false;
1433 if (IsEdgePairSeparating(C1, D1, C2, D2))
1434 return false;
1435
1436 // 2. Test face normals:
1437 // Tetrahedron 1 faces are: A1B1D1, B1C1D1, C1A1D1, A1C1B1
1438 // Tetrahedron 2 faces are: A2B2D2, B2C2D2, C2A2D2, A2C2B2
1439 mini::SVector<ScalarType, kDims> n = Normalized(Cross(B1 - A1, D1 - A1));
1440 if (TestAxis(n))
1441 return false;
1442 n = Normalized(Cross(C1 - B1, D1 - B1));
1443 if (TestAxis(n))
1444 return false;
1445 n = Normalized(Cross(A1 - C1, D1 - C1));
1446 if (TestAxis(n))
1447 return false;
1448 n = Normalized(Cross(C1 - A1, B1 - A1));
1449 if (TestAxis(n))
1450 return false;
1451
1452 n = Normalized(Cross(B2 - A2, D2 - A2));
1453 if (TestAxis(n))
1454 return false;
1455 n = Normalized(Cross(C2 - B2, D2 - B2));
1456 if (TestAxis(n))
1457 return false;
1458 n = Normalized(Cross(A2 - C2, D2 - C2));
1459 if (TestAxis(n))
1460 return false;
1461 n = Normalized(Cross(C2 - A2, B2 - A2));
1462 return not TestAxis(n);
1463}
1464
1465template <
1466 mini::CMatrix TMatrixA,
1467 mini::CMatrix TMatrixB,
1468 mini::CMatrix TMatrixC,
1469 mini::CMatrix TMatrixSC>
1470PBAT_HOST_DEVICE bool TriangleSphere(
1471 TMatrixA const& A,
1472 TMatrixB const& B,
1473 TMatrixC const& C,
1474 TMatrixSC const& SC,
1475 typename TMatrixSC::ScalarType R)
1476{
1477 using ScalarType = typename TMatrixSC::ScalarType;
1478 auto const X = ClosestPointQueries::PointInTriangle(SC, A, B, C);
1479 ScalarType const d2 = SquaredNorm(X - SC);
1480 ScalarType const r2 = R * R;
1481 return d2 < r2;
1482}
1483
1484template <
1485 mini::CMatrix TMatrixA,
1486 mini::CMatrix TMatrixB,
1487 mini::CMatrix TMatrixC,
1488 mini::CMatrix TMatrixD,
1489 mini::CMatrix TMatrixSC>
1490PBAT_HOST_DEVICE bool TetrahedronSphere(
1491 TMatrixA const& A,
1492 TMatrixB const& B,
1493 TMatrixC const& C,
1494 TMatrixD const& D,
1495 TMatrixSC const& SC,
1496 typename TMatrixSC::ScalarType R)
1497{
1498 using ScalarType = typename TMatrixSC::ScalarType;
1499 auto const X = ClosestPointQueries::PointInTetrahedron(SC, A, B, C, D);
1500 ScalarType const d2 = SquaredNorm(X - SC);
1501 ScalarType const r2 = R * R;
1502 return d2 < r2;
1503}
1504
1505} // namespace OverlapQueries
1506} // namespace geometry
1507} // namespace pbat
1508
1509#endif // PBAT_GEOMETRY_OVERLAPQUERIES_H
ClosestPointQueries.h
This file contains functions to answer closest point queries.
ConstexprFor.h
Compile-time for loops.
IntersectionQueries.h
This file contains functions to answer intersection queries.
Mini.h
This file includes all the mini linear algebra headers.
pbat::common::ForRange
constexpr void ForRange(F &&f)
Compile-time for loop over a range of values.
Definition ConstexprFor.h:55
pbat::geometry::ClosestPointQueries::PointOnAxisAlignedBoundingBox
PBAT_HOST_DEVICE auto PointOnAxisAlignedBoundingBox(TMatrixX const &X, TMatrixL const &L, TMatrixU const &U) -> mini::SVector< typename TMatrixX::ScalarType, TMatrixX::kRows >
Obtain the point on the axis-aligned bounding box (AABB) defined by the lower and upper corners close...
Definition ClosestPointQueries.h:280
pbat::geometry::ClosestPointQueries::PointInTetrahedron
PBAT_HOST_DEVICE auto PointInTetrahedron(TMatrixP const &P, TMatrixA const &A, TMatrixB const &B, TMatrixC const &C, TMatrixD const &D) -> mini::SVector< typename TMatrixP::ScalarType, TMatrixP::kRows >
Obtain the point in the tetrahedron ABCD closest to the point P. The order of ABCD must be such that ...
Definition ClosestPointQueries.h:397
pbat::geometry::ClosestPointQueries::PointInTriangle
PBAT_HOST_DEVICE auto PointInTriangle(TMatrixP const &P, TMatrixA const &A, TMatrixB const &B, TMatrixC const &C) -> mini::SVector< typename TMatrixP::ScalarType, TMatrixP::kRows >
Obtain the point on the triangle ABC closest to the point P.
Definition ClosestPointQueries.h:384
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::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::OverlapQueries
This namespace contains functions to answer overlap queries.
Definition OverlapQueries.h:27
pbat::geometry::OverlapQueries::Triangles3D
PBAT_HOST_DEVICE bool Triangles3D(TMatrixA1 const &A1, TMatrixB1 const &B1, TMatrixC1 const &C1, TMatrixA2 const &A2, TMatrixB2 const &B2, TMatrixC2 const &C2)
Tests for overlap between triangle A1B1C1 and triangle A2B2C2, in 3D.
Definition OverlapQueries.h:1122
pbat::geometry::OverlapQueries::AxisAlignedBoundingBoxes
PBAT_HOST_DEVICE bool AxisAlignedBoundingBoxes(TMatrixL1 const &L1, TMatrixU1 const &U1, TMatrixL2 const &L2, TMatrixU2 const &U2)
Tests for overlap between axis-aligned bounding box (L1,U1) and axis-aligned bounding box (L2,...
Definition OverlapQueries.h:608
pbat::geometry::OverlapQueries::TriangleSphere
PBAT_HOST_DEVICE bool TriangleSphere(TMatrixA const &A, TMatrixB const &B, TMatrixC const &C, TMatrixSC const &SC, typename TMatrixSC::ScalarType R)
Tests for overlap between a triangle ABC and a sphere with center SC of radius R.
Definition OverlapQueries.h:1470
pbat::geometry::OverlapQueries::LineSegmentSphere
PBAT_HOST_DEVICE bool LineSegmentSphere(TMatrixP const &P, TMatrixQ const &Q, TMatrixC const &C, typename TMatrixC::ScalarType R)
Tests for overlap between line segment PQ and sphere (C,R)
Definition OverlapQueries.h:632
pbat::geometry::OverlapQueries::TetrahedronAxisAlignedBoundingBox
PBAT_HOST_DEVICE bool TetrahedronAxisAlignedBoundingBox(TMatrixA const &A, TMatrixB const &B, TMatrixC const &C, TMatrixD const &D, TMatrixL const &L, TMatrixU const &U)
Tests for overlap between tetrahedron ABCD and axis-aligned bounding box (L,U), in at least 3D.
Definition OverlapQueries.h:937
pbat::geometry::OverlapQueries::LineSegmentSweptTriangle3D
PBAT_HOST_DEVICE bool LineSegmentSweptTriangle3D(TMatrixP const &P, TMatrixQ const &Q, TMatrixA1 const &A1, TMatrixB1 const &B1, TMatrixC1 const &C1, TMatrixA2 const &A2, TMatrixB2 const &B2, TMatrixC2 const &C2)
Tests overlap between line segment PQ and the swept volume spanned by linear interpolation of A1B1C1 ...
Definition OverlapQueries.h:724
pbat::geometry::OverlapQueries::PointTetrahedron3D
PBAT_HOST_DEVICE bool PointTetrahedron3D(TMatrixP const &P, TMatrixA const &A, TMatrixB const &B, TMatrixC const &C, TMatrixD const &D)
Checks if point P is contained in tetrahedron ABCD, in at least 3D.
Definition OverlapQueries.h:562
pbat::geometry::OverlapQueries::TetrahedronSphere
PBAT_HOST_DEVICE bool TetrahedronSphere(TMatrixA const &A, TMatrixB const &B, TMatrixC const &C, TMatrixD const &D, TMatrixSC const &SC, typename TMatrixSC::ScalarType R)
Tests for overlap between a tetrahedron ABCD and a sphere with center SC of radius R.
Definition OverlapQueries.h:1490
pbat::geometry::OverlapQueries::PointTriangle
PBAT_HOST_DEVICE bool PointTriangle(TMatrixP const &P, TMatrixA const &A, TMatrixB const &B, TMatrixC const &C)
Tests for overlap between point P and triangle ABC.
Definition OverlapQueries.h:548
pbat::geometry::OverlapQueries::LineSegmentTriangle3D
PBAT_HOST_DEVICE bool LineSegmentTriangle3D(TMatrixP const &P, TMatrixQ const &Q, TMatrixA const &A, TMatrixB const &B, TMatrixC const &C)
Detects if the line segment PQ passes through the triangle ABC, in 3D.
Definition OverlapQueries.h:705
pbat::geometry::OverlapQueries::PlaneAxisAlignedBoundingBox
PBAT_HOST_DEVICE bool PlaneAxisAlignedBoundingBox(TMatrixP const &P, TMatrixN const &n, TMatrixL const &L, TMatrixU const &U)
Tests for overlap between plane (P,n) and axis-aligned bounding box (low,up)
Definition OverlapQueries.h:803
pbat::geometry::OverlapQueries::TriangleAxisAlignedBoundingBox
PBAT_HOST_DEVICE bool TriangleAxisAlignedBoundingBox(TMatrixA const &A, TMatrixB const &B, TMatrixC const &C, TMatrixL const &L, TMatrixU const &U)
Tests for overlap between triangle ABC and axis-aligned bounding box (low,up)
Definition OverlapQueries.h:828
pbat::geometry::OverlapQueries::Spheres
PBAT_HOST_DEVICE bool Spheres(TMatrixC1 const &C1, typename TMatrixC1::ScalarType R1, TMatrixC2 const &C2, typename TMatrixC2::ScalarType R2)
Tests for overlap between sphere (C1,R1) and sphere (C2,R2).
Definition OverlapQueries.h:590
pbat::geometry::OverlapQueries::LineSegmentAxisAlignedBoundingBox
PBAT_HOST_DEVICE bool LineSegmentAxisAlignedBoundingBox(TMatrixP const &P, TMatrixQ const &Q, TMatrixL const &L, TMatrixU const &U)
Tests for overlap between line segment PQ and axis-aligned bounding box (L,U)
Definition OverlapQueries.h:664
pbat::geometry::OverlapQueries::SphereAxisAlignedBoundingBox
PBAT_HOST_DEVICE bool SphereAxisAlignedBoundingBox(TMatrixC const &C, typename TMatrixC::ScalarType R, TMatrixL const &L, TMatrixU const &U)
Tests for overlap between sphere (c,r) and axis-aligned bounding box (low,up)
Definition OverlapQueries.h:619
pbat::geometry::OverlapQueries::PointAxisAlignedBoundingBox
PBAT_HOST_DEVICE bool PointAxisAlignedBoundingBox(TMatrixP const &P, TMatrixL const &L, TMatrixU const &U)
Tests for overlap between point P and axis-aligned bounding box (L,U)
Definition OverlapQueries.h:536
pbat::geometry::OverlapQueries::TriangleTetrahedron
PBAT_HOST_DEVICE bool TriangleTetrahedron(TMatrixA const &A, TMatrixB const &B, TMatrixC const &C, TMatrixI const &I, TMatrixJ const &J, TMatrixK const &K, TMatrixL const &L)
Tests for overlap between triangle ABC and tetrahedron IJKL, in at least 3D.
Definition OverlapQueries.h:1145
pbat::geometry::OverlapQueries::Tetrahedra
PBAT_HOST_DEVICE bool Tetrahedra(TMatrixA1 const &A1, TMatrixB1 const &B1, TMatrixC1 const &C1, TMatrixD1 const &D1, TMatrixA2 const &A2, TMatrixB2 const &B2, TMatrixC2 const &C2, TMatrixD2 const &D2)
Tests for overlap between tetrahedron A1B1C1D1 and tetrahedron A2B2C2D2, in at least 3D.
Definition OverlapQueries.h:1288
pbat::geometry::OverlapQueries::Triangles2D
PBAT_HOST_DEVICE bool Triangles2D(TMatrixA1 const &A1, TMatrixB1 const &B1, TMatrixC1 const &C1, TMatrixA2 const &A2, TMatrixB2 const &B2, TMatrixC2 const &C2)
Tests for overlap between triangle A1B1C1 and triangle A2B2C2, in 2D.
Definition OverlapQueries.h:1067
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