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#include "geometry/PxConvexMeshGeometry.h"
31#include "geometry/PxSphereGeometry.h"
32#include "GuSweepTests.h"
33#include "GuHeightFieldUtil.h"
34#include "CmScaling.h"
35#include "GuConvexMesh.h"
36#include "GuIntersectionRayPlane.h"
37#include "GuVecBox.h"
38#include "GuVecCapsule.h"
39#include "GuVecConvexHull.h"
40#include "GuSweepMTD.h"
41#include "GuSweepSphereCapsule.h"
42#include "GuSweepCapsuleCapsule.h"
43#include "GuSweepTriangleUtils.h"
44#include "GuSweepCapsuleTriangle.h"
45#include "GuInternal.h"
46#include "GuGJKRaycast.h"
47
48using namespace physx;
49using namespace Gu;
50using namespace Cm;
51using namespace physx::shdfnd::aos;
52
53static const PxReal gEpsilon = .01f;
54
55static PxU32 computeSweepConvexPlane(
56 const PxConvexMeshGeometry& convexGeom, ConvexHullData* hullData, const PxU32& nbPolys, const PxTransform& pose,
57 const PxVec3& impact_, const PxVec3& unitDir)
58{
59 PX_ASSERT(nbPolys);
60
61 const PxVec3 impact = impact_ - unitDir * gEpsilon;
62
63 const PxVec3 localPoint = pose.transformInv(input: impact);
64 const PxVec3 localDir = pose.rotateInv(input: unitDir);
65
66 const FastVertex2ShapeScaling scaling(convexGeom.scale);
67
68 PxU32 minIndex = 0;
69 PxReal minD = PX_MAX_REAL;
70 for(PxU32 j=0; j<nbPolys; j++)
71 {
72 const PxPlane& pl = hullData->mPolygons[j].mPlane;
73
74 PxPlane plane;
75 scaling.transformPlaneToShapeSpace(nIn: pl.n, dIn: pl.d, nOut&: plane.n, dOut&: plane.d);
76
77 PxReal d = plane.distance(p: localPoint);
78 if(d<0.0f)
79 continue;
80
81 const PxReal tweak = plane.n.dot(v: localDir) * gEpsilon;
82 d += tweak;
83
84 if(d<minD)
85 {
86 minIndex = j;
87 minD = d;
88 }
89 }
90 return minIndex;
91}
92
93static PX_FORCE_INLINE bool computeFaceIndex(PxSweepHit& sweepHit, const PxHitFlags hitFlags, const PxConvexMeshGeometry& convexGeom, ConvexHullData* hullData, const PxTransform& pose, const PxVec3& unitDir)
94{
95 if(hitFlags & PxHitFlag::eFACE_INDEX)
96 {
97 // PT: compute closest polygon using the same tweak as in swept-capsule-vs-mesh
98 sweepHit.faceIndex = computeSweepConvexPlane(convexGeom, hullData, nbPolys: hullData->mNbPolygons, pose, impact_: sweepHit.position, unitDir);
99 sweepHit.flags |= PxHitFlag::eFACE_INDEX;
100 }
101 return true;
102}
103
104static PX_FORCE_INLINE bool hasInitialOverlap(PxSweepHit& sweepHit, const PxVec3& unitDir,
105 const FloatVArg toi,
106 const Vec3VArg normal, const Vec3VArg closestA,
107 const PsTransformV& convexPose,
108 const bool isMtd, const bool impactPointOnTheOtherShape)
109{
110 sweepHit.flags = PxHitFlag::eNORMAL;
111
112 const FloatV zero = FZero();
113 if(FAllGrtrOrEq(a: zero, b: toi))
114 {
115 //ML: initial overlap
116 if(isMtd)
117 {
118 sweepHit.flags |= PxHitFlag::ePOSITION;
119 const FloatV length = toi;
120 const Vec3V worldPointA = convexPose.transform(input: closestA);
121 const Vec3V worldNormal = V3Normalize(a: convexPose.rotate(input: normal));
122 if(impactPointOnTheOtherShape)
123 {
124 const Vec3V destWorldPointA = V3NegScaleSub(a: worldNormal, b: length, c: worldPointA);
125 V3StoreU(a: worldNormal, f&: sweepHit.normal);
126 V3StoreU(a: destWorldPointA, f&: sweepHit.position);
127 }
128 else
129 {
130 const Vec3V destNormal = V3Neg(f: worldNormal);
131 V3StoreU(a: destNormal, f&: sweepHit.normal);
132 V3StoreU(a: worldPointA, f&: sweepHit.position);
133 }
134 FStore(a: length, f: &sweepHit.distance);
135 }
136 else
137 {
138 sweepHit.distance = 0.0f;
139 sweepHit.normal = -unitDir;
140 }
141 sweepHit.faceIndex = 0xffffffff;
142 return true;
143 }
144 return false;
145}
146
147///////////////////////////////////////////////// sweepCapsule/Sphere //////////////////////////////////////////////////////
148bool sweepCapsule_SphereGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS)
149{
150 PX_UNUSED(capsuleGeom_);
151 PX_UNUSED(capsulePose_);
152
153 PX_ASSERT(geom.getType() == PxGeometryType::eSPHERE);
154 const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom);
155
156 const Sphere sphere(pose.p, sphereGeom.radius+inflation);
157
158 if(!sweepSphereCapsule(sphere, lss, dir: -unitDir, length: distance, d&: sweepHit.distance, ip&: sweepHit.position, nrm&: sweepHit.normal, hitFlags))
159 return false;
160
161 const bool isMtd = hitFlags & PxHitFlag::eMTD;
162
163 if(isMtd)
164 {
165 sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
166
167 if(sweepHit.distance == 0.f)
168 {
169 //intialOverlap
170 if(lss.p0 == lss.p1)
171 {
172 //sphere
173 return computeSphere_SphereMTD(sphere0: sphere, sphere1: Sphere(lss.p0, lss.radius), hit&: sweepHit);
174 }
175 else
176 {
177 //capsule
178 return computeSphere_CapsuleMTD(sphere, capsule: lss, hit&: sweepHit);
179 }
180 }
181 }
182 else
183 {
184 if(sweepHit.distance!=0.0f)
185 sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
186 else
187 sweepHit.flags = PxHitFlag::eNORMAL;
188 }
189 return true;
190}
191
192bool sweepCapsule_PlaneGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS)
193{
194 PX_UNUSED(capsuleGeom_);
195 PX_UNUSED(capsulePose_);
196
197 PX_ASSERT(geom.getType() == PxGeometryType::ePLANE);
198 PX_UNUSED(geom);
199// const PxPlaneGeometry& planeGeom = static_cast<const PxPlaneGeometry&>(geom);
200
201 const PxPlane& worldPlane = getPlane(pose);
202
203 const PxF32 capsuleRadius = lss.radius + inflation;
204
205 PxU32 index = 0;
206 PxVec3 pts[2];
207
208 PxReal minDp = PX_MAX_REAL;
209
210 sweepHit.faceIndex = 0xFFFFffff; // spec says face index is undefined for planes
211
212 // Find extreme point on the capsule
213 // AP: removed if (lss.p0 == lss.p1 clause because it wasn't properly computing minDp)
214 pts[0] = lss.p0;
215 pts[1] = lss.p1;
216 for(PxU32 i=0; i<2; i++)
217 {
218 const PxReal dp = pts[i].dot(v: worldPlane.n);
219 if(dp<minDp)
220 {
221 minDp = dp;
222 index = i;
223 }
224 }
225
226 const bool isMtd = hitFlags & PxHitFlag::eMTD;
227
228 if(isMtd)
229 {
230 //initial overlap with the plane
231 if(minDp <= capsuleRadius - worldPlane.d)
232 {
233 sweepHit.flags = PxHitFlag::eNORMAL| PxHitFlag::ePOSITION;
234 return computePlane_CapsuleMTD(plane: worldPlane, capsule: lss, hit&: sweepHit);
235 }
236 }
237 else
238 {
239 if(!(hitFlags & PxHitFlag::eASSUME_NO_INITIAL_OVERLAP))
240 {
241 // test if the capsule initially overlaps with plane
242 if(minDp <= capsuleRadius - worldPlane.d)
243 {
244 sweepHit.flags = PxHitFlag::eNORMAL;
245 sweepHit.distance = 0.0f;
246 sweepHit.normal = -unitDir;
247 return true;
248 }
249 }
250 }
251
252 const PxVec3 ptOnCapsule = pts[index] - worldPlane.n*capsuleRadius;
253
254 // Raycast extreme vertex against plane
255 bool hitPlane = intersectRayPlane(orig: ptOnCapsule, dir: unitDir, plane: worldPlane, distanceAlongLine&: sweepHit.distance, pointOnPlane: &sweepHit.position);
256 if(hitPlane && sweepHit.distance > 0 && sweepHit.distance <= distance)
257 {
258 sweepHit.normal = worldPlane.n;
259 sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
260 return true;
261 }
262 return false;
263}
264
265bool sweepCapsule_CapsuleGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS)
266{
267 PX_UNUSED(capsuleGeom_);
268 PX_UNUSED(capsulePose_);
269
270 PX_ASSERT(geom.getType() == PxGeometryType::eCAPSULE);
271 const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom);
272
273 Capsule staticCapsule;
274 getCapsule(capsule&: staticCapsule, capsuleGeom, pose);
275 staticCapsule.radius +=inflation;
276
277 const bool isMtd = hitFlags & PxHitFlag::eMTD;
278
279 PxU16 outFlags;
280 if(!sweepCapsuleCapsule(capsule0: lss, capsule1: staticCapsule, dir: -unitDir, length: distance, min_dist&: sweepHit.distance, ip&: sweepHit.position, normal&: sweepHit.normal, inHitFlags: hitFlags, outHitFlags&: outFlags))
281 return false;
282
283 sweepHit.flags = PxHitFlags(outFlags);
284 if(sweepHit.distance == 0.0f)
285 {
286 //initial overlap
287 if(isMtd)
288 {
289 sweepHit.flags |= PxHitFlag::ePOSITION;
290 return computeCapsule_CapsuleMTD(capsule0: lss, capsule1: staticCapsule, hit&: sweepHit);
291 }
292 }
293 return true;
294}
295
296bool sweepCapsule_ConvexGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS)
297{
298 PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
299
300 using namespace Ps::aos;
301
302 PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
303 const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
304
305 ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
306 ConvexHullData* hullData = &convexMesh->getHull();
307
308 const Vec3V zeroV = V3Zero();
309 const FloatV zero = FZero();
310 const FloatV dist = FLoad(f: distance);
311 const Vec3V worldDir = V3LoadU(f: unitDir);
312
313 const PsTransformV capPose = loadTransformU(transform: capsulePose_);
314 const PsTransformV convexPose = loadTransformU(transform: pose);
315
316 const PsMatTransformV aToB(convexPose.transformInv(src: capPose));
317
318 const FloatV capsuleHalfHeight = FLoad(f: capsuleGeom_.halfHeight);
319 const FloatV capsuleRadius = FLoad(f: lss.radius);
320
321 const Vec3V vScale = V3LoadU_SafeReadW(f: convexGeom.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
322 const QuatV vQuat = QuatVLoadU(v: &convexGeom.scale.rotation.x);
323
324 CapsuleV capsule(aToB.p, aToB.rotate( input: V3Scale(a: V3UnitX(), b: capsuleHalfHeight)), capsuleRadius);
325 ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, convexGeom.scale.isIdentity());
326
327 const Vec3V dir = convexPose.rotateInv(input: V3Neg(f: V3Scale(a: worldDir, b: dist)));
328
329 bool isMtd = hitFlags & PxHitFlag::eMTD;
330
331 FloatV toi;
332 Vec3V closestA, normal;//closestA and normal is in the local space of convex hull
333 LocalConvex<CapsuleV> convexA(capsule);
334 LocalConvex<ConvexHullV> convexB(convexHull);
335 const Vec3V initialSearchDir = V3Sub(a: capsule.getCenter(), b: convexHull.getCenter());
336 if(!gjkRaycastPenetration< LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(a: convexA, b: convexB, initialDir: initialSearchDir, initialLambda: zero, s: zeroV, r: dir, lambda&: toi, normal, closestA, inflation: lss.radius + inflation, initialOverlap: isMtd))
337 return false;
338
339 if(hasInitialOverlap(sweepHit, unitDir, toi, normal, closestA, convexPose, isMtd, impactPointOnTheOtherShape: true))
340 return true;
341
342 sweepHit.flags |= PxHitFlag::ePOSITION;
343 const Vec3V worldPointA = convexPose.transform(input: closestA);
344 const FloatV length = FMul(a: dist, b: toi);
345 const Vec3V destNormal = V3Normalize(a: convexPose.rotate(input: normal));
346 const Vec3V destWorldPointA = V3ScaleAdd(a: worldDir, b: length, c: worldPointA);
347 V3StoreU(a: destNormal, f&: sweepHit.normal);
348 V3StoreU(a: destWorldPointA, f&: sweepHit.position);
349 FStore(a: length, f: &sweepHit.distance);
350
351 return computeFaceIndex(sweepHit, hitFlags, convexGeom, hullData, pose, unitDir);
352}
353
354///////////////////////////////////////////////// sweepBox //////////////////////////////////////////////////////
355
356bool sweepBox_PlaneGeom(GU_BOX_SWEEP_FUNC_PARAMS)
357{
358 PX_ASSERT(geom.getType() == PxGeometryType::ePLANE);
359 PX_UNUSED(geom);
360 PX_UNUSED(boxPose_);
361 PX_UNUSED(boxGeom_);
362
363// const PxPlaneGeometry& planeGeom = static_cast<const PxPlaneGeometry&>(geom);
364
365 sweepHit.faceIndex = 0xFFFFffff; // spec says face index is undefined for planes
366
367 PxPlane worldPlane = getPlane(pose);
368 worldPlane.d -=inflation;
369
370 // Find extreme point on the box
371 PxVec3 boxPts[8];
372 box.computeBoxPoints(pts: boxPts);
373 PxU32 index = 0;
374 PxReal minDp = PX_MAX_REAL;
375 for(PxU32 i=0;i<8;i++)
376 {
377 const PxReal dp = boxPts[i].dot(v: worldPlane.n);
378
379 if(dp<minDp)
380 {
381 minDp = dp;
382 index = i;
383 }
384 }
385
386 bool isMtd = hitFlags & PxHitFlag::eMTD;
387
388 if(isMtd)
389 {
390 // test if box initially overlap with plane
391 if(minDp <= -worldPlane.d)
392 {
393 sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
394 //compute Mtd;
395 return computePlane_BoxMTD(plane: worldPlane, box, hit&: sweepHit);
396 }
397 }
398 else
399 {
400 if(!(hitFlags & PxHitFlag::eASSUME_NO_INITIAL_OVERLAP))
401 {
402 // test if box initially overlap with plane
403 if(minDp <= -worldPlane.d)
404 {
405 sweepHit.flags = PxHitFlag::eNORMAL;
406 sweepHit.distance = 0.0f;
407 sweepHit.normal = -unitDir;
408 return true;
409 }
410 }
411 }
412
413 // Raycast extreme vertex against plane
414 bool hitPlane = intersectRayPlane(orig: boxPts[index], dir: unitDir, plane: worldPlane, distanceAlongLine&: sweepHit.distance, pointOnPlane: &sweepHit.position);
415 if(hitPlane && sweepHit.distance > 0 && sweepHit.distance <= distance)
416 {
417 sweepHit.normal = worldPlane.n;
418 sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
419 return true;
420 }
421 return false;
422}
423
424bool sweepBox_ConvexGeom(GU_BOX_SWEEP_FUNC_PARAMS)
425{
426 PX_UNUSED(boxGeom_);
427
428 using namespace Ps::aos;
429 PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
430 const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
431
432 ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
433 ConvexHullData* hullData = &convexMesh->getHull();
434
435 const Vec3V zeroV = V3Zero();
436 const FloatV zero = FZero();
437
438 const PsTransformV boxPose = loadTransformU(transform: boxPose_);
439 const PsTransformV convexPose = loadTransformU(transform: pose);
440
441 const PsMatTransformV aToB(convexPose.transformInv(src: boxPose));
442
443 const Vec3V boxExtents = V3LoadU(f: box.extents);
444
445 const Vec3V vScale = V3LoadU_SafeReadW(f: convexGeom.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
446 const QuatV vQuat = QuatVLoadU(v: &convexGeom.scale.rotation.x);
447
448 BoxV boxV(zeroV, boxExtents);
449 ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, convexGeom.scale.isIdentity());
450
451 const Vec3V worldDir = V3LoadU(f: unitDir);
452 const FloatV dist = FLoad(f: distance);
453 const Vec3V dir = convexPose.rotateInv(input: V3Neg(f: V3Scale(a: worldDir, b: dist)));
454
455 bool isMtd = hitFlags & PxHitFlag::eMTD;
456
457 FloatV toi;
458 Vec3V closestA, normal;
459 RelativeConvex<BoxV> convexA(boxV, aToB);
460 LocalConvex<ConvexHullV> convexB(convexHull);
461 if(!gjkRaycastPenetration< RelativeConvex<BoxV>,LocalConvex<ConvexHullV> >(a: convexA, b: convexB, initialDir: aToB.p, initialLambda: zero, s: zeroV, r: dir, lambda&: toi, normal, closestA, inflation: inflation, initialOverlap: isMtd))
462 return false;
463
464 if(hasInitialOverlap(sweepHit, unitDir, toi, normal, closestA, convexPose, isMtd, impactPointOnTheOtherShape: true))
465 return true;
466
467 sweepHit.flags |= PxHitFlag::ePOSITION;
468 const Vec3V destNormal = V3Normalize(a: convexPose.rotate(input: normal));
469 const FloatV length = FMul(a: dist, b: toi);
470 const Vec3V worldPointA = convexPose.transform(input: closestA);
471 const Vec3V destWorldPointA = V3ScaleAdd(a: worldDir, b: length, c: worldPointA);
472 V3StoreU(a: destNormal, f&: sweepHit.normal);
473 V3StoreU(a: destWorldPointA, f&: sweepHit.position);
474 FStore(a: length, f: &sweepHit.distance);
475
476 return computeFaceIndex(sweepHit, hitFlags, convexGeom, hullData, pose, unitDir);
477}
478
479/////////////////////////////////////////////////////////////////////////////////////////////////////////////
480
481bool Gu::sweepCapsuleTriangles(GU_SWEEP_TRIANGLES_FUNC_PARAMS(PxCapsuleGeometry))
482{
483 Capsule capsule;
484 getCapsule(capsule, capsuleGeom: geom, pose);
485 capsule.radius +=inflation;
486
487 // Compute swept box
488 Box capsuleBox;
489 computeBoxAroundCapsule(capsule, box&: capsuleBox);
490
491 BoxPadded sweptBounds;
492 computeSweptBox(box&: sweptBounds, extents: capsuleBox.extents, center: capsuleBox.center, rot: capsuleBox.rot, unitDir, distance);
493
494 PxVec3 triNormal;
495 return sweepCapsuleTriangles_Precise(nbTris, triangles, capsule, unitDir, distance, cachedIndex, hit, triNormalOut&: triNormal, hitFlags, isDoubleSided: doubleSided, cullBox: &sweptBounds);
496}
497
498/////////////////////////////////////////////////////////////////////////////////////////////////////////////
499
500bool sweepConvex_SphereGeom(GU_CONVEX_SWEEP_FUNC_PARAMS)
501{
502 PX_ASSERT(geom.getType() == PxGeometryType::eSPHERE);
503 const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom);
504
505 ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
506 ConvexHullData* hullData = &convexMesh->getHull();
507
508 const Vec3V zeroV = V3Zero();
509 const FloatV zero= FZero();
510
511 const Vec3V vScale = V3LoadU_SafeReadW(f: convexGeom.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
512 const QuatV vQuat = QuatVLoadU(v: &convexGeom.scale.rotation.x);
513
514 const FloatV sphereRadius = FLoad(f: sphereGeom.radius);
515
516 const PsTransformV sphereTransf = loadTransformU(transform: pose);
517 const PsTransformV convexTransf = loadTransformU(transform: convexPose);
518
519 const PsMatTransformV aToB(convexTransf.transformInv(src: sphereTransf));
520
521 const Vec3V worldDir = V3LoadU(f: unitDir);
522 const FloatV dist = FLoad(f: distance);
523 const Vec3V dir = convexTransf.rotateInv(input: V3Scale(a: worldDir, b: dist));
524
525 ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, convexGeom.scale.isIdentity());
526 //CapsuleV capsule(zeroV, sphereRadius);
527 CapsuleV capsule(aToB.p, sphereRadius);
528
529 const bool isMtd = hitFlags & PxHitFlag::eMTD;
530
531 FloatV toi;
532 Vec3V closestA, normal;
533 LocalConvex<CapsuleV> convexA(capsule);
534 LocalConvex<ConvexHullV> convexB(convexHull);
535 const Vec3V initialSearchDir = V3Sub(a: capsule.getCenter(), b: convexHull.getCenter());
536 if(!gjkRaycastPenetration< LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(a: convexA, b: convexB, initialDir: initialSearchDir, initialLambda: zero, s: zeroV, r: dir, lambda&: toi, normal, closestA, inflation: sphereGeom.radius+inflation, initialOverlap: isMtd))
537 return false;
538
539 if(hasInitialOverlap(sweepHit, unitDir, toi, normal, closestA, convexPose, isMtd, impactPointOnTheOtherShape: false))
540 return true;
541
542 sweepHit.flags |= PxHitFlag::ePOSITION;
543 const Vec3V destNormal = V3Neg(f: V3Normalize(a: convexTransf.rotate(input: normal)));
544 const FloatV length = FMul(a: dist, b: toi);
545 const Vec3V destWorldPointA = convexTransf.transform(input: closestA);
546 V3StoreU(a: destNormal, f&: sweepHit.normal);
547 V3StoreU(a: destWorldPointA, f&: sweepHit.position);
548 FStore(a: length, f: &sweepHit.distance);
549 sweepHit.faceIndex = 0xffffffff;
550 return true;
551}
552
553bool sweepConvex_PlaneGeom(GU_CONVEX_SWEEP_FUNC_PARAMS)
554{
555 PX_ASSERT(geom.getType() == PxGeometryType::ePLANE);
556 PX_UNUSED(hitFlags);
557 PX_UNUSED(geom);
558
559 ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
560 ConvexHullData* hullData = &convexMesh->getHull();
561
562 sweepHit.faceIndex = 0xFFFFffff; // spec says face index is undefined for planes
563
564 const PxVec3* PX_RESTRICT hullVertices = hullData->getHullVertices();
565 PxU32 numHullVertices = hullData->mNbHullVertices;
566
567 const bool isMtd = hitFlags & PxHitFlag::eMTD;
568
569 const FastVertex2ShapeScaling convexScaling(convexGeom.scale);
570
571 PxPlane plane = getPlane(pose);
572 plane.d -=inflation;
573
574 sweepHit.distance = distance;
575 bool status = false;
576 bool initialOverlap = false;
577 while(numHullVertices--)
578 {
579 const PxVec3& vertex = *hullVertices++;
580 const PxVec3 worldPt = convexPose.transform(input: convexScaling * vertex);
581 float t;
582 PxVec3 pointOnPlane;
583 if(intersectRayPlane(orig: worldPt, dir: unitDir, plane, distanceAlongLine&: t, pointOnPlane: &pointOnPlane))
584 {
585 if(plane.distance(p: worldPt) <= 0.0f)
586 {
587 initialOverlap = true;
588 break;
589 //// Convex touches plane
590 //sweepHit.distance = 0.0f;
591 //sweepHit.flags = PxHitFlag::eNORMAL;
592 //sweepHit.normal = -unitDir;
593 //return true;
594 }
595
596 if(t > 0.0f && t <= sweepHit.distance)
597 {
598 sweepHit.distance = t;
599 sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
600 sweepHit.position = pointOnPlane;
601 sweepHit.normal = plane.n;
602 status = true;
603 }
604 }
605 }
606
607 if(initialOverlap)
608 {
609 if(isMtd)
610 {
611 sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
612 return computePlane_ConvexMTD(plane, convexGeom, convexPose, hit&: sweepHit);
613 }
614 else
615 {
616 sweepHit.distance = 0.0f;
617 sweepHit.flags = PxHitFlag::eNORMAL;
618 sweepHit.normal = -unitDir;
619 return true;
620 }
621 }
622 return status;
623}
624
625bool sweepConvex_CapsuleGeom(GU_CONVEX_SWEEP_FUNC_PARAMS)
626{
627 PX_ASSERT(geom.getType() == PxGeometryType::eCAPSULE);
628 const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom);
629
630 Capsule capsule;
631 getCapsule(capsule, capsuleGeom, pose);
632
633 // remove PxHitFlag::eFACE_INDEX, not neeeded to compute.
634 PxHitFlags tempHitFlags = hitFlags;
635 tempHitFlags &= ~PxHitFlag::eFACE_INDEX;
636
637 if(!sweepCapsule_ConvexGeom(geom: convexGeom, pose: convexPose, capsuleGeom_: capsuleGeom, capsulePose_: pose, lss: capsule, unitDir: -unitDir, distance, sweepHit, hitFlags: tempHitFlags, inflation))
638 return false;
639
640 if(sweepHit.flags & PxHitFlag::ePOSITION)
641 sweepHit.position += unitDir * sweepHit.distance;
642
643 sweepHit.normal = -sweepHit.normal;
644 sweepHit.faceIndex = 0xffffffff;
645 return true;
646}
647
648bool sweepConvex_BoxGeom(GU_CONVEX_SWEEP_FUNC_PARAMS)
649{
650 PX_ASSERT(geom.getType() == PxGeometryType::eBOX);
651 const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);
652
653 Box box;
654 buildFrom(dst&: box, center: pose.p, extents: boxGeom.halfExtents, q: pose.q);
655
656 // remove PxHitFlag::eFACE_INDEX, not neeeded to compute.
657 PxHitFlags tempHitFlags = hitFlags;
658 tempHitFlags &= ~PxHitFlag::eFACE_INDEX;
659
660 if(!sweepBox_ConvexGeom(geom: convexGeom, pose: convexPose, boxGeom_: boxGeom, boxPose_: pose, box, unitDir: -unitDir, distance, sweepHit, hitFlags: tempHitFlags, inflation))
661 return false;
662
663 if(sweepHit.flags & PxHitFlag::ePOSITION)
664 sweepHit.position += unitDir * sweepHit.distance;
665
666 sweepHit.normal = -sweepHit.normal;
667 sweepHit.faceIndex = 0xffffffff;
668 return true;
669}
670
671bool sweepConvex_ConvexGeom(GU_CONVEX_SWEEP_FUNC_PARAMS)
672{
673 using namespace Ps::aos;
674 PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
675 const PxConvexMeshGeometry& otherConvexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
676 ConvexMesh& otherConvexMesh = *static_cast<ConvexMesh*>(otherConvexGeom.convexMesh);
677
678 ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
679 ConvexHullData* hullData = &convexMesh->getHull();
680
681 ConvexHullData* otherHullData = &otherConvexMesh.getHull();
682
683 const Vec3V zeroV = V3Zero();
684 const FloatV zero = FZero();
685
686 const Vec3V otherVScale = V3LoadU_SafeReadW(f: otherConvexGeom.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
687 const QuatV otherVQuat = QuatVLoadU(v: &otherConvexGeom.scale.rotation.x);
688
689 const Vec3V vScale = V3LoadU_SafeReadW(f: convexGeom.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
690 const QuatV vQuat = QuatVLoadU(v: &convexGeom.scale.rotation.x);
691
692 const PsTransformV otherTransf = loadTransformU(transform: pose);
693 const PsTransformV convexTransf = loadTransformU(transform: convexPose);
694
695 const Vec3V worldDir = V3LoadU(f: unitDir);
696 const FloatV dist = FLoad(f: distance);
697 const Vec3V dir = convexTransf.rotateInv(input: V3Scale(a: worldDir, b: dist));
698
699 const PsMatTransformV aToB(convexTransf.transformInv(src: otherTransf));
700
701 ConvexHullV otherConvexHull(otherHullData, zeroV, otherVScale, otherVQuat, otherConvexGeom.scale.isIdentity());
702 ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, convexGeom.scale.isIdentity());
703
704 const bool isMtd = hitFlags & PxHitFlag::eMTD;
705
706 FloatV toi;
707 Vec3V closestA, normal;
708 RelativeConvex<ConvexHullV> convexA(otherConvexHull, aToB);
709 LocalConvex<ConvexHullV> convexB(convexHull);
710 if(!gjkRaycastPenetration< RelativeConvex<ConvexHullV>, LocalConvex<ConvexHullV> >(a: convexA, b: convexB, initialDir: aToB.p, initialLambda: zero, s: zeroV, r: dir, lambda&: toi, normal, closestA, inflation: inflation, initialOverlap: isMtd))
711 return false;
712
713 if(hasInitialOverlap(sweepHit, unitDir, toi, normal, closestA, convexPose, isMtd, impactPointOnTheOtherShape: false))
714 return true;
715
716 sweepHit.flags |= PxHitFlag::ePOSITION;
717 const Vec3V worldPointA = convexTransf.transform(input: closestA);
718 const Vec3V destNormal = V3Neg(f: V3Normalize(a: convexTransf.rotate(input: normal)));
719 const FloatV length = FMul(a: dist, b: toi);
720 V3StoreU(a: destNormal, f&: sweepHit.normal);
721 V3StoreU(a: worldPointA, f&: sweepHit.position);
722 FStore(a: length, f: &sweepHit.distance);
723
724 return computeFaceIndex(sweepHit, hitFlags, convexGeom: otherConvexGeom, hullData: otherHullData, pose, unitDir);
725}
726

source code of qtquick3dphysics/src/3rdparty/PhysX/source/geomutils/src/GuSweepSharedTests.cpp