1//
2// Redistribution and use in source and binary forms, with or without
3// modification, are permitted provided that the following conditions
4// are met:
5// * Redistributions of source code must retain the above copyright
6// notice, this list of conditions and the following disclaimer.
7// * Redistributions in binary form must reproduce the above copyright
8// notice, this list of conditions and the following disclaimer in the
9// documentation and/or other materials provided with the distribution.
10// * Neither the name of NVIDIA CORPORATION nor the names of its
11// contributors may be used to endorse or promote products derived
12// from this software without specific prior written permission.
13//
14// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
15// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
17// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
18// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
19// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
20// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
21// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
22// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
24// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25//
26// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
27// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
28// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
29
30#ifndef GU_SWEEP_TRIANGLE_UTILS_H
31#define GU_SWEEP_TRIANGLE_UTILS_H
32
33#include "geometry/PxTriangle.h"
34#include "PxQueryReport.h"
35
36#include "CmPhysXCommon.h"
37#include "GuSweepSharedTests.h"
38#include "GuInternal.h"
39
40namespace physx
41{
42
43namespace Gu
44{
45 // PT: computes proper impact data for sphere-sweep-vs-tri, after the closest tri has been found.
46 void computeSphereTriImpactData(PxVec3& hit, PxVec3& normal, const PxVec3& center, const PxVec3& dir, float t, const PxTriangle& tri);
47
48 // PT: computes proper impact data for box-sweep-vs-tri, after the closest tri has been found.
49 void computeBoxTriImpactData(PxVec3& hit, PxVec3& normal, const PxVec3& boxExtents, const PxVec3& localDir, const PxTriangle& triInBoxSpace, PxReal impactDist);
50
51 // PT: computes impact normal between two edges. Produces better normals than just the EE cross product.
52 // This version properly computes the closest points between two colliding edges and makes a normal from these.
53 void computeEdgeEdgeNormal(PxVec3& normal, const PxVec3& p1, const PxVec3& p2_p1, const PxVec3& p3, const PxVec3& p4_p3, const PxVec3& dir, float d);
54
55 // PT: small function just to avoid duplicating the code.
56 // Returns index of first triangle we should process (when processing arrays of input triangles)
57 PX_FORCE_INLINE PxU32 getInitIndex(const PxU32* PX_RESTRICT cachedIndex, PxU32 nbTris)
58 {
59 PxU32 initIndex = 0; // PT: by default the first triangle to process is just the first one in the array
60 if(cachedIndex) // PT: but if we cached the last closest triangle from a previous call...
61 {
62 PX_ASSERT(*cachedIndex < nbTris);
63 PX_UNUSED(nbTris);
64 initIndex = *cachedIndex; // PT: ...then we should start with that one, to potentially shrink the ray as early as possible
65 }
66 return initIndex;
67 }
68
69 // PT: quick triangle rejection for sphere-based sweeps.
70 // Please refer to %SDKRoot%\InternalDocumentation\GU\cullTriangle.png for details & diagram.
71 PX_FORCE_INLINE bool cullTriangle(const PxVec3* PX_RESTRICT triVerts, const PxVec3& dir, PxReal radius, PxReal t, const PxReal dpc0)
72 {
73 // PT: project triangle on axis
74 const PxReal dp0 = triVerts[0].dot(v: dir);
75 const PxReal dp1 = triVerts[1].dot(v: dir);
76 const PxReal dp2 = triVerts[2].dot(v: dir);
77
78 // PT: keep min value = earliest possible impact distance
79 PxReal dp = dp0;
80 dp = physx::intrinsics::selectMin(a: dp, b: dp1);
81 dp = physx::intrinsics::selectMin(a: dp, b: dp2);
82
83 // PT: make sure we keep triangles that are about as close as best current distance
84 radius += 0.001f + GU_EPSILON_SAME_DISTANCE;
85
86 // PT: if earliest possible impact distance for this triangle is already larger than
87 // sphere's current best known impact distance, we can skip the triangle
88 if(dp>dpc0 + t + radius)
89 {
90 //PX_ASSERT(resx == 0.0f);
91 return false;
92 }
93
94 // PT: if triangle is fully located before the sphere's initial position, skip it too
95 const PxReal dpc1 = dpc0 - radius;
96 if(dp0<dpc1 && dp1<dpc1 && dp2<dpc1)
97 {
98 //PX_ASSERT(resx == 0.0f);
99 return false;
100 }
101
102 //PX_ASSERT(resx != 0.0f);
103 return true;
104 }
105
106 // PT: quick quad rejection for sphere-based sweeps. Same as for triangle, adapted for one more vertex.
107 PX_FORCE_INLINE bool cullQuad(const PxVec3* PX_RESTRICT quadVerts, const PxVec3& dir, PxReal radius, PxReal t, const PxReal dpc0)
108 {
109 // PT: project quad on axis
110 const PxReal dp0 = quadVerts[0].dot(v: dir);
111 const PxReal dp1 = quadVerts[1].dot(v: dir);
112 const PxReal dp2 = quadVerts[2].dot(v: dir);
113 const PxReal dp3 = quadVerts[3].dot(v: dir);
114
115 // PT: keep min value = earliest possible impact distance
116 PxReal dp = dp0;
117 dp = physx::intrinsics::selectMin(a: dp, b: dp1);
118 dp = physx::intrinsics::selectMin(a: dp, b: dp2);
119 dp = physx::intrinsics::selectMin(a: dp, b: dp3);
120
121 // PT: make sure we keep quads that are about as close as best current distance
122 radius += 0.001f;
123
124 // PT: if earliest possible impact distance for this quad is already larger than
125 // sphere's current best known impact distance, we can skip the quad
126 if(dp>dpc0 + t + radius)
127 return false;
128
129 // PT: if quad is fully located before the sphere's initial position, skip it too
130 const float dpc1 = dpc0 - radius;
131 if(dp0<dpc1 && dp1<dpc1 && dp2<dpc1 && dp3<dpc1)
132 return false;
133
134 return true;
135 }
136
137 // PT: computes distance between a point 'point' and a segment. The segment is defined as a starting point 'p0'
138 // and a direction vector 'dir' plus a length 't'. Segment's endpoint is p0 + dir * t.
139 //
140 // point
141 // o
142 // __/|
143 // __/ / |
144 // __/ / |(B)
145 // __/ (A)/ |
146 // __/ / | dir
147 // p0 o/---------o---------------o-- -->
148 // t (t<=fT) t (t>fT)
149 // return (A)^2 return (B)^2
150 //
151 // |<-------------->|
152 // fT
153 //
154 PX_FORCE_INLINE PxReal squareDistance(const PxVec3& p0, const PxVec3& dir, PxReal t, const PxVec3& point)
155 {
156 PxVec3 diff = point - p0;
157 PxReal fT = diff.dot(v: dir);
158 fT = physx::intrinsics::selectMax(a: fT, b: 0.0f);
159 fT = physx::intrinsics::selectMin(a: fT, b: t);
160 diff -= fT*dir;
161 return diff.magnitudeSquared();
162 }
163
164 // PT: quick triangle culling for sphere-based sweeps
165 // Please refer to %SDKRoot%\InternalDocumentation\GU\coarseCulling.png for details & diagram.
166 PX_FORCE_INLINE bool coarseCullingTri(const PxVec3& center, const PxVec3& dir, PxReal t, PxReal radius, const PxVec3* PX_RESTRICT triVerts)
167 {
168 // PT: compute center of triangle ### could be precomputed?
169 const PxVec3 triCenter = (triVerts[0] + triVerts[1] + triVerts[2]) * (1.0f/3.0f);
170
171 // PT: distance between the triangle center and the swept path (an LSS)
172 // Same as: distancePointSegmentSquared(center, center+dir*t, TriCenter);
173 PxReal d = PxSqrt(a: squareDistance(p0: center, dir, t, point: triCenter)) - radius - 0.0001f;
174
175 if (d < 0.0f) // The triangle center lies inside the swept sphere
176 return true;
177
178 d*=d;
179
180 // PT: coarse capsule-vs-triangle overlap test ### distances could be precomputed?
181 if(1)
182 {
183 if(d <= (triCenter-triVerts[0]).magnitudeSquared())
184 return true;
185 if(d <= (triCenter-triVerts[1]).magnitudeSquared())
186 return true;
187 if(d <= (triCenter-triVerts[2]).magnitudeSquared())
188 return true;
189 }
190 else
191 {
192 const float d0 = (triCenter-triVerts[0]).magnitudeSquared();
193 const float d1 = (triCenter-triVerts[1]).magnitudeSquared();
194 const float d2 = (triCenter-triVerts[2]).magnitudeSquared();
195 float triRadius = physx::intrinsics::selectMax(a: d0, b: d1);
196 triRadius = physx::intrinsics::selectMax(a: triRadius, b: d2);
197 if(d <= triRadius)
198 return true;
199 }
200 return false;
201 }
202
203 // PT: quick quad culling for sphere-based sweeps. Same as for triangle, adapted for one more vertex.
204 PX_FORCE_INLINE bool coarseCullingQuad(const PxVec3& center, const PxVec3& dir, PxReal t, PxReal radius, const PxVec3* PX_RESTRICT quadVerts)
205 {
206 // PT: compute center of quad ### could be precomputed?
207 const PxVec3 quadCenter = (quadVerts[0] + quadVerts[1] + quadVerts[2] + quadVerts[3]) * (1.0f/4.0f);
208
209 // PT: distance between the quad center and the swept path (an LSS)
210 PxReal d = PxSqrt(a: squareDistance(p0: center, dir, t, point: quadCenter)) - radius - 0.0001f;
211
212 if (d < 0.0f) // The quad center lies inside the swept sphere
213 return true;
214
215 d*=d;
216
217 // PT: coarse capsule-vs-quad overlap test ### distances could be precomputed?
218 if(1)
219 {
220 if(d <= (quadCenter-quadVerts[0]).magnitudeSquared())
221 return true;
222 if(d <= (quadCenter-quadVerts[1]).magnitudeSquared())
223 return true;
224 if(d <= (quadCenter-quadVerts[2]).magnitudeSquared())
225 return true;
226 if(d <= (quadCenter-quadVerts[3]).magnitudeSquared())
227 return true;
228 }
229 return false;
230 }
231
232 // PT: combined triangle culling for sphere-based sweeps
233 PX_FORCE_INLINE bool rejectTriangle(const PxVec3& center, const PxVec3& unitDir, PxReal curT, PxReal radius, const PxVec3* PX_RESTRICT triVerts, const PxReal dpc0)
234 {
235 if(!coarseCullingTri(center, dir: unitDir, t: curT, radius, triVerts))
236 return true;
237 if(!cullTriangle(triVerts, dir: unitDir, radius, t: curT, dpc0))
238 return true;
239 return false;
240 }
241
242 // PT: combined quad culling for sphere-based sweeps
243 PX_FORCE_INLINE bool rejectQuad(const PxVec3& center, const PxVec3& unitDir, PxReal curT, PxReal radius, const PxVec3* PX_RESTRICT quadVerts, const PxReal dpc0)
244 {
245 if(!coarseCullingQuad(center, dir: unitDir, t: curT, radius, quadVerts))
246 return true;
247 if(!cullQuad(quadVerts, dir: unitDir, radius, t: curT, dpc0))
248 return true;
249 return false;
250 }
251
252 PX_FORCE_INLINE bool shouldFlipNormal(const PxVec3& normal, bool meshBothSides, bool isDoubleSided, const PxVec3& triangleNormal, const PxVec3& dir)
253 {
254 // PT: this function assumes that input normal is opposed to the ray/sweep direction. This is always
255 // what we want except when we hit a single-sided back face with 'meshBothSides' enabled.
256
257 if(!meshBothSides || isDoubleSided)
258 return false;
259
260 PX_ASSERT(normal.dot(dir) <= 0.0f); // PT: if this fails, the logic below cannot be applied
261 PX_UNUSED(normal);
262 return triangleNormal.dot(v: dir) > 0.0f; // PT: true for back-facing hits
263 }
264
265 PX_FORCE_INLINE bool shouldFlipNormal(const PxVec3& normal, bool meshBothSides, bool isDoubleSided, const PxTriangle& triangle, const PxVec3& dir, const PxTransform* pose)
266 {
267 // PT: this function assumes that input normal is opposed to the ray/sweep direction. This is always
268 // what we want except when we hit a single-sided back face with 'meshBothSides' enabled.
269
270 if(!meshBothSides || isDoubleSided)
271 return false;
272
273 PX_ASSERT(normal.dot(dir) <= 0.0f); // PT: if this fails, the logic below cannot be applied
274 PX_UNUSED(normal);
275
276 PxVec3 triangleNormal;
277 triangle.denormalizedNormal(normal&: triangleNormal);
278
279 if(pose)
280 triangleNormal = pose->rotate(input: triangleNormal);
281
282 return triangleNormal.dot(v: dir) > 0.0f; // PT: true for back-facing hits
283 }
284
285 // PT: implements the spec for IO sweeps in a single place (to ensure consistency)
286 PX_FORCE_INLINE bool setInitialOverlapResults(PxSweepHit& hit, const PxVec3& unitDir, PxU32 faceIndex)
287 {
288 // PT: please write these fields in the order they are listed in the struct.
289 hit.faceIndex = faceIndex;
290 hit.flags = PxHitFlag::eNORMAL|PxHitFlag::eFACE_INDEX;
291 hit.normal = -unitDir;
292 hit.distance = 0.0f;
293 return true; // PT: true indicates a hit, saves some lines in calling code
294 }
295
296 PX_FORCE_INLINE void computeBoxLocalImpact( PxVec3& pos, PxVec3& normal, PxHitFlags& outFlags,
297 const Box& box, const PxVec3& localDir, const PxTriangle& triInBoxSpace,
298 const PxHitFlags inFlags, bool isDoubleSided, bool meshBothSides, PxReal impactDist)
299 {
300 if(inFlags & (PxHitFlag::eNORMAL|PxHitFlag::ePOSITION))
301 {
302 PxVec3 localPos, localNormal;
303 computeBoxTriImpactData(hit&: localPos, normal&: localNormal, boxExtents: box.extents, localDir, triInBoxSpace, impactDist);
304
305 if(inFlags & PxHitFlag::eNORMAL)
306 {
307 localNormal.normalize();
308
309 // PT: doing this after the 'rotate' minimizes errors when normal and dir are close to perpendicular
310 // ....but we must do it before the rotate now, because triangleNormal is in box space (and thus we
311 // need the normal with the proper orientation, in box space. We can't fix it after it's been rotated
312 // to box space.
313 // Technically this one is only here because of the EE cross product in the feature-based sweep.
314 // PT: TODO: revisit corresponding code in computeImpactData, get rid of ambiguity
315 // PT: TODO: this may not be needed anymore
316 if((localNormal.dot(v: localDir))>0.0f)
317 localNormal = -localNormal;
318
319 // PT: this one is to ensure the normal respects the mesh-both-sides/double-sided convention
320 if(shouldFlipNormal(normal: localNormal, meshBothSides, isDoubleSided, triangle: triInBoxSpace, dir: localDir, NULL))
321 localNormal = -localNormal;
322
323 normal = box.rotate(src: localNormal);
324 outFlags |= PxHitFlag::eNORMAL;
325 }
326
327 if(inFlags & PxHitFlag::ePOSITION)
328 {
329 pos = box.transform(src: localPos);
330 outFlags |= PxHitFlag::ePOSITION;
331 }
332 }
333 }
334
335} // namespace Gu
336
337}
338
339#endif
340

source code of qtquick3dphysics/src/3rdparty/PhysX/source/geomutils/src/sweep/GuSweepTriangleUtils.h