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#ifndef PXD_FEATHERSTONE_ARTICULATION_H
32#define PXD_FEATHERSTONE_ARTICULATION_H
33
34#include "foundation/PxVec3.h"
35#include "foundation/PxQuat.h"
36#include "foundation/PxTransform.h"
37#include "PsVecMath.h"
38#include "CmUtils.h"
39#include "PxArticulationJoint.h"
40#include "DyVArticulation.h"
41#include "DyFeatherstoneArticulationUtils.h"
42#include "DyFeatherstoneArticulationJointData.h"
43#include "solver/PxSolverDefs.h"
44
45#ifndef FEATHERSTONE_DEBUG
46#define FEATHERSTONE_DEBUG 0
47#endif
48
49namespace physx
50{
51
52class PxContactJoint;
53//struct PxSolverConstraintDesc;
54class PxcConstraintBlockStream;
55class PxcScratchAllocator;
56class PxsConstraintBlockManager;
57struct SolverConstraint1DExtStep;
58struct PxSolverConstraintPrepDesc;
59struct PxSolverBody;
60struct PxSolverBodyData;
61class PxConstraintAllocator;
62class PxsContactManagerOutputIterator;
63
64struct PxSolverConstraintDesc;
65
66namespace Dy
67{
68//#if PX_VC
69//#pragma warning(push)
70//#pragma warning( disable : 4324 ) // Padding was added at the end of a structure because of a __declspec(align) value.
71//#endif
72
73
74 //class ArticulationJointCoreData;
75 class ArticulationLinkData;
76 struct SpatialSubspaceMatrix;
77 struct SolverConstraint1DExt;
78 struct SolverConstraint1DStep;
79
80 class FeatherstoneArticulation;
81 struct SpatialMatrix;
82 struct SpatialTransform;
83 struct Constraint;
84 class ThreadContext;
85
86/* struct DyScratchAllocator
87 {
88 char* base;
89 size_t size;
90 size_t taken;
91
92 DyScratchAllocator(char* p, size_t s) : base(p), size(s), taken(0) {}
93
94 PxU32 round16(PxU32 size_) { return (size_ + 15)&(~15); }
95
96 template<class T> T* alloc(PxU32 count)
97 {
98 const PxU32 sizeReq = round16(sizeof(T)*count);
99 PX_ASSERT((taken+ sizeReq) < size);
100 T* result = reinterpret_cast<T*>(base + taken);
101 taken += sizeReq;
102 return result;
103 }
104 };*/
105
106 //This stuct is used in TGS
107/* class ArticulationTempData
108 {
109 public:
110 Cm::SpatialVectorF mBaseLinkMotionVelocite;
111 Ps::Array<Cm::SpatialVectorF> mZAForce;
112 Ps::Array<PxReal> mJointPosition; // joint position
113 Ps::Array<PxReal> mJointVelocity; // joint velocity
114 Ps::Array<PxTransform> mLinkTransform; // this is the transform list for links
115 };*/
116
117 struct ArticulationInternalConstraint
118 {
119 //Common/shared directional info between, frictions and drives
120 Cm::UnAlignedSpatialVector row0; //24 24
121 Cm::UnAlignedSpatialVector row1; //24 48
122 Cm::UnAlignedSpatialVector deltaVA; //24 72
123 Cm::UnAlignedSpatialVector deltaVB; //24 96
124 //Response information
125 PxReal recipResponse; //4 100
126 PxReal response; //4 104
127 //Joint limit values
128 PxReal lowLimit; //4 108
129 PxReal highLimit; //4 112
130 PxReal lowImpulse; //4 116 changed
131 PxReal highImpulse; //4 120 changed
132 PxReal erp; //4 124
133 //Joint spring drive info
134 PxReal driveTargetVel; //4 128
135 PxReal driveBiasCoefficient; //4 132
136 PxReal driveTarget; //4 136
137 PxReal driveVelMultiplier; //4 140
138 PxReal driveBackPropagateScale; //4 144
139 PxReal driveImpulseMultiplier; //4 148
140 PxReal maxDriveForce; //4 152
141 PxReal driveForce; //4 156
142
143 PxReal maxFrictionForce; //4 160
144 PxReal frictionForce; //4 164
145 PxReal frictionForceCoefficient; //4 168
146
147 bool isLinearConstraint; //1 169
148 PxU8 padding[7]; //11 176
149 };
150
151 struct ArticulationInternalLockedAxis
152 {
153 //How much an impulse will effect angular velocity
154 Cm::UnAlignedSpatialVector deltaVA; //24 24
155 Cm::UnAlignedSpatialVector deltaVB; //24 48
156 //Jacobian axis that is locked
157 PxVec3 axis; //12 60
158 //Response information
159 PxReal recipResponse; //4 64
160 //Initial error
161 PxReal error; //4 68
162 //Bias scale
163 PxReal biasScale; //4 72
164
165 PxU32 pad[2]; //4 80
166 };
167
168 class ArticulationData
169 {
170 public:
171
172 ArticulationData() :
173 mLinksData(NULL), mJointData(NULL), mJointTranData(NULL),
174 mDt(0.f), mDofs(0xffffffff), mLocks(0), mDataDirty(true)
175 {
176 mRootPreMotionVelocity = Cm::SpatialVectorF::Zero();
177 }
178
179 ~ArticulationData();
180
181 PX_FORCE_INLINE void init();
182 void resizeLinkData(const PxU32 linkCount);
183 void resizeJointData(const PxU32 dofs);
184
185 PX_FORCE_INLINE PxReal* getJointAccelerations() { return mJointAcceleration.begin(); }
186 PX_FORCE_INLINE const PxReal* getJointAccelerations() const { return mJointAcceleration.begin(); }
187 PX_FORCE_INLINE PxReal* getJointVelocities() { return mJointVelocity.begin(); }
188 PX_FORCE_INLINE const PxReal* getJointVelocities() const { return mJointVelocity.begin(); }
189 PX_FORCE_INLINE PxReal* getJointDeltaVelocities() { return mJointDeltaVelocity.begin(); }
190 PX_FORCE_INLINE const PxReal* getJointDeltaVelocities() const { return mJointDeltaVelocity.begin(); }
191 PX_FORCE_INLINE PxReal* getJointPositions() { return mJointPosition.begin(); }
192 PX_FORCE_INLINE const PxReal* getJointPositions() const { return mJointPosition.begin(); }
193 PX_FORCE_INLINE PxReal* getJointForces() { return mJointForce.begin(); }
194 PX_FORCE_INLINE const PxReal* getJointForces() const { return mJointForce.begin(); }
195 //PX_FORCE_INLINE PxReal* getJointFrictionForces() { return mJointFrictionForce.begin(); }
196
197 PX_FORCE_INLINE ArticulationInternalConstraint& getInternalConstraint(const PxU32 dofId) { return mInternalConstraints[dofId]; }
198 PX_FORCE_INLINE const ArticulationInternalConstraint& getInternalConstraint(const PxU32 dofId) const { return mInternalConstraints[dofId]; }
199
200 PX_FORCE_INLINE ArticulationInternalLockedAxis& getInternalLocks(const PxU32 dofId) { return mInternalLockedAxes[dofId]; }
201 PX_FORCE_INLINE const ArticulationInternalLockedAxis& getInternalLocks(const PxU32 dofId) const { return mInternalLockedAxes[dofId]; }
202
203 PX_FORCE_INLINE Cm::SpatialVectorF* getMotionVelocities() { return mMotionVelocities.begin(); }
204 PX_FORCE_INLINE Cm::SpatialVectorF* getMotionAccelerations() { return mMotionAccelerations.begin(); }
205 PX_FORCE_INLINE const Cm::SpatialVectorF* getMotionAccelerations() const { return mMotionAccelerations.begin(); }
206 PX_FORCE_INLINE Cm::SpatialVectorF* getCorioliseVectors() { return mCorioliseVectors.begin(); }
207 PX_FORCE_INLINE Cm::SpatialVectorF* getSpatialZAVectors() { return mZAForces.begin(); }
208 PX_FORCE_INLINE Cm::SpatialVectorF* getTransmittedForces() { return mJointTransmittedForce.begin(); }
209
210 PX_FORCE_INLINE Cm::SpatialVectorF* getPosIterMotionVelocities() { return mPosIterMotionVelocities.begin(); }
211 PX_FORCE_INLINE PxReal* getPosIterJointDeltaVelocities() { return mPosIterJointDeltaVelocities.begin(); }
212
213 PX_FORCE_INLINE Cm::SpatialVectorF& getPosIterMotionVelocity(const PxU32 index) { return mPosIterMotionVelocities[index]; }
214 PX_FORCE_INLINE const Cm::SpatialVectorF& getMotionVelocity(const PxU32 index) const { return mMotionVelocities[index]; }
215 PX_FORCE_INLINE const Cm::SpatialVectorF& getMotionAcceleration(const PxU32 index) const { return mMotionAccelerations[index]; }
216 PX_FORCE_INLINE const Cm::SpatialVectorF& getCorioliseVector(const PxU32 index) const { return mCorioliseVectors[index]; }
217 PX_FORCE_INLINE const Cm::SpatialVectorF& getSpatialZAVector(const PxU32 index) const { return mZAForces[index]; }
218 PX_FORCE_INLINE const Cm::SpatialVectorF& getTransmittedForce(const PxU32 index) const { return mJointTransmittedForce[index]; }
219
220 PX_FORCE_INLINE Cm::SpatialVectorF& getMotionVelocity(const PxU32 index) { return mMotionVelocities[index]; }
221 PX_FORCE_INLINE Cm::SpatialVectorF& getMotionAcceleration(const PxU32 index) { return mMotionAccelerations[index]; }
222 PX_FORCE_INLINE Cm::SpatialVectorF& getCorioliseVector(const PxU32 index) { return mCorioliseVectors[index]; }
223 PX_FORCE_INLINE Cm::SpatialVectorF& getSpatialZAVector(const PxU32 index) { return mZAForces[index]; }
224 PX_FORCE_INLINE Cm::SpatialVectorF& getTransmittedForce(const PxU32 index) { return mJointTransmittedForce[index]; }
225
226 //PX_FORCE_INLINE Dy::SpatialMatrix* getTempSpatialMatrix() { mTempSpatialMatrix.begin(); }
227
228 PX_FORCE_INLINE PxTransform& getPreTransform(const PxU32 index) { return mPreTransform[index]; }
229 PX_FORCE_INLINE const PxTransform& getPreTransform(const PxU32 index) const { return mPreTransform[index]; }
230// PX_FORCE_INLINE void setPreTransform(const PxU32 index, const PxTransform& t){ mPreTransform[index] = t; }
231 PX_FORCE_INLINE PxTransform* getPreTransform() { return mPreTransform.begin(); }
232
233 PX_FORCE_INLINE const Cm::SpatialVectorF& getDeltaMotionVector(const PxU32 index) const { return mDeltaMotionVector[index]; }
234 PX_FORCE_INLINE void setDeltaMotionVector(const PxU32 index, const Cm::SpatialVectorF& vec) { mDeltaMotionVector[index] = vec; }
235 PX_FORCE_INLINE Cm::SpatialVectorF* getDeltaMotionVector() { return mDeltaMotionVector.begin(); }
236
237 PX_FORCE_INLINE ArticulationLink* getLinks() const { return mLinks; }
238 PX_FORCE_INLINE PxU32 getLinkCount() const { return mLinkCount; }
239
240 PX_FORCE_INLINE ArticulationLink& getLink(PxU32 index) const { return mLinks[index]; }
241
242 PX_FORCE_INLINE ArticulationLinkData* getLinkData() const { return mLinksData; }
243 ArticulationLinkData& getLinkData(PxU32 index) const;
244
245 PX_FORCE_INLINE ArticulationJointCoreData* getJointData() const { return mJointData; }
246 PX_FORCE_INLINE ArticulationJointCoreData& getJointData(PxU32 index) const { return mJointData[index]; }
247
248 PX_FORCE_INLINE ArticulationJointTargetData* getJointTranData() const { return mJointTranData; }
249 PX_FORCE_INLINE ArticulationJointTargetData& getJointTranData(PxU32 index) const { return mJointTranData[index]; }
250 // PT: PX-1399
251 PX_FORCE_INLINE PxArticulationFlags getArticulationFlags() const { return *mFlags; }
252
253 PX_FORCE_INLINE Cm::SpatialVector* getExternalAccelerations() { return mExternalAcceleration; }
254
255 PX_FORCE_INLINE PxU32 getSolverDataSize() const { return mSolverDataSize; }
256 PX_FORCE_INLINE Cm::SpatialVector& getExternalAcceleration(const PxU32 linkID) { return mExternalAcceleration[linkID]; }
257
258 PX_FORCE_INLINE PxReal getDt() const { return mDt; }
259 PX_FORCE_INLINE void setDt(const PxReal dt) { mDt = dt; }
260
261 PX_FORCE_INLINE bool getDataDirty() const { return mDataDirty; }
262 PX_FORCE_INLINE void setDataDirty(const bool dirty) { mDataDirty = dirty; }
263
264 PX_FORCE_INLINE PxU32 getDofs() const { return mDofs; }
265 PX_FORCE_INLINE void setDofs(const PxU32 dof) { mDofs = dof; }
266
267 PX_FORCE_INLINE PxU32 getLocks() const { return mLocks; }
268 PX_FORCE_INLINE void setLocks(const PxU32 locks) { mLocks = locks; }
269
270 PX_FORCE_INLINE FeatherstoneArticulation* getArticulation() { return mArticulation; }
271 PX_FORCE_INLINE void setArticulation(FeatherstoneArticulation* articulation) { mArticulation = articulation; }
272
273 PX_FORCE_INLINE const SpatialMatrix& getBaseInvSpatialArticulatedInertiaW() const { return mBaseInvSpatialArticulatedInertiaW; }
274
275 PX_FORCE_INLINE PxTransform* getAccumulatedPoses() { return mAccumulatedPoses.begin(); }
276 PX_FORCE_INLINE const PxTransform* getAccumulatedPoses() const { return mAccumulatedPoses.begin(); }
277
278 PX_FORCE_INLINE SpatialImpulseResponseMatrix* getImpulseResponseMatrixWorld() { return mResponseMatrixW.begin(); }
279
280 PX_FORCE_INLINE const SpatialImpulseResponseMatrix* getImpulseResponseMatrixWorld() const { return mResponseMatrixW.begin(); }
281
282 PX_FORCE_INLINE const SpatialMatrix& getWorldSpatialArticulatedInertia(const PxU32 linkID) const { return mWorldSpatialArticulatedInertia[linkID]; }
283
284
285 PX_FORCE_INLINE const InvStIs& getInvStIs(const PxU32 linkID) const { return mInvStIs[linkID]; }
286
287 PX_FORCE_INLINE const SpatialSubspaceMatrix& getMotionMatrix(const PxU32 linkID) const { return mMotionMatrix[linkID]; }
288 PX_FORCE_INLINE const SpatialSubspaceMatrix& getWorldMotionMatrix(const PxU32 linkID) const { return mWorldMotionMatrix[linkID]; }
289
290 PX_FORCE_INLINE const IsInvD& getWorldIsInvD(const PxU32 linkID) const { return mIsInvDW[linkID]; }
291
292 private:
293 Cm::SpatialVectorF mRootPreMotionVelocity;
294 Ps::Array<PxReal> mJointAcceleration; // joint acceleration
295 Ps::Array<PxReal> mJointVelocity; // joint velocity
296 Ps::Array<PxReal> mJointDeltaVelocity; // joint velocity change due to contacts
297 Ps::Array<PxReal> mJointPosition; // joint position
298 Ps::Array<PxReal> mJointForce; // joint force
299 //Ps::Array<PxReal> mJointFrictionForce; // joint friction force
300
301 Ps::Array<PxReal> mPosIterJointDeltaVelocities; //joint delta velocity after postion iternation before velocity iteration
302 Ps::Array<Cm::SpatialVectorF> mPosIterMotionVelocities; //link motion velocites after position iteration before velocity iteration
303 Ps::Array<Cm::SpatialVectorF> mMotionVelocities; //link motion velocites
304 Ps::Array<Cm::SpatialVectorF> mMotionAccelerations; //link motion accelerations
305 Ps::Array<Cm::SpatialVectorF> mCorioliseVectors; //link coriolise vector
306 Ps::Array<Cm::SpatialVectorF> mZAForces; //link spatial zero acceleration force/ spatial articulated force
307 Ps::Array<Cm::SpatialVectorF> mJointTransmittedForce;
308 Ps::Array<ArticulationInternalConstraint> mInternalConstraints;
309 Ps::Array<ArticulationInternalLockedAxis> mInternalLockedAxes;
310
311 Ps::Array<Cm::SpatialVectorF> mDeltaMotionVector; //this is for TGS solver
312 Ps::Array<PxTransform> mPreTransform; //this is the previous transform list for links
313 Ps::Array<SpatialImpulseResponseMatrix> mResponseMatrixW;
314 Ps::Array<SpatialMatrix> mWorldSpatialArticulatedInertia;
315 Ps::Array<InvStIs> mInvStIs;
316 Ps::Array<SpatialSubspaceMatrix> mMotionMatrix;
317 Ps::Array<SpatialSubspaceMatrix> mWorldMotionMatrix;
318 Ps::Array<IsInvD> mIsInvDW;
319
320 Ps::Array<PxU32> mNbStaticConstraints;
321 Ps::Array<PxU32> mStaticConstraintStartIndex;
322
323 Ps::Array<PxQuat> mRelativeQuat;
324
325
326 //Ps::Array<SpatialMatrix> mTempSpatialMatrix;
327
328 ArticulationLink* mLinks;
329 PxU32 mLinkCount;
330 ArticulationLinkData* mLinksData;
331 ArticulationJointCoreData* mJointData;
332 ArticulationJointTargetData* mJointTranData;
333 PxReal mDt;
334 PxU32 mDofs;
335 PxU32 mLocks;
336 const PxArticulationFlags* mFlags; // PT: PX-1399
337 Cm::SpatialVector* mExternalAcceleration;
338 PxU32 mSolverDataSize;
339 bool mDataDirty; //this means we need to call commonInit()
340 bool mJointDirty; //this means joint delta velocity has been changed by contacts so we need to update joint velocity/joint acceleration
341 FeatherstoneArticulation* mArticulation;
342
343 Ps::Array<PxTransform> mAccumulatedPoses;
344 Ps::Array<PxQuat> mDeltaQ;
345 //PxReal mAccumulatedDt;
346
347 SpatialMatrix mBaseInvSpatialArticulatedInertiaW;
348
349
350
351 friend class FeatherstoneArticulation;
352 };
353
354 void ArticulationData::init()
355 {
356 //zero delta motion vector for TGS solver
357 PxMemZero(dest: getDeltaMotionVector(), count: sizeof(Cm::SpatialVectorF) * mLinkCount);
358
359 //zero joint velocity delta, which will be changed in pxcFsApplyImpulse if there are any contact with links
360 PxMemZero(dest: getJointDeltaVelocities(), count: sizeof(PxReal) * mDofs);
361 mJointDirty = false;
362 }
363
364 struct ScratchData
365 {
366 public:
367 ScratchData()
368 {
369 motionVelocities = NULL;
370 motionAccelerations = NULL;
371 coriolisVectors = NULL;
372 spatialZAVectors = NULL;
373 externalAccels = NULL;
374 compositeSpatialInertias = NULL;
375
376 jointVelocities = NULL;
377 jointAccelerations = NULL;
378 jointForces = NULL;
379 jointPositions = NULL;
380 jointFrictionForces = NULL;
381 }
382
383 Cm::SpatialVectorF* motionVelocities;
384 Cm::SpatialVectorF* motionAccelerations;
385 Cm::SpatialVectorF* coriolisVectors;
386 Cm::SpatialVectorF* spatialZAVectors;
387 Cm::SpatialVector* externalAccels;
388 Dy::SpatialMatrix* compositeSpatialInertias;
389
390 PxReal* jointVelocities;
391 PxReal* jointAccelerations;
392 PxReal* jointForces;
393 PxReal* jointPositions;
394 PxReal* jointFrictionForces;
395 };
396
397#if PX_VC
398#pragma warning(push)
399#pragma warning( disable : 4324 ) // Padding was added at the end of a structure because of a __declspec(align) value.
400#endif
401
402 //Articulation dirty flag - used to tag which properties of the articulation are dirty. Used only to transfer selected data to the GPU...
403 struct ArticulationDirtyFlag
404 {
405 enum Enum
406 {
407 eDIRTY_JOINTS = 1<<0,
408 eDIRTY_POSITIONS = 1<<1,
409 eDIRTY_VELOCITIES = 1<<2,
410 eDIRTY_ACCELERATIONS = 1<<3,
411 eDIRTY_FORCES = 1<<4,
412 eDIRTY_ROOT = 1<<5,
413 eDIRTY_LINKS = 1<<6,
414 eIN_DIRTY_LIST = 1<<7,
415 eDIRTY_DOFS = (eDIRTY_POSITIONS | eDIRTY_VELOCITIES | eDIRTY_ACCELERATIONS | eDIRTY_FORCES)
416 };
417 };
418
419 PX_INLINE void computeArticJacobianAxes(PxVec3 row[3], const PxQuat& qa, const PxQuat& qb)
420 {
421 // Compute jacobian matrix for (qa* qb) [[* means conjugate in this expr]]
422 // d/dt (qa* qb) = 1/2 L(qa*) R(qb) (omega_b - omega_a)
423 // result is L(qa*) R(qb), where L(q) and R(q) are left/right q multiply matrix
424
425 const PxReal wa = qa.w, wb = qb.w;
426 const PxVec3 va(qa.x, qa.y, qa.z), vb(qb.x, qb.y, qb.z);
427
428 const PxVec3 c = vb*wa + va*wb;
429 const PxReal d0 = wa*wb;
430 const PxReal d1 = va.dot(v: vb);
431 const PxReal d = d0 - d1;
432
433 row[0] = (va * vb.x + vb * va.x + PxVec3(d, c.z, -c.y)) * 0.5f;
434 row[1] = (va * vb.y + vb * va.y + PxVec3(-c.z, d, c.x)) * 0.5f;
435 row[2] = (va * vb.z + vb * va.z + PxVec3(c.y, -c.x, d)) * 0.5f;
436
437 if ((d0 + d1) != 0.0f) // check if relative rotation is 180 degrees which can lead to singular matrix
438 return;
439 else
440 {
441 row[0].x += PX_EPS_F32;
442 row[1].y += PX_EPS_F32;
443 row[2].z += PX_EPS_F32;
444 }
445 }
446
447
448 PX_ALIGN_PREFIX(64)
449 class FeatherstoneArticulation : public ArticulationV
450 {
451 PX_NOCOPY(FeatherstoneArticulation)
452 public:
453 // public interface
454
455 explicit FeatherstoneArticulation(void*);
456 ~FeatherstoneArticulation();
457
458 // get data sizes for allocation at higher levels
459 virtual void getDataSizes(PxU32 linkCount, PxU32& solverDataSize, PxU32& totalSize, PxU32& scratchSize);
460
461 virtual bool resize(const PxU32 linkCount);
462
463 virtual void onUpdateSolverDesc();
464
465 virtual PxU32 getDofs();
466
467 virtual PxU32 getDof(const PxU32 linkID);
468
469 virtual bool applyCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag);
470
471 virtual void copyInternalStateToCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag);
472
473 virtual void packJointData(const PxReal* maximum, PxReal* reduced);
474
475 virtual void unpackJointData(const PxReal* reduced, PxReal* maximum);
476
477 virtual void initializeCommonData();
478
479 //gravity as input, joint force as output
480 virtual void getGeneralizedGravityForce(const PxVec3& gravity, PxArticulationCache& cache);
481
482 //joint velocity as input, generalised force(coriolis and centrigugal force) as output
483 virtual void getCoriolisAndCentrifugalForce(PxArticulationCache& cache);
484
485 //external force as input, joint force as output
486 virtual void getGeneralizedExternalForce(PxArticulationCache& /*cache*/);
487
488 //joint force as input, joint acceleration as output
489 virtual void getJointAcceleration(const PxVec3& gravity, PxArticulationCache& cache);
490
491 //joint acceleration as input, joint force as out
492 virtual void getJointForce(PxArticulationCache& cache);
493
494 virtual void getDenseJacobian(PxArticulationCache& cache, PxU32 & nRows, PxU32 & nCols);
495
496 //These two functions are for closed loop system
497 void getKMatrix(ArticulationJointCore* loopJoint, const PxU32 parentIndex, const PxU32 childIndex, PxArticulationCache& cache);
498
499 virtual void getCoefficientMatrix(const PxReal dt, const PxU32 linkID, const PxContactJoint* contactJoints, const PxU32 nbContacts, PxArticulationCache& cache);
500
501 virtual void getCoefficientMatrixWithLoopJoints(ArticulationLoopConstraint* lConstraints, const PxU32 nbJoints, PxArticulationCache& cache);
502
503 virtual bool getLambda(ArticulationLoopConstraint* lConstraints, const PxU32 nbJoints, PxArticulationCache& cache, PxArticulationCache& rollBackCache,
504 const PxReal* jointTorque, const PxVec3& gravity, const PxU32 maxIter);
505
506 virtual void getGeneralizedMassMatrix(PxArticulationCache& cache);
507
508 virtual void getGeneralizedMassMatrixCRB(PxArticulationCache& cache);
509
510 virtual bool storeStaticConstraint(const PxSolverConstraintDesc& desc);
511
512 virtual bool willStoreStaticConstraint() { return true; }
513
514 virtual void teleportRootLink();
515
516 virtual void getImpulseResponse(
517 PxU32 linkID,
518 Cm::SpatialVectorF* Z,
519 const Cm::SpatialVector& impulse,
520 Cm::SpatialVector& deltaV) const;
521
522 virtual void getImpulseResponse(
523 PxU32 linkID,
524 Cm::SpatialVectorV* /*Z*/,
525 const Cm::SpatialVectorV& impulse,
526 Cm::SpatialVectorV& deltaV) const;
527
528 virtual void getImpulseSelfResponse(
529 PxU32 linkID0,
530 PxU32 linkID1,
531 Cm::SpatialVectorF* Z,
532 const Cm::SpatialVector& impulse0,
533 const Cm::SpatialVector& impulse1,
534 Cm::SpatialVector& deltaV0,
535 Cm::SpatialVector& deltaV1) const;
536
537 virtual Cm::SpatialVectorV getLinkVelocity(const PxU32 linkID) const;
538
539 virtual Cm::SpatialVectorV getLinkMotionVector(const PxU32 linkID) const;
540
541 //this is called by island gen to determine whether the articulation should be awake or sleep
542 virtual Cm::SpatialVector getMotionVelocity(const PxU32 linkID) const;
543
544 virtual Cm::SpatialVector getMotionAcceleration(const PxU32 linkID) const;
545
546 virtual void fillIndexedManager(const PxU32 linkId, Dy::ArticulationLinkHandle& handle, PxU8& indexType);
547
548 virtual PxReal getLinkMaxPenBias(const PxU32 linkID) const;
549
550 static PxU32 computeUnconstrainedVelocities(
551 const ArticulationSolverDesc& desc,
552 PxReal dt,
553 PxConstraintAllocator& allocator,
554 PxSolverConstraintDesc* constraintDesc,
555 PxU32& acCount,
556 const PxVec3& gravity, PxU64 contextID,
557 Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
558
559 static void computeUnconstrainedVelocitiesTGS(
560 const ArticulationSolverDesc& desc,
561 PxReal dt, const PxVec3& gravity,
562 PxU64 contextID, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
563
564 static PxU32 setupSolverConstraintsTGS(const ArticulationSolverDesc& articDesc,
565 PxcConstraintBlockStream& stream,
566 PxSolverConstraintDesc* constraintDesc,
567 PxReal dt,
568 PxReal invDt,
569 PxReal totalDt,
570 PxU32& acCount,
571 PxsConstraintBlockManager& constraintBlockManager,
572 Cm::SpatialVectorF* Z);
573
574 static void saveVelocity(const ArticulationSolverDesc& d, Cm::SpatialVectorF* deltaV);
575
576 static void saveVelocityTGS(const ArticulationSolverDesc& d, PxReal invDtF32);
577
578 static void updateBodies(const ArticulationSolverDesc& desc, PxReal dt);
579
580 static void updateBodiesTGS(const ArticulationSolverDesc& desc, PxReal dt);
581
582 static void updateBodies(FeatherstoneArticulation* articulation, PxReal dt, bool integrateJointPosition);
583
584 static void recordDeltaMotion(const ArticulationSolverDesc& desc, const PxReal dt, Cm::SpatialVectorF* deltaV, const PxReal totalInvDt);
585
586 static void deltaMotionToMotionVelocity(const ArticulationSolverDesc& desc, PxReal invDt);
587
588 virtual void pxcFsApplyImpulse(PxU32 linkID, Ps::aos::Vec3V linear,
589 Ps::aos::Vec3V angular, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
590
591 virtual void pxcFsApplyImpulses(PxU32 linkID, const Ps::aos::Vec3V& linear,
592 const Ps::aos::Vec3V& angular, PxU32 linkID2, const Ps::aos::Vec3V& linear2,
593 const Ps::aos::Vec3V& angular2, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
594
595 virtual void pxcFsApplyImpulses(Cm::SpatialVectorF* Z);
596
597 virtual Cm::SpatialVectorV pxcFsGetVelocity(PxU32 linkID);
598
599 virtual void pxcFsGetVelocities(PxU32 linkID, PxU32 linkID1, Cm::SpatialVectorV& v0, Cm::SpatialVectorV& v1);
600
601 virtual Cm::SpatialVectorV pxcFsGetVelocityTGS(PxU32 linkID);
602
603 virtual const PxTransform& getCurrentTransform(PxU32 linkID) const;
604
605 virtual const PxQuat& getDeltaQ(PxU32 linkID) const;
606
607 //Applies a set of N impulses, all in local space and updates the links' motion and joint velocities
608 void applyImpulses(Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
609 void getDeltaV(Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
610
611 //This method calculate the velocity change due to collision/constraint impulse, record joint velocity and acceleration
612 static Cm::SpatialVectorF propagateVelocityW(const PxVec3& c2p, const Dy::SpatialMatrix& spatialInertia,
613 const InvStIs& invStIs, const SpatialSubspaceMatrix& motionMatrix, const Cm::SpatialVectorF& Z,
614 PxReal* jointVelocity, const Cm::SpatialVectorF& hDeltaV);
615
616 //This method calculate the velocity change due to collision/constraint impulse
617 static Cm::SpatialVectorF propagateVelocityTestImpulseW(const PxVec3& c2p, const Dy::SpatialMatrix& spatialInertia, const InvStIs& invStIs,
618 const SpatialSubspaceMatrix& motionMatrix, const Cm::SpatialVectorF& Z, const Cm::SpatialVectorF& hDeltaV);
619
620
621 ////This method calculate zero acceration impulse due to test/actual impluse
622 //static Cm::SpatialVectorF propagateImpulse(const IsInvD& isInvD, const SpatialTransform& childToParent,
623 // const SpatialSubspaceMatrix& motionMatrix, const Cm::SpatialVectorF& Z);
624
625 static Cm::SpatialVectorF propagateImpulseW(const IsInvD& isInvD, const PxVec3& childToParent,
626 const SpatialSubspaceMatrix& motionMatrix, const Cm::SpatialVectorF& Z);
627
628 bool applyCacheToDest(ArticulationData& data, PxArticulationCache& cache,
629 PxReal* jVelocities, PxReal* jAcceleration, PxReal* jPosition, PxReal* jointForce,
630 const PxArticulationCacheFlags flag);
631
632 PX_FORCE_INLINE ArticulationData& getArticulationData() { return mArticulationData; }
633 PX_FORCE_INLINE const ArticulationData& getArticulationData() const { return mArticulationData; }
634
635 //void setGpuRemapId(const PxU32 id) { mGpuRemapId = id; }
636 //PxU32 getGpuRemapId() { return mGpuRemapId; }
637
638 static PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF translateSpatialVector(const PxVec3& offset, const Cm::SpatialVectorF& vec)
639 {
640 return Cm::SpatialVectorF(vec.top, vec.bottom + offset.cross(v: vec.top));
641 }
642
643 static PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::UnAlignedSpatialVector translateSpatialVector(const PxVec3& offset, const Cm::UnAlignedSpatialVector& vec)
644 {
645 return Cm::UnAlignedSpatialVector(vec.top, vec.bottom + offset.cross(v: vec.top));
646 }
647
648 static PX_FORCE_INLINE PxMat33 constructSkewSymmetricMatrix(const PxVec3 r)
649 {
650 return PxMat33(PxVec3(0.0f, r.z, -r.y),
651 PxVec3(-r.z, 0.0f, r.x),
652 PxVec3(r.y, -r.x, 0.0f));
653 }
654
655 bool raiseGPUDirtyFlag(ArticulationDirtyFlag::Enum flag)
656 {
657 bool nothingRaised = !(mGPUDirtyFlags);
658 mGPUDirtyFlags |= flag;
659 return nothingRaised;
660 }
661
662 void clearGPUDirtyFlags()
663 {
664 mGPUDirtyFlags = 0;
665 }
666
667 protected:
668 void constraintPrep(ArticulationLoopConstraint* lConstraints, const PxU32 nbJoints,
669 Cm::SpatialVectorF* Z, PxSolverConstraintPrepDesc& prepDesc, PxSolverBody& sBody,
670 PxSolverBodyData& sBodyData, PxSolverConstraintDesc* desc, PxConstraintAllocator& allocator);
671
672 void updateArticulation(ScratchData& scratchData,
673 const PxVec3& gravity,
674 Cm::SpatialVectorF* Z,
675 Cm::SpatialVectorF* DeltaV);
676
677 void computeUnconstrainedVelocitiesInternal(
678 const PxVec3& gravity,
679 Cm::SpatialVectorF* Z, Cm::SpatialVectorF* DeltaV);
680
681 //copy joint data from fromJointData to toJointData
682 void copyJointData(ArticulationData& data, PxReal* toJointData, const PxReal* fromJointData);
683
684 void computeDofs();
685 //this function calculates motion subspace matrix(s) for all tree joint
686 void jcalc(ArticulationData& data, bool forceUpdate = false);
687
688 //this function calculates loop joint constraint subspace matrix(s) and active force
689 //subspace matrix
690 void jcalcLoopJointSubspace(ArticulationJointCore* joint, ArticulationJointCoreData& jointDatum, SpatialSubspaceMatrix& T);
691
692 void computeSpatialInertia(ArticulationData& data);
693
694 //compute zero acceleration force
695 void computeZ(ArticulationData& data, const PxVec3& gravity, ScratchData& scratchData);
696
697 //compute drag force
698 void computeD(ArticulationData& data, ScratchData& scratchData,
699 Cm::SpatialVectorF* tZ, Cm::SpatialVectorF* tDeltaV);
700
701 void solveInternalConstraints(const PxReal dt, const PxReal invDt, Cm::SpatialVectorF* impulses, Cm::SpatialVectorF* DeltaV,
702 bool velocityIteration, bool isTGS, const PxReal elapsedTime);
703
704 Cm::SpatialVectorF solveInternalConstraintRecursive(const PxReal dt, const PxReal invDt, Cm::SpatialVectorF* impulses, Cm::SpatialVectorF* DeltaV,
705 bool velocityIteration, bool isTGS, const PxReal elapsedTime, const PxU32 linkID, const Cm::SpatialVectorF& parentDeltaV,
706 PxU32& dofId, PxU32& lockId);
707
708 void writebackInternalConstraints(bool isTGS);
709
710 void concludeInternalConstraints(bool isTGS);
711
712 //compute coriolis force
713 void computeC(ArticulationData& data, ScratchData& scratchData);
714
715 //compute relative transform child to parent
716 void computeRelativeTransformC2P(ArticulationData& data);
717 //compute relative transform child to base
718 void computeRelativeTransformC2B(ArticulationData& data);
719
720 void computeLinkVelocities(ArticulationData& data, ScratchData& scratchData);
721
722 void initLinks(ArticulationData& data, const PxVec3& gravity,
723 ScratchData& scratchData, Cm::SpatialVectorF* tZ, Cm::SpatialVectorF* tDeltaV);
724
725 void computeIs(ArticulationLinkData& linkDatum, ArticulationJointCoreData& jointDatum, const PxU32 linkID);
726 static SpatialMatrix computePropagateSpatialInertia(const PxU8 jointType, ArticulationJointCoreData& jointDatum,
727 const SpatialMatrix& articulatedInertia, const Cm::SpatialVectorF* linkIs, InvStIs& invStIs, IsInvD& isInvD,
728 const SpatialSubspaceMatrix& motionMatrix);
729
730 static void transformInertia(const SpatialTransform& sTod, SpatialMatrix& inertia);
731
732 static void translateInertia(const PxMat33& offset, SpatialMatrix& inertia);
733
734 void computeArticulatedSpatialInertia(ArticulationData& data);
735
736 void computeArticulatedResponseMatrix(ArticulationData& data);
737
738 void computeArticulatedSpatialZ(ArticulationData& data, ScratchData& scratchData);
739
740 /*void computeJointAcceleration(ArticulationLinkData& linkDatum, ArticulationJointCoreData& jointDatum,
741 const Cm::SpatialVectorF& pMotionAcceleration, PxReal* jointAcceleration, const PxU32 linkID);*/
742
743 void computeJointAccelerationW(ArticulationLinkData& linkDatum, ArticulationJointCoreData& jointDatum,
744 const Cm::SpatialVectorF& pMotionAcceleration, PxReal* jointAcceleration, const PxU32 linkID);
745
746 //compute joint acceleration, joint velocity and link acceleration, velocity based
747 //on spatial force and spatial articulated inertia tensor
748 void computeLinkAcceleration(ArticulationData& data, ScratchData& scratchData);
749
750 //void computeTempLinkAcceleration(ArticulationData& data, ScratchData& scratchData);
751 void computeJointTransmittedFrictionForce(ArticulationData& data, ScratchData& scratchData,
752 Cm::SpatialVectorF* Z, Cm::SpatialVectorF* DeltaV);
753 //void computeJointFriction(ArticulationData& data, ScratchData& scratchData);
754
755 //void copyFromBodyCore();
756 //void copyToBodyCore();
757 void updateBodies();
758
759 void applyExternalImpulse(ArticulationLink* links, const PxU32 linkCount, const bool fixBase,
760 ArticulationData& data, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV, const PxReal dt, const PxVec3& gravity, Cm::SpatialVector* acceleration);
761
762 static Cm::SpatialVectorF getDeltaVWithDeltaJV(const bool fixBase, const PxU32 linkID,
763 const ArticulationData& data, Cm::SpatialVectorF* Z,
764 PxReal* jointVelocities);
765
766 static Cm::SpatialVectorF getDeltaV(const bool fixBase, const PxU32 linkID,
767 const ArticulationData& data, Cm::SpatialVectorF* Z);
768
769 //impulse need to be in the linkID space
770 static void getZ(const PxU32 linkID, const ArticulationData& data,
771 Cm::SpatialVectorF* Z, const Cm::SpatialVectorF& impulse);
772
773 ////impulse0 and impulse1 are in linkID0 and linkID1 space respectively
774 //static void getZ(
775 // const ArticulationData& data,
776 // Cm::SpatialVectorF* Z,
777 // PxU32 linkID0_,
778 // const Cm::SpatialVector& impulse0,
779 // PxU32 linkID1_,
780 // const Cm::SpatialVector& impulse1);
781
782 //This method use in impulse self response. The input impulse is in the link space
783 static Cm::SpatialVectorF getImpulseResponseW(
784 const PxU32 linkID,
785 const ArticulationData& data,
786 const Cm::SpatialVectorF& impulse);
787
788 //This method use in impulse self response. The input impulse is in the link space
789 static Cm::SpatialVectorF getImpulseResponseWithJ(
790 const PxU32 linkID,
791 const bool fixBase,
792 const ArticulationData& data,
793 Cm::SpatialVectorF* Z,
794 const Cm::SpatialVectorF& impulse,
795 PxReal* jointVelocities);
796
797 void getImpulseSelfResponseInv(const bool fixBase,
798 PxU32 linkID0,
799 PxU32 linkID1,
800 Cm::SpatialVectorF* Z,
801 const Cm::SpatialVector& impulse0,
802 const Cm::SpatialVector& impulse1,
803 Cm::SpatialVector& deltaV0,
804 Cm::SpatialVector& deltaV1,
805 PxReal* jointVelocities);
806
807 void getImpulseResponseSlowInv(Dy::ArticulationLink* links,
808 const ArticulationData& data,
809 PxU32 linkID0_,
810 const Cm::SpatialVector& impulse0,
811 Cm::SpatialVector& deltaV0,
812 PxU32 linkID1_,
813 const Cm::SpatialVector& impulse1,
814 Cm::SpatialVector& deltaV1,
815 PxReal* jointVelocities);
816
817 Cm::SpatialVectorF getImpulseResponseInv(const bool fixBase,
818 const PxU32 linkID, Cm::SpatialVectorF* Z,
819 const Cm::SpatialVector& impulse,
820 PxReal* jointVelocites);
821
822 void inverseDynamic(ArticulationData& data, const PxVec3& gravity,
823 ScratchData& scratchData, bool computeCoriolis);
824
825 void inverseDynamicFloatingBase(ArticulationData& data, const PxVec3& gravity,
826 ScratchData& scratchData, bool computeCoriolis);
827
828 //compute link body force with motion velocity and acceleration
829 void computeZAForceInv(ArticulationData& data, ScratchData& scratchData);
830 void initCompositeSpatialInertia(ArticulationData& data, Dy::SpatialMatrix* compositeSpatialInertia);
831 void computeCompositeSpatialInertiaAndZAForceInv(ArticulationData& data, ScratchData& scratchData);
832
833 void computeRelativeGeneralizedForceInv(ArticulationData& data, ScratchData& scratchData);
834
835 //provided joint velocity and joint acceleartion, compute link acceleration
836 void computeLinkAccelerationInv(ArticulationData& data, ScratchData& scratchData);
837
838 void computeGeneralizedForceInv(ArticulationData& data, ScratchData& scratchData);
839
840 void calculateMassMatrixColInv(ScratchData& scratchData);
841
842 void calculateHFixBase(PxArticulationCache& cache);
843
844 void calculateHFloatingBase(PxArticulationCache& cache);
845
846 //joint limits
847 void enforcePrismaticLimits(PxReal* jPosition, ArticulationJointCore* joint);
848
849 public:
850 static void getImpulseSelfResponse(ArticulationLink* links,
851 const bool fixBase,
852 Cm::SpatialVectorF* Z,
853 const ArticulationData& data,
854 PxU32 linkID0,
855 const Cm::SpatialVectorV& impulse0,
856 Cm::SpatialVectorV& deltaV0,
857 PxU32 linkID1,
858 const Cm::SpatialVectorV& impulse1,
859 Cm::SpatialVectorV& deltaV1);
860
861 static void getImpulseResponseSlow(Dy::ArticulationLink* links,
862 const ArticulationData& data,
863 PxU32 linkID0_,
864 const Cm::SpatialVector& impulse0,
865 Cm::SpatialVector& deltaV0,
866 PxU32 linkID1_,
867 const Cm::SpatialVector& impulse1,
868 Cm::SpatialVector& deltaV1,
869 Cm::SpatialVectorF* Z);
870
871 void createHardLimit(
872 ArticulationLink* links,
873 const bool fixBase,
874 Cm::SpatialVectorF* Z,
875 ArticulationData& data,
876 PxU32 linkIndex,
877 SolverConstraint1DExt& s,
878 const PxVec3& axis,
879 PxReal err,
880 PxReal recipDt);
881
882 void createHardLimits(
883 SolverConstraint1DExt& s0,
884 SolverConstraint1DExt& s1,
885 const PxVec3& axis,
886 PxReal err0,
887 PxReal err1,
888 PxReal recipDt,
889 const Cm::SpatialVectorV& deltaVA,
890 const Cm::SpatialVectorV& deltaVB,
891 PxReal recipResponse);
892
893 void createTangentialSpring(
894 ArticulationLink* links,
895 const bool fixBase,
896 Cm::SpatialVectorF* Z,
897 ArticulationData& data,
898 PxU32 linkIndex,
899 SolverConstraint1DExt& s,
900 const PxVec3& axis,
901 PxReal stiffness,
902 PxReal damping,
903 PxReal dt);
904
905 PxU32 setupSolverConstraints(
906 ArticulationLink* links,
907 const PxU32 linkCount,
908 const bool fixBase,
909 ArticulationData& data,
910 Cm::SpatialVectorF* Z,
911 PxU32& acCount);
912
913 void setupInternalConstraints(
914 ArticulationLink* links,
915 const PxU32 linkCount,
916 const bool fixBase,
917 ArticulationData& data,
918 Cm::SpatialVectorF* Z,
919 PxReal stepDt,
920 PxReal dt,
921 PxReal invDt,
922 PxReal erp,
923 bool isTGSSolver);
924
925 void setupInternalConstraintsRecursive(
926 ArticulationLink* links,
927 const PxU32 linkCount,
928 const bool fixBase,
929 ArticulationData& data,
930 Cm::SpatialVectorF* Z,
931 const PxReal stepDt,
932 const PxReal dt,
933 const PxReal invDt,
934 const PxReal erp,
935 const PxReal cfm,
936 const bool isTGSSolver,
937 const PxU32 linkID,
938 const PxReal maxForceScale);
939
940 virtual void prepareStaticConstraints(const PxReal dt, const PxReal invDt, PxsContactManagerOutputIterator& outputs,
941 Dy::ThreadContext& threadContext, PxReal correlationDist, PxReal bounceThreshold, PxReal frictionOffsetThreshold, PxReal solverOffsetSlop,
942 PxReal ccdMaxSeparation, PxSolverBodyData* solverBodyData, PxsConstraintBlockManager& blockManager,
943 Dy::ConstraintWriteback* constraintWritebackPool);
944
945 virtual void prepareStaticConstraintsTGS(const PxReal stepDt, const PxReal totalDt, const PxReal invStepDt, const PxReal invTotalDt,
946 PxsContactManagerOutputIterator& outputs, Dy::ThreadContext& threadContext, PxReal correlationDist, PxReal bounceThreshold,
947 PxReal frictionOffsetThreshold, PxTGSSolverBodyData* solverBodyData,
948 PxTGSSolverBodyTxInertia* txInertia, PxsConstraintBlockManager& blockManager, Dy::ConstraintWriteback* constraintWritebackPool,
949 const PxU32 nbSubsteps, const PxReal lengthScale);
950
951 void createHardLimitTGS(
952 SolverConstraint1DExtStep& s,
953 const PxVec3& axis,
954 PxReal err,
955 PxReal recipDt,
956 const Cm::SpatialVectorV& deltaVA,
957 const Cm::SpatialVectorV& deltaVB,
958 PxReal recipResponse);
959
960 void createTangentialSpringTGS(
961 ArticulationLink* links,
962 const bool fixBase,
963 Cm::SpatialVectorF* Z,
964 ArticulationData& data,
965 PxU32 linkIndex,
966 SolverConstraint1DExtStep& s,
967 const PxVec3& axis,
968 PxReal stiffness,
969 PxReal damping,
970 PxReal dt);
971
972 //integration
973 void propagateLinksDown(ArticulationData& data, PxReal* jointVelocities, PxReal* jointPositions,
974 Cm::SpatialVectorF* motionVelocities);
975
976 void updateJointProperties(
977 const PxReal* jointDeltaVelocities,
978 const Cm::SpatialVectorF* motionVelocities,
979 PxReal* jointVelocities,
980 PxReal* jointAccelerations);
981
982 void recomputeAccelerations(const PxReal dt);
983 Cm::SpatialVector recomputeAcceleration(const PxU32 linkID, const PxReal dt) const;
984
985 void computeAndEnforceJointPositions(ArticulationData& data, PxReal* jointPositions);
986
987 //update link position based on joint position provided by the cache
988 void teleportLinks(ArticulationData& data);
989
990 void computeLinkVelocities(ArticulationData& data);
991
992 PxU8* allocateScratchSpatialData(PxcScratchAllocator* allocator,
993 const PxU32 linkCount, ScratchData& scratchData, bool fallBackToHeap = false);
994
995// void allocateScratchSpatialData(DyScratchAllocator& allocator,
996// const PxU32 linkCount, ScratchData& scratchData);
997
998 //This method calculate the velocity change from parent to child using parent current motion velocity
999 PxTransform propagateTransform(const PxU32 linkID, ArticulationLink* links, ArticulationJointCoreData& jointDatum,
1000 Cm::SpatialVectorF* motionVelocities, const PxReal dt, const PxTransform& pBody2World, const PxTransform& currentTransform,
1001 PxReal* jointVelocity, PxReal* jointDeltaVelocities, PxReal* jointPosition, const SpatialSubspaceMatrix& motionMatrix,
1002 const SpatialSubspaceMatrix& worldMotionMatrix);
1003
1004 static void updateRootBody(const Cm::SpatialVectorF& motionVelocity,
1005 const PxTransform& preTransform, ArticulationData& data, const PxReal dt);
1006
1007 ArticulationData mArticulationData;
1008
1009 Ps::Array<char> mScratchMemory;
1010 bool mHasSphericalJoint;
1011
1012 Ps::Array<PxSolverConstraintDesc> mStaticConstraints;
1013 //this is the id remap pxgbodysim and pxgariculation. if application delete the articulation, we need to
1014 //put back this id to the id pool
1015 //PxU32 mGpuRemapId;
1016
1017 PxU32 mGPUDirtyFlags;
1018
1019
1020
1021 } PX_ALIGN_SUFFIX(64);
1022
1023#if PX_VC
1024#pragma warning(pop)
1025#endif
1026
1027 void PxvRegisterArticulationsReducedCoordinate();
1028
1029} //namespace Dy
1030
1031}
1032
1033#endif
1034

source code of qtquick3dphysics/src/3rdparty/PhysX/source/lowleveldynamics/include/DyFeatherstoneArticulation.h