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/PxSphereGeometry.h"
31#include "geometry/PxBoxGeometry.h"
32#include "geometry/PxCapsuleGeometry.h"
33#include "geometry/PxPlaneGeometry.h"
34#include "geometry/PxConvexMeshGeometry.h"
35
36#include "PsIntrinsics.h"
37#include "PsAllocator.h"
38#include "PsUtilities.h"
39#include "PsVecMath.h"
40
41#include "GuOverlapTests.h"
42#include "GuHeightFieldUtil.h"
43#include "GuIntersectionBoxBox.h"
44#include "GuIntersectionTriangleBox.h"
45#include "GuDistancePointSegment.h"
46#include "GuDistanceSegmentBox.h"
47#include "GuDistanceSegmentSegment.h"
48#include "GuDistanceSegmentSegmentSIMD.h"
49#include "GuCapsule.h"
50#include "GuEdgeCache.h"
51#include "GuBoxConversion.h"
52#include "GuInternal.h"
53#include "GuConvexUtilsInternal.h"
54#include "GuVecTriangle.h"
55#include "GuVecSphere.h"
56#include "GuVecCapsule.h"
57#include "GuVecConvexHull.h"
58#include "GuConvexMesh.h"
59
60using namespace physx;
61using namespace Cm;
62using namespace Gu;
63using namespace Ps::aos;
64
65static bool intersectHeightFieldSphere(const HeightFieldUtil& hfUtil, const Sphere& sphereInHfShape)
66{
67 const HeightField& hf = hfUtil.getHeightField();
68
69 // sample the sphere center in the heightfield to find out
70 // if we have penetration with more than the sphere radius
71 if(hfUtil.isShapePointOnHeightField(x: sphereInHfShape.center.x, z: sphereInHfShape.center.z))
72 {
73 // The sphere origin projects within the bounds of the heightfield in the X-Z plane
74 PxReal sampleHeight = hfUtil.getHeightAtShapePoint(x: sphereInHfShape.center.x, z: sphereInHfShape.center.z);
75 PxReal deltaHeight = sphereInHfShape.center.y - sampleHeight;
76 if(hf.isDeltaHeightInsideExtent(dy: deltaHeight))
77 {
78 // The sphere origin is 'below' the heightfield surface
79 PxU32 faceIndex = hfUtil.getFaceIndexAtShapePoint(x: sphereInHfShape.center.x, z: sphereInHfShape.center.z);
80 if(faceIndex != 0xffffffff)
81 {
82 return true;
83 }
84 return false;
85 }
86 }
87
88 const PxReal radiusSquared = sphereInHfShape.radius * sphereInHfShape.radius;
89
90 const PxVec3 sphereInHF = hfUtil.shape2hfp(v: sphereInHfShape.center);
91
92 const PxReal radiusOverRowScale = sphereInHfShape.radius * PxAbs(a: hfUtil.getOneOverRowScale());
93 const PxReal radiusOverColumnScale = sphereInHfShape.radius * PxAbs(a: hfUtil.getOneOverColumnScale());
94
95 const PxU32 minRow = hf.getMinRow(x: sphereInHF.x - radiusOverRowScale);
96 const PxU32 maxRow = hf.getMaxRow(x: sphereInHF.x + radiusOverRowScale);
97 const PxU32 minColumn = hf.getMinColumn(z: sphereInHF.z - radiusOverColumnScale);
98 const PxU32 maxColumn = hf.getMaxColumn(z: sphereInHF.z + radiusOverColumnScale);
99
100 for(PxU32 r = minRow; r < maxRow; r++)
101 {
102 for(PxU32 c = minColumn; c < maxColumn; c++)
103 {
104
105 // x--x--x
106 // | x |
107 // x x x
108 // | x |
109 // x--x--x
110 PxVec3 pcp[11];
111 PxU32 npcp = 0;
112 npcp = hfUtil.findClosestPointsOnCell(row: r, column: c, point: sphereInHfShape.center, closestPoints: pcp, NULL, testFaces: true, testEdges: true, skipEdgesIfFaceHits: true);
113
114 for(PxU32 pi = 0; pi < npcp; pi++)
115 {
116 PxVec3 d = sphereInHfShape.center - pcp[pi];
117
118 PxReal ll = d.magnitudeSquared();
119
120 if(ll > radiusSquared)
121 // Too far
122 continue;
123
124 return true;
125 }
126 }
127 }
128 return false;
129}
130
131static bool intersectHeightFieldCapsule(const HeightFieldUtil& hfUtil, const PxCapsuleGeometry& capsuleGeom, const PxTransform& capsulePose)
132{
133 const HeightField& hf = hfUtil.getHeightField();
134
135 PxVec3 verticesInHfShape[2];
136 PxVec3 capsuleOrigin, capsuleExtent;
137 {
138 const PxVec3 capsuleHalfHeightVector = getCapsuleHalfHeightVector(transform: capsulePose, capsuleGeom);
139
140 capsuleOrigin = capsulePose.p + capsuleHalfHeightVector;
141 capsuleExtent = -capsuleHalfHeightVector*2.0f;
142
143 verticesInHfShape[0] = capsuleOrigin;
144 verticesInHfShape[1] = capsulePose.p - capsuleHalfHeightVector;
145 }
146
147 const PxReal radius = capsuleGeom.radius;
148 const PxReal radiusOverRowScale = radius * PxAbs(a: hfUtil.getOneOverRowScale());
149 const PxReal radiusOverColumnScale = radius * PxAbs(a: hfUtil.getOneOverColumnScale());
150
151 PxU32 absMinRow = 0xffffffff;
152 PxU32 absMaxRow = 0;
153 PxU32 absMinColumn = 0xffffffff;
154 PxU32 absMaxColumn = 0;
155
156 PxReal radiusSquared = radius * radius;
157
158 // test both of capsule's corner vertices+radius for HF overlap
159 for(PxU32 i = 0; i<2; i++)
160 {
161 const PxVec3& sphereInHfShape = verticesInHfShape[i];
162
163 // we have to do this first to update the absMin / absMax correctly even if
164 // we decide to continue from inside the deep penetration code.
165
166 const PxVec3 sphereInHF = hfUtil.shape2hfp(v: sphereInHfShape);
167
168 const PxU32 minRow = hf.getMinRow(x: sphereInHF.x - radiusOverRowScale);
169 const PxU32 maxRow = hf.getMaxRow(x: sphereInHF.x + radiusOverRowScale);
170 const PxU32 minColumn = hf.getMinColumn(z: sphereInHF.z - radiusOverColumnScale);
171 const PxU32 maxColumn = hf.getMaxColumn(z: sphereInHF.z + radiusOverColumnScale);
172
173 if(minRow < absMinRow) absMinRow = minRow;
174 if(minColumn < absMinColumn) absMinColumn = minColumn;
175 if(maxRow > absMaxRow) absMaxRow = maxRow;
176 if(maxColumn > absMaxColumn) absMaxColumn = maxColumn;
177
178 if(hfUtil.isShapePointOnHeightField(x: sphereInHfShape.x, z: sphereInHfShape.z))
179 {
180 // The sphere origin projects within the bounds of the heightfield in the X-Z plane
181 const PxReal sampleHeight = hfUtil.getHeightAtShapePoint(x: sphereInHfShape.x, z: sphereInHfShape.z);
182 const PxReal deltaHeight = sphereInHfShape.y - sampleHeight;
183 if(hf.isDeltaHeightInsideExtent(dy: deltaHeight))
184 {
185 // The sphere origin is 'below' the heightfield surface
186 const PxU32 faceIndex = hfUtil.getFaceIndexAtShapePoint(x: sphereInHfShape.x, z: sphereInHfShape.z);
187 if(faceIndex != 0xffffffff)
188 {
189 return true;
190 }
191 continue;
192 }
193 }
194
195 for(PxU32 r = minRow; r < maxRow; r++)
196 {
197 for(PxU32 c = minColumn; c < maxColumn; c++)
198 {
199
200 // x--x--x
201 // | x |
202 // x x x
203 // | x |
204 // x--x--x
205 PxVec3 pcp[11];
206 PxU32 npcp = 0;
207 npcp = hfUtil.findClosestPointsOnCell(row: r, column: c, point: sphereInHfShape, closestPoints: pcp, NULL, testFaces: true, testEdges: true, skipEdgesIfFaceHits: true);
208
209 for(PxU32 pi = 0; pi < npcp; pi++)
210 {
211 const PxVec3 d = sphereInHfShape - pcp[pi];
212
213 if(hf.isDeltaHeightOppositeExtent(dy: d.y))
214 {
215 // We are 'above' the heightfield
216
217 const PxReal ll = d.magnitudeSquared();
218 if(ll > radiusSquared)
219 // Too far above
220 continue;
221
222 return true;
223 }
224 }
225 }
226 }
227 }
228
229 const Vec3V p1 = V3LoadU(f: capsuleOrigin);
230 const Vec3V d1 = V3LoadU(f: capsuleExtent);
231
232 // now test capsule's inflated segment for overlap with HF edges
233 PxU32 row, column;
234 for(row = absMinRow; row <= absMaxRow; row++)
235 {
236 for(column = absMinColumn; column <= absMaxColumn; column++)
237 {
238 const PxU32 vertexIndex = row * hf.getNbColumnsFast() + column;
239 const PxU32 firstEdge = 3 * vertexIndex;
240 // omg I am sorry about this code but I can't find a simpler way:
241 // last column will only test edge 2
242 // last row will only test edge 0
243 // and most importantly last row and column will not go inside the for
244 const PxU32 minEi = (column == absMaxColumn) ? 2u : 0;
245 const PxU32 maxEi = (row == absMaxRow ) ? 1u : 3u;
246 for(PxU32 ei = minEi; ei < maxEi; ei++)
247 {
248 const PxU32 edgeIndex = firstEdge + ei;
249
250 const PxU32 cell = vertexIndex;
251 PX_ASSERT(cell == edgeIndex / 3);
252 const PxU32 row_ = row;
253 PX_ASSERT(row_ == cell / hf.getNbColumnsFast());
254 const PxU32 column_ = column;
255 PX_ASSERT(column_ == cell % hf.getNbColumnsFast());
256
257 const PxU32 faceIndex = hfUtil.getEdgeFaceIndex(edgeIndex, cell, row: row_, column: column_);
258 if(faceIndex != 0xffffffff)
259 {
260 PxVec3 origin;
261 PxVec3 direction;
262 hfUtil.getEdge(edgeIndex, cell, row: row_, column: column_, origin, extent&: direction);
263
264 const Vec3V p2 = V3LoadU(f: origin);
265 const Vec3V d2 = V3LoadU(f: direction);
266 FloatV s, t;
267 const FloatV llV = Gu::distanceSegmentSegmentSquared(p1, d1, p2, d2, param0&: s, param1&: t);
268
269 PxReal ll;
270 FStore(a: llV, f: &ll);
271
272 if(ll < radiusSquared)
273 return true;
274 }
275 }
276 }
277 }
278 return false;
279}
280
281namespace physx
282{
283namespace Gu
284{
285 const PxReal signs[24] =
286 {
287 -1,-1,-1,
288 -1,-1, 1,
289 -1, 1,-1,
290 -1, 1, 1,
291 1,-1,-1,
292 1,-1, 1,
293 1, 1,-1,
294 1, 1, 1,
295 };
296
297 const char edges[24] =
298 {
299 0,1,
300 1,3,
301 3,2,
302 2,0,
303 4,5,
304 5,7,
305 7,6,
306 6,4,
307 0,4,
308 1,5,
309 2,6,
310 3,7,
311 };
312
313 struct TriggerTraceSegmentCallback
314 {
315 bool intersection;
316
317 PX_INLINE TriggerTraceSegmentCallback() : intersection(false)
318 {
319 }
320
321 PX_INLINE bool underFaceHit(
322 const HeightFieldUtil&, const PxVec3&,
323 const PxVec3&, PxF32, PxF32, PxF32, PxU32)
324 {
325 return true;
326 }
327
328 PX_INLINE bool faceHit(const HeightFieldUtil&, const PxVec3&, PxU32, PxReal, PxReal)
329 {
330 intersection = true;
331 return false;
332 }
333 bool onEvent(PxU32 , PxU32* )
334 {
335 return true;
336 }
337 };
338
339
340 class OverlapHeightfieldTraceSegmentHelper
341 {
342 PX_NOCOPY(OverlapHeightfieldTraceSegmentHelper)
343 public:
344 OverlapHeightfieldTraceSegmentHelper(const HeightFieldTraceUtil& hfUtil) : mHfUtil(hfUtil)
345 {
346 mHfUtil.computeLocalBounds(bounds&: mLocalBounds);
347 }
348
349 PX_INLINE void traceSegment(const PxVec3& aP0, const PxVec3& aP1, TriggerTraceSegmentCallback* aCallback) const
350 {
351 mHfUtil.traceSegment<TriggerTraceSegmentCallback, false, false>(aP0, rayDir: aP1 - aP0, rayLength: 1.0f, aCallback, hfLocalBounds: mLocalBounds, backfaceCull: false, NULL);
352 }
353
354 private:
355 const HeightFieldTraceUtil& mHfUtil;
356 PxBounds3 mLocalBounds;
357 };
358
359} // namespace
360}
361
362static bool intersectHeightFieldBox(const HeightFieldTraceUtil& hfUtil, const Box& boxInHfShape)
363{
364 const HeightField& hf = hfUtil.getHeightField();
365
366 // Get box vertices
367 PxVec3 boxVertices[8];
368 for(PxU32 i=0; i<8; i++)
369 boxVertices[i] = PxVec3(boxInHfShape.extents.x*signs[3*i], boxInHfShape.extents.y*signs[3*i+1], boxInHfShape.extents.z*signs[3*i+2]);
370
371 // Transform box vertices to HeightFieldShape space
372 PxVec3 boxVerticesInHfShape[8];
373 for(PxU32 i=0; i<8; i++)
374 boxVerticesInHfShape[i] = boxInHfShape.transform(src: boxVertices[i]);
375
376 // Test box vertices.
377 {
378 for(PxU32 i=0; i<8; i++)
379 {
380 const PxVec3& boxVertexInHfShape = boxVerticesInHfShape[i];
381 if(hfUtil.isShapePointOnHeightField(x: boxVertexInHfShape.x, z: boxVertexInHfShape.z))
382 {
383 const PxReal y = hfUtil.getHeightAtShapePoint(x: boxVertexInHfShape.x, z: boxVertexInHfShape.z);
384 const PxReal dy = boxVertexInHfShape.y - y;
385 if(hf.isDeltaHeightInsideExtent(dy))
386 {
387 PxU32 faceIndex = hfUtil.getFaceIndexAtShapePoint(x: boxVertexInHfShape.x, z: boxVertexInHfShape.z);
388 if(faceIndex != 0xffffffff)
389 {
390 return true;
391 }
392 }
393 }
394 }
395 }
396
397 // Test box edges.
398 {
399 OverlapHeightfieldTraceSegmentHelper traceSegmentHelper(hfUtil);
400
401 for(PxU32 i=0; i<12; i++)
402 {
403 const PxVec3 v0 = boxVerticesInHfShape[PxU8(edges[2*i])];
404 const PxVec3 v1 = boxVerticesInHfShape[PxU8(edges[2*i+1])];
405 TriggerTraceSegmentCallback cb;
406 traceSegmentHelper.traceSegment(aP0: v0, aP1: v1, aCallback: &cb);
407 if(cb.intersection)
408 return true;
409 }
410 }
411
412 // Test HeightField vertices.
413 {
414 PsTransformV _hfShape2BoxShape;
415 const PxQuat bq(boxInHfShape.rot);
416 const QuatV q1 = QuatVLoadU(v: &bq.x);
417 const Vec3V p1 = V3LoadU(i: &boxInHfShape.center.x);
418 const PsTransformV _boxPose(p1, q1);
419 _hfShape2BoxShape = _boxPose.getInverse();
420
421 PxReal minx(PX_MAX_REAL);
422 PxReal minz(PX_MAX_REAL);
423 PxReal maxx(-PX_MAX_REAL);
424 PxReal maxz(-PX_MAX_REAL);
425
426 for(PxU32 i=0; i<8; i++)
427 {
428 const PxVec3& boxVertexInHfShape = boxVerticesInHfShape[i];
429
430/* if(boxVertexInHfShape.x < minx) minx = boxVertexInHfShape.x;
431 if(boxVertexInHfShape.z < minz) minz = boxVertexInHfShape.z;
432 if(boxVertexInHfShape.x > maxx) maxx = boxVertexInHfShape.x;
433 if(boxVertexInHfShape.z > maxz) maxz = boxVertexInHfShape.z;*/
434 minx = physx::intrinsics::selectMin(a: boxVertexInHfShape.x, b: minx);
435 minz = physx::intrinsics::selectMin(a: boxVertexInHfShape.z, b: minz);
436 maxx = physx::intrinsics::selectMax(a: boxVertexInHfShape.x, b: maxx);
437 maxz = physx::intrinsics::selectMax(a: boxVertexInHfShape.z, b: maxz);
438 }
439
440 const PxReal oneOverRowScale = hfUtil.getOneOverRowScale();
441 const PxReal oneOverColumnScale = hfUtil.getOneOverColumnScale();
442 const PxU32 minRow = hf.getMinRow(x: minx * oneOverRowScale);
443 const PxU32 maxRow = hf.getMaxRow(x: maxx * oneOverRowScale);
444 const PxU32 minColumn = hf.getMinColumn(z: minz * oneOverColumnScale);
445 const PxU32 maxColumn = hf.getMaxColumn(z: maxz * oneOverColumnScale);
446
447 const Vec4V extentV = V4LoadXYZW(x: boxInHfShape.extents.x, y: boxInHfShape.extents.y, z: boxInHfShape.extents.z, PX_MAX_REAL);
448 const PxHeightFieldGeometry& geom = hfUtil.getHeightFieldGeometry();
449
450 for(PxU32 row = minRow; row <= maxRow; row++)
451 {
452 for(PxU32 column = minColumn; column <= maxColumn; column++)
453 {
454 PxU32 vertexIndex = row * hf.getNbColumnsFast() + column;
455 if(hfUtil.isQueryVertex(vertexIndex, row, column))
456 {
457 // check if hf vertex is inside the box
458 const Vec4V hfVertex = V4LoadXYZW(x: geom.rowScale * row, y: geom.heightScale * hf.getHeight(vertexIndex), z: geom.columnScale * column, w: 0.0f);
459 const Vec4V hfVertexInBoxShape = Vec4V_From_Vec3V(f: _hfShape2BoxShape.transform(input: Vec3V_From_Vec4V(v: hfVertex)));
460 const Vec4V hfVertexInBoxShapeAbs = V4Abs(a: hfVertexInBoxShape);
461
462 if(V4AllGrtr(a: extentV, b: hfVertexInBoxShapeAbs))
463 {
464 return true;
465 }
466 }
467 }
468 }
469 }
470 return false;
471}
472
473static Matrix34 multiplyInverseRTLeft(const Matrix34& left, const Matrix34& right)
474{
475// t = left.M % (right.t - left.t);
476 PxVec3 t = left.rotateTranspose(other: right.p - left.p);
477
478// M.setMultiplyTransposeLeft(left.M, right.M);
479 const PxMat33& left33 = left.m;
480 const PxMat33& right33 = right.m;
481 PxMat33 multiplyTransposeLeft33 = (left33.getTranspose()) * right33;
482
483 return Matrix34(multiplyTransposeLeft33, t);
484}
485
486static bool intersectHeightFieldConvex(
487 const HeightFieldTraceUtil& hfUtil, const PxTransform& _hfAbsPose, const ConvexMesh& convexMesh,
488 const PxTransform& _convexAbsPose, const PxMeshScale& convexMeshScaling)
489{
490 const Matrix34 hfAbsPose34(_hfAbsPose);
491 const Matrix34 convexAbsPose34(_convexAbsPose);
492 const Matrix34 vertexToShapeSkew34(convexMeshScaling.toMat33());
493 const Matrix34 temp34 = convexAbsPose34 * vertexToShapeSkew34;
494 const Matrix34 convexShape2HfShapeSkew34 = multiplyInverseRTLeft(left: hfAbsPose34, right: temp34);
495
496 const ConvexHullData* hull = &convexMesh.getHull();
497
498 // Allocate space for transformed vertices.
499 PxVec3* convexVerticesInHfShape = reinterpret_cast<PxVec3*>(PxAlloca(hull->mNbHullVertices*sizeof(PxVec3)));
500
501 // Transform vertices to height field shape
502 const PxVec3* hullVerts = hull->getHullVertices();
503 for(PxU32 i=0; i<hull->mNbHullVertices; i++)
504 convexVerticesInHfShape[i] = convexShape2HfShapeSkew34.transform(other: hullVerts[i]);
505
506 // Compute bounds of convex in hf space
507 PxBounds3 convexBoundsInHfShape;
508 computeBoundsAroundVertices(bounds&: convexBoundsInHfShape, nbVerts: hull->mNbHullVertices, verts: convexVerticesInHfShape);
509
510 // Compute the height field extreme over the bounds area.
511 const HeightField& hf = hfUtil.getHeightField();
512 PxReal hfExtreme = -PX_MAX_REAL;
513 const PxReal oneOverRowScale = hfUtil.getOneOverRowScale();
514 const PxReal oneOverColumnScale = hfUtil.getOneOverColumnScale();
515 const PxReal rowScale = (1.0f / hfUtil.getOneOverRowScale());
516 const PxReal columnScale = (1.0f / hfUtil.getOneOverColumnScale());
517 const PxReal heightScale = (1.0f / hfUtil.getOneOverHeightScale());
518
519 // negative scale support
520 PxU32 minRow;
521 PxU32 maxRow;
522 if(oneOverRowScale > 0.0f)
523 {
524 minRow = hf.getMinRow(x: convexBoundsInHfShape.minimum.x * oneOverRowScale);
525 maxRow = hf.getMaxRow(x: convexBoundsInHfShape.maximum.x * oneOverRowScale);
526 }
527 else
528 {
529 minRow = hf.getMinRow(x: convexBoundsInHfShape.maximum.x * oneOverRowScale);
530 maxRow = hf.getMaxRow(x: convexBoundsInHfShape.minimum.x * oneOverRowScale);
531 }
532
533 PxU32 minColumn;
534 PxU32 maxColumn;
535 if(oneOverColumnScale > 0.0f)
536 {
537 minColumn = hf.getMinColumn(z: convexBoundsInHfShape.minimum.z * oneOverColumnScale);
538 maxColumn = hf.getMaxColumn(z: convexBoundsInHfShape.maximum.z * oneOverColumnScale);
539 }
540 else
541 {
542 minColumn = hf.getMinColumn(z: convexBoundsInHfShape.maximum.z * oneOverColumnScale);
543 maxColumn = hf.getMaxColumn(z: convexBoundsInHfShape.minimum.z * oneOverColumnScale);
544 }
545
546 for(PxU32 row = minRow; row <= maxRow; row++)
547 {
548 for(PxU32 column = minColumn; column <= maxColumn; column++)
549 {
550 const PxReal h = hf.getHeight(vertexIndex: row * hf.getNbColumnsFast() + column);
551 hfExtreme = PxMax(a: hfExtreme, b: h);
552 }
553 }
554 hfExtreme *= heightScale;
555
556
557 // Return if convex is on the wrong side of the extreme.
558 if(convexBoundsInHfShape.minimum.y > hfExtreme)
559 return false;
560
561 // Test convex vertices
562 {
563 for(PxU32 i=0; i<hull->mNbHullVertices; i++)
564 {
565 const PxVec3& convexVertexInHfShape = convexVerticesInHfShape[i];
566 bool insideExtreme = convexVertexInHfShape.y < hfExtreme;
567 if(insideExtreme && hfUtil.isShapePointOnHeightField(x: convexVertexInHfShape.x, z: convexVertexInHfShape.z))
568 {
569 const PxReal y = hfUtil.getHeightAtShapePoint(x: convexVertexInHfShape.x, z: convexVertexInHfShape.z);
570 const PxReal dy = convexVertexInHfShape.y - y;
571 if(hf.isDeltaHeightInsideExtent(dy))
572 {
573 const PxU32 faceIndex = hfUtil.getFaceIndexAtShapePoint(x: convexVertexInHfShape.x, z: convexVertexInHfShape.z);
574 if(faceIndex != 0xffffffff)
575 return true;
576 }
577 }
578 }
579 }
580
581 // Test convex edges.
582 {
583 EdgeCache edgeCache;
584 PxU32 numPolygons = hull->mNbPolygons;
585 const HullPolygonData* polygons = hull->mPolygons;
586 const PxU8* const vertexData = hull->getVertexData8();
587
588 OverlapHeightfieldTraceSegmentHelper traceSegmentHelper(hfUtil);
589
590 while(numPolygons--)
591 {
592 const HullPolygonData& polygon = *polygons++;
593
594 const PxU8* verts = vertexData + polygon.mVRef8;
595
596 PxU32 numEdges = polygon.mNbVerts;
597
598 PxU32 a = numEdges - 1;
599 PxU32 b = 0;
600 while(numEdges--)
601 {
602 PxU8 vi0 = verts[a];
603 PxU8 vi1 = verts[b];
604
605 if(vi1 < vi0)
606 {
607 PxU8 tmp = vi0;
608 vi0 = vi1;
609 vi1 = tmp;
610 }
611
612 if(edgeCache.isInCache(vertex0: vi0, vertex1: vi1)) //avoid processing edges 2x if possible (this will typically have cache misses about 5% of the time leading to 5% redundant work.
613 continue;
614
615 const PxVec3& sv0 = convexVerticesInHfShape[vi0];
616 const PxVec3& sv1 = convexVerticesInHfShape[vi1];
617 a = b;
618 b++;
619
620
621 if((sv0.y > hfExtreme) && (sv1.y > hfExtreme))
622 continue;
623
624 const PxVec3 v0 = sv0;
625 const PxVec3 v1 = sv1;
626 TriggerTraceSegmentCallback cb;
627 traceSegmentHelper.traceSegment(aP0: v0, aP1: v1, aCallback: &cb);
628 if(cb.intersection)
629 return true;
630 }
631 }
632 }
633
634 // Test HeightField vertices
635 {
636 const Matrix34 tmp34 = multiplyInverseRTLeft(left: convexAbsPose34, right: hfAbsPose34);
637 const Matrix34 hfShape2ConvexShapeSkew34 = vertexToShapeSkew34 * tmp34;
638
639 for(PxU32 row = minRow; row <= maxRow; row++)
640 {
641 for(PxU32 column = minColumn; column <= maxColumn; column++)
642 {
643 const PxU32 hfVertexIndex = row * hf.getNbColumnsFast() + column;
644 if(hfUtil.isQueryVertex(vertexIndex: hfVertexIndex, row, column))
645 {
646 // Check if hf vertex is inside the convex
647 const PxVec3 hfVertex(rowScale * row, heightScale * hf.getHeight(vertexIndex: hfVertexIndex), columnScale * column);
648 const PxVec3 hfVertexInConvexShape = hfShape2ConvexShapeSkew34.transform(other: hfVertex);
649
650 bool inside = true;
651 for(PxU32 poly = 0; poly < hull->mNbPolygons; poly++)
652 {
653 PxReal d = hull->mPolygons[poly].mPlane.distance(p: hfVertexInConvexShape);
654 if(d >= 0)
655 {
656 inside = false;
657 break;
658 }
659 }
660 if(inside)
661 return true;
662 }
663 }
664 }
665 }
666 return false;
667}
668
669bool Gu::checkOverlapAABB_heightFieldGeom(const PxGeometry& geom, const PxTransform& pose, const PxBounds3& box)
670{
671 PX_ASSERT(geom.getType() == PxGeometryType::eHEIGHTFIELD);
672 const PxHeightFieldGeometry& hfGeom = static_cast<const PxHeightFieldGeometry&>(geom);
673
674 const Matrix34 invAbsPose(pose.getInverse());
675
676 const Box boxInHfShape(
677 invAbsPose.transform(other: box.getCenter()),
678 box.getExtents(),
679 invAbsPose.m);
680
681 HeightFieldTraceUtil hfUtil(hfGeom);
682 return intersectHeightFieldBox(hfUtil, boxInHfShape);
683}
684
685bool GeomOverlapCallback_SphereHeightfield(GU_OVERLAP_FUNC_PARAMS)
686{
687 PX_ASSERT(geom0.getType()==PxGeometryType::eSPHERE);
688 PX_ASSERT(geom1.getType()==PxGeometryType::eHEIGHTFIELD);
689 PX_UNUSED(cache);
690
691 const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom0);
692 const PxHeightFieldGeometry& hfGeom = static_cast<const PxHeightFieldGeometry&>(geom1);
693
694 const Sphere sphereInHf(pose1.transformInv(input: pose0.p), sphereGeom.radius);
695
696 HeightFieldUtil hfUtil(hfGeom);
697 return intersectHeightFieldSphere(hfUtil, sphereInHfShape: sphereInHf);
698}
699
700bool GeomOverlapCallback_CapsuleHeightfield(GU_OVERLAP_FUNC_PARAMS)
701{
702 PX_ASSERT(geom0.getType()==PxGeometryType::eCAPSULE);
703 PX_ASSERT(geom1.getType()==PxGeometryType::eHEIGHTFIELD);
704 PX_UNUSED(cache);
705
706 const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom0);
707 const PxHeightFieldGeometry& hfGeom = static_cast<const PxHeightFieldGeometry&>(geom1);
708
709 const PxTransform capsuleShapeToHfShape = pose1.transformInv(src: pose0);
710
711 const HeightFieldUtil hfUtil(hfGeom);
712 return intersectHeightFieldCapsule(hfUtil, capsuleGeom, capsulePose: capsuleShapeToHfShape);
713}
714
715bool GeomOverlapCallback_BoxHeightfield(GU_OVERLAP_FUNC_PARAMS)
716{
717 PX_ASSERT(geom0.getType()==PxGeometryType::eBOX);
718 PX_ASSERT(geom1.getType()==PxGeometryType::eHEIGHTFIELD);
719 PX_UNUSED(cache);
720
721 const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom0);
722 const PxHeightFieldGeometry& hfGeom = static_cast<const PxHeightFieldGeometry&>(geom1);
723
724 const PxTransform boxShape2HfShape = pose1.transformInv(src: pose0);
725
726 Box box;
727 buildFrom(dst&: box, center: boxShape2HfShape.p, extents: boxGeom.halfExtents, q: boxShape2HfShape.q);
728
729 HeightFieldTraceUtil hfUtil(hfGeom);
730 return intersectHeightFieldBox(hfUtil, boxInHfShape: box);
731}
732
733///////////////////////////////////////////////////////////////////////////////
734bool GeomOverlapCallback_ConvexHeightfield(GU_OVERLAP_FUNC_PARAMS)
735{
736 PX_ASSERT(geom0.getType()==PxGeometryType::eCONVEXMESH);
737 PX_ASSERT(geom1.getType()==PxGeometryType::eHEIGHTFIELD);
738 PX_UNUSED(cache);
739
740 const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom0);
741 const PxHeightFieldGeometry& hfGeom = static_cast<const PxHeightFieldGeometry&>(geom1);
742
743 ConvexMesh* cm = static_cast<ConvexMesh*>(convexGeom.convexMesh);
744
745 HeightFieldTraceUtil hfUtil(hfGeom);
746 return intersectHeightFieldConvex(hfUtil, hfAbsPose: pose1, convexMesh: *cm, convexAbsPose: pose0, convexMeshScaling: convexGeom.scale);
747}
748

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