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 "GuSweepTests.h"
31#include "GuHeightFieldUtil.h"
32#include "GuEntityReport.h"
33#include "GuVecCapsule.h"
34#include "GuSweepMTD.h"
35#include "GuSweepTriangleUtils.h"
36#include "GuVecBox.h"
37#include "CmScaling.h"
38#include "GuSweepCapsuleTriangle.h"
39#include "GuInternal.h"
40#include "GuGJKRaycast.h"
41
42using namespace physx;
43using namespace Gu;
44using namespace Cm;
45using namespace physx::shdfnd::aos;
46
47#include "GuSweepConvexTri.h"
48
49#define AbortTraversal false
50#define ContinueTraversal true
51
52///////////////////////////////////////////////////////////////////////////////
53
54class HeightFieldTraceSegmentSweepHelper
55{
56 PX_NOCOPY(HeightFieldTraceSegmentSweepHelper)
57public:
58 HeightFieldTraceSegmentSweepHelper(const HeightFieldTraceUtil& hfUtil, const PxVec3& aabbExtentHfLocalSpace)
59 : mHfUtil(hfUtil), mOverlapObjectExtent(aabbExtentHfLocalSpace)
60 {
61 mHfUtil.computeLocalBounds(bounds&: mLocalBounds);
62 // extend the bounds
63 mLocalBounds.minimum = mLocalBounds.minimum - aabbExtentHfLocalSpace;
64 mLocalBounds.maximum = mLocalBounds.maximum + aabbExtentHfLocalSpace;
65 }
66
67 template<class T>
68 PX_INLINE void traceSegment(const PxVec3& aP0, const PxVec3& rayDirNorm, const float rayLength, T* aCallback) const
69 {
70 mHfUtil.traceSegment<T, false, true>(aP0, rayDirNorm, rayLength, aCallback, mLocalBounds, false, &mOverlapObjectExtent);
71 }
72
73private:
74 const HeightFieldTraceUtil& mHfUtil;
75 const PxVec3& mOverlapObjectExtent;
76 PxBounds3 mLocalBounds;
77};
78
79///////////////////////////////////////////////////////////////////////////////
80
81class HeightFieldTraceSegmentReport : public EntityReport<PxU32>
82{
83 PX_NOCOPY(HeightFieldTraceSegmentReport)
84public:
85
86 HeightFieldTraceSegmentReport(const HeightFieldUtil& hfUtil, const PxHitFlags hitFlags) :
87 mHfUtil (hfUtil),
88 mHitFlags (hitFlags),
89 mStatus (false),
90 mInitialOverlap (false),
91 mIsDoubleSided ((hfUtil.getHeightFieldGeometry().heightFieldFlags & PxMeshGeometryFlag::eDOUBLE_SIDED) || (hitFlags & PxHitFlag::eMESH_BOTH_SIDES)),
92 mIsAnyHit (hitFlags & PxHitFlag::eMESH_ANY)
93 {
94 }
95
96 bool underFaceHit(const Gu::HeightFieldUtil&, const PxVec3&, const PxVec3&, PxF32, PxF32, PxF32, PxU32)
97 {
98 return true;
99 }
100
101 bool faceHit(const Gu::HeightFieldUtil&, const PxVec3&, PxU32, PxReal, PxReal)
102 {
103 return true;
104 }
105
106 protected:
107 const HeightFieldUtil& mHfUtil;
108 const PxHitFlags mHitFlags;
109 bool mStatus;
110 bool mInitialOverlap;
111 const bool mIsDoubleSided;
112 const bool mIsAnyHit;
113};
114
115///////////////////////////////////////////////////////////////////////////////
116
117class CapsuleTraceSegmentReport : public HeightFieldTraceSegmentReport
118{
119 PX_NOCOPY(CapsuleTraceSegmentReport)
120public:
121 CapsuleTraceSegmentReport( const HeightFieldUtil& hfUtil, const PxHitFlags hitFlags,
122 const Capsule& inflatedCapsule,
123 const PxVec3& unitDir, PxSweepHit& sweepHit, const PxTransform& pose, PxReal distance) :
124 HeightFieldTraceSegmentReport (hfUtil, hitFlags),
125 mInflatedCapsule (inflatedCapsule),
126 mUnitDir (unitDir),
127 mSweepHit (sweepHit),
128 mPose (pose),
129 mDistance (distance)
130 {
131 mSweepHit.faceIndex = 0xFFFFffff;
132 }
133
134 virtual PxAgain onEvent(PxU32 nb, PxU32* indices)
135 {
136 PX_ALIGN_PREFIX(16) PxU8 tribuf[HF_SWEEP_REPORT_BUFFER_SIZE*sizeof(PxTriangle)] PX_ALIGN_SUFFIX(16);
137 PxTriangle* tmpT = reinterpret_cast<PxTriangle*>(tribuf);
138 PX_ASSERT(nb <= HF_SWEEP_REPORT_BUFFER_SIZE);
139 for(PxU32 i=0; i<nb; i++)
140 {
141 const PxU32 triangleIndex = indices[i];
142 mHfUtil.getTriangle(mPose, worldTri&: tmpT[i], NULL, NULL, triangleIndex, worldSpaceTranslation: true);
143 }
144
145 PxSweepHit h; // PT: TODO: ctor!
146 // PT: this one is safe because cullbox is NULL (no need to allocate one more triangle)
147 // PT: TODO: is it ok to pass the initial distance here?
148 PxVec3 bestNormal;
149 const bool status = sweepCapsuleTriangles_Precise(nbTris: nb, triangles: tmpT, capsule: mInflatedCapsule, unitDir: mUnitDir, distance: mDistance, NULL, hit&: h, triNormalOut&: bestNormal, hitFlags: mHitFlags, isDoubleSided: mIsDoubleSided);
150 if(status && (h.distance <= mSweepHit.distance))
151 {
152 mSweepHit.faceIndex = indices[h.faceIndex];
153 mSweepHit.normal = h.normal;
154 mSweepHit.position = h.position;
155 mSweepHit.distance = h.distance;
156
157 mStatus = true;
158 if(h.distance == 0.0f)
159 {
160 mInitialOverlap = true;
161 return AbortTraversal;
162 }
163
164 if(mIsAnyHit)
165 return AbortTraversal;
166 }
167 return ContinueTraversal;
168 }
169
170 bool finalizeHit(PxSweepHit& sweepHit, const PxHeightFieldGeometry& hfGeom, const PxTransform& pose, const Capsule& lss, const Capsule& inflatedCapsule, const PxVec3& unitDir)
171 {
172 if(!mStatus)
173 return false;
174
175 if(mInitialOverlap)
176 {
177 // PT: TODO: consider using 'setInitialOverlapResults' here
178 sweepHit.flags = PxHitFlag::eNORMAL | PxHitFlag::eFACE_INDEX;
179
180 if(mHitFlags & PxHitFlag::eMTD)
181 {
182 const Vec3V p0 = V3LoadU(f: lss.p0);
183 const Vec3V p1 = V3LoadU(f: lss.p1);
184 const FloatV radius = FLoad(f: lss.radius);
185 CapsuleV capsuleV;
186 capsuleV.initialize(p0: p0, p1: p1, radius: radius);
187
188 //calculate MTD
189 const bool hasContacts = computeCapsule_HeightFieldMTD(heightFieldGeom: hfGeom, pose, capsuleV, inflatedRadius: inflatedCapsule.radius, isDoubleSided: mIsDoubleSided, flags: GuHfQueryFlags::eWORLD_SPACE, hit&: sweepHit);
190
191 //ML: the center of mass is below the surface, we won't have MTD contact generate
192 if(!hasContacts)
193 {
194 sweepHit.distance = 0.0f;
195 sweepHit.normal = -unitDir;
196 }
197 else
198 {
199 sweepHit.flags |= PxHitFlag::ePOSITION;
200 }
201 }
202 else
203 {
204 sweepHit.distance = 0.0f;
205 sweepHit.normal = -unitDir;
206 }
207 }
208 else
209 {
210 sweepHit.flags = PxHitFlag::eNORMAL| PxHitFlag::ePOSITION | PxHitFlag::eFACE_INDEX;
211 }
212 return true;
213 }
214
215 private:
216 const Capsule& mInflatedCapsule;
217 const PxVec3& mUnitDir;
218 PxSweepHit& mSweepHit;
219 const PxTransform& mPose;
220 const PxReal mDistance;
221};
222
223bool sweepCapsule_HeightFieldGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS)
224{
225 PX_UNUSED(capsuleGeom_);
226 PX_UNUSED(capsulePose_);
227
228 PX_ASSERT(geom.getType() == PxGeometryType::eHEIGHTFIELD);
229 const PxHeightFieldGeometry& hfGeom = static_cast<const PxHeightFieldGeometry&>(geom);
230
231 const Capsule inflatedCapsule(lss.p0, lss.p1, lss.radius + inflation);
232
233 // Compute swept box
234 Box capsuleBox;
235 computeBoxAroundCapsule(capsule: inflatedCapsule, box&: capsuleBox);
236
237 const PxVec3 capsuleAABBExtents = capsuleBox.computeAABBExtent();
238
239 const HeightFieldTraceUtil hfUtil(hfGeom);
240 CapsuleTraceSegmentReport myReport(hfUtil, hitFlags, inflatedCapsule, unitDir, sweepHit, pose, distance);
241
242 sweepHit.distance = PX_MAX_F32;
243
244 // need hf local space stuff
245 const PxTransform inversePose = pose.getInverse();
246 const PxVec3 centerLocalSpace = inversePose.transform(input: capsuleBox.center);
247 const PxVec3 sweepDirLocalSpace = inversePose.rotate(input: unitDir);
248 const PxVec3 capsuleAABBBExtentHfLocalSpace = PxBounds3::basisExtent(center: centerLocalSpace, basis: PxMat33Padded(inversePose.q), extent: capsuleAABBExtents).getExtents();
249
250 HeightFieldTraceSegmentSweepHelper traceSegmentHelper(hfUtil, capsuleAABBBExtentHfLocalSpace);
251 traceSegmentHelper.traceSegment<CapsuleTraceSegmentReport>(aP0: centerLocalSpace, rayDirNorm: sweepDirLocalSpace, rayLength: distance, aCallback: &myReport);
252
253 return myReport.finalizeHit(sweepHit, hfGeom, pose, lss, inflatedCapsule, unitDir);
254}
255
256///////////////////////////////////////////////////////////////////////////////
257
258class ConvexTraceSegmentReport : public HeightFieldTraceSegmentReport
259{
260 PX_NOCOPY(ConvexTraceSegmentReport)
261public:
262 ConvexTraceSegmentReport( const HeightFieldUtil& hfUtil, const ConvexHullData& hull, const PxMeshScale& convexScale,
263 const PxTransform& convexPose, const PxTransform& heightFieldPose,
264 const PxVec3& unitDir, PxReal distance, PxHitFlags hitFlags, PxReal inflation) :
265 HeightFieldTraceSegmentReport (hfUtil, hitFlags),
266 mUnitDir (unitDir),
267 mInflation (inflation)
268 {
269 using namespace Ps::aos;
270 mSweepHit.faceIndex = 0xFFFFffff;
271 mSweepHit.distance = distance;
272 const Vec3V worldDir = V3LoadU(f: unitDir);
273 const FloatV dist = FLoad(f: distance);
274 const QuatV q0 = QuatVLoadU(v: &heightFieldPose.q.x);
275 const Vec3V p0 = V3LoadU(i: &heightFieldPose.p.x);
276
277 const QuatV q1 = QuatVLoadU(v: &convexPose.q.x);
278 const Vec3V p1 = V3LoadU(i: &convexPose.p.x);
279
280 const PsTransformV meshTransf(p0, q0);
281 const PsTransformV convexTransf(p1, q1);
282
283 mMeshToConvex = convexTransf.transformInv(src: meshTransf);
284 mConvexPoseV = convexTransf;
285 mConvexSpaceDir = convexTransf.rotateInv(input: V3Neg(f: V3Scale(a: worldDir, b: dist)));
286 mDistance = dist;
287
288 const Vec3V vScale = V3LoadU_SafeReadW(f: convexScale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
289 const QuatV vQuat = QuatVLoadU(v: &convexScale.rotation.x);
290
291 mMeshSpaceUnitDir = heightFieldPose.rotateInv(input: unitDir);
292 mConvexHull.initialize(hullData: &hull, center: V3Zero(), scale: vScale, scaleRot: vQuat, idtScale: convexScale.isIdentity());
293 }
294
295 virtual PxAgain onEvent(PxU32 nbEntities, PxU32* entities)
296 {
297 const PxTransform idt(PxIdentity);
298 for(PxU32 i=0; i<nbEntities; i++)
299 {
300 PxTriangle tri;
301 mHfUtil.getTriangle(idt, worldTri&: tri, NULL, NULL, triangleIndex: entities[i], worldSpaceTranslation: false, worldSpaceRotation: false); // First parameter not needed if local space triangle is enough
302
303 // use mSweepHit.distance as max sweep distance so far, mSweepHit.distance will be clipped by this function
304 if(sweepConvexVsTriangle( v0: tri.verts[0], v1: tri.verts[1], v2: tri.verts[2], convexHull&: mConvexHull, meshToConvex: mMeshToConvex, convexTransfV: mConvexPoseV,
305 convexSpaceDir: mConvexSpaceDir, unitDir: mUnitDir, meshSpaceUnitDir: mMeshSpaceUnitDir, fullDistance: mDistance, shrunkDistance: mSweepHit.distance, hit&: mSweepHit, isDoubleSided: mIsDoubleSided,
306 inflation: mInflation, initialOverlap&: mInitialOverlap, faceIndex: entities[i]))
307 {
308 mStatus = true;
309 if(mIsAnyHit || mSweepHit.distance == 0.0f)
310 return AbortTraversal;
311 }
312 }
313 return ContinueTraversal;
314 }
315
316 bool finalizeHit(PxSweepHit& sweepHit,
317 const PxHeightFieldGeometry& hfGeom, const PxTransform& pose,
318 const PxConvexMeshGeometry& convexGeom, const PxTransform& convexPose,
319 const PxVec3& unitDir, PxReal inflation)
320 {
321 if(!mStatus)
322 return false;
323
324 if(mInitialOverlap)
325 {
326 if(mHitFlags & PxHitFlag::eMTD)
327 {
328 const bool hasContacts = computeConvex_HeightFieldMTD(heightFieldGeom: hfGeom, pose, convexGeom, convexTransform: convexPose, inflation, isDoubleSided: mIsDoubleSided, flags: GuHfQueryFlags::eWORLD_SPACE, hit&: sweepHit);
329
330 sweepHit.faceIndex = mSweepHit.faceIndex;
331 sweepHit.flags = PxHitFlag::eNORMAL | PxHitFlag::eFACE_INDEX;
332 if(!hasContacts)
333 {
334 sweepHit.distance = 0.0f;
335 sweepHit.normal = -unitDir;
336 }
337 else
338 {
339 sweepHit.flags |= PxHitFlag::ePOSITION;
340 }
341 }
342 else
343 {
344 setInitialOverlapResults(hit&: sweepHit, unitDir, faceIndex: mSweepHit.faceIndex); // hit index must be set to closest for IO
345 }
346 }
347 else
348 {
349 sweepHit = mSweepHit;
350 sweepHit.normal = -sweepHit.normal;
351 sweepHit.normal.normalize();
352 }
353 return true;
354 }
355
356 private:
357 PsMatTransformV mMeshToConvex;
358 PsTransformV mConvexPoseV;
359 ConvexHullV mConvexHull;
360 PxSweepHit mSweepHit;
361 Vec3V mConvexSpaceDir;
362 FloatV mDistance;
363 const PxVec3 mUnitDir;
364 PxVec3 mMeshSpaceUnitDir;
365 const PxReal mInflation;
366};
367
368bool sweepConvex_HeightFieldGeom(GU_CONVEX_SWEEP_FUNC_PARAMS)
369{
370 PX_ASSERT(geom.getType() == PxGeometryType::eHEIGHTFIELD);
371 const PxHeightFieldGeometry& hfGeom = static_cast<const PxHeightFieldGeometry&>(geom);
372
373 const Matrix34 convexTM(convexPose);
374 const Matrix34 meshTM(pose);
375
376 ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
377
378 const bool idtScaleConvex = convexGeom.scale.isIdentity();
379
380 FastVertex2ShapeScaling convexScaling;
381 if(!idtScaleConvex)
382 convexScaling.init(scale: convexGeom.scale);
383
384 PX_ASSERT(!convexMesh->getLocalBoundsFast().isEmpty());
385 const PxBounds3 hullAABBLocalSpace = convexMesh->getLocalBoundsFast().transformFast(matrix: convexScaling.getVertex2ShapeSkew());
386
387 const HeightFieldTraceUtil hfUtil(hfGeom);
388 ConvexTraceSegmentReport entityReport(
389 hfUtil, convexMesh->getHull(), convexGeom.scale, convexPose, pose, -unitDir, distance, hitFlags, inflation);
390
391 // need hf local space stuff
392 const PxBounds3 hullAABB = PxBounds3::transformFast(transform: convexPose, bounds: hullAABBLocalSpace);
393 const PxVec3 aabbExtents = hullAABB.getExtents() + PxVec3(inflation);
394 const PxTransform inversePose = pose.getInverse();
395 const PxVec3 centerLocalSpace = inversePose.transform(input: hullAABB.getCenter());
396 const PxVec3 sweepDirLocalSpace = inversePose.rotate(input: unitDir);
397 const PxVec3 convexAABBExtentHfLocalSpace = PxBounds3::basisExtent(center: centerLocalSpace, basis: PxMat33Padded(inversePose.q), extent: aabbExtents).getExtents();
398
399 HeightFieldTraceSegmentSweepHelper traceSegmentHelper(hfUtil, convexAABBExtentHfLocalSpace);
400 traceSegmentHelper.traceSegment<ConvexTraceSegmentReport>(aP0: centerLocalSpace, rayDirNorm: sweepDirLocalSpace, rayLength: distance, aCallback: &entityReport);
401
402 return entityReport.finalizeHit(sweepHit, hfGeom, pose, convexGeom, convexPose, unitDir, inflation);
403}
404
405///////////////////////////////////////////////////////////////////////////////
406
407#if PX_VC
408 #pragma warning(push)
409 #pragma warning( disable : 4324 ) // Padding was added at the end of a structure because of a __declspec(align) value.
410#endif
411
412class BoxTraceSegmentReport : public HeightFieldTraceSegmentReport
413{
414 PX_NOCOPY(BoxTraceSegmentReport)
415public:
416 BoxTraceSegmentReport( const HeightFieldUtil& hfUtil, const PxHitFlags hitFlags,
417 const PsTransformV& worldToBoxV, const PxTransform& pose, const BoxV& box, const PxVec3& localMotion,
418 PxSweepHit& sweepHit, PxReal inflation) :
419 HeightFieldTraceSegmentReport (hfUtil, hitFlags),
420 mWorldToBoxV (worldToBoxV),
421 mPose (pose),
422 mBox (box),
423 mLocalMotion (localMotion),
424 mSweepHit (sweepHit),
425 mInflation (inflation)
426 {
427 mMinToi = FMax();
428 mSweepHit.faceIndex = 0xFFFFffff;
429 }
430
431 virtual PxAgain onEvent(PxU32 nb, PxU32* indices)
432 {
433 const FloatV zero = FZero();
434 const Vec3V zeroV = V3Zero();
435 const Vec3V dir = V3LoadU(f: mLocalMotion);
436 //FloatV minToi = FMax();
437 FloatV toi;
438 Vec3V closestA, normal;//closestA and normal is in the local space of box
439
440 for(PxU32 i=0; i<nb; i++)
441 {
442 const PxU32 triangleIndex = indices[i];
443
444 PxTriangle currentTriangle; // in world space
445 mHfUtil.getTriangle(mPose, worldTri&: currentTriangle, NULL, NULL, triangleIndex, worldSpaceTranslation: true, worldSpaceRotation: true);
446
447 const Vec3V localV0 = V3LoadU(f: currentTriangle.verts[0]);
448 const Vec3V localV1 = V3LoadU(f: currentTriangle.verts[1]);
449 const Vec3V localV2 = V3LoadU(f: currentTriangle.verts[2]);
450
451 const Vec3V triV0 = mWorldToBoxV.transform(input: localV0);
452 const Vec3V triV1 = mWorldToBoxV.transform(input: localV1);
453 const Vec3V triV2 = mWorldToBoxV.transform(input: localV2);
454
455 if(!mIsDoubleSided)
456 {
457 const Vec3V triNormal = V3Cross(a: V3Sub(a: triV2, b: triV1),b: V3Sub(a: triV0, b: triV1));
458 if(FAllGrtrOrEq(a: V3Dot(a: triNormal, b: dir), b: zero))
459 continue;
460 }
461
462 const TriangleV triangle(triV0, triV1, triV2);
463
464 ////move triangle to box space
465 //const Vec3V localV0 = Vec3V_From_PxVec3(WorldToBox.transform(currentTriangle.verts[0]));
466 //const Vec3V localV1 = Vec3V_From_PxVec3(WorldToBox.transform(currentTriangle.verts[1]));
467 //const Vec3V localV2 = Vec3V_From_PxVec3(WorldToBox.transform(currentTriangle.verts[2]));
468
469 //TriangleV triangle(localV0, localV1, localV2);
470 const LocalConvex<TriangleV> convexA(triangle);
471 const LocalConvex<BoxV> convexB(mBox);
472 const Vec3V initialSearchDir = V3Sub(a: triangle.getCenter(), b: mBox.getCenter());
473
474 if(gjkRaycastPenetration< LocalConvex<TriangleV>, LocalConvex<BoxV> >(a: convexA, b: convexB, initialDir: initialSearchDir, initialLambda: zero, s: zeroV, r: dir, lambda&: toi, normal, closestA, inflation: mInflation, initialOverlap: false))
475 {
476 mStatus = true;
477 if(FAllGrtr(a: toi, b: zero))
478 {
479 if(FAllGrtr(a: mMinToi, b: toi))
480 {
481 mMinToi = toi;
482 FStore(a: toi, f: &mSweepHit.distance);
483 V3StoreU(a: normal, f&: mSweepHit.normal);
484 V3StoreU(a: closestA, f&: mSweepHit.position);
485 mSweepHit.faceIndex = triangleIndex;
486
487 if(mIsAnyHit)
488 return AbortTraversal;
489 }
490 }
491 else
492 {
493 mSweepHit.distance = 0.0f;
494 mSweepHit.faceIndex = triangleIndex;
495 mInitialOverlap = true;
496 return AbortTraversal;
497 }
498 }
499 }
500 return ContinueTraversal;
501 }
502
503 bool finalizeHit(PxSweepHit& sweepHit,
504 const PxHeightFieldGeometry& hfGeom, const PxTransform& pose,
505 const PxTransform& boxPose_, const Box& box,
506 const PxVec3& unitDir, PxReal distance, PxReal inflation)
507 {
508 if(!mStatus)
509 return false;
510
511 if(mInitialOverlap)
512 {
513 // PT: TODO: consider using 'setInitialOverlapResults' here
514
515 sweepHit.flags = PxHitFlag::eNORMAL | PxHitFlag::eFACE_INDEX;
516
517 if(mHitFlags & PxHitFlag::eMTD)
518 {
519 const bool hasContacts = computeBox_HeightFieldMTD(heightFieldGeom: hfGeom, pose, box, boxTransform: boxPose_, inflation, isDoubleSided: mIsDoubleSided, flags: GuHfQueryFlags::eWORLD_SPACE, hit&: sweepHit);
520
521 //ML: the center of mass is below the surface, we won't have MTD contact generate
522 if(!hasContacts)
523 {
524 sweepHit.distance = 0.0f;
525 sweepHit.normal = -unitDir;
526 }
527 else
528 {
529 sweepHit.flags |= PxHitFlag::ePOSITION;
530 }
531 }
532 else
533 {
534 sweepHit.distance = 0.0f;
535 sweepHit.normal = -unitDir;
536 }
537 }
538 else
539 {
540 PxVec3 n = sweepHit.normal.getNormalized();
541 if((n.dot(v: mLocalMotion))>0.0f)
542 n = -n;
543
544 sweepHit.distance *= distance; // stored as toi [0,1] during computation -> scale
545 sweepHit.normal = boxPose_.rotate(input: n);
546 sweepHit.position = boxPose_.transform(input: sweepHit.position);
547 sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL | PxHitFlag::eFACE_INDEX;
548 }
549 return true;
550 }
551
552 private:
553 const PsTransformV& mWorldToBoxV;
554 const PxTransform& mPose;
555 const BoxV& mBox;
556 FloatV mMinToi;
557 const PxVec3 mLocalMotion;
558 PxSweepHit& mSweepHit;
559 const PxReal mInflation;
560};
561
562#if PX_VC
563 #pragma warning(pop)
564#endif
565
566bool sweepBox_HeightFieldGeom(GU_BOX_SWEEP_FUNC_PARAMS)
567{
568 PX_ASSERT(geom.getType() == PxGeometryType::eHEIGHTFIELD);
569 PX_UNUSED(boxGeom_);
570 PX_UNUSED(hitFlags);
571
572 const PxHeightFieldGeometry& hfGeom = static_cast<const PxHeightFieldGeometry&>(geom);
573
574 const PxVec3 boxAABBExtent = box.computeAABBExtent() + PxVec3(inflation);
575
576 // Move to AABB space
577 PX_ALIGN_PREFIX(16) PxTransform WorldToBox PX_ALIGN_SUFFIX(16);
578 WorldToBox = boxPose_.getInverse();
579
580 const QuatV q1 = QuatVLoadA(v: &WorldToBox.q.x);
581 const Vec3V p1 = V3LoadA(f: &WorldToBox.p.x);
582 const PsTransformV WorldToBoxV(p1, q1);
583
584 const PxVec3 motion = unitDir * distance;
585 const PxVec3 localMotion = WorldToBox.rotate(input: motion);
586
587 const BoxV boxV(V3Zero(), V3LoadU(f: box.extents));
588
589 sweepHit.distance = PX_MAX_F32;
590
591 const HeightFieldTraceUtil hfUtil(hfGeom);
592 BoxTraceSegmentReport myReport(hfUtil, hitFlags, WorldToBoxV, pose, boxV, localMotion, sweepHit, inflation);
593
594 // need hf local space stuff
595 const PxTransform inversePose = pose.getInverse();
596 const PxVec3 centerLocalSpace = inversePose.transform(input: box.center);
597 const PxVec3 sweepDirLocalSpace = inversePose.rotate(input: unitDir);
598 const PxVec3 boxAABBExtentInHfLocalSpace = PxBounds3::basisExtent(center: centerLocalSpace, basis: PxMat33Padded(inversePose.q), extent: boxAABBExtent).getExtents();
599
600 HeightFieldTraceSegmentSweepHelper traceSegmentHelper(hfUtil, boxAABBExtentInHfLocalSpace);
601 traceSegmentHelper.traceSegment<BoxTraceSegmentReport>(aP0: centerLocalSpace, rayDirNorm: sweepDirLocalSpace, rayLength: distance, aCallback: &myReport);
602
603 return myReport.finalizeHit(sweepHit, hfGeom, pose, boxPose_, box, unitDir, distance, inflation);
604}
605
606///////////////////////////////////////////////////////////////////////////////
607

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