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
31#include "foundation/PxPreprocessor.h"
32#include "PsVecMath.h"
33#include "DyArticulationContactPrep.h"
34#include "DySolverConstraintDesc.h"
35#include "DySolverConstraint1D.h"
36#include "DySolverContact.h"
37#include "DySolverContactPF.h"
38#include "DyArticulationHelper.h"
39#include "PxcNpWorkUnit.h"
40#include "PxsMaterialManager.h"
41#include "PxsMaterialCombiner.h"
42#include "DyCorrelationBuffer.h"
43#include "DySolverConstraintExtShared.h"
44
45using namespace physx;
46using namespace Gu;
47
48// constraint-gen only, since these use getVelocityFast methods
49// which aren't valid during the solver phase
50
51namespace physx
52{
53
54namespace Dy
55{
56
57
58bool setupFinalizeExtSolverContactsCoulomb(
59 const ContactBuffer& buffer,
60 const CorrelationBuffer& c,
61 const PxTransform& bodyFrame0,
62 const PxTransform& bodyFrame1,
63 PxU8* workspace,
64 PxReal invDt,
65 PxReal bounceThresholdF32,
66 const SolverExtBody& b0,
67 const SolverExtBody& b1,
68 PxU32 frictionCountPerPoint,
69 PxReal invMassScale0, PxReal invInertiaScale0,
70 PxReal invMassScale1, PxReal invInertiaScale1,
71 PxReal restDist,
72 PxReal ccdMaxDistance,
73 Cm::SpatialVectorF* Z)
74{
75 // NOTE II: the friction patches are sparse (some of them have no contact patches, and
76 // therefore did not get written back to the cache) but the patch addresses are dense,
77 // corresponding to valid patches
78
79 const FloatV ccdMaxSeparation = FLoad(f: ccdMaxDistance);
80
81 PxU8* PX_RESTRICT ptr = workspace;
82
83 const PxF32 maxPenBias0 = b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b0.mBodyData->penBiasClamp : b0.mArticulation->getLinkMaxPenBias(linkID: b0.mLinkIndex);
84 const PxF32 maxPenBias1 = b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b1.mBodyData->penBiasClamp : b1.mArticulation->getLinkMaxPenBias(linkID: b1.mLinkIndex);
85
86 const FloatV maxPenBias = FLoad(f: PxMax(a: maxPenBias0, b: maxPenBias1)/invDt);
87
88 const FloatV restDistance = FLoad(f: restDist);
89 const FloatV bounceThreshold = FLoad(f: bounceThresholdF32);
90
91 const Cm::SpatialVectorV vel0 = b0.getVelocity();
92 const Cm::SpatialVectorV vel1 = b1.getVelocity();
93
94 const FloatV invDtV = FLoad(f: invDt);
95 const FloatV pt8 = FLoad(f: 0.8f);
96
97 const FloatV invDtp8 = FMul(a: invDtV, b: pt8);
98
99 const Vec3V bodyFrame0p = V3LoadU(f: bodyFrame0.p);
100 const Vec3V bodyFrame1p = V3LoadU(f: bodyFrame1.p);
101
102 Ps::prefetchLine(ptr: c.contactID);
103 Ps::prefetchLine(ptr: c.contactID, offset: 128);
104
105 const PxU32 frictionPatchCount = c.frictionPatchCount;
106
107 const PxU32 pointStride = sizeof(SolverContactPointExt);
108 const PxU32 frictionStride = sizeof(SolverContactFrictionExt);
109 const PxU8 pointHeaderType = DY_SC_TYPE_EXT_CONTACT;
110 const PxU8 frictionHeaderType = DY_SC_TYPE_EXT_FRICTION;
111
112 const FloatV d0 = FLoad(f: invMassScale0);
113 const FloatV d1 = FLoad(f: invMassScale1);
114 const FloatV angD0 = FLoad(f: invInertiaScale0);
115 const FloatV angD1 = FLoad(f: invInertiaScale1);
116
117 PxU8 flags = 0;
118
119 for(PxU32 i=0;i< frictionPatchCount;i++)
120 {
121 const PxU32 contactCount = c.frictionPatchContactCounts[i];
122 if(contactCount == 0)
123 continue;
124
125 const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;
126
127 const Vec3V normalV = Ps::aos::V3LoadA(f: contactBase0->normal);
128 const Vec3V normal = V3LoadA(f: contactBase0->normal);
129
130 const PxReal combinedRestitution = contactBase0->restitution;
131
132
133 SolverContactCoulombHeader* PX_RESTRICT header = reinterpret_cast<SolverContactCoulombHeader*>(ptr);
134 ptr += sizeof(SolverContactCoulombHeader);
135
136 Ps::prefetchLine(ptr, offset: 128);
137 Ps::prefetchLine(ptr, offset: 256);
138 Ps::prefetchLine(ptr, offset: 384);
139
140 const FloatV restitution = FLoad(f: combinedRestitution);
141
142
143 header->numNormalConstr = PxU8(contactCount);
144 header->type = pointHeaderType;
145 //header->setRestitution(combinedRestitution);
146
147 header->setDominance0(d0);
148 header->setDominance1(d1);
149 header->angDom0 = invInertiaScale0;
150 header->angDom1 = invInertiaScale1;
151 header->flags = flags;
152
153 header->setNormal(normalV);
154
155 for(PxU32 patch=c.correlationListHeads[i];
156 patch!=CorrelationBuffer::LIST_END;
157 patch = c.contactPatches[patch].next)
158 {
159 const PxU32 count = c.contactPatches[patch].count;
160 const Gu::ContactPoint* contactBase = buffer.contacts + c.contactPatches[patch].start;
161
162 PxU8* p = ptr;
163 for(PxU32 j=0;j<count;j++)
164 {
165 const Gu::ContactPoint& contact = contactBase[j];
166
167 SolverContactPointExt* PX_RESTRICT solverContact = reinterpret_cast<SolverContactPointExt*>(p);
168 p += pointStride;
169
170 setupExtSolverContact(b0, b1, d0, d1, angD0, angD1, bodyFrame0p, bodyFrame1p, normal, invDt: invDtV, invDtp8, restDistance, maxPenBias, restitution,
171 bounceThreshold, contact, solverContact&: *solverContact, ccdMaxSeparation, Z, v0: vel0, v1: vel1);
172 }
173 ptr = p;
174 }
175 }
176
177 //construct all the frictions
178
179 PxU8* PX_RESTRICT ptr2 = workspace;
180
181 const PxF32 orthoThreshold = 0.70710678f;
182 const PxF32 eps = 0.00001f;
183 bool hasFriction = false;
184
185 for(PxU32 i=0;i< frictionPatchCount;i++)
186 {
187 const PxU32 contactCount = c.frictionPatchContactCounts[i];
188 if(contactCount == 0)
189 continue;
190
191 SolverContactCoulombHeader* header = reinterpret_cast<SolverContactCoulombHeader*>(ptr2);
192 header->frictionOffset = PxU16(ptr - ptr2);
193 ptr2 += sizeof(SolverContactCoulombHeader) + header->numNormalConstr * pointStride;
194
195 const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;
196
197 PxVec3 normal = contactBase0->normal;
198
199 const PxReal staticFriction = contactBase0->staticFriction;
200 const bool disableStrongFriction = !!(contactBase0->materialFlags & PxMaterialFlag::eDISABLE_FRICTION);
201 const bool haveFriction = (disableStrongFriction == 0);
202
203 SolverFrictionHeader* frictionHeader = reinterpret_cast<SolverFrictionHeader*>(ptr);
204 frictionHeader->numNormalConstr = Ps::to8(value: c.frictionPatchContactCounts[i]);
205 frictionHeader->numFrictionConstr = Ps::to8(value: haveFriction ? c.frictionPatchContactCounts[i] * frictionCountPerPoint : 0);
206 frictionHeader->flags = flags;
207 ptr += sizeof(SolverFrictionHeader);
208 PxF32* forceBuffer = reinterpret_cast<PxF32*>(ptr);
209 ptr += frictionHeader->getAppliedForcePaddingSize(numConstr: c.frictionPatchContactCounts[i]);
210 PxMemZero(dest: forceBuffer, count: sizeof(PxF32) * c.frictionPatchContactCounts[i]);
211 Ps::prefetchLine(ptr, offset: 128);
212 Ps::prefetchLine(ptr, offset: 256);
213 Ps::prefetchLine(ptr, offset: 384);
214
215
216 const PxVec3 t0Fallback1(0.f, -normal.z, normal.y);
217 const PxVec3 t0Fallback2(-normal.y, normal.x, 0.f) ;
218 const PxVec3 tFallback1 = orthoThreshold > PxAbs(a: normal.x) ? t0Fallback1 : t0Fallback2;
219 const PxVec3 vrel = b0.getLinVel() - b1.getLinVel();
220 const PxVec3 t0_ = vrel - normal * (normal.dot(v: vrel));
221 const PxReal sqDist = t0_.dot(v: t0_);
222 const PxVec3 tDir0 = (sqDist > eps ? t0_: tFallback1).getNormalized();
223 const PxVec3 tDir1 = tDir0.cross(v: normal);
224 PxVec3 tFallback[2] = {tDir0, tDir1};
225
226 PxU32 ind = 0;
227
228 if(haveFriction)
229 {
230 hasFriction = true;
231 frictionHeader->setStaticFriction(staticFriction);
232 frictionHeader->invMass0D0 = invMassScale0;
233 frictionHeader->invMass1D1 = invMassScale1;
234 frictionHeader->angDom0 = invInertiaScale0;
235 frictionHeader->angDom1 = invInertiaScale1;
236 frictionHeader->type = frictionHeaderType;
237
238 PxU32 totalPatchContactCount = 0;
239
240 for(PxU32 patch=c.correlationListHeads[i];
241 patch!=CorrelationBuffer::LIST_END;
242 patch = c.contactPatches[patch].next)
243 {
244 const PxU32 count = c.contactPatches[patch].count;
245 const PxU32 start = c.contactPatches[patch].start;
246 const Gu::ContactPoint* contactBase = buffer.contacts + start;
247
248 PxU8* p = ptr;
249
250 for(PxU32 j =0; j < count; j++)
251 {
252 const Gu::ContactPoint& contact = contactBase[j];
253 const Vec3V ra = V3Sub(a: V3LoadA(f: contact.point), b: bodyFrame0p);
254 const Vec3V rb = V3Sub(a: V3LoadA(f: contact.point), b: bodyFrame1p);
255
256 const Vec3V targetVel = V3LoadA(f: contact.targetVel);
257 const Vec3V pVRa = V3Add(a: vel0.linear, b: V3Cross(a: vel0.angular, b: ra));
258 const Vec3V pVRb = V3Add(a: vel1.linear, b: V3Cross(a: vel1.angular, b: rb));
259 //const PxVec3 vrel = pVRa - pVRb;
260
261 for(PxU32 k = 0; k < frictionCountPerPoint; ++k)
262 {
263 SolverContactFrictionExt* PX_RESTRICT f0 = reinterpret_cast<SolverContactFrictionExt*>(p);
264 p += frictionStride;
265
266 Vec3V t0 = V3LoadU(f: tFallback[ind]);
267 ind = 1 - ind;
268 const Vec3V raXn = V3Cross(a: ra, b: t0);
269 const Vec3V rbXn = V3Cross(a: rb, b: t0);
270 Cm::SpatialVectorV deltaV0, deltaV1;
271
272 const Cm::SpatialVectorV resp0 = createImpulseResponseVector(linear: t0, angular: raXn, body: b0);
273 const Cm::SpatialVectorV resp1 = createImpulseResponseVector(linear: V3Neg(f: t0), angular: V3Neg(f: rbXn), body: b1);
274
275 FloatV unitResponse = getImpulseResponse(b0, impulse0: resp0, deltaV0, dom0: d0, angDom0: angD0,
276 b1, impulse1: resp1, deltaV1, dom1: d1, angDom1: angD1, Z: reinterpret_cast<Cm::SpatialVectorV*>(Z));
277
278 FloatV tv = V3Dot(a: targetVel, b: t0);
279 if(b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
280 tv = FAdd(a: tv, b: V3Dot(a: pVRa, b: t0));
281 else if(b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
282 tv = FSub(a: tv, b: V3Dot(a: pVRb, b: t0));
283
284 const FloatV recipResponse = FSel(c: FIsGrtr(a: unitResponse, b: FZero()), a: FRecip(a: unitResponse), b: FZero());
285
286 f0->raXnXYZ_velMultiplierW = V4SetW(v: Vec4V_From_Vec3V(f: resp0.angular), f: recipResponse);
287 f0->rbXnXYZ_biasW = V4SetW(v: V4Neg(f: Vec4V_From_Vec3V(f: resp1.angular)), f: FZero());
288 f0->normalXYZ_appliedForceW = V4SetW(v: Vec4V_From_Vec3V(f: t0), f: FZero());
289 FStore(a: tv, f: &f0->targetVel);
290 f0->linDeltaVA = deltaV0.linear;
291 f0->angDeltaVA = deltaV0.angular;
292 f0->linDeltaVB = deltaV1.linear;
293 f0->angDeltaVB = deltaV1.angular;
294 }
295 }
296
297 totalPatchContactCount += c.contactPatches[patch].count;
298
299 ptr = p;
300 }
301 }
302 }
303 //PX_ASSERT(ptr - workspace == n.solverConstraintSize);
304 return hasFriction;
305}
306
307
308}
309
310}
311

source code of qtquick3dphysics/src/3rdparty/PhysX/source/lowleveldynamics/src/DyArticulationContactPrepPF.cpp