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 "PsMathUtils.h"
32#include "CmConeLimitHelper.h"
33#include "DySolverConstraint1D.h"
34#include "DyFeatherstoneArticulation.h"
35#include "PxsRigidBody.h"
36#include "PxcConstraintBlockStream.h"
37#include "DyArticulationContactPrep.h"
38#include "DyDynamics.h"
39#include "DyArticulationReference.h"
40#include "DyArticulationPImpl.h"
41#include "foundation/PxProfiler.h"
42#include "DyArticulationFnsSimd.h"
43#include "PsFoundation.h"
44#include "extensions/PxContactJoint.h"
45#include "DyFeatherstoneArticulationLink.h"
46#include "DyFeatherstoneArticulationJointData.h"
47#include "DyConstraint.h"
48#include "DyConstraintPrep.h"
49#include "DySolverContext.h"
50
51namespace physx
52{
53
54namespace Dy
55{
56 void PxcFsFlushVelocity(FeatherstoneArticulation& articulation, Cm::SpatialVectorF* deltaV);
57
58 void FeatherstoneArticulation::computeLinkAccelerationInv(ArticulationData& data, ScratchData& scratchData)
59 {
60 Cm::SpatialVectorF* motionAccelerations = scratchData.motionAccelerations;
61
62 Cm::SpatialVectorF* coriolisVectors = scratchData.coriolisVectors;
63
64 PxReal* jointAccelerations = scratchData.jointAccelerations;
65
66 motionAccelerations[0] = Cm::SpatialVectorF::Zero();
67
68 for (PxU32 linkID = 1; linkID < data.getLinkCount(); ++linkID)
69 {
70 ArticulationLink& link = data.getLink(index: linkID);
71
72 Cm::SpatialVectorF pMotionAcceleration = translateSpatialVector(offset: -data.getLinkData(index: linkID).rw, vec: motionAccelerations[link.parent]);
73
74 Cm::SpatialVectorF motionAcceleration(PxVec3(0.f), PxVec3(0.f));
75
76 if (jointAccelerations)
77 {
78 ArticulationJointCoreData& jointDatum = data.getJointData(index: linkID);
79 const PxReal* jAcceleration = &jointAccelerations[jointDatum.jointOffset];
80 for (PxU32 ind = 0; ind < jointDatum.dof; ++ind)
81 {
82 motionAcceleration.top += data.mWorldMotionMatrix[linkID][ind].top * jAcceleration[ind];
83 motionAcceleration.bottom += data.mWorldMotionMatrix[linkID][ind].bottom * jAcceleration[ind];
84 }
85 }
86
87 motionAccelerations[linkID] = pMotionAcceleration + coriolisVectors[linkID] + motionAcceleration;
88 }
89 }
90
91 //generalized force
92 void FeatherstoneArticulation::computeGeneralizedForceInv(ArticulationData& data, ScratchData& scratchData)
93 {
94 const PxU32 linkCount = data.getLinkCount();
95
96 Cm::SpatialVectorF* spatialZAForces = scratchData.spatialZAVectors;
97 PxReal* jointForces = scratchData.jointForces;
98
99 for (PxU32 linkID = (linkCount - 1); linkID > 0; --linkID)
100 {
101 ArticulationLink& link = data.getLink(index: linkID);
102
103 //joint force
104
105 spatialZAForces[link.parent] += translateSpatialVector(offset: data.getLinkData(index: linkID).rw, vec: spatialZAForces[linkID]);
106
107 ArticulationJointCoreData& jointDatum = data.getJointData(index: linkID);
108 //compute generalized force
109 PxReal* force = &jointForces[jointDatum.jointOffset];
110
111 for (PxU32 ind = 0; ind < jointDatum.dof; ++ind)
112 {
113 force[ind] = data.mWorldMotionMatrix[linkID][ind].innerProduct(v: spatialZAForces[linkID]);
114 }
115 }
116 }
117
118 void FeatherstoneArticulation::computeZAForceInv(ArticulationData& data, ScratchData& scratchData)
119 {
120 const PxU32 linkCount = data.getLinkCount();
121
122 Cm::SpatialVectorF* motionAccelerations = scratchData.motionAccelerations;
123
124 Cm::SpatialVectorF* biasForce = scratchData.spatialZAVectors;
125
126 for (PxU32 linkID = 0; linkID < linkCount; ++linkID)
127 {
128 ArticulationLink& link = data.getLink(index: linkID);
129
130 PxsBodyCore& core = *link.bodyCore;
131
132 const PxVec3& ii = core.inverseInertia;
133
134 const PxReal m = core.inverseMass == 0.f ? 0.f : 1.0f / core.inverseMass;
135 const PxVec3 inertiaTensor = PxVec3(ii.x == 0.f ? 0.f : (1.f / ii.x), ii.y == 0.f ? 0.f : (1.f / ii.y), ii.z == 0.f ? 0.f : (1.f / ii.z));
136
137
138 Cm::SpatialVectorF Ia;
139 Ia.bottom = core.body2World.rotate(input: core.body2World.rotateInv(input: motionAccelerations[linkID].top).multiply(a: inertiaTensor));
140 Ia.top = motionAccelerations[linkID].bottom * m;
141
142 biasForce[linkID] +=Ia;
143 }
144 }
145
146 void FeatherstoneArticulation::initCompositeSpatialInertia(ArticulationData& data, Dy::SpatialMatrix* compositeSpatialInertia)
147 {
148 const PxU32 linkCount = data.getLinkCount();
149
150 for (PxU32 linkID = 0; linkID < linkCount; ++linkID)
151 {
152 SpatialMatrix& spatialInertia = compositeSpatialInertia[linkID];
153
154 ArticulationLink& link = data.getLink(index: linkID);
155
156 PxsBodyCore& core = *link.bodyCore;
157
158 const PxVec3& ii = core.inverseInertia;
159
160 const PxReal m = core.inverseMass == 0.f ? 0.f : 1.0f / core.inverseMass;
161
162 //construct mass matric
163 spatialInertia.topLeft = PxMat33(PxZero);
164 spatialInertia.topRight = PxMat33::createDiagonal(d: PxVec3(m));
165
166 //construct inertia matrix
167 PxMat33 rot(data.getLink(index: linkID).bodyCore->body2World.q);
168 PxMat33& I = spatialInertia.bottomLeft;
169 const PxVec3 inertiaTensor = PxVec3(ii.x == 0.f ? 0.f : (1.f / ii.x), ii.y == 0.f ? 0.f : (1.f / ii.y), ii.z == 0.f ? 0.f : (1.f / ii.z));
170 Cm::transformInertiaTensor(invD: inertiaTensor, M: rot, mIInv&: I);
171 }
172 }
173
174 void FeatherstoneArticulation::computeCompositeSpatialInertiaAndZAForceInv(ArticulationData& data, ScratchData& scratchData)
175 {
176 ArticulationLink* links = data.getLinks();
177 const PxU32 linkCount = data.getLinkCount();
178 const PxU32 startIndex = PxU32(linkCount - 1);
179
180 Dy::SpatialMatrix* compositeSpatialInertia = scratchData.compositeSpatialInertias;
181 Cm::SpatialVectorF* zaForce = scratchData.spatialZAVectors;
182
183 initCompositeSpatialInertia(data, compositeSpatialInertia);
184
185 for (PxU32 linkID = startIndex; linkID > 0; --linkID)
186 {
187 ArticulationLink& link = links[linkID];
188
189 Dy::SpatialMatrix cSpatialInertia = compositeSpatialInertia[linkID];
190 translateInertia(offset: FeatherstoneArticulation::constructSkewSymmetricMatrix(r: data.mLinksData[linkID].rw), inertia&: cSpatialInertia);
191
192 //compute parent's composite spatial inertia
193 compositeSpatialInertia[link.parent] += cSpatialInertia;
194
195 //compute zero acceleration force. This is the force that would be required to support the
196 //motion of all the bodies in childen set if root node acceleration happened to be zero
197 zaForce[link.parent] += translateSpatialVector(offset: data.getLinkData(index: linkID).rw, vec: zaForce[linkID]);
198 }
199 }
200
201 void FeatherstoneArticulation::computeRelativeGeneralizedForceInv(ArticulationData& data, ScratchData& scratchData)
202 {
203 Cm::SpatialVectorF* motionAccelerations = scratchData.motionAccelerations;
204 Dy::SpatialMatrix* compositeSpatialInertia = scratchData.compositeSpatialInertias;
205 Cm::SpatialVectorF* zaForce = scratchData.spatialZAVectors;
206 PxReal* jointForces = scratchData.jointForces;
207
208 Dy::SpatialMatrix invInertia = compositeSpatialInertia[0].invertInertia();
209 motionAccelerations[0] = -(invInertia * zaForce[0]);
210
211 const PxU32 linkCount = data.getLinkCount();
212 ArticulationLink* links = data.getLinks();
213
214 for (PxU32 linkID = 1; linkID < linkCount; ++linkID)
215 {
216 ArticulationLink& link = links[linkID];
217
218 //const SpatialTransform p2c = data.mChildToParent[linkID].getTranspose();
219
220 motionAccelerations[linkID] = translateSpatialVector(offset: -data.mLinksData[linkID].rw, vec: motionAccelerations[link.parent]);
221
222
223 zaForce[linkID] = compositeSpatialInertia[linkID] * motionAccelerations[linkID] + zaForce[linkID];
224 ArticulationJointCoreData& jointDatum = data.getJointData(index: linkID);
225 //compute generalized force
226 PxReal* jForce = &jointForces[jointDatum.jointOffset];
227
228 for (PxU32 ind = 0; ind < jointDatum.dof; ++ind)
229 {
230 jForce[ind] = data.mWorldMotionMatrix[linkID][ind].innerProduct(v: zaForce[linkID]);
231 }
232 }
233 }
234
235 void FeatherstoneArticulation::inverseDynamic(ArticulationData& data, const PxVec3& gravity,
236 ScratchData& scratchData, bool computeCoriolis)
237 {
238 //pass 1
239 computeLinkVelocities(data, scratchData);
240
241 if(computeCoriolis)
242 computeC(data, scratchData);
243 else
244 PxMemZero(dest: scratchData.coriolisVectors, count: sizeof(Cm::SpatialVectorF)*data.getLinkCount());
245
246 computeZ(data, gravity, scratchData);
247
248 computeLinkAccelerationInv(data, scratchData);
249
250 computeZAForceInv(data, scratchData);
251
252 //pass 2
253 computeGeneralizedForceInv(data, scratchData);
254 }
255
256 void FeatherstoneArticulation::inverseDynamicFloatingBase(ArticulationData& data, const PxVec3& gravity,
257 ScratchData& scratchData, bool computeCoriolis)
258 {
259 //pass 1
260 computeLinkVelocities(data, scratchData);
261
262 if(computeCoriolis)
263 computeC(data, scratchData);
264 else
265 PxMemZero(dest: scratchData.coriolisVectors, count: sizeof(Cm::SpatialVectorF)*data.getLinkCount());
266
267 computeZ(data, gravity, scratchData);
268 //no gravity, no external accelerations because we have turned those in force in
269 //computeZ
270 computeLinkAccelerationInv(data, scratchData);
271
272 computeZAForceInv(data, scratchData);
273
274 //pass 2
275 computeCompositeSpatialInertiaAndZAForceInv(data, scratchData);
276
277 //pass 3
278 computeRelativeGeneralizedForceInv(data, scratchData);
279 }
280
281
282 bool FeatherstoneArticulation::applyCacheToDest(ArticulationData& data, PxArticulationCache& cache,
283 PxReal* jVelocities, PxReal* jAccelerations, PxReal* jPositions, PxReal* jointForces,
284 const PxArticulationCacheFlags flag)
285 {
286 bool needsScheduling = !mGPUDirtyFlags;
287
288 if (flag & PxArticulationCache::eVELOCITY)
289 {
290 copyJointData(data, toJointData: jVelocities, fromJointData: cache.jointVelocity);
291 mGPUDirtyFlags |= ArticulationDirtyFlag::eDIRTY_VELOCITIES;
292 }
293
294 if (flag & PxArticulationCache::eACCELERATION)
295 {
296 copyJointData(data, toJointData: jAccelerations, fromJointData: cache.jointAcceleration);
297 mGPUDirtyFlags |= ArticulationDirtyFlag::eDIRTY_ACCELERATIONS;
298 }
299
300 if (flag & PxArticulationCache::eROOT)
301 {
302 ArticulationLink& rLink = mArticulationData.getLink(index: 0);
303 if (flag & PxArticulationCache::ePOSITION)
304 rLink.bodyCore->body2World = cache.rootLinkData->transform * rLink.bodyCore->getBody2Actor();
305 if (flag & PxArticulationCache::eVELOCITY)
306 {
307 rLink.bodyCore->linearVelocity = cache.rootLinkData->worldLinVel;
308 rLink.bodyCore->angularVelocity = cache.rootLinkData->worldAngVel;
309 }
310 mGPUDirtyFlags |= ArticulationDirtyFlag::eDIRTY_ROOT;
311 }
312
313 if (flag & PxArticulationCache::ePOSITION)
314 {
315 copyJointData(data, toJointData: jPositions, fromJointData: cache.jointPosition);
316 //When we update the joint positions, we also have to update the link state, so need to make links
317 //dirty!
318 mGPUDirtyFlags |= (ArticulationDirtyFlag::eDIRTY_POSITIONS);
319 }
320
321 if (flag & PxArticulationCache::eFORCE)
322 {
323 copyJointData(data, toJointData: jointForces, fromJointData: cache.jointForce);
324 mGPUDirtyFlags |= ArticulationDirtyFlag::eDIRTY_FORCES;
325 }
326
327 if ((flag & PxArticulationCache::ePOSITION))
328 {
329 //update link's position based on the joint position
330 teleportLinks(data);
331 }
332
333 if ((flag & PxArticulationCache::eVELOCITY) || (flag & PxArticulationCache::ePOSITION))
334 {
335 computeLinkVelocities(data);
336 }
337
338 return needsScheduling;
339 }
340
341 void FeatherstoneArticulation::packJointData(const PxReal* maximum, PxReal* reduced)
342 {
343 const PxU32 linkCount = mArticulationData.getLinkCount();
344
345 for (PxU32 linkID = 1; linkID < linkCount; linkID++)
346 {
347
348 ArticulationLink& linkDatum = mArticulationData.getLink(index: linkID);
349 ArticulationJointCore* joint = linkDatum.inboundJoint;
350 ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(index: linkID);
351
352 const PxReal* maxJointData = &maximum[(linkID - 1) * DY_MAX_DOF];
353 PxReal* reducedJointData = &reduced[jointDatum.jointOffset];
354
355 PxU32 count = 0;
356 for (PxU32 j = 0; j < DY_MAX_DOF; ++j)
357 {
358 PxArticulationMotions motion = PxArticulationMotions(joint->motion[j]);
359 if (motion != PxArticulationMotion::eLOCKED)
360 {
361 reducedJointData[count] = maxJointData[j];
362 count++;
363 }
364 }
365
366 PX_ASSERT(count == jointDatum.dof);
367 }
368
369 }
370
371 void FeatherstoneArticulation::unpackJointData(const PxReal* reduced, PxReal* maximum)
372 {
373 const PxU32 linkCount = mArticulationData.getLinkCount();
374
375 for (PxU32 linkID = 1; linkID < linkCount; linkID++)
376 {
377 ArticulationLink& linkDatum = mArticulationData.getLink(index: linkID);
378 ArticulationJointCore* joint = linkDatum.inboundJoint;
379 ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(index: linkID);
380
381 PxReal* maxJointData = &maximum[(linkID - 1) * DY_MAX_DOF];
382 const PxReal* reducedJointData = &reduced[jointDatum.jointOffset];
383
384 PxU32 count = 0;
385 for (PxU32 j = 0; j < DY_MAX_DOF; ++j)
386 {
387 PxArticulationMotions motion = PxArticulationMotions(joint->motion[j]);
388 if (motion != PxArticulationMotion::eLOCKED)
389 {
390 maxJointData[j] = reducedJointData[count];
391 count++;
392 }
393 else
394 {
395 maxJointData[j] = 0.f;
396 }
397 }
398
399 PX_ASSERT(count == jointDatum.dof);
400 }
401 }
402
403 void FeatherstoneArticulation::initializeCommonData()
404 {
405 jcalc(data&: mArticulationData);
406 computeRelativeTransformC2P(data&: mArticulationData);
407
408 computeRelativeTransformC2B(data&: mArticulationData);
409
410 computeSpatialInertia(data&: mArticulationData);
411
412 mArticulationData.setDataDirty(false);
413 }
414
415 void FeatherstoneArticulation::getGeneralizedGravityForce(const PxVec3& gravity, PxArticulationCache& cache)
416 {
417
418 if (mArticulationData.getDataDirty())
419 {
420 Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, messageFmt: "Articulation::getGeneralisedGravityForce() commonInit need to be called first to initialize data!");
421 return;
422 }
423
424#if FEATHERSTONE_DEBUG
425 PxReal* jointForce = reinterpret_cast<PxReal*>(PX_ALLOC(sizeof(PxReal) * mArticulationData.getDofs(), "jointForce"));
426 {
427
428 const PxU32 linkCount = mArticulationData.getLinkCount();
429
430 PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);
431
432 ScratchData scratchData;
433 PxU8* tempMemory = allocateScratchSpatialData(allocator, linkCount, scratchData);
434
435 scratchData.jointVelocities = NULL;
436 scratchData.jointAccelerations = NULL;
437 scratchData.jointForces = jointForce;
438
439 const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
440 if (fixBase)
441 inverseDynamic(mArticulationData, gravity, scratchData, false);
442 else
443 inverseDynamicFloatingBase(mArticulationData, gravity, scratchData, false);
444
445 allocator->free(tempMemory);
446 }
447#endif
448
449 const PxVec3 tGravity = -gravity;
450 PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);
451 const PxU32 linkCount = mArticulationData.getLinkCount();
452
453 const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
454 if (fixBase)
455 {
456 Cm::SpatialVectorF* spatialZAForces = reinterpret_cast<Cm::SpatialVectorF*>(allocator->alloc(requestedSize: sizeof(Cm::SpatialVectorF) * linkCount));
457
458 for (PxU32 linkID = 0; linkID < linkCount; ++linkID)
459 {
460 ArticulationLink& link = mArticulationData.getLink(index: linkID);
461
462 PxsBodyCore& core = *link.bodyCore;
463
464 const PxReal m = 1.0f / core.inverseMass;
465
466 const PxVec3 linkGravity = tGravity;
467
468 spatialZAForces[linkID].top = m*linkGravity;
469 spatialZAForces[linkID].bottom = PxVec3(0.f);
470 }
471
472 ScratchData scratchData;
473 scratchData.spatialZAVectors = spatialZAForces;
474 scratchData.jointForces = cache.jointForce;
475
476 computeGeneralizedForceInv(data&: mArticulationData, scratchData);
477
478 //release spatialZA vectors
479 allocator->free(addr: spatialZAForces);
480 }
481 else
482 {
483 ScratchData scratchData;
484 PxU8* tempMemory = allocateScratchSpatialData(allocator, linkCount, scratchData);
485
486 scratchData.jointVelocities = NULL;
487 scratchData.jointAccelerations = NULL;
488 scratchData.jointForces = cache.jointForce;
489 scratchData.externalAccels = NULL;
490
491 inverseDynamicFloatingBase(data&: mArticulationData, gravity: tGravity, scratchData, computeCoriolis: false);
492
493 allocator->free(addr: tempMemory);
494 }
495
496#if FEATHERSTONE_DEBUG
497 //compare joint force
498 const PxU32 totalDofs = mArticulationData.getDofs();
499 for (PxU32 i = 0; i < totalDofs; ++i)
500 {
501 const PxReal dif = jointForce[i] - cache.jointForce[i];
502 PX_ASSERT(PxAbs(dif) < 5e-3f);
503 }
504
505 PX_FREE(jointForce);
506#endif
507
508 }
509
510 //gravity, acceleration and external force(external acceleration) are zero
511 void FeatherstoneArticulation::getCoriolisAndCentrifugalForce(PxArticulationCache& cache)
512 {
513 if (mArticulationData.getDataDirty())
514 {
515 Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, messageFmt: "Articulation::getCoriolisAndCentrifugalForce() commonInit need to be called first to initialize data!");
516 return;
517 }
518
519 const PxU32 linkCount = mArticulationData.getLinkCount();
520
521 PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);
522
523 ScratchData scratchData;
524 PxU8* tempMemory = allocateScratchSpatialData(allocator, linkCount, scratchData);
525
526 scratchData.jointVelocities = cache.jointVelocity;
527 scratchData.jointAccelerations = NULL;
528 scratchData.jointForces = cache.jointForce;
529 scratchData.externalAccels = NULL;
530
531 const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
532 if (fixBase)
533 inverseDynamic(data&: mArticulationData, gravity: PxVec3(0.f), scratchData, computeCoriolis: true);
534 else
535 inverseDynamicFloatingBase(data&: mArticulationData, gravity: PxVec3(0.f), scratchData, computeCoriolis: true);
536
537 allocator->free(addr: tempMemory);
538 }
539
540 //gravity, joint acceleration and joint velocity are zero
541 void FeatherstoneArticulation::getGeneralizedExternalForce(PxArticulationCache& cache)
542 {
543 if (mArticulationData.getDataDirty())
544 {
545 Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, messageFmt: "Articulation::getCoriolisAndCentrifugalForce() commonInit need to be called first to initialize data!");
546 return;
547 }
548
549 const PxU32 linkCount = mArticulationData.getLinkCount();
550
551 PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);
552
553 ScratchData scratchData;
554 PxU8* tempMemory = allocateScratchSpatialData(allocator, linkCount, scratchData);
555
556 scratchData.jointVelocities = NULL;
557 scratchData.jointAccelerations = NULL;
558 scratchData.jointForces = cache.jointForce;
559
560 Cm::SpatialVector* accels = reinterpret_cast<Cm::SpatialVector*>(allocator->alloc(requestedSize: sizeof(Cm::SpatialVector) * linkCount));
561
562 //turn external forces to external accels
563 for (PxU32 i = 0; i < linkCount; ++i)
564 {
565 ArticulationLink& link = mArticulationData.getLink(index: i);
566 PxsBodyCore& core = *link.bodyCore;
567
568 const PxSpatialForce& force = cache.externalForces[i];
569 Cm::SpatialVector& accel = accels[i];
570
571 accel.linear = force.force * core.inverseMass;
572
573 PxMat33 inverseInertiaWorldSpace;
574 Cm::transformInertiaTensor(invD: core.inverseInertia, M: PxMat33(core.body2World.q), mIInv&: inverseInertiaWorldSpace);
575
576 accel.angular = inverseInertiaWorldSpace * force.torque;
577 }
578
579 scratchData.externalAccels = accels;
580
581 const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
582 if (fixBase)
583 inverseDynamic(data&: mArticulationData, gravity: PxVec3(0.f), scratchData, computeCoriolis: false);
584 else
585 inverseDynamicFloatingBase(data&: mArticulationData, gravity: PxVec3(0.f), scratchData, computeCoriolis: false);
586
587 allocator->free(addr: tempMemory);
588 allocator->free(addr: accels);
589 }
590
591 //provided joint acceleration, calculate joint force
592 void FeatherstoneArticulation::getJointForce(PxArticulationCache& cache)
593 {
594 if (mArticulationData.getDataDirty())
595 {
596 Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, messageFmt: "ArticulationHelper::getJointForce() commonInit need to be called first to initialize data!");
597 return;
598 }
599
600 //const PxU32 size = sizeof(PxReal) * mArticulationData.getDofs();
601 PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);
602 //PxReal* jointVelocities = reinterpret_cast<PxReal*>(allocator->alloc(size));
603
604 ScratchData scratchData;
605 scratchData.jointVelocities = NULL;//jont velocity will be zero
606 scratchData.jointAccelerations = cache.jointAcceleration; //input
607 scratchData.jointForces = cache.jointForce; //output
608 scratchData.externalAccels = NULL;
609
610 PxU8* tempMemory = allocateScratchSpatialData(allocator, linkCount: mArticulationData.getLinkCount(), scratchData);
611
612 //make sure joint velocity be zero
613 //PxMemZero(jointVelocities, sizeof(PxReal) * mArticulationData.getDofs());
614 const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
615
616 if (fixBase)
617 inverseDynamic(data&: mArticulationData, gravity: PxVec3(0.f), scratchData, computeCoriolis: false);
618 else
619 inverseDynamicFloatingBase(data&: mArticulationData, gravity: PxVec3(0.f), scratchData, computeCoriolis: false);
620
621 //allocator->free(jointVelocities);
622 allocator->free(addr: tempMemory);
623 }
624
625 void FeatherstoneArticulation::jcalcLoopJointSubspace(ArticulationJointCore* joint,
626 ArticulationJointCoreData& jointDatum, SpatialSubspaceMatrix& T)
627 {
628 const PxVec3 childOffset = -joint->childPose.p;
629 const PxVec3 zero(0.f);
630
631 //if the column is free, we put zero for it, this is for computing K(coefficient matrix)
632 T.setNumColumns(6);
633
634 //transpose(Tc)*S = 0
635 //transpose(Ta)*S = 1
636 switch (joint->jointType)
637 {
638 case PxArticulationJointType::ePRISMATIC:
639 {
640 PX_ASSERT(jointDatum.dof == 1);
641
642 const PxVec3 rx = (joint->childPose.rotate(input: PxVec3(1.f, 0.f, 0.f))).getNormalized();
643 const PxVec3 ry = (joint->childPose.rotate(input: PxVec3(0.f, 1.f, 0.f))).getNormalized();
644 const PxVec3 rz = (joint->childPose.rotate(input: PxVec3(0.f, 0.f, 1.f))).getNormalized();
645
646 //joint->activeForceSubspace.setNumColumns(1);
647
648 if (jointDatum.jointAxis[0][3] == 1.f)
649 {
650 //x is the free translation axis
651 T.setColumn(index: 0, top: rx, bottom: zero);
652 T.setColumn(index: 1, top: ry, bottom: zero);
653 T.setColumn(index: 2, top: rz, bottom: zero);
654 T.setColumn(index: 3, top: zero, bottom: zero);
655 T.setColumn(index: 4, top: zero, bottom: ry);
656 T.setColumn(index: 5, top: zero, bottom: rz);
657
658 //joint->activeForceSubspace.setColumn(0, PxVec3(0.f), rx);
659 }
660 else if (jointDatum.jointAxis[0][4] == 1.f)
661 {
662 //y is the free translation axis
663 T.setColumn(index: 0, top: rx, bottom: zero);
664 T.setColumn(index: 1, top: ry, bottom: zero);
665 T.setColumn(index: 2, top: rz, bottom: zero);
666 T.setColumn(index: 3, top: zero, bottom: rx);
667 T.setColumn(index: 4, top: zero, bottom: zero);
668 T.setColumn(index: 5, top: zero, bottom: rz);
669
670 //joint->activeForceSubspace.setColumn(0, PxVec3(0.f), ry);
671 }
672 else if (jointDatum.jointAxis[0][5] == 1.f)
673 {
674 //z is the free translation axis
675 T.setColumn(index: 0, top: rx, bottom: zero);
676 T.setColumn(index: 1, top: ry, bottom: zero);
677 T.setColumn(index: 2, top: rx, bottom: zero);
678 T.setColumn(index: 3, top: zero, bottom: rx);
679 T.setColumn(index: 4, top: zero, bottom: ry);
680 T.setColumn(index: 5, top: zero, bottom: zero);
681
682 //joint->activeForceSubspace.setColumn(0, PxVec3(0.f), rz);
683 }
684
685 break;
686 }
687 case PxArticulationJointType::eREVOLUTE:
688 {
689 //joint->activeForceSubspace.setNumColumns(1);
690
691 const PxVec3 rx = (joint->childPose.rotate(input: PxVec3(1.f, 0.f, 0.f))).getNormalized();
692 const PxVec3 ry = (joint->childPose.rotate(input: PxVec3(0.f, 1.f, 0.f))).getNormalized();
693 const PxVec3 rz = (joint->childPose.rotate(input: PxVec3(0.f, 0.f, 1.f))).getNormalized();
694
695 const PxVec3 rxXd = rx.cross(v: childOffset);
696 const PxVec3 ryXd = ry.cross(v: childOffset);
697 const PxVec3 rzXd = rz.cross(v: childOffset);
698
699 if (jointDatum.jointAxis[0][0] == 1.f)
700 {
701 //x is the free rotation axis
702
703 T.setColumn(index: 0, top: zero, bottom: zero);
704 T.setColumn(index: 1, top: ry, bottom: zero);
705 T.setColumn(index: 2, top: rz, bottom: zero);
706
707 //joint->activeForceSubspace.setColumn(0, rx, PxVec3(0.f));
708
709 }
710 else if (jointDatum.jointAxis[0][1] == 1.f)
711 {
712 //y is the free rotation axis
713 T.setColumn(index: 0, top: rx, bottom: zero);
714 T.setColumn(index: 1, top: zero, bottom: zero);
715 T.setColumn(index: 2, top: rz, bottom: zero);
716
717 //joint->activeForceSubspace.setColumn(0, ry, PxVec3(0.f));
718 }
719 else if (jointDatum.jointAxis[0][2] == 1.f)
720 {
721 //z is the rotation axis
722 T.setColumn(index: 0, top: rx, bottom: zero);
723 T.setColumn(index: 1, top: ry, bottom: zero);
724 T.setColumn(index: 2, top: zero, bottom: zero);
725
726 //joint->activeForceSubspace.setColumn(0, rz, PxVec3(0.f));
727 }
728
729 T.setColumn(index: 3, top: rxXd, bottom: rx);
730 T.setColumn(index: 4, top: ryXd, bottom: ry);
731 T.setColumn(index: 5, top: rzXd, bottom: rz);
732
733 break;
734 }
735 case PxArticulationJointType::eSPHERICAL:
736 {
737 //joint->activeForceSubspace.setNumColumns(3);
738
739 const PxVec3 rx = (joint->childPose.rotate(input: PxVec3(1.f, 0.f, 0.f))).getNormalized();
740 const PxVec3 ry = (joint->childPose.rotate(input: PxVec3(0.f, 1.f, 0.f))).getNormalized();
741 const PxVec3 rz = (joint->childPose.rotate(input: PxVec3(0.f, 0.f, 1.f))).getNormalized();
742
743 const PxVec3 rxXd = rx.cross(v: childOffset);
744 const PxVec3 ryXd = ry.cross(v: childOffset);
745 const PxVec3 rzXd = rz.cross(v: childOffset);
746
747 T.setColumn(index: 0, top: zero, bottom: zero);
748 T.setColumn(index: 1, top: zero, bottom: zero);
749 T.setColumn(index: 2, top: zero, bottom: zero);
750
751 T.setColumn(index: 3, top: rxXd, bottom: rx);
752 T.setColumn(index: 4, top: ryXd, bottom: ry);
753 T.setColumn(index: 5, top: rzXd, bottom: rz);
754
755 //need to implement constraint force subspace matrix and active force subspace matrix
756
757 break;
758 }
759 case PxArticulationJointType::eFIX:
760 {
761 //joint->activeForceSubspace.setNumColumns(0);
762 //T.setNumColumns(6);
763
764 /* const PxVec3 rx = (joint->childPose.rotate(PxVec3(1.f, 0.f, 0.f))).getNormalized();
765 const PxVec3 ry = (joint->childPose.rotate(PxVec3(0.f, 1.f, 0.f))).getNormalized();
766 const PxVec3 rz = (joint->childPose.rotate(PxVec3(0.f, 0.f, 1.f))).getNormalized();
767
768 T.setColumn(0, rx, PxVec3(0.f));
769 T.setColumn(1, ry, PxVec3(0.f));
770 T.setColumn(2, rz, PxVec3(0.f));
771 T.setColumn(3, PxVec3(0.f), rx);
772 T.setColumn(4, PxVec3(0.f), ry);
773 T.setColumn(5, PxVec3(0.f), rz);
774 */
775
776 T.setColumn(index: 0, top: PxVec3(1.f, 0.f, 0.f), bottom: zero);
777 T.setColumn(index: 1, top: PxVec3(0.f, 1.f, 0.f), bottom: zero);
778 T.setColumn(index: 2, top: PxVec3(0.f, 0.f, 1.f), bottom: zero);
779 T.setColumn(index: 3, top: zero, bottom: PxVec3(1.f, 0.f, 0.f));
780 T.setColumn(index: 4, top: zero, bottom: PxVec3(0.f, 1.f, 0.f));
781 T.setColumn(index: 5, top: zero, bottom: PxVec3(0.f, 0.f, 1.f));
782
783 PX_ASSERT(jointDatum.dof == 0);
784 break;
785 }
786 default:
787 break;
788
789 }
790 }
791
792 //This method supports just one loopJoint
793 void FeatherstoneArticulation::getKMatrix(ArticulationJointCore* loopJoint, const PxU32 parentIndex, const PxU32 childIndex, PxArticulationCache& cache)
794 {
795 PX_UNUSED(loopJoint);
796 PX_UNUSED(parentIndex);
797 PX_UNUSED(childIndex);
798 PX_UNUSED(cache);
799
800 ////initialize all tree links motion subspace matrix
801 //jcalc(mArticulationData);
802
803 ////linkID is the parent link, ground is the child link so child link is the fix base
804 //ArticulationLinkData& pLinkDatum = mArticulationData.getLinkData(parentIndex);
805
806 //ArticulationLink& cLink = mArticulationData.getLink(childIndex);
807 //ArticulationLinkData& cLinkDatum = mArticulationData.getLinkData(childIndex);
808 //
809 //ArticulationJointCoreData loopJointDatum;
810 //loopJointDatum.computeJointDof(loopJoint);
811
812 ////this is constraintForceSubspace in child body space(T)
813 //SpatialSubspaceMatrix T;
814
815 ////loop joint constraint subspace matrix(T)
816 //jcalcLoopJointSubspace(loopJoint, loopJointDatum, T);
817
818 //const PxU32 linkCount = mArticulationData.getLinkCount();
819 ////set Jacobian matrix to be zero
820 //PxMemZero(cache.jacobian, sizeof(PxKinematicJacobian) * linkCount);
821
822 ////transform T to world space
823 //PxTransform& body2World = cLink.bodyCore->body2World;
824
825 //for (PxU32 ind = 0; ind < T.getNumColumns(); ++ind)
826 //{
827 // Cm::SpatialVectorF& column = T[ind];
828 // T.setColumn(ind, body2World.rotate(column.top), body2World.rotate(column.bottom));
829 //}
830
831 //const Cm::SpatialVectorF& pAccel = pLinkDatum.motionAcceleration;
832 //const Cm::SpatialVectorF& cAccel = cLinkDatum.motionAcceleration;
833
834 //const Cm::SpatialVectorF& pVel = pLinkDatum.motionVelocity;
835 //const Cm::SpatialVectorF& cVel = cLinkDatum.motionVelocity;
836
837 //Cm::SpatialVectorF k = (pAccel - cAccel) + pVel.cross(cVel);
838 //k = T.transposeMultiply(k);
839 //k = -k;
840
841 //PxU32 i = childIndex;
842 //PxU32 j = parentIndex;
843
844 //PxU32* index = NULL;
845
846 //while (i != j)
847 //{
848 // if (i > j)
849 // index = &i;
850 // else
851 // index = &j;
852
853 // const PxU32 linkIndex = *index;
854
855 // PxKinematicJacobian* K = cache.jacobian + linkIndex;
856
857 // ArticulationLink& link = mArticulationData.getLink(linkIndex);
858
859 // ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(linkIndex);
860
861 // SpatialSubspaceMatrix& S = jointDatum.motionMatrix;
862
863 // PxTransform& tBody2World = link.bodyCore->body2World;
864
865 // Cm::SpatialVectorF res;
866 // for (PxU32 ind = 0; ind < S.getNumColumns(); ++ind)
867 // {
868 // Cm::SpatialVectorF& sCol = S[ind];
869
870 // //transform spatial axis into world space
871 // sCol.top = tBody2World.rotate(sCol.top);
872 // sCol.bottom = tBody2World.rotate(sCol.bottom);
873
874 // res = T.transposeMultiply(sCol);
875 // res = -res;
876
877 // PxReal* kSubMatrix = K->j[ind];
878
879 // kSubMatrix[0] = res.top.x; kSubMatrix[1] = res.top.y; kSubMatrix[2] = res.top.z;
880 // kSubMatrix[3] = res.bottom.x; kSubMatrix[4] = res.bottom.y; kSubMatrix[5] = res.bottom.z;
881 // }
882
883 // //overwrite either i or j to its parent index
884 // *index = link.parent;
885 //}
886 }
887
888
889 void FeatherstoneArticulation::getCoefficientMatrix(const PxReal dt, const PxU32 linkID, const PxContactJoint* contactJoints, const PxU32 nbContacts, PxArticulationCache& cache)
890 {
891 if (mArticulationData.getDataDirty())
892 {
893 Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, messageFmt: "ArticulationHelper::getCoefficientMatrix() commonInit need to be called first to initialize data!");
894 return;
895 }
896
897 computeArticulatedSpatialInertia(data&: mArticulationData);
898
899 ArticulationLink* links = mArticulationData.getLinks();
900
901 const PxU32 linkCount = mArticulationData.getLinkCount();
902
903 PxReal* coefficientMatrix = cache.coefficientMatrix;
904
905 const PxU32 elementCount = mArticulationData.getDofs();
906
907 //zero coefficient matrix
908 PxMemZero(dest: coefficientMatrix, count: sizeof(PxReal) * elementCount * nbContacts);
909
910 const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
911
912 for (PxU32 a = 0; a < nbContacts; ++a)
913 {
914 PxJacobianRow row;
915 contactJoints[a].computeJacobians(jacobian: &row);
916
917 //impulse lin is contact normal, and ang is raxn. R is body2World, R(t) is world2Body
918 //| R(t), 0 |
919 //| R(t)*r, R(t)|
920 //r is the vector from center of mass to contact point
921 //p(impluse) = |n|
922 // |0|
923
924 //transform p(impluse) from work space to the local space of link
925 ArticulationLink& link = links[linkID];
926 PxTransform& body2World = link.bodyCore->body2World;
927
928 PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);
929 ScratchData scratchData;
930 PxU8* tempMemory = allocateScratchSpatialData(allocator, linkCount, scratchData);
931
932 Cm::SpatialVectorF* Z = scratchData.spatialZAVectors;
933
934 //make sure all links' spatial zero acceleration impulse are zero
935 PxMemZero(dest: Z, count: sizeof(Cm::SpatialVectorF) * linkCount);
936
937 const Cm::SpatialVectorF impl(body2World.rotateInv(input: row.linear0), body2World.rotateInv(input: row.angular0));
938
939 getZ(linkID, data: mArticulationData, Z, impulse: impl);
940
941 const PxU32 totalDofs = mArticulationData.getDofs();
942
943 const PxU32 size = sizeof(PxReal) * totalDofs;
944
945 PxU8* tData = reinterpret_cast<PxU8*>(allocator->alloc(requestedSize: size * 2));
946
947 PxReal* jointVelocities = reinterpret_cast<PxReal*>(tData);
948 PxReal* jointAccelerations = reinterpret_cast<PxReal*>(tData + size);
949 //zero joint Velocites
950 PxMemZero(dest: jointVelocities, count: size);
951
952 getDeltaVWithDeltaJV(fixBase, linkID, data: mArticulationData, Z, jointVelocities);
953
954 const PxReal invDt = 1.f / dt;
955 //calculate joint acceleration due to velocity change
956 for (PxU32 i = 0; i < totalDofs; ++i)
957 {
958 jointAccelerations[i] = jointVelocities[i] * invDt;
959 }
960
961 //compute individual link's spatial inertia tensor. This is very important
962 computeSpatialInertia(data&: mArticulationData);
963
964 PxReal* coeCol = &coefficientMatrix[elementCount * a];
965
966 //this means the joint force calculated by the inverse dynamic
967 //will be just influenced by joint acceleration change
968 scratchData.jointVelocities = NULL;
969 scratchData.externalAccels = NULL;
970
971 //Input
972 scratchData.jointAccelerations = jointAccelerations;
973
974 //a column of the coefficient matrix is the joint force
975 scratchData.jointForces = coeCol;
976
977 if (fixBase)
978 {
979 inverseDynamic(data&: mArticulationData, gravity: PxVec3(0.f), scratchData, computeCoriolis: false);
980 }
981 else
982 {
983 inverseDynamicFloatingBase(data&: mArticulationData, gravity: PxVec3(0.f), scratchData, computeCoriolis: false);
984 }
985
986 allocator->free(addr: tData);
987 allocator->free(addr: tempMemory);
988 }
989 }
990
991 void FeatherstoneArticulation::getImpulseResponseSlowInv(Dy::ArticulationLink* links,
992 const ArticulationData& data,
993 PxU32 linkID0_,
994 const Cm::SpatialVector& impulse0,
995 Cm::SpatialVector& deltaV0,
996 PxU32 linkID1_,
997 const Cm::SpatialVector& impulse1,
998 Cm::SpatialVector& deltaV1,
999 PxReal* jointVelocities)
1000 {
1001 PX_UNUSED(jointVelocities);
1002 PxU32 stack[DY_ARTICULATION_MAX_SIZE];
1003 Cm::SpatialVectorF Z[DY_ARTICULATION_MAX_SIZE];
1004 //Cm::SpatialVectorF ZZV[DY_ARTICULATION_MAX_SIZE];
1005
1006 PxU32 i0, i1, ic;
1007
1008 PxU32 linkID0 = linkID0_;
1009 PxU32 linkID1 = linkID1_;
1010
1011
1012 for (i0 = linkID0, i1 = linkID1; i0 != i1;) // find common path
1013 {
1014 if (i0<i1)
1015 i1 = links[i1].parent;
1016 else
1017 i0 = links[i0].parent;
1018 }
1019
1020 PxU32 common = i0;
1021
1022 Cm::SpatialVectorF Z0(-impulse0.linear, -impulse0.angular);
1023 Cm::SpatialVectorF Z1(-impulse1.linear, -impulse1.angular);
1024
1025 Z[linkID0] = Z0;
1026 Z[linkID1] = Z1;
1027
1028 //for (i0 = linkID0; i0 != common; i0 = links[i0].parent)
1029 for (i0 = 0; linkID0 != common; linkID0 = links[linkID0].parent)
1030 {
1031 Z0 = FeatherstoneArticulation::propagateImpulseW(isInvD: data.getWorldIsInvD(linkID: linkID0), childToParent: data.getLinkData(index: linkID0).rw, motionMatrix: data.getWorldMotionMatrix(linkID: linkID0), Z: Z0);
1032 Z[links[linkID0].parent] = Z0;
1033 stack[i0++] = linkID0;
1034 }
1035
1036 for (i1 = i0; linkID1 != common; linkID1 = links[linkID1].parent)
1037 {
1038 Z1 = FeatherstoneArticulation::propagateImpulseW(isInvD: data.getWorldIsInvD(linkID: linkID1), childToParent: data.getLinkData(index: linkID1).rw, motionMatrix: data.getWorldMotionMatrix(linkID: linkID1), Z: Z1);
1039 Z[links[linkID1].parent] = Z1;
1040 stack[i1++] = linkID1;
1041 }
1042
1043 //KS - we can replace the following section of code with the impulse response matrix - until next comment!
1044
1045 Cm::SpatialVectorF ZZ = Z0 + Z1;
1046 Z[common] = ZZ;
1047 for (ic = i1; common; common = links[common].parent)
1048 {
1049 Z[links[common].parent] = FeatherstoneArticulation::propagateImpulseW(isInvD: data.getWorldIsInvD(linkID: common), childToParent: data.getLinkData(index: common).rw, motionMatrix: data.getMotionMatrix(linkID: common), Z: Z[common]);
1050 stack[ic++] = common;
1051 }
1052
1053 if(data.getArticulationFlags() & PxArticulationFlag::eFIX_BASE)
1054 Z[0] = Cm::SpatialVectorF(PxVec3(0.f), PxVec3(0.f));
1055
1056 //SpatialMatrix inverseArticulatedInertia = data.getLinkData(0).spatialArticulatedInertia.getInverse();
1057 const SpatialMatrix& inverseArticulatedInertia = data.getBaseInvSpatialArticulatedInertiaW();
1058 Cm::SpatialVectorF v = inverseArticulatedInertia * (-Z[0]);
1059
1060 for (PxU32 index = ic; (index--) > i1;)
1061 {
1062 const PxU32 id = stack[index];
1063 v = FeatherstoneArticulation::propagateVelocityW(c2p: data.getLinkData(index: id).rw, spatialInertia: data.mWorldSpatialArticulatedInertia[id],
1064 invStIs: data.mInvStIs[id], motionMatrix: data.getWorldMotionMatrix(linkID: id), Z: Z[id], jointVelocity: jointVelocities, hDeltaV: v);
1065 }
1066
1067 //Replace everything to here with the impulse response matrix multiply
1068
1069 Cm::SpatialVectorF dv1 = v;
1070 for (PxU32 index = i1; (index--) > i0;)
1071 {
1072 const PxU32 id = stack[index];
1073 dv1 = FeatherstoneArticulation::propagateVelocityW(c2p: data.getLinkData(index: id).rw, spatialInertia: data.mWorldSpatialArticulatedInertia[id],
1074 invStIs: data.mInvStIs[id], motionMatrix: data.getWorldMotionMatrix(linkID: id), Z: Z[id], jointVelocity: jointVelocities, hDeltaV: v);
1075 }
1076
1077 Cm::SpatialVectorF dv0 = v;
1078 for (PxU32 index = i0; (index--) > 0;)
1079 {
1080 const PxU32 id = stack[index];
1081 dv0 = FeatherstoneArticulation::propagateVelocityW(c2p: data.getLinkData(index: id).rw, spatialInertia: data.mWorldSpatialArticulatedInertia[id],
1082 invStIs: data.mInvStIs[id], motionMatrix: data.getWorldMotionMatrix(linkID: id), Z: Z[id], jointVelocity: jointVelocities, hDeltaV: v);
1083 }
1084
1085 deltaV0.linear = dv0.bottom;
1086 deltaV0.angular = dv0.top;
1087
1088 deltaV1.linear = dv1.bottom;
1089 deltaV1.angular = dv1.top;
1090 }
1091
1092 void FeatherstoneArticulation::getImpulseSelfResponseInv(const bool fixBase,
1093 PxU32 linkID0,
1094 PxU32 linkID1,
1095 Cm::SpatialVectorF* Z,
1096 const Cm::SpatialVector& impulse0,
1097 const Cm::SpatialVector& impulse1,
1098 Cm::SpatialVector& deltaV0,
1099 Cm::SpatialVector& deltaV1,
1100 PxReal* jointVelocities)
1101 {
1102 ArticulationLink* links = mArticulationData.getLinks();
1103
1104 //transform p(impluse) from work space to the local space of link
1105 ArticulationLink& link = links[linkID1];
1106 //ArticulationLinkData& linkDatum = mArticulationData.getLinkData(linkID1);
1107
1108 if (link.parent == linkID0)
1109 {
1110 PX_ASSERT(linkID0 == link.parent);
1111 PX_ASSERT(linkID0 < linkID1);
1112
1113 //impulse is in world space
1114 const Cm::SpatialVector& imp1 = impulse1;
1115 const Cm::SpatialVector& imp0 = impulse0;
1116
1117
1118 Cm::SpatialVectorF pImpulse(imp0.linear, imp0.angular);
1119
1120 PX_ASSERT(linkID0 == link.parent);
1121
1122 //initialize child link spatial zero acceleration impulse
1123 Cm::SpatialVectorF Z1(-imp1.linear, -imp1.angular);
1124 //this calculate parent link spatial zero acceleration impulse
1125 Cm::SpatialVectorF Z0 = FeatherstoneArticulation::propagateImpulseW(isInvD: mArticulationData.mIsInvDW[linkID1], childToParent: mArticulationData.getLinkData(index: linkID1).rw, motionMatrix: mArticulationData.mWorldMotionMatrix[linkID1], Z: Z1);
1126
1127 //in parent space
1128 const Cm::SpatialVectorF impulseDif = pImpulse - Z0;
1129
1130 Cm::SpatialVectorF delV0(PxVec3(0.f), PxVec3(0.f));
1131 Cm::SpatialVectorF delV1(PxVec3(0.f), PxVec3(0.f));
1132
1133 //calculate velocity change start from the parent link to the root
1134 delV0 = FeatherstoneArticulation::getImpulseResponseWithJ(linkID: linkID0, fixBase, data: mArticulationData, Z, impulse: impulseDif, jointVelocities);
1135
1136 //calculate velocity change for child link
1137 delV1 = FeatherstoneArticulation::propagateVelocityW(c2p: mArticulationData.getLinkData(index: linkID1).rw,
1138 spatialInertia: mArticulationData.mWorldSpatialArticulatedInertia[linkID1], invStIs: mArticulationData.mInvStIs[linkID1],
1139 motionMatrix: mArticulationData.mWorldMotionMatrix[linkID1], Z: Z1, jointVelocity: jointVelocities, hDeltaV: delV0);
1140
1141 //translate delV0 and delV1 into world space again
1142 deltaV0.linear = delV0.bottom;
1143 deltaV0.angular = delV0.top;
1144 deltaV1.linear = delV1.bottom;
1145 deltaV1.angular = delV1.top;
1146 }
1147 else
1148 {
1149 getImpulseResponseSlowInv(links, data: mArticulationData, linkID0_: linkID0, impulse0, deltaV0, linkID1_: linkID1,impulse1, deltaV1, jointVelocities );
1150 }
1151 }
1152
1153 Cm::SpatialVectorF FeatherstoneArticulation::getImpulseResponseInv(
1154 const bool fixBase, const PxU32 linkID,
1155 Cm::SpatialVectorF* Z,
1156 const Cm::SpatialVector& impulse,
1157 PxReal* jointVelocities)
1158 {
1159 //impulse lin is contact normal, and ang is raxn. R is body2World, R(t) is world2Body
1160 //| R(t), 0 |
1161 //| R(t)*r, R(t)|
1162 //r is the vector from center of mass to contact point
1163 //p(impluse) = |n|
1164 // |0|
1165
1166 ArticulationLink* links = mArticulationData.getLinks();
1167 //ArticulationLinkData* linkData = mArticulationData.getLinkData();
1168 ArticulationJointCoreData* jointData = mArticulationData.getJointData();
1169 const PxU32 linkCount = mArticulationData.getLinkCount();
1170
1171 //make sure all links' spatial zero acceleration impulse are zero
1172 PxMemZero(dest: Z, count: sizeof(Cm::SpatialVectorF) * linkCount);
1173
1174 Z[linkID] = Cm::SpatialVectorF(-impulse.linear, -impulse.angular);
1175
1176 for (PxU32 i = linkID; i; i = links[i].parent)
1177 {
1178 ArticulationLink& tLink = links[i];
1179 //ArticulationLinkData& tLinkDatum = linkData[i];
1180 Z[tLink.parent] = propagateImpulseW(isInvD: mArticulationData.mIsInvDW[i], childToParent: mArticulationData.mLinksData[i].rw,
1181 motionMatrix: mArticulationData.mWorldMotionMatrix[i], Z: Z[i]);
1182 }
1183
1184 //set velocity change of the root link to be zero
1185 Cm::SpatialVectorF deltaV = Cm::SpatialVectorF(PxVec3(0.f), PxVec3(0.f));
1186 if (!fixBase)
1187 deltaV = mArticulationData.mBaseInvSpatialArticulatedInertiaW * (-Z[0]);
1188
1189 for (ArticulationBitField i = links[linkID].pathToRoot - 1; i; i &= (i - 1))
1190 {
1191 //index of child of link h on path to link linkID
1192 const PxU32 index = ArticulationLowestSetBit(val: i);
1193 ArticulationJointCoreData& tJointDatum = jointData[index];
1194
1195 PxReal* jVelocity = &jointVelocities[tJointDatum.jointOffset];
1196
1197 PX_ASSERT(links[index].parent < index);
1198 deltaV = propagateVelocityW(c2p: mArticulationData.mLinksData[index].rw, spatialInertia: mArticulationData.mWorldSpatialArticulatedInertia[index],
1199 invStIs: mArticulationData.mInvStIs[index], motionMatrix: mArticulationData.mWorldMotionMatrix[index], Z: Z[index], jointVelocity: jVelocity, hDeltaV: deltaV);
1200 }
1201
1202 return deltaV;
1203
1204 }
1205
1206
1207 void FeatherstoneArticulation::getCoefficientMatrixWithLoopJoints(ArticulationLoopConstraint* lConstraints, const PxU32 nbConstraints, PxArticulationCache& cache)
1208 {
1209 if (mArticulationData.getDataDirty())
1210 {
1211 Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, messageFmt: "ArticulationHelper::getCoefficientMatrix() commonInit need to be called first to initialize data!");
1212 return;
1213 }
1214
1215 computeArticulatedSpatialInertia(data&: mArticulationData);
1216
1217 const PxU32 linkCount = mArticulationData.getLinkCount();
1218
1219 PxReal* coefficientMatrix = cache.coefficientMatrix;
1220
1221 const PxU32 elementCount = mArticulationData.getDofs();
1222
1223 //zero coefficient matrix
1224 PxMemZero(dest: coefficientMatrix, count: sizeof(PxReal) * elementCount * nbConstraints);
1225
1226 const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
1227
1228 PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);
1229 ScratchData scratchData;
1230 PxU8* tempMemory = allocateScratchSpatialData(allocator, linkCount, scratchData);
1231
1232 Cm::SpatialVectorF* Z = scratchData.spatialZAVectors;
1233 const PxU32 totalDofs = mArticulationData.getDofs();
1234
1235 const PxU32 size = sizeof(PxReal) * totalDofs;
1236
1237 PxU8* tData = reinterpret_cast<PxU8*>(allocator->alloc(requestedSize: size * 2));
1238
1239 const PxReal invDt = 1.f / mArticulationData.getDt();
1240 PxReal* jointVelocities = reinterpret_cast<PxReal*>(tData);
1241 PxReal* jointAccelerations = reinterpret_cast<PxReal*>(tData + size);
1242
1243 for (PxU32 a = 0; a < nbConstraints; ++a)
1244 {
1245 ArticulationLoopConstraint& lConstraint = lConstraints[a];
1246 Constraint* aConstraint = lConstraint.constraint;
1247
1248 Px1DConstraint rows[MAX_CONSTRAINT_ROWS];
1249
1250 PxMemZero(dest: rows, count: sizeof(Px1DConstraint)*MAX_CONSTRAINT_ROWS);
1251
1252 for (PxU32 i = 0; i<MAX_CONSTRAINT_ROWS; i++)
1253 {
1254 Px1DConstraint& c = rows[i];
1255 //Px1DConstraintInit(c);
1256 c.minImpulse = -PX_MAX_REAL;
1257 c.maxImpulse = PX_MAX_REAL;
1258 }
1259
1260 const PxTransform body2World0 = aConstraint->body0 ? aConstraint->bodyCore0->body2World : PxTransform(PxIdentity);
1261 const PxTransform body2World1 = aConstraint->body1 ? aConstraint->bodyCore1->body2World : PxTransform(PxIdentity);
1262
1263 PxVec3 body0WorldOffset(0.f);
1264 PxVec3 ra, rb;
1265 PxConstraintInvMassScale invMassScales;
1266 PxU32 constraintCount = (*aConstraint->solverPrep)(rows,
1267 body0WorldOffset,
1268 MAX_CONSTRAINT_ROWS,
1269 invMassScales,
1270 aConstraint->constantBlock,
1271 body2World0, body2World1, !!(aConstraint->flags & PxConstraintFlag::eENABLE_EXTENDED_LIMITS), ra, rb);
1272
1273 const PxU32 linkIndex0 = lConstraint.linkIndex0;
1274 const PxU32 linkIndex1 = lConstraint.linkIndex1;
1275
1276 //zero joint Velocites
1277 PxMemZero(dest: jointVelocities, count: size);
1278
1279 for (PxU32 j = 0; j < constraintCount; ++j)
1280 {
1281 Px1DConstraint& row = rows[j];
1282
1283 if (linkIndex0 != 0x80000000 && linkIndex1 != 0x80000000)
1284 {
1285 const bool flip = linkIndex0 > linkIndex1;
1286
1287 Cm::SpatialVector impulse0(row.linear0, row.angular0);
1288 Cm::SpatialVector impulse1(row.linear1, row.angular1);
1289
1290 Cm::SpatialVector deltaV0, deltaV1;
1291
1292 if (flip)
1293 {
1294 getImpulseSelfResponseInv(fixBase, linkID0: linkIndex1, linkID1: linkIndex0, Z, impulse0: impulse1, impulse1: impulse0,
1295 deltaV0&: deltaV1, deltaV1&: deltaV0, jointVelocities);
1296 }
1297 else
1298 {
1299 getImpulseSelfResponseInv(fixBase, linkID0: linkIndex0, linkID1: linkIndex1, Z, impulse0, impulse1,
1300 deltaV0, deltaV1, jointVelocities);
1301 }
1302 }
1303 else
1304 {
1305 if (linkIndex0 == 0x80000000)
1306 {
1307 Cm::SpatialVector impulse1(row.linear1, row.angular1);
1308 getImpulseResponseInv(fixBase, linkID: linkIndex1, Z, impulse: impulse1, jointVelocities);
1309 }
1310 else
1311 {
1312 Cm::SpatialVector impulse0(row.linear0, row.angular0);
1313 getImpulseResponseInv(fixBase, linkID: linkIndex0, Z, impulse: impulse0, jointVelocities);
1314 }
1315 }
1316 }
1317
1318 //calculate joint acceleration due to velocity change
1319 for (PxU32 i = 0; i < totalDofs; ++i)
1320 {
1321 jointAccelerations[i] = jointVelocities[i] * invDt;
1322 }
1323
1324 //reset spatial inertia
1325 computeSpatialInertia(data&: mArticulationData);
1326
1327 PxReal* coeCol = &coefficientMatrix[elementCount * a];
1328
1329 //this means the joint force calculated by the inverse dynamic
1330 //will be just influenced by joint acceleration change
1331 scratchData.jointVelocities = NULL;
1332 scratchData.externalAccels = NULL;
1333
1334 //Input
1335 scratchData.jointAccelerations = jointAccelerations;
1336
1337 //a column of the coefficient matrix is the joint force
1338 scratchData.jointForces = coeCol;
1339
1340 if (fixBase)
1341 {
1342 inverseDynamic(data&: mArticulationData, gravity: PxVec3(0.f), scratchData, computeCoriolis: false);
1343 }
1344 else
1345 {
1346 inverseDynamicFloatingBase(data&: mArticulationData, gravity: PxVec3(0.f), scratchData, computeCoriolis: false);
1347 }
1348
1349 allocator->free(addr: tData);
1350 allocator->free(addr: tempMemory);
1351 }
1352 }
1353
1354 void FeatherstoneArticulation::constraintPrep(ArticulationLoopConstraint* lConstraints,
1355 const PxU32 nbJoints, Cm::SpatialVectorF* Z, PxSolverConstraintPrepDesc& prepDesc,
1356 PxSolverBody& sBody, PxSolverBodyData& sBodyData, PxSolverConstraintDesc* descs,
1357 PxConstraintAllocator& allocator)
1358 {
1359 const PxReal dt = mArticulationData.getDt();
1360 const PxReal invDt = 1.f / dt;
1361 //constraint prep
1362 for (PxU32 a = 0; a < nbJoints; ++a)
1363 {
1364 ArticulationLoopConstraint& lConstraint = lConstraints[a];
1365 Constraint* aConstraint = lConstraint.constraint;
1366
1367 PxSolverConstraintDesc& desc = descs[a];
1368 prepDesc.desc = &desc;
1369 prepDesc.linBreakForce = aConstraint->linBreakForce;
1370 prepDesc.angBreakForce = aConstraint->angBreakForce;
1371 prepDesc.writeback = &mContext->getConstraintWriteBackPool()[aConstraint->index];
1372 prepDesc.disablePreprocessing = !!(aConstraint->flags & PxConstraintFlag::eDISABLE_PREPROCESSING);
1373 prepDesc.improvedSlerp = !!(aConstraint->flags & PxConstraintFlag::eIMPROVED_SLERP);
1374 prepDesc.driveLimitsAreForces = !!(aConstraint->flags & PxConstraintFlag::eDRIVE_LIMITS_ARE_FORCES);
1375 prepDesc.extendedLimits = !!(aConstraint->flags & PxConstraintFlag::eENABLE_EXTENDED_LIMITS);
1376 prepDesc.minResponseThreshold = aConstraint->minResponseThreshold;
1377
1378 Px1DConstraint rows[MAX_CONSTRAINT_ROWS];
1379
1380 PxMemZero(dest: rows, count: sizeof(Px1DConstraint)*MAX_CONSTRAINT_ROWS);
1381
1382 for (PxU32 i = 0; i < MAX_CONSTRAINT_ROWS; i++)
1383 {
1384 Px1DConstraint& c = rows[i];
1385 //Px1DConstraintInit(c);
1386 c.minImpulse = -PX_MAX_REAL;
1387 c.maxImpulse = PX_MAX_REAL;
1388 }
1389
1390 prepDesc.invMassScales.linear0 = prepDesc.invMassScales.linear1 = prepDesc.invMassScales.angular0 = prepDesc.invMassScales.angular1 = 1.f;
1391
1392 const PxTransform body2World0 = aConstraint->body0 ? aConstraint->bodyCore0->body2World : PxTransform(PxIdentity);
1393 const PxTransform body2World1 = aConstraint->body1 ? aConstraint->bodyCore1->body2World : PxTransform(PxIdentity);
1394
1395 PxVec3 body0WorldOffset(0.f);
1396 PxVec3 ra, rb;
1397 PxConstraintInvMassScale invMassScales;
1398 PxU32 constraintCount = (*aConstraint->solverPrep)(rows,
1399 body0WorldOffset,
1400 MAX_CONSTRAINT_ROWS,
1401 invMassScales,
1402 aConstraint->constantBlock,
1403 body2World0, body2World1, !!(aConstraint->flags & PxConstraintFlag::eENABLE_EXTENDED_LIMITS),
1404 ra, rb);
1405
1406 prepDesc.body0WorldOffset = body0WorldOffset;
1407 prepDesc.bodyFrame0 = body2World0;
1408 prepDesc.bodyFrame1 = body2World1;
1409 prepDesc.numRows = constraintCount;
1410 prepDesc.rows = rows;
1411
1412 const PxU32 linkIndex0 = lConstraint.linkIndex0;
1413 const PxU32 linkIndex1 = lConstraint.linkIndex1;
1414
1415 if (linkIndex0 != 0x80000000 && linkIndex1 != 0x80000000)
1416 {
1417 desc.articulationA = this;
1418 desc.articulationB = this;
1419 desc.linkIndexA = PxU16(linkIndex0);
1420 desc.linkIndexB = PxU16(linkIndex1);
1421
1422 desc.bodyA = reinterpret_cast<PxSolverBody*>(this);
1423 desc.bodyB = reinterpret_cast<PxSolverBody*>(this);
1424
1425 prepDesc.bodyState0 = PxSolverConstraintPrepDescBase::eARTICULATION;
1426 prepDesc.bodyState1 = PxSolverConstraintPrepDescBase::eARTICULATION;
1427
1428 }
1429 else if (linkIndex0 == 0x80000000)
1430 {
1431 desc.articulationA = NULL;
1432 desc.articulationB = this;
1433
1434 desc.linkIndexA = 0xffff;
1435 desc.linkIndexB = PxU16(linkIndex1);
1436
1437 desc.bodyA = &sBody;
1438 desc.bodyB = reinterpret_cast<PxSolverBody*>(this);
1439
1440 prepDesc.bodyState0 = PxSolverConstraintPrepDescBase::eSTATIC_BODY;
1441 prepDesc.bodyState1 = PxSolverConstraintPrepDescBase::eARTICULATION;
1442 }
1443 else if (linkIndex1 == 0x80000000)
1444 {
1445 desc.articulationA = this;
1446 desc.articulationB = NULL;
1447
1448 desc.linkIndexA = PxU16(linkIndex0);
1449 desc.linkIndexB = 0xffff;
1450
1451 desc.bodyA = reinterpret_cast<PxSolverBody*>(this);
1452 desc.bodyB = &sBody;
1453
1454 prepDesc.bodyState0 = PxSolverConstraintPrepDescBase::eARTICULATION;
1455 prepDesc.bodyState1 = PxSolverConstraintPrepDescBase::eSTATIC_BODY;
1456
1457 }
1458
1459 prepDesc.body0 = desc.bodyA;
1460 prepDesc.body1 = desc.bodyB;
1461 prepDesc.data0 = &sBodyData;
1462 prepDesc.data1 = &sBodyData;
1463
1464 ConstraintHelper::setupSolverConstraint(prepDesc, allocator, dt, invdt: invDt, Z);
1465 }
1466
1467 }
1468
1469 class BlockBasedAllocator
1470 {
1471 struct AllocationPage
1472 {
1473 static const PxU32 PageSize = 32 * 1024;
1474 PxU8 mPage[PageSize];
1475
1476 PxU32 currentIndex;
1477
1478 AllocationPage() : currentIndex(0) {}
1479
1480 PxU8* allocate(const PxU32 size)
1481 {
1482 PxU32 alignedSize = (size + 15)&(~15);
1483 if ((currentIndex + alignedSize) < PageSize)
1484 {
1485 PxU8* ret = &mPage[currentIndex];
1486 currentIndex += alignedSize;
1487 return ret;
1488 }
1489 return NULL;
1490 }
1491 };
1492
1493 AllocationPage* currentPage;
1494
1495 physx::shdfnd::Array<AllocationPage*> mAllocatedBlocks;
1496 PxU32 mCurrentIndex;
1497
1498 public:
1499 BlockBasedAllocator() : currentPage(NULL), mCurrentIndex(0)
1500 {
1501 }
1502
1503 virtual PxU8* allocate(const PxU32 byteSize)
1504 {
1505 if (currentPage)
1506 {
1507 PxU8* data = currentPage->allocate(size: byteSize);
1508 if (data)
1509 return data;
1510 }
1511
1512 if (mCurrentIndex < mAllocatedBlocks.size())
1513 {
1514 currentPage = mAllocatedBlocks[mCurrentIndex++];
1515 currentPage->currentIndex = 0;
1516 return currentPage->allocate(size: byteSize);
1517 }
1518 currentPage = PX_PLACEMENT_NEW(PX_ALLOC(sizeof(AllocationPage), PX_DEBUG_EXP("AllocationPage")), AllocationPage)();
1519 mAllocatedBlocks.pushBack(a: currentPage);
1520 mCurrentIndex = mAllocatedBlocks.size();
1521
1522 return currentPage->allocate(size: byteSize);
1523 }
1524
1525 void release() { for (PxU32 a = 0; a < mAllocatedBlocks.size(); ++a) PX_FREE(mAllocatedBlocks[a]); mAllocatedBlocks.clear(); currentPage = NULL; mCurrentIndex = 0; }
1526
1527 void reset() { currentPage = NULL; mCurrentIndex = 0; }
1528
1529 virtual ~BlockBasedAllocator()
1530 {
1531 release();
1532 }
1533 };
1534
1535 class ArticulationBlockAllocator : public PxConstraintAllocator
1536 {
1537 BlockBasedAllocator mConstraintAllocator;
1538 BlockBasedAllocator mFrictionAllocator[2];
1539
1540 PxU32 currIdx;
1541
1542 public:
1543
1544 ArticulationBlockAllocator() : currIdx(0)
1545 {
1546 }
1547
1548 virtual ~ArticulationBlockAllocator() {}
1549
1550 virtual PxU8* reserveConstraintData(const PxU32 size)
1551 {
1552 return reinterpret_cast<PxU8*>(mConstraintAllocator.allocate(byteSize: size));
1553 }
1554
1555 virtual PxU8* reserveFrictionData(const PxU32 byteSize)
1556 {
1557 return reinterpret_cast<PxU8*>(mFrictionAllocator[currIdx].allocate(byteSize));
1558 }
1559
1560 void release() { currIdx = 1 - currIdx; mConstraintAllocator.release(); mFrictionAllocator[currIdx].release(); }
1561
1562 PX_NOCOPY(ArticulationBlockAllocator)
1563
1564 };
1565
1566 void solveExt1D(const PxSolverConstraintDesc& desc, SolverContext& cache);
1567 void writeBack1D(const PxSolverConstraintDesc& desc, SolverContext&, PxSolverBodyData&, PxSolverBodyData&);
1568 void conclude1D(const PxSolverConstraintDesc& desc, SolverContext& /*cache*/);
1569 void clearExt1D(const PxSolverConstraintDesc& desc, SolverContext& cache);
1570
1571 bool FeatherstoneArticulation::getLambda(ArticulationLoopConstraint* lConstraints, const PxU32 nbJoints,
1572 PxArticulationCache& cache, PxArticulationCache& initialState,
1573 const PxReal* jointTorque, const PxVec3& gravity, const PxU32 maxIter)
1574 {
1575 const PxReal dt = mArticulationData.getDt();
1576 const PxReal invDt = 1.f / dt;
1577 const PxU32 totalDofs = mArticulationData.getDofs();
1578
1579 const PxU32 linkCount = mArticulationData.getLinkCount();
1580
1581 ArticulationBlockAllocator bAlloc;
1582
1583 PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);
1584
1585 Cm::SpatialVectorF* Z = reinterpret_cast<Cm::SpatialVectorF*>(allocator->alloc(requestedSize: sizeof(Cm::SpatialVectorF) * linkCount, fallBackToHeap: true));
1586 Cm::SpatialVectorF* deltaV = reinterpret_cast<Cm::SpatialVectorF*>(allocator->alloc(requestedSize: sizeof(Cm::SpatialVectorF) * linkCount, fallBackToHeap: true));
1587
1588 PxReal* prevoiusLambdas =reinterpret_cast<PxReal*>(allocator->alloc(requestedSize: sizeof(PxReal)*nbJoints * 2, fallBackToHeap: true));
1589 PxReal* lambdas = cache.lambda;
1590
1591 //this is the joint force changed caused by contact force based on impulse strength is 1
1592 PxReal* J = cache.coefficientMatrix;
1593
1594 PxSolverBody staticSolverBody;
1595 PxMemZero(dest: &staticSolverBody, count: sizeof(PxSolverBody));
1596 PxSolverBodyData staticSolverBodyData;
1597 PxMemZero(dest: &staticSolverBodyData, count: sizeof(PxSolverBodyData));
1598 staticSolverBodyData.maxContactImpulse = PX_MAX_F32;
1599 staticSolverBodyData.penBiasClamp = -PX_MAX_F32;
1600 staticSolverBodyData.body2World = PxTransform(PxIdentity);
1601
1602 Dy::SolverContext context;
1603 context.Z = Z;
1604 context.deltaV = deltaV;
1605 context.doFriction = false;
1606
1607
1608 PxSolverConstraintDesc* desc = reinterpret_cast<PxSolverConstraintDesc*>(allocator->alloc(requestedSize: sizeof(PxSolverConstraintDesc) * nbJoints, fallBackToHeap: true));
1609 ArticulationSolverDesc artiDesc;
1610
1611 PxSolverConstraintDesc* constraintDescs = reinterpret_cast<PxSolverConstraintDesc*>(allocator->alloc(requestedSize: sizeof(PxSolverConstraintDesc) * mArticulationData.getLinkCount()-1, fallBackToHeap: true));
1612
1613 //run forward dynamic to calculate the lamba
1614
1615 artiDesc.articulation = this;
1616 PxU32 acCount = 0;
1617 computeUnconstrainedVelocities(desc: artiDesc, dt, allocator&: bAlloc, constraintDesc: constraintDescs, acCount,
1618 gravity, contextID: 0, Z, deltaV);
1619
1620 ScratchData scratchData;
1621 scratchData.motionVelocities = mArticulationData.getMotionVelocities();
1622 scratchData.motionAccelerations = mArticulationData.getMotionAccelerations();
1623 scratchData.coriolisVectors = mArticulationData.getCorioliseVectors();
1624 scratchData.spatialZAVectors = mArticulationData.getSpatialZAVectors();
1625 scratchData.jointAccelerations = mArticulationData.getJointAccelerations();
1626 scratchData.jointVelocities = mArticulationData.getJointVelocities();
1627 scratchData.jointPositions = mArticulationData.getJointPositions();
1628 scratchData.jointForces = mArticulationData.getJointForces();
1629 scratchData.externalAccels = mArticulationData.getExternalAccelerations();
1630
1631 //prepare constraint data
1632 PxSolverConstraintPrepDesc prepDesc;
1633 constraintPrep(lConstraints, nbJoints, Z, prepDesc, sBody&: staticSolverBody,
1634 sBodyData&: staticSolverBodyData, descs: desc, allocator&: bAlloc);
1635
1636 for (PxU32 i = 0; i < nbJoints; ++i)
1637 {
1638 prevoiusLambdas[i] = PX_MAX_F32;
1639 }
1640
1641 bool found = true;
1642
1643 for (PxU32 iter = 0; iter < maxIter; ++iter)
1644 {
1645 found = true;
1646 for (PxU32 i = 0; i < nbJoints; ++i)
1647 {
1648 clearExt1D(desc: desc[i], cache&: context);
1649 }
1650
1651 //solve
1652 for (PxU32 itr = 0; itr < 4; itr++)
1653 {
1654 for (PxU32 i = 0; i < nbJoints; ++i)
1655 {
1656 solveExt1D(desc: desc[i], cache&: context);
1657 }
1658 }
1659 for (PxU32 i = 0; i < nbJoints; ++i)
1660 {
1661 conclude1D(desc: desc[i], context);
1662 }
1663
1664 PxcFsFlushVelocity(articulation&: *this, deltaV);
1665
1666 for (PxU32 i = 0; i < nbJoints; ++i)
1667 {
1668 solveExt1D(desc: desc[i], cache&: context);
1669 writeBack1D(desc: desc[i], context, staticSolverBodyData, staticSolverBodyData);
1670 }
1671
1672 PxReal eps = 1e-5f;
1673 for (PxU32 i = 0; i < nbJoints; ++i)
1674 {
1675 Dy::Constraint* constraint = lConstraints->constraint;
1676
1677 Dy::ConstraintWriteback& solverOutput = mContext->getConstraintWriteBackPool()[constraint->index];
1678 PxVec3 linearForce = solverOutput.linearImpulse * invDt;
1679
1680 //linear force is normalize so lambda is the magnitude of linear force
1681 lambdas[i] = linearForce.magnitude() * dt;
1682
1683 const PxReal dif = PxAbs(a: prevoiusLambdas[i] - lambdas[i]);
1684 if (dif > eps)
1685 found = false;
1686
1687 prevoiusLambdas[i] = lambdas[i];
1688 }
1689
1690 if (found)
1691 break;
1692
1693 //joint force
1694 PxReal* jf3 = cache.jointForce;
1695
1696 //zero the joint force buffer
1697 PxMemZero(dest: jf3, count: sizeof(PxReal)*totalDofs);
1698
1699 for (PxU32 colInd = 0; colInd < nbJoints; ++colInd)
1700 {
1701 PxReal* col = &J[colInd * totalDofs];
1702
1703 for (PxU32 j = 0; j < totalDofs; ++j)
1704 {
1705 jf3[j] += col[j] * lambdas[colInd];
1706 }
1707 }
1708
1709 //jointTorque is M(q)*qddot + C(q,qdot)t - g(q)
1710 //jointTorque - J*lambda.
1711 for (PxU32 j = 0; j < totalDofs; ++j)
1712 {
1713 jf3[j] = jointTorque[j] - jf3[j];
1714 }
1715
1716 //reset all joint velocities/
1717 applyCache(cache&: initialState, flag: PxArticulationCache::eALL);
1718
1719 //copy constraint torque to internal data
1720 applyCache(cache, flag: PxArticulationCache::eFORCE);
1721
1722 mArticulationData.init();
1723
1724 computeLinkVelocities(data&: mArticulationData, scratchData);
1725 computeZ(data&: mArticulationData, gravity, scratchData);
1726 computeArticulatedSpatialZ(data&: mArticulationData, scratchData);
1727 computeLinkAcceleration(data&: mArticulationData, scratchData);
1728
1729 //zero zero acceleration vector in the articulation data so that we can use this buffer to accumulated
1730 //impulse for the contacts/constraints in the PGS/TGS solvers
1731 PxMemZero(dest: mArticulationData.getSpatialZAVectors(), count: sizeof(Cm::SpatialVectorF) * linkCount);
1732 }
1733
1734 allocator->free(addr: constraintDescs);
1735 allocator->free(addr: prevoiusLambdas);
1736 allocator->free(addr: Z);
1737 allocator->free(addr: deltaV);
1738 allocator->free(addr: desc);
1739 bAlloc.release();
1740
1741 //roll back to the current stage
1742 applyCache(cache&: initialState, flag: PxArticulationCache::eALL);
1743
1744 return found;
1745
1746 }
1747
1748 //i is the current link ID, we need to compute the row/column related to the joint i with all the other joints
1749 PxU32 computeHi(ArticulationData& data, const PxU32 linkID, PxReal* massMatrix, Cm::SpatialVectorF* f)
1750 {
1751 ArticulationLink* links = data.getLinks();
1752
1753 ArticulationJointCoreData& jointDatum = data.getJointData(index: linkID);
1754
1755 const PxU32 totalDofs = data.getDofs();
1756
1757 //Hii
1758 for (PxU32 ind = 0; ind < jointDatum.dof; ++ind)
1759 {
1760 const PxU32 row = (jointDatum.jointOffset + ind)* totalDofs;
1761 const Cm::SpatialVectorF& tf = f[ind];
1762 for (PxU32 ind2 = 0; ind2 < jointDatum.dof; ++ind2)
1763 {
1764 const PxU32 col = jointDatum.jointOffset + ind2;
1765 const Cm::UnAlignedSpatialVector& sa = data.getWorldMotionMatrix(linkID)[ind2];
1766 massMatrix[row + col] = sa.innerProduct(v: tf);
1767 }
1768 }
1769
1770 PxU32 j = linkID;
1771
1772 ArticulationLink* jLink = &links[j];
1773 while (jLink->parent != 0)
1774 {
1775 for (PxU32 ind = 0; ind < jointDatum.dof; ++ind)
1776 {
1777 //f[ind] = data.getChildToParent(j) * f[ind];
1778 f[ind] = FeatherstoneArticulation::translateSpatialVector(offset: data.getLinkData(index: j).rw, vec: f[ind]);
1779 }
1780
1781 //assign j to the parent link
1782 j = jLink->parent;
1783 jLink = &links[j];
1784
1785 //Hij
1786 ArticulationJointCoreData& pJointDatum = data.getJointData(index: j);
1787
1788 for (PxU32 ind = 0; ind < pJointDatum.dof; ++ind)
1789 {
1790 const Cm::UnAlignedSpatialVector& sa = data.getWorldMotionMatrix(linkID: j)[ind];
1791 const PxU32 col = pJointDatum.jointOffset + ind;
1792
1793 for (PxU32 ind2 = 0; ind2 < jointDatum.dof; ++ind2)
1794 {
1795 const PxU32 row = (jointDatum.jointOffset + ind2)* totalDofs;
1796
1797 Cm::SpatialVectorF& fcol = f[ind2];
1798
1799 massMatrix[row + col] = sa.innerProduct(v: fcol);
1800 }
1801 }
1802
1803 //Hji = transpose(Hij)
1804 {
1805 for (PxU32 ind = 0; ind < pJointDatum.dof; ++ind)
1806 {
1807 const PxU32 pRow = (pJointDatum.jointOffset + ind)* totalDofs;
1808 const PxU32 col = pJointDatum.jointOffset + ind;
1809
1810 for (PxU32 ind2 = 0; ind2 < jointDatum.dof; ++ind2)
1811 {
1812 const PxU32 pCol = jointDatum.jointOffset + ind2;
1813 const PxU32 row = (jointDatum.jointOffset + ind2) * totalDofs;
1814
1815 massMatrix[pRow + pCol] = massMatrix[row + col];
1816 }
1817 }
1818 }
1819
1820 }
1821 return j;
1822 }
1823
1824 void FeatherstoneArticulation::calculateHFixBase(PxArticulationCache& cache)
1825 {
1826 const PxU32 elementCount = mArticulationData.getDofs();
1827
1828 PxReal* massMatrix = cache.massMatrix;
1829
1830 PxMemZero(dest: massMatrix, count: sizeof(PxReal) * elementCount * elementCount);
1831
1832 const PxU32 linkCount = mArticulationData.getLinkCount();
1833
1834 PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);
1835
1836 ArticulationLink* links = mArticulationData.getLinks();
1837
1838 const PxU32 startIndex = PxU32(linkCount - 1);
1839
1840 Dy::SpatialMatrix* compositeSpatialInertia = reinterpret_cast<Dy::SpatialMatrix*>(allocator->alloc(requestedSize: sizeof(Dy::SpatialMatrix) * linkCount));
1841
1842 //initialize composite spatial inertial
1843 initCompositeSpatialInertia(data&: mArticulationData, compositeSpatialInertia);
1844
1845 Cm::SpatialVectorF F[6];
1846 for (PxU32 i = startIndex; i > 0; --i)
1847 {
1848 ArticulationLink& link = links[i];
1849
1850 Dy::SpatialMatrix cSpatialInertia = compositeSpatialInertia[i];
1851 //transform current link's spatial inertia to parent's space
1852 FeatherstoneArticulation::translateInertia(offset: FeatherstoneArticulation::constructSkewSymmetricMatrix(r: mArticulationData.mLinksData[i].rw), inertia&: cSpatialInertia);
1853
1854 //compute parent's composite spatial inertia
1855 compositeSpatialInertia[link.parent] += cSpatialInertia;
1856
1857 Dy::SpatialMatrix& tSpatialInertia = compositeSpatialInertia[i];
1858
1859 ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(index: i);
1860
1861 for (PxU32 ind = 0; ind < jointDatum.dof; ++ind)
1862 {
1863 Cm::UnAlignedSpatialVector& sa = mArticulationData.mWorldMotionMatrix[i][ind];
1864 Cm::UnAlignedSpatialVector tmp = tSpatialInertia* sa;
1865 F[ind].top = tmp.top;
1866 F[ind].bottom = tmp.bottom;
1867 }
1868
1869 //Hii, Hij, Hji
1870 computeHi(data&: mArticulationData, linkID: i, massMatrix, f: F);
1871 }
1872
1873 allocator->free(addr: compositeSpatialInertia);
1874 }
1875
1876
1877 void FeatherstoneArticulation::calculateHFloatingBase(PxArticulationCache& cache)
1878 {
1879 const PxU32 elementCount = mArticulationData.getDofs();
1880
1881 PxReal* massMatrix = cache.massMatrix;
1882
1883 PxMemZero(dest: massMatrix, count: sizeof(PxReal) * elementCount * elementCount);
1884
1885 const PxU32 linkCount = mArticulationData.getLinkCount();
1886
1887 PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);
1888
1889 ArticulationLink* links = mArticulationData.getLinks();
1890 ArticulationLinkData* linkData = mArticulationData.getLinkData();
1891
1892 const PxU32 startIndex = PxU32(linkCount - 1);
1893
1894 Dy::SpatialMatrix* compositeSpatialInertia = reinterpret_cast<Dy::SpatialMatrix*>(allocator->alloc(requestedSize: sizeof(Dy::SpatialMatrix) * linkCount));
1895 Cm::SpatialVectorF* F = reinterpret_cast<Cm::SpatialVectorF*>(allocator->alloc(requestedSize: sizeof(Cm::SpatialVectorF) * elementCount));
1896
1897 //initialize composite spatial inertial
1898 initCompositeSpatialInertia(data&: mArticulationData, compositeSpatialInertia);
1899
1900 for (PxU32 i = startIndex; i > 0; --i)
1901 {
1902 ArticulationLink& link = links[i];
1903
1904 Dy::SpatialMatrix cSpatialInertia = compositeSpatialInertia[i];
1905 //transform current link's spatial inertia to parent's space
1906 FeatherstoneArticulation::translateInertia(offset: FeatherstoneArticulation::constructSkewSymmetricMatrix(r: mArticulationData.mLinksData[i].rw), inertia&: cSpatialInertia);
1907
1908 //compute parent's composite spatial inertia
1909 compositeSpatialInertia[link.parent] += cSpatialInertia;
1910
1911 Dy::SpatialMatrix& tSpatialInertia = compositeSpatialInertia[i];
1912
1913 ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(index: i);
1914
1915 Cm::SpatialVectorF* f = &F[jointDatum.jointOffset];
1916
1917 for (PxU32 ind = 0; ind < jointDatum.dof; ++ind)
1918 {
1919 Cm::UnAlignedSpatialVector& sa = mArticulationData.mWorldMotionMatrix[i][ind];
1920 Cm::UnAlignedSpatialVector tmp = tSpatialInertia* sa;
1921 f[ind].top = tmp.top;
1922 f[ind].bottom = tmp.bottom;
1923 }
1924
1925 //Hii, Hij, Hji
1926 const PxU32 j = computeHi(data&: mArticulationData, linkID: i, massMatrix, f);
1927
1928 //transform F to the base link space
1929 ArticulationLinkData& fDatum = linkData[j];
1930
1931 for (PxU32 ind = 0; ind < jointDatum.dof; ++ind)
1932 {
1933 f[ind] = translateSpatialVector(offset: fDatum.childToBase, vec: f[ind]);
1934 }
1935 }
1936
1937 //Ib = base link composite inertia tensor
1938 //compute transpose(F) * inv(Ib) *F
1939 Dy::SpatialMatrix invI0 = compositeSpatialInertia[0].invertInertia();
1940
1941 //H - transpose(F) * inv(Ib) * F;
1942 for (PxU32 row = 0; row < elementCount; ++row)
1943 {
1944 const Cm::SpatialVectorF& f = F[row];
1945 for (PxU32 col = 0; col < elementCount; ++col)
1946 {
1947 const Cm::SpatialVectorF invIf = invI0 * F[col];
1948 const PxReal v = f.innerProduct(v: invIf);
1949 const PxU32 index = row * elementCount + col;
1950 massMatrix[index] = massMatrix[index] - v;
1951 }
1952 }
1953
1954 allocator->free(addr: compositeSpatialInertia);
1955 allocator->free(addr: F);
1956
1957 }
1958
1959 //calcualte a single column of H, jointForce is equal to a single column of H
1960 void FeatherstoneArticulation::calculateMassMatrixColInv(ScratchData& scratchData)
1961 {
1962 const PxU32 linkCount = mArticulationData.getLinkCount();
1963
1964 Cm::SpatialVectorF* motionAccelerations = scratchData.motionAccelerations;
1965 Cm::SpatialVectorF* spatialZAForces = scratchData.spatialZAVectors;
1966
1967 //Input
1968 PxReal* jointAccelerations = scratchData.jointAccelerations;
1969
1970 //set base link motion acceleration to be zero because H should
1971 //be just affected by joint position/link position
1972 motionAccelerations[0] = Cm::SpatialVectorF::Zero();
1973 spatialZAForces[0] = Cm::SpatialVectorF::Zero();
1974
1975 for (PxU32 linkID = 1; linkID < linkCount; ++linkID)
1976 {
1977 ArticulationLink& link = mArticulationData.getLink(index: linkID);
1978 ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(index: linkID);
1979
1980 //parent motion accelerations into child space
1981 Cm::SpatialVectorF accel = translateSpatialVector(offset: -mArticulationData.getLinkData(index: linkID).rw, vec: motionAccelerations[link.parent]);
1982
1983 const PxReal* jAcceleration = &jointAccelerations[jointDatum.jointOffset];
1984
1985 for (PxU32 ind = 0; ind < jointDatum.dof; ++ind)
1986 {
1987 accel.top += mArticulationData.mWorldMotionMatrix[linkID][ind].top * jAcceleration[ind];
1988 accel.bottom += mArticulationData.mWorldMotionMatrix[linkID][ind].bottom * jAcceleration[ind];
1989 }
1990
1991 motionAccelerations[linkID] = accel;
1992
1993 spatialZAForces[linkID] = mArticulationData.mWorldSpatialArticulatedInertia[linkID] * accel;
1994 }
1995
1996 computeGeneralizedForceInv(data&: mArticulationData, scratchData);
1997
1998 }
1999
2000 void FeatherstoneArticulation::getGeneralizedMassMatrixCRB(PxArticulationCache& cache)
2001 {
2002 if (mArticulationData.getDataDirty())
2003 {
2004 Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, messageFmt: "ArticulationHelper::getGeneralizedMassMatrix() commonInit need to be called first to initialize data!");
2005 return;
2006 }
2007
2008 const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
2009 if (fixBase)
2010 {
2011 calculateHFixBase(cache);
2012 }
2013 else
2014 {
2015 calculateHFloatingBase(cache);
2016 }
2017
2018 }
2019
2020 void FeatherstoneArticulation::getGeneralizedMassMatrix( PxArticulationCache& cache)
2021 {
2022 if (mArticulationData.getDataDirty())
2023 {
2024 Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, messageFmt: "ArticulationHelper::getGeneralizedMassMatrix() commonInit need to be called first to initialize data!");
2025 return;
2026 }
2027
2028
2029 //calculate each column for mass matrix
2030 PxReal* massMatrix = cache.massMatrix;
2031
2032 const PxU32 linkCount = mArticulationData.getLinkCount();
2033
2034 const PxU32 elementCount = mArticulationData.getDofs();
2035
2036 const PxU32 size = sizeof(PxReal) * elementCount;
2037 PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);
2038
2039 ScratchData scratchData;
2040 PxU8* tempMemory = allocateScratchSpatialData(allocator, linkCount, scratchData);
2041
2042 PxReal* jointAccelerations = reinterpret_cast<PxReal*>(allocator->alloc(requestedSize: size));
2043
2044 scratchData.jointAccelerations = jointAccelerations;
2045 scratchData.jointVelocities = NULL;
2046 scratchData.externalAccels = NULL;
2047
2048 const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
2049
2050 //initialize jointAcceleration to be zero
2051 PxMemZero(dest: jointAccelerations, count: size);
2052
2053 for (PxU32 colInd = 0; colInd < elementCount; ++colInd)
2054 {
2055 PxReal* col = &massMatrix[colInd * elementCount];
2056
2057 scratchData.jointForces = col;
2058
2059 //set joint acceleration 1 in the col + 1 and zero elsewhere
2060 jointAccelerations[colInd] = 1;
2061
2062 if (fixBase)
2063 {
2064 //jointAcceleration is Q, HQ = ID(model, qdot, Q).
2065 calculateMassMatrixColInv(scratchData);
2066 }
2067 else
2068 {
2069 inverseDynamicFloatingBase(data&: mArticulationData, gravity: PxVec3(0.f), scratchData, computeCoriolis: false);
2070 }
2071
2072 //reset joint acceleration to be zero
2073 jointAccelerations[colInd] = 0;
2074 }
2075
2076 allocator->free(addr: jointAccelerations);
2077 allocator->free(addr: tempMemory);
2078 }
2079
2080
2081
2082} //namespace Dy
2083
2084}
2085

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