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 "geomutils/GuContactBuffer.h"
31
32#include "GuVecCapsule.h"
33#include "GuGeometryUnion.h"
34#include "GuContactMethodImpl.h"
35#include "GuDistanceSegmentSegmentSIMD.h"
36
37using namespace physx;
38using namespace Gu;
39using namespace Ps;
40using namespace aos;
41
42static Vec4V pcmDistancePointSegmentTValue22( const Vec3VArg a0, const Vec3VArg b0,
43 const Vec3VArg a1, const Vec3VArg b1,
44 const Vec3VArg p0, const Vec3VArg p1,
45 const Vec3VArg p2, const Vec3VArg p3)
46{
47 const Vec4V zero = V4Zero();
48 const Vec3V ap00 = V3Sub(a: p0, b: a0);
49 const Vec3V ap10 = V3Sub(a: p1, b: a0);
50 const Vec3V ap01 = V3Sub(a: p2, b: a1);
51 const Vec3V ap11 = V3Sub(a: p3, b: a1);
52
53 const Vec3V ab0 = V3Sub(a: b0, b: a0);
54 const Vec3V ab1 = V3Sub(a: b1, b: a1);
55
56/* const FloatV nom00 = V3Dot(ap00, ab0);
57 const FloatV nom10 = V3Dot(ap10, ab0);
58 const FloatV nom01 = V3Dot(ap01, ab1);
59 const FloatV nom11 = V3Dot(ap11, ab1);*/
60
61 const Vec4V combinedDot = V3Dot4(a0: ap00, b0: ab0, a1: ap10, b1: ab0, a2: ap01, b2: ab1, a3: ap11, b3: ab1);
62 const FloatV nom00 = V4GetX(f: combinedDot);
63 const FloatV nom10 = V4GetY(f: combinedDot);
64 const FloatV nom01 = V4GetZ(f: combinedDot);
65 const FloatV nom11 = V4GetW(f: combinedDot);
66
67 const FloatV denom0 = V3Dot(a: ab0, b: ab0);
68 const FloatV denom1 = V3Dot(a: ab1, b: ab1);
69
70 const Vec4V nom = V4Merge(x: nom00, y: nom10, z: nom01, w: nom11);
71 const Vec4V denom = V4Merge(x: denom0, y: denom0, z: denom1, w: denom1);
72
73 const Vec4V tValue = V4Div(a: nom, b: denom);
74 return V4Sel(c: V4IsEq(a: denom, b: zero), a: zero, b: tValue);
75}
76
77namespace physx
78{
79namespace Gu
80{
81 static void storeContact(const Vec3VArg contact, const Vec3VArg normal, const FloatVArg separation, Gu::ContactBuffer& buffer)
82 {
83 Gu::ContactPoint& point = buffer.contacts[buffer.count++];
84
85 const Vec4V normalSep = Ps::aos::V4SetW(v: Vec4V_From_Vec3V(f: normal), f: separation);
86
87 V4StoreA(a: normalSep, f: &point.normal.x);
88 V3StoreA(a: contact, f&: point.point);
89 point.internalFaceIndex1 = PXC_CONTACT_NO_FACE_INDEX;
90 }
91
92bool pcmContactCapsuleCapsule(GU_CONTACT_METHOD_ARGS)
93{
94 PX_UNUSED(renderOutput);
95 PX_UNUSED(cache);
96
97 // Get actual shape data
98 const PxCapsuleGeometry& shapeCapsule0 = shape0.get<const PxCapsuleGeometry>();
99 const PxCapsuleGeometry& shapeCapsule1 = shape1.get<const PxCapsuleGeometry>();
100
101 PX_ASSERT(transform1.q.isSane());
102 PX_ASSERT(transform0.q.isSane());
103
104 const Vec3V _p0 = V3LoadA(f: &transform0.p.x);
105 const QuatV q0 = QuatVLoadA(v: &transform0.q.x);
106
107 const Vec3V _p1 = V3LoadA(f: &transform1.p.x);
108 const QuatV q1 = QuatVLoadA(v: &transform1.q.x);
109
110 /*PsTransformV transf0(p0, q0);
111 PsTransformV transf1(p1, q1);*/
112
113 const FloatV r0 = FLoad(f: shapeCapsule0.radius);
114 const FloatV halfHeight0 = FLoad(f: shapeCapsule0.halfHeight);
115
116 const FloatV r1 = FLoad(f: shapeCapsule1.radius);
117 const FloatV halfHeight1 = FLoad(f: shapeCapsule1.halfHeight);
118
119 const FloatV cDist = FLoad(f: params.mContactDistance);
120
121 const Vec3V positionOffset = V3Scale(a: V3Add(a: _p0, b: _p1), b: FHalf());
122 const Vec3V p0 = V3Sub(a: _p0, b: positionOffset);
123 const Vec3V p1 = V3Sub(a: _p1, b: positionOffset);
124
125 const FloatV zero = FZero();
126 //const FloatV one = FOne();
127 const Vec3V zeroV = V3Zero();
128
129 /*const Vec3V positionOffset = V3Scale(V3Add(transf0.p, transf1.p), FloatV_From_F32(0.5f));
130 transf0.p = V3Sub(transf0.p, positionOffset);
131 transf1.p = V3Sub(transf1.p, positionOffset);*/
132
133 const Vec3V basisVector0 = QuatGetBasisVector0(q: q0);
134 const Vec3V tmp0 = V3Scale(a: basisVector0, b: halfHeight0);
135 const Vec3V s0 = V3Add(a: p0, b: tmp0);
136 const Vec3V e0 = V3Sub(a: p0, b: tmp0);
137 const Vec3V d0 = V3Sub(a: e0, b: s0);
138
139 const Vec3V basisVector1 = QuatGetBasisVector0(q: q1);
140 const Vec3V tmp1 = V3Scale(a: basisVector1, b: halfHeight1);
141 const Vec3V s1 = V3Add(a: p1, b: tmp1);
142 const Vec3V e1 = V3Sub(a: p1, b: tmp1);
143
144 const Vec3V d1 = V3Sub(a: e1, b: s1);
145
146 const FloatV sumRadius = FAdd(a: r0, b: r1);
147 const FloatV inflatedSum = FAdd(a: sumRadius, b: cDist);
148 const FloatV inflatedSumSquared = FMul(a: inflatedSum, b: inflatedSum);
149 const FloatV a = V3Dot(a: d0, b: d0);//squared length of segment1
150 const FloatV e = V3Dot(a: d1, b: d1);//squared length of segment2
151 const FloatV eps = FLoad(f: 1e-6f);//FEps();
152
153 FloatV t0, t1;
154 const FloatV sqDist0 = distanceSegmentSegmentSquared(p1: s0, d1: d0, p2: s1, d2: d1, param0&: t0, param1&: t1);
155
156 if(FAllGrtrOrEq(a: inflatedSumSquared, b: sqDist0))
157 {
158 const Vec4V zeroV4 = V4Zero();
159 const Vec4V oneV4 = V4One();
160 //check to see whether these two capsule are paralle
161 const FloatV parallelTolerance = FLoad(f: 0.9998f);
162
163 const BoolV con0 = FIsGrtr(a: eps, b: a);
164 const BoolV con1 = FIsGrtr(a: eps, b: e);
165 const Vec3V dir0 = V3Sel(c: con0, a: zeroV, b: V3ScaleInv(a: d0, b: FSqrt(a)));
166 const Vec3V dir1 = V3Sel(c: con1, a: zeroV, b: V3ScaleInv(a: d1, b: FSqrt(a: e)));
167
168 const FloatV cos = FAbs(a: V3Dot(a: dir0, b: dir1));
169 if(FAllGrtr(a: cos, b: parallelTolerance))//paralle
170 {
171 //project s, e into s1e1
172 const Vec4V t= pcmDistancePointSegmentTValue22(a0: s0, b0: e0, a1: s1, b1: e1,
173 p0: s1, p1: e1, p2: s0, p3: e0);
174
175 const BoolV con = BAnd(a: V4IsGrtrOrEq(a: t, b: zeroV4), b: V4IsGrtrOrEq(a: oneV4, b: t));
176 const BoolV con00 = BGetX(f: con);
177 const BoolV con01 = BGetY(f: con);
178 const BoolV con10 = BGetZ(f: con);
179 const BoolV con11 = BGetW(f: con);
180
181 /* PX_ALIGN(16, PxU32 conditions[4]);
182 F32Array_Aligned_From_Vec4V(con, (PxF32*)conditions);*/
183
184 PxU32 numContact=0;
185
186 if(BAllEqTTTT(a: con00))
187 {
188 const Vec3V projS1 = V3ScaleAdd(a: d0, b: V4GetX(f: t), c: s0);
189 const Vec3V v = V3Sub(a: projS1, b: s1);
190 const FloatV sqDist = V3Dot(a: v, b: v);
191 const BoolV bCon = BAnd(a: FIsGrtr(a: sqDist, b: eps), b: FIsGrtr(a: inflatedSumSquared, b: sqDist));
192
193 if(BAllEqTTTT(a: bCon))
194 {
195 const FloatV dist = FSqrt(a: sqDist);
196 const FloatV pen = FSub(a: dist, b: sumRadius);
197 const Vec3V normal = V3ScaleInv(a: v, b: dist);
198 PX_ASSERT(isFiniteVec3V(normal));
199 const Vec3V _p = V3NegScaleSub(a: normal, b: r0, c: projS1);
200 const Vec3V p = V3Add(a: _p, b: positionOffset);
201
202 storeContact(contact: p, normal, separation: pen, buffer&: contactBuffer);
203 numContact++;
204 }
205 }
206 if(BAllEqTTTT(a: con01))
207 {
208 const Vec3V projE1 = V3ScaleAdd(a: d0, b: V4GetY(f: t), c: s0);
209 const Vec3V v = V3Sub(a: projE1, b: e1);
210 const FloatV sqDist = V3Dot(a: v, b: v);
211 const BoolV bCon = BAnd(a: FIsGrtr(a: sqDist, b: eps), b: FIsGrtr(a: inflatedSumSquared, b: sqDist));
212
213 if(BAllEqTTTT(a: bCon))
214 {
215 const FloatV dist = FSqrt(a: sqDist);
216 const FloatV pen = FSub(a: dist, b: sumRadius);
217 const Vec3V normal = V3ScaleInv(a: v, b: dist);
218 PX_ASSERT(isFiniteVec3V(normal));
219 const Vec3V _p = V3NegScaleSub(a: normal, b: r0, c: projE1);
220 const Vec3V p = V3Add(a: _p, b: positionOffset);
221 storeContact(contact: p, normal, separation: pen, buffer&: contactBuffer);
222 numContact++;
223 }
224 }
225
226 if(BAllEqTTTT(a: con10))
227 {
228 const Vec3V projS0 = V3ScaleAdd(a: d1, b: V4GetZ(f: t), c: s1);
229 const Vec3V v = V3Sub(a: s0, b: projS0);
230 const FloatV sqDist = V3Dot(a: v, b: v);
231 const BoolV bCon = BAnd(a: FIsGrtr(a: sqDist, b: eps), b: FIsGrtr(a: inflatedSumSquared, b: sqDist));
232
233 if(BAllEqTTTT(a: bCon))
234 {
235 const FloatV dist = FSqrt(a: sqDist);
236 const FloatV pen = FSub(a: dist, b: sumRadius);
237 const Vec3V normal = V3ScaleInv(a: v, b: dist);
238 PX_ASSERT(isFiniteVec3V(normal));
239 const Vec3V _p = V3NegScaleSub(a: normal, b: r0, c: s0);
240 const Vec3V p = V3Add(a: _p, b: positionOffset);
241 //const Vec3V p = V3ScaleAdd(normal, r0, s0);
242 storeContact(contact: p, normal, separation: pen, buffer&: contactBuffer);
243 numContact++;
244 }
245 }
246
247 if(BAllEqTTTT(a: con11))
248 {
249 const Vec3V projE0 = V3ScaleAdd(a: d1, b: V4GetW(f: t), c: s1);
250 const Vec3V v = V3Sub(a: e0, b: projE0);
251 const FloatV sqDist = V3Dot(a: v, b: v);
252 const BoolV bCon = BAnd(a: FIsGrtr(a: sqDist, b: eps), b: FIsGrtr(a: inflatedSumSquared, b: sqDist));
253
254 if(BAllEqTTTT(a: bCon))
255 {
256 const FloatV dist = FSqrt(a: sqDist);
257 const FloatV pen = FSub(a: dist, b: sumRadius);
258 const Vec3V normal = V3ScaleInv(a: v, b: dist);
259 PX_ASSERT(isFiniteVec3V(normal));
260 const Vec3V _p = V3NegScaleSub(a: normal, b: r0, c: e0);
261 const Vec3V p = V3Add(a: _p, b: positionOffset);
262 //const Vec3V p = V3ScaleAdd(normal, r0, e0);
263 storeContact(contact: p, normal, separation: pen, buffer&: contactBuffer);
264 numContact++;
265 }
266 }
267
268 if(numContact)
269 return true;
270 }
271
272 const Vec3V closestA = V3ScaleAdd(a: d0, b: t0, c: s0);
273 const Vec3V closestB = V3ScaleAdd(a: d1, b: t1, c: s1);
274
275 const BoolV con = FIsGrtr(a: eps, b: sqDist0);
276 //const Vec3V normal = V3Sel(FIsEq(dist, zero), V3Sel(FIsGrtr(a, eps), V3Normalise(d0), V3Scale(V3Sub(closestA, closestB), FRecip(dist)));
277 const Vec3V _normal = V3Sel(c: con, a: V3Sel(c: FIsGrtr(a, b: eps), a: d0, b: V3UnitX()), b: V3Sub(a: closestA, b: closestB));
278 const Vec3V normal = V3Normalize(a: _normal);
279 PX_ASSERT(isFiniteVec3V(normal));
280 const Vec3V _point = V3NegScaleSub(a: normal, b: r0, c: closestA);
281 const Vec3V p = V3Add(a: _point, b: positionOffset);
282 const FloatV dist = FSel(c: con, a: zero, b: FSqrt(a: sqDist0));
283 const FloatV pen = FSub(a: dist, b: sumRadius);
284 //PX_ASSERT(FAllGrtrOrEq(zero, pen));
285 storeContact(contact: p, normal, separation: pen, buffer&: contactBuffer);
286 return true;
287 }
288 return false;
289}
290}//Gu
291}//physx
292

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