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_GJKRAYCAST_H
31#define GU_GJKRAYCAST_H
32
33#include "GuGJKType.h"
34#include "GuGJKSimplex.h"
35#include "GuConvexSupportTable.h"
36#include "GuGJKPenetration.h"
37#include "GuEPA.h"
38
39
40namespace physx
41{
42
43
44namespace Gu
45{
46
47 /*
48 ConvexA is in the local space of ConvexB
49 lambda : the time of impact(TOI)
50 initialLambda : the start time of impact value (disable)
51 s : the sweep ray origin
52 r : the normalized sweep ray direction scaled by the sweep distance. r should be in ConvexB's space
53 normal : the contact normal
54 inflation : the amount by which we inflate the swept shape. If the inflated shapes aren't initially-touching,
55 the TOI will return the time at which both shapes are at a distance equal to inflation separated. If inflation is 0
56 the TOI will return the time at which both shapes are touching.
57
58 */
59 template<class ConvexA, class ConvexB>
60 bool gjkRaycast(const ConvexA& a, const ConvexB& b, const Ps::aos::Vec3VArg initialDir, const Ps::aos::FloatVArg initialLambda, const Ps::aos::Vec3VArg s, const Ps::aos::Vec3VArg r, Ps::aos::FloatV& lambda, Ps::aos::Vec3V& normal, Ps::aos::Vec3V& closestA, const PxReal _inflation)
61 {
62 PX_UNUSED(initialLambda);
63
64 using namespace Ps::aos;
65
66 const FloatV inflation = FLoad(f: _inflation);
67 const Vec3V zeroV = V3Zero();
68 const FloatV zero = FZero();
69 const FloatV one = FOne();
70 const BoolV bTrue = BTTTT();
71
72 const FloatV maxDist = FLoad(PX_MAX_REAL);
73
74 FloatV _lambda = zero;//initialLambda;
75 Vec3V x = V3ScaleAdd(a: r, b: _lambda, c: s);
76 PxU32 size=1;
77
78 //const Vec3V dir = V3Sub(a.getCenter(), b.getCenter());
79 const Vec3V _initialSearchDir = V3Sel(c: FIsGrtr(a: V3Dot(a: initialDir, b: initialDir), b: FEps()), a: initialDir, b: V3UnitX());
80 const Vec3V initialSearchDir = V3Normalize(a: _initialSearchDir);
81
82 const Vec3V initialSupportA(a.ConvexA::support(V3Neg(f: initialSearchDir)));
83 const Vec3V initialSupportB( b.ConvexB::support(initialSearchDir));
84
85 Vec3V Q[4] = {V3Sub(a: initialSupportA, b: initialSupportB), zeroV, zeroV, zeroV}; //simplex set
86 Vec3V A[4] = {initialSupportA, zeroV, zeroV, zeroV}; //ConvexHull a simplex set
87 Vec3V B[4] = {initialSupportB, zeroV, zeroV, zeroV}; //ConvexHull b simplex set
88
89
90 Vec3V v = V3Neg(f: Q[0]);
91 Vec3V supportA = initialSupportA;
92 Vec3V supportB = initialSupportB;
93 Vec3V support = Q[0];
94
95 const FloatV minMargin = FMin(a.ConvexA::getSweepMargin(), b.ConvexB::getSweepMargin());
96 const FloatV eps1 = FMul(a: minMargin, b: FLoad(f: 0.1f));
97 const FloatV inflationPlusEps(FAdd(a: eps1, b: inflation));
98 const FloatV eps2 = FMul(a: eps1, b: eps1);
99
100 const FloatV inflation2 = FMul(a: inflationPlusEps, b: inflationPlusEps);
101
102 Vec3V clos(Q[0]);
103 Vec3V preClos = clos;
104 //Vec3V closA(initialSupportA);
105 FloatV sDist = V3Dot(a: v, b: v);
106 FloatV minDist = sDist;
107 //Vec3V closAA = initialSupportA;
108 //Vec3V closBB = initialSupportB;
109
110 BoolV bNotTerminated = FIsGrtr(a: sDist, b: eps2);
111 BoolV bNotDegenerated = bTrue;
112
113 Vec3V nor = v;
114
115 while(BAllEqTTTT(a: bNotTerminated))
116 {
117
118 minDist = sDist;
119 preClos = clos;
120
121 const Vec3V vNorm = V3Normalize(a: v);
122 const Vec3V nvNorm = V3Neg(f: vNorm);
123
124 supportA=a.ConvexA::support(vNorm);
125 supportB=V3Add(x, b.ConvexB::support(nvNorm));
126
127 //calculate the support point
128 support = V3Sub(a: supportA, b: supportB);
129 const Vec3V w = V3Neg(f: support);
130 const FloatV vw = FSub(a: V3Dot(a: vNorm, b: w), b: inflationPlusEps);
131 if(FAllGrtr(a: vw, b: zero))
132 {
133 const FloatV vr = V3Dot(a: vNorm, b: r);
134 if(FAllGrtrOrEq(a: vr, b: zero))
135 {
136 return false;
137 }
138 else
139 {
140 const FloatV _oldLambda = _lambda;
141 _lambda = FSub(a: _lambda, b: FDiv(a: vw, b: vr));
142 if(FAllGrtr(a: _lambda, b: _oldLambda))
143 {
144 if(FAllGrtr(a: _lambda, b: one))
145 {
146 return false;
147 }
148 const Vec3V bPreCenter = x;
149 x = V3ScaleAdd(a: r, b: _lambda, c: s);
150
151 const Vec3V offSet =V3Sub(a: x, b: bPreCenter);
152 const Vec3V b0 = V3Add(a: B[0], b: offSet);
153 const Vec3V b1 = V3Add(a: B[1], b: offSet);
154 const Vec3V b2 = V3Add(a: B[2], b: offSet);
155
156 B[0] = b0;
157 B[1] = b1;
158 B[2] = b2;
159
160 Q[0]=V3Sub(a: A[0], b: b0);
161 Q[1]=V3Sub(a: A[1], b: b1);
162 Q[2]=V3Sub(a: A[2], b: b2);
163
164 supportB = V3Add(x, b.ConvexB::support(nvNorm));
165 support = V3Sub(a: supportA, b: supportB);
166 minDist = maxDist;
167 nor = v;
168 //size=0;
169 }
170 }
171 }
172
173 PX_ASSERT(size < 4);
174 A[size]=supportA;
175 B[size]=supportB;
176 Q[size++]=support;
177
178 //calculate the closest point between two convex hull
179 clos = GJKCPairDoSimplex(Q, A, B, support, size);
180 v = V3Neg(f: clos);
181 sDist = V3Dot(a: clos, b: clos);
182
183 bNotDegenerated = FIsGrtr(a: minDist, b: sDist);
184 bNotTerminated = BAnd(a: FIsGrtr(a: sDist, b: inflation2), b: bNotDegenerated);
185 }
186
187
188 const BoolV aQuadratic = a.isMarginEqRadius();
189 //ML:if the Minkowski sum of two objects are too close to the original(eps2 > sDist), we can't take v because we will lose lots of precision. Therefore, we will take
190 //previous configuration's normal which should give us a reasonable approximation. This effectively means that, when we do a sweep with inflation, we always keep v because
191 //the shapes converge separated. If we do a sweep without inflation, we will usually use the previous configuration's normal.
192 nor = V3Sel(c: BAnd(a: FIsGrtr(a: sDist, b: eps2), b: bNotDegenerated), a: v, b: nor);
193 nor = V3Neg(f: V3NormalizeSafe(a: nor, unsafeReturnValue: V3Zero()));
194 normal = nor;
195 lambda = _lambda;
196
197 const Vec3V closestP = V3Sel(c: bNotDegenerated, a: clos, b: preClos);
198 Vec3V closA = zeroV, closB = zeroV;
199 getClosestPoint(Q, A, B, closest: closestP, closestA&: closA, closestB&: closB, size);
200 closestA = V3Sel(aQuadratic, V3NegScaleSub(nor, a.getMargin(), closA), closA);
201
202 return true;
203 }
204
205
206
207 /*
208 ConvexA is in the local space of ConvexB
209 lambda : the time of impact(TOI)
210 initialLambda : the start time of impact value (disable)
211 s : the sweep ray origin in ConvexB's space
212 r : the normalized sweep ray direction scaled by the sweep distance. r should be in ConvexB's space
213 normal : the contact normal in ConvexB's space
214 closestA : the tounching contact in ConvexB's space
215 inflation : the amount by which we inflate the swept shape. If the inflated shapes aren't initially-touching,
216 the TOI will return the time at which both shapes are at a distance equal to inflation separated. If inflation is 0
217 the TOI will return the time at which both shapes are touching.
218
219 */
220 template<class ConvexA, class ConvexB>
221 bool gjkRaycastPenetration(const ConvexA& a, const ConvexB& b, const Ps::aos::Vec3VArg initialDir, const Ps::aos::FloatVArg initialLambda, const Ps::aos::Vec3VArg s, const Ps::aos::Vec3VArg r, Ps::aos::FloatV& lambda,
222 Ps::aos::Vec3V& normal, Ps::aos::Vec3V& closestA, const PxReal _inflation, const bool initialOverlap)
223 {
224 using namespace Ps::aos;
225 Vec3V closA;
226 Vec3V norm;
227 FloatV _lambda;
228 if(gjkRaycast(a, b, initialDir, initialLambda, s, r, _lambda, norm, closA, _inflation))
229 {
230 const FloatV zero = FZero();
231 lambda = _lambda;
232 if(FAllEq(a: _lambda, b: zero) && initialOverlap)
233 {
234
235 //time of impact is zero, the sweep shape is intesect, use epa to get the normal and contact point
236 const FloatV contactDist = getSweepContactEps(a.getMargin(), b.getMargin());
237
238 FloatV sDist(zero);
239 PxU8 aIndices[4];
240 PxU8 bIndices[4];
241 PxU8 size=0;
242 GjkOutput output;
243
244 //PX_COMPILE_TIME_ASSERT(typename Shrink<ConvexB>::Type != Gu::BoxV);
245
246 typename ConvexA::ConvexGeomType convexA = a.getGjkConvex();
247 typename ConvexB::ConvexGeomType convexB = b.getGjkConvex();
248
249 GjkStatus status = gjkPenetration<typename ConvexA::ConvexGeomType, typename ConvexB::ConvexGeomType>(convexA, convexB,
250 initialDir, contactDist, false, aIndices, bIndices, size, output);
251 //norm = V3Neg(norm);
252 if(status == GJK_CONTACT)
253 {
254 closA = output.closestA;
255 sDist = output.penDep;
256 norm = output.normal;
257 }
258 else if(status == EPA_CONTACT)
259 {
260 status = epaPenetration(a, b, aIndices, bIndices, size, false, FLoad(f: 1.f), output);
261
262 if (status == EPA_CONTACT || status == EPA_DEGENERATE)
263 {
264 closA = output.closestA;
265 sDist = output.penDep;
266 norm = output.normal;
267 }
268 else
269 {
270 //ML: if EPA fail, we will use the ray direction as the normal and set pentration to be zero
271 closA = V3Zero();
272 sDist = zero;
273 norm = V3Normalize(a: V3Neg(f: r));
274 }
275 }
276 else
277 {
278 //ML:: this will be gjk degenerate case. However, we can still
279 //reply on the previous iteration's closest feature information
280 closA = output.closestA;
281 sDist = output.penDep;
282 norm = output.normal;
283 }
284 lambda = FMin(a: zero, b: sDist);
285 }
286 closestA = closA;
287 normal = norm;
288 return true;
289 }
290 return false;
291 }
292}
293}
294
295#endif
296

source code of qtquick3dphysics/src/3rdparty/PhysX/source/geomutils/src/gjk/GuGJKRaycast.h