Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
AABox4.h
Go to the documentation of this file.
1// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
2// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
3// SPDX-License-Identifier: MIT
4
5#pragma once
6
8
10
13JPH_INLINE UVec4 AABox4VsBox(const AABox &inBox1, Vec4Arg inBox2MinX, Vec4Arg inBox2MinY, Vec4Arg inBox2MinZ, Vec4Arg inBox2MaxX, Vec4Arg inBox2MaxY, Vec4Arg inBox2MaxZ)
14{
15 // Splat values of box 1
16 Vec4 box1_minx = inBox1.mMin.SplatX();
17 Vec4 box1_miny = inBox1.mMin.SplatY();
18 Vec4 box1_minz = inBox1.mMin.SplatZ();
19 Vec4 box1_maxx = inBox1.mMax.SplatX();
20 Vec4 box1_maxy = inBox1.mMax.SplatY();
21 Vec4 box1_maxz = inBox1.mMax.SplatZ();
22
23 // Test separation over each axis
24 UVec4 nooverlapx = UVec4::sOr(Vec4::sGreater(box1_minx, inBox2MaxX), Vec4::sGreater(inBox2MinX, box1_maxx));
25 UVec4 nooverlapy = UVec4::sOr(Vec4::sGreater(box1_miny, inBox2MaxY), Vec4::sGreater(inBox2MinY, box1_maxy));
26 UVec4 nooverlapz = UVec4::sOr(Vec4::sGreater(box1_minz, inBox2MaxZ), Vec4::sGreater(inBox2MinZ, box1_maxz));
27
28 // Return overlap
29 return UVec4::sNot(UVec4::sOr(UVec4::sOr(nooverlapx, nooverlapy), nooverlapz));
30}
31
33JPH_INLINE void AABox4Scale(Vec3Arg inScale, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, Vec4 &outBoundsMinX, Vec4 &outBoundsMinY, Vec4 &outBoundsMinZ, Vec4 &outBoundsMaxX, Vec4 &outBoundsMaxY, Vec4 &outBoundsMaxZ)
34{
35 Vec4 scale_x = inScale.SplatX();
36 Vec4 scaled_min_x = scale_x * inBoxMinX;
37 Vec4 scaled_max_x = scale_x * inBoxMaxX;
38 outBoundsMinX = Vec4::sMin(scaled_min_x, scaled_max_x); // Negative scale can flip min and max
39 outBoundsMaxX = Vec4::sMax(scaled_min_x, scaled_max_x);
40
41 Vec4 scale_y = inScale.SplatY();
42 Vec4 scaled_min_y = scale_y * inBoxMinY;
43 Vec4 scaled_max_y = scale_y * inBoxMaxY;
44 outBoundsMinY = Vec4::sMin(scaled_min_y, scaled_max_y);
45 outBoundsMaxY = Vec4::sMax(scaled_min_y, scaled_max_y);
46
47 Vec4 scale_z = inScale.SplatZ();
48 Vec4 scaled_min_z = scale_z * inBoxMinZ;
49 Vec4 scaled_max_z = scale_z * inBoxMaxZ;
50 outBoundsMinZ = Vec4::sMin(scaled_min_z, scaled_max_z);
51 outBoundsMaxZ = Vec4::sMax(scaled_min_z, scaled_max_z);
52}
53
55JPH_INLINE void AABox4EnlargeWithExtent(Vec3Arg inExtent, Vec4 &ioBoundsMinX, Vec4 &ioBoundsMinY, Vec4 &ioBoundsMinZ, Vec4 &ioBoundsMaxX, Vec4 &ioBoundsMaxY, Vec4 &ioBoundsMaxZ)
56{
57 Vec4 extent_x = inExtent.SplatX();
58 ioBoundsMinX -= extent_x;
59 ioBoundsMaxX += extent_x;
60
61 Vec4 extent_y = inExtent.SplatY();
62 ioBoundsMinY -= extent_y;
63 ioBoundsMaxY += extent_y;
64
65 Vec4 extent_z = inExtent.SplatZ();
66 ioBoundsMinZ -= extent_z;
67 ioBoundsMaxZ += extent_z;
68}
69
71JPH_INLINE UVec4 AABox4VsPoint(Vec3Arg inPoint, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
72{
73 // Splat point to 4 component vectors
74 Vec4 point_x = Vec4(inPoint).SplatX();
75 Vec4 point_y = Vec4(inPoint).SplatY();
76 Vec4 point_z = Vec4(inPoint).SplatZ();
77
78 // Test if point overlaps with box
79 UVec4 overlapx = UVec4::sAnd(Vec4::sGreaterOrEqual(point_x, inBoxMinX), Vec4::sLessOrEqual(point_x, inBoxMaxX));
80 UVec4 overlapy = UVec4::sAnd(Vec4::sGreaterOrEqual(point_y, inBoxMinY), Vec4::sLessOrEqual(point_y, inBoxMaxY));
81 UVec4 overlapz = UVec4::sAnd(Vec4::sGreaterOrEqual(point_z, inBoxMinZ), Vec4::sLessOrEqual(point_z, inBoxMaxZ));
82
83 // Test if all are overlapping
84 return UVec4::sAnd(UVec4::sAnd(overlapx, overlapy), overlapz);
85}
86
88JPH_INLINE UVec4 AABox4VsBox(Mat44Arg inOrientation, Vec3Arg inHalfExtents, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, float inEpsilon = 1.0e-6f)
89{
90 // Taken from: Real Time Collision Detection - Christer Ericson
91 // Chapter 4.4.1, page 103-105.
92 // Note that the code is swapped around: A is the aabox and B is the oriented box (this saves us from having to invert the orientation of the oriented box)
93
94 // Compute translation vector t (the translation of B in the space of A)
95 Vec4 t[3] {
96 inOrientation.GetTranslation().SplatX() - 0.5f * (inBoxMinX + inBoxMaxX),
97 inOrientation.GetTranslation().SplatY() - 0.5f * (inBoxMinY + inBoxMaxY),
98 inOrientation.GetTranslation().SplatZ() - 0.5f * (inBoxMinZ + inBoxMaxZ) };
99
100 // Compute common subexpressions. Add in an epsilon term to
101 // counteract arithmetic errors when two edges are parallel and
102 // their cross product is (near) null (see text for details)
103 Vec3 epsilon = Vec3::sReplicate(inEpsilon);
104 Vec3 abs_r[3] { inOrientation.GetAxisX().Abs() + epsilon, inOrientation.GetAxisY().Abs() + epsilon, inOrientation.GetAxisZ().Abs() + epsilon };
105
106 // Half extents for a
107 Vec4 a_half_extents[3] {
108 0.5f * (inBoxMaxX - inBoxMinX),
109 0.5f * (inBoxMaxY - inBoxMinY),
110 0.5f * (inBoxMaxZ - inBoxMinZ) };
111
112 // Half extents of b
113 Vec4 b_half_extents_x = inHalfExtents.SplatX();
114 Vec4 b_half_extents_y = inHalfExtents.SplatY();
115 Vec4 b_half_extents_z = inHalfExtents.SplatZ();
116
117 // Each component corresponds to 1 overlapping OBB vs ABB
118 UVec4 overlaps = UVec4(0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff);
119
120 // Test axes L = A0, L = A1, L = A2
121 Vec4 ra, rb;
122 for (int i = 0; i < 3; i++)
123 {
124 ra = a_half_extents[i];
125 rb = b_half_extents_x * abs_r[0][i] + b_half_extents_y * abs_r[1][i] + b_half_extents_z * abs_r[2][i];
126 overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual(t[i].Abs(), ra + rb));
127 }
128
129 // Test axes L = B0, L = B1, L = B2
130 for (int i = 0; i < 3; i++)
131 {
132 ra = a_half_extents[0] * abs_r[i][0] + a_half_extents[1] * abs_r[i][1] + a_half_extents[2] * abs_r[i][2];
133 rb = Vec4::sReplicate(inHalfExtents[i]);
134 overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(0, i) + t[1] * inOrientation(1, i) + t[2] * inOrientation(2, i)).Abs(), ra + rb));
135 }
136
137 // Test axis L = A0 x B0
138 ra = a_half_extents[1] * abs_r[0][2] + a_half_extents[2] * abs_r[0][1];
139 rb = b_half_extents_y * abs_r[2][0] + b_half_extents_z * abs_r[1][0];
140 overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 0) - t[1] * inOrientation(2, 0)).Abs(), ra + rb));
141
142 // Test axis L = A0 x B1
143 ra = a_half_extents[1] * abs_r[1][2] + a_half_extents[2] * abs_r[1][1];
144 rb = b_half_extents_x * abs_r[2][0] + b_half_extents_z * abs_r[0][0];
145 overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 1) - t[1] * inOrientation(2, 1)).Abs(), ra + rb));
146
147 // Test axis L = A0 x B2
148 ra = a_half_extents[1] * abs_r[2][2] + a_half_extents[2] * abs_r[2][1];
149 rb = b_half_extents_x * abs_r[1][0] + b_half_extents_y * abs_r[0][0];
150 overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 2) - t[1] * inOrientation(2, 2)).Abs(), ra + rb));
151
152 // Test axis L = A1 x B0
153 ra = a_half_extents[0] * abs_r[0][2] + a_half_extents[2] * abs_r[0][0];
154 rb = b_half_extents_y * abs_r[2][1] + b_half_extents_z * abs_r[1][1];
155 overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 0) - t[2] * inOrientation(0, 0)).Abs(), ra + rb));
156
157 // Test axis L = A1 x B1
158 ra = a_half_extents[0] * abs_r[1][2] + a_half_extents[2] * abs_r[1][0];
159 rb = b_half_extents_x * abs_r[2][1] + b_half_extents_z * abs_r[0][1];
160 overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 1) - t[2] * inOrientation(0, 1)).Abs(), ra + rb));
161
162 // Test axis L = A1 x B2
163 ra = a_half_extents[0] * abs_r[2][2] + a_half_extents[2] * abs_r[2][0];
164 rb = b_half_extents_x * abs_r[1][1] + b_half_extents_y * abs_r[0][1];
165 overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 2) - t[2] * inOrientation(0, 2)).Abs(), ra + rb));
166
167 // Test axis L = A2 x B0
168 ra = a_half_extents[0] * abs_r[0][1] + a_half_extents[1] * abs_r[0][0];
169 rb = b_half_extents_y * abs_r[2][2] + b_half_extents_z * abs_r[1][2];
170 overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 0) - t[0] * inOrientation(1, 0)).Abs(), ra + rb));
171
172 // Test axis L = A2 x B1
173 ra = a_half_extents[0] * abs_r[1][1] + a_half_extents[1] * abs_r[1][0];
174 rb = b_half_extents_x * abs_r[2][2] + b_half_extents_z * abs_r[0][2];
175 overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 1) - t[0] * inOrientation(1, 1)).Abs(), ra + rb));
176
177 // Test axis L = A2 x B2
178 ra = a_half_extents[0] * abs_r[2][1] + a_half_extents[1] * abs_r[2][0];
179 rb = b_half_extents_x * abs_r[1][2] + b_half_extents_y * abs_r[0][2];
180 overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 2) - t[0] * inOrientation(1, 2)).Abs(), ra + rb));
181
182 // Return if the OBB vs AABBs are intersecting
183 return overlaps;
184}
185
187JPH_INLINE UVec4 AABox4VsBox(const OrientedBox &inBox, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, float inEpsilon = 1.0e-6f)
188{
189 return AABox4VsBox(inBox.mOrientation, inBox.mHalfExtents, inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ, inEpsilon);
190}
191
193JPH_INLINE Vec4 AABox4DistanceSqToPoint(Vec4Arg inPointX, Vec4Arg inPointY, Vec4Arg inPointZ, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
194{
195 // Get closest point on box
196 Vec4 closest_x = Vec4::sMin(Vec4::sMax(inPointX, inBoxMinX), inBoxMaxX);
197 Vec4 closest_y = Vec4::sMin(Vec4::sMax(inPointY, inBoxMinY), inBoxMaxY);
198 Vec4 closest_z = Vec4::sMin(Vec4::sMax(inPointZ, inBoxMinZ), inBoxMaxZ);
199
200 // Return the squared distance between the box and point
201 return Square(closest_x - inPointX) + Square(closest_y - inPointY) + Square(closest_z - inPointZ);
202}
203
205JPH_INLINE Vec4 AABox4DistanceSqToPoint(Vec3 inPoint, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
206{
207 return AABox4DistanceSqToPoint(inPoint.SplatX(), inPoint.SplatY(), inPoint.SplatZ(), inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ);
208}
209
211JPH_INLINE UVec4 AABox4VsSphere(Vec4Arg inCenterX, Vec4Arg inCenterY, Vec4Arg inCenterZ, Vec4Arg inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
212{
213 // Test the distance from the center of the sphere to the box is smaller than the radius
214 Vec4 distance_sq = AABox4DistanceSqToPoint(inCenterX, inCenterY, inCenterZ, inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ);
215 return Vec4::sLessOrEqual(distance_sq, inRadiusSq);
216}
217
219JPH_INLINE UVec4 AABox4VsSphere(Vec3Arg inCenter, float inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
220{
221 return AABox4VsSphere(inCenter.SplatX(), inCenter.SplatY(), inCenter.SplatZ(), Vec4::sReplicate(inRadiusSq), inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ);
222}
223
JPH_INLINE UVec4 AABox4VsSphere(Vec4Arg inCenterX, Vec4Arg inCenterY, Vec4Arg inCenterZ, Vec4Arg inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
Test 4 AABoxes vs a sphere.
Definition: AABox4.h:211
JPH_INLINE UVec4 AABox4VsPoint(Vec3Arg inPoint, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
Test if 4 bounding boxes overlap with a point.
Definition: AABox4.h:71
JPH_NAMESPACE_BEGIN JPH_INLINE UVec4 AABox4VsBox(const AABox &inBox1, Vec4Arg inBox2MinX, Vec4Arg inBox2MinY, Vec4Arg inBox2MinZ, Vec4Arg inBox2MaxX, Vec4Arg inBox2MaxY, Vec4Arg inBox2MaxZ)
Definition: AABox4.h:13
JPH_INLINE Vec4 AABox4DistanceSqToPoint(Vec4Arg inPointX, Vec4Arg inPointY, Vec4Arg inPointZ, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
Get the squared distance between 4 AABoxes and a point.
Definition: AABox4.h:193
JPH_INLINE void AABox4EnlargeWithExtent(Vec3Arg inExtent, Vec4 &ioBoundsMinX, Vec4 &ioBoundsMinY, Vec4 &ioBoundsMinZ, Vec4 &ioBoundsMaxX, Vec4 &ioBoundsMaxY, Vec4 &ioBoundsMaxZ)
Enlarge 4 bounding boxes with extent (add to both sides)
Definition: AABox4.h:55
JPH_INLINE void AABox4Scale(Vec3Arg inScale, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, Vec4 &outBoundsMinX, Vec4 &outBoundsMinY, Vec4 &outBoundsMinZ, Vec4 &outBoundsMaxX, Vec4 &outBoundsMaxY, Vec4 &outBoundsMaxZ)
Scale 4 axis aligned boxes.
Definition: AABox4.h:33
#define JPH_NAMESPACE_END
Definition: Core.h:367
#define JPH_NAMESPACE_BEGIN
Definition: Core.h:361
constexpr T Square(T inV)
Square a value.
Definition: Math.h:52
Axis aligned box.
Definition: AABox.h:16
Vec3 mMin
Bounding box min and max.
Definition: AABox.h:300
Vec3 mMax
Definition: AABox.h:301
Holds a 4x4 matrix of floats, but supports also operations on the 3x3 upper left part of the matrix.
Definition: Mat44.h:13
JPH_INLINE Vec3 GetAxisY() const
Definition: Mat44.h:148
JPH_INLINE Vec3 GetAxisZ() const
Definition: Mat44.h:150
JPH_INLINE Vec3 GetAxisX() const
Access to the columns.
Definition: Mat44.h:146
JPH_INLINE Vec3 GetTranslation() const
Definition: Mat44.h:152
Oriented box.
Definition: OrientedBox.h:18
Mat44 mOrientation
Transform that positions and rotates the local space axis aligned box into world space.
Definition: OrientedBox.h:35
Vec3 mHalfExtents
Half extents (half the size of the edge) of the local space axis aligned box.
Definition: OrientedBox.h:36
Definition: UVec4.h:12
static JPH_INLINE UVec4 sNot(UVec4Arg inV1)
Logical not (component wise)
Definition: UVec4.inl:209
static JPH_INLINE UVec4 sAnd(UVec4Arg inV1, UVec4Arg inV2)
Logical and (component wise)
Definition: UVec4.inl:194
static JPH_INLINE UVec4 sOr(UVec4Arg inV1, UVec4Arg inV2)
Logical or (component wise)
Definition: UVec4.inl:166
Definition: Vec3.h:16
JPH_INLINE Vec4 SplatX() const
Replicate the X component to all components.
Definition: Vec3.inl:521
JPH_INLINE Vec3 Abs() const
Return the absolute value of each of the components.
Definition: Vec3.inl:564
JPH_INLINE Vec4 SplatZ() const
Replicate the Z component to all components.
Definition: Vec3.inl:543
JPH_INLINE Vec4 SplatY() const
Replicate the Y component to all components.
Definition: Vec3.inl:532
static JPH_INLINE Vec3 sReplicate(float inV)
Replicate inV across all components.
Definition: Vec3.inl:118
Definition: Vec4.h:14
JPH_INLINE Vec4 SplatX() const
Replicate the X component to all components.
Definition: Vec4.inl:547
static JPH_INLINE UVec4 sGreater(Vec4Arg inV1, Vec4Arg inV2)
Greater than (component wise)
Definition: Vec4.inl:208
static JPH_INLINE UVec4 sLessOrEqual(Vec4Arg inV1, Vec4Arg inV2)
Less than or equal (component wise)
Definition: Vec4.inl:194
JPH_INLINE Vec4 SplatY() const
Replicate the Y component to all components.
Definition: Vec4.inl:558
static JPH_INLINE UVec4 sGreaterOrEqual(Vec4Arg inV1, Vec4Arg inV2)
Greater than or equal (component wise)
Definition: Vec4.inl:222
static JPH_INLINE Vec4 sMin(Vec4Arg inV1, Vec4Arg inV2)
Return the minimum value of each of the components.
Definition: Vec4.inl:138
JPH_INLINE Vec4 SplatZ() const
Replicate the Z component to all components.
Definition: Vec4.inl:569
static JPH_INLINE Vec4 sMax(Vec4Arg inV1, Vec4Arg inV2)
Return the maximum of each of the components.
Definition: Vec4.inl:152
static JPH_INLINE Vec4 sReplicate(float inV)
Replicate inV across all components.
Definition: Vec4.inl:74