RavEngine
Loading...
Searching...
No Matches
DyFeatherstoneArticulation.h
1// Redistribution and use in source and binary forms, with or without
2// modification, are permitted provided that the following conditions
3// are met:
4// * Redistributions of source code must retain the above copyright
5// notice, this list of conditions and the following disclaimer.
6// * Redistributions in binary form must reproduce the above copyright
7// notice, this list of conditions and the following disclaimer in the
8// documentation and/or other materials provided with the distribution.
9// * Neither the name of NVIDIA CORPORATION nor the names of its
10// contributors may be used to endorse or promote products derived
11// from this software without specific prior written permission.
12//
13// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
14// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
15// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
16// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
17// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
18// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
19// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
20// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
21// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
22// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
23// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
24//
25// Copyright (c) 2008-2022 NVIDIA Corporation. All rights reserved.
26// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
27// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
28
29#ifndef DY_FEATHERSTONE_ARTICULATION_H
30#define DY_FEATHERSTONE_ARTICULATION_H
31
32#include "foundation/PxVec3.h"
33#include "foundation/PxQuat.h"
34#include "foundation/PxTransform.h"
35#include "foundation/PxVecMath.h"
36#include "CmUtils.h"
37#include "DyVArticulation.h"
38#include "DyFeatherstoneArticulationUtils.h"
39#include "DyFeatherstoneArticulationJointData.h"
40#include "solver/PxSolverDefs.h"
41#include "DyArticulationTendon.h"
42#include "CmSpatialVector.h"
43
44#ifndef FEATHERSTONE_DEBUG
45#define FEATHERSTONE_DEBUG 0
46#endif
47
48#define DY_STATIC_CONTACTS_IN_INTERNAL_SOLVER true
49
50namespace physx
51{
52
53class PxContactJoint;
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 ArticulationLinkData;
75 struct SpatialSubspaceMatrix;
76 struct SolverConstraint1DExt;
77 struct SolverConstraint1DStep;
78
79 class FeatherstoneArticulation;
80 struct SpatialMatrix;
81 struct SpatialTransform;
82 struct Constraint;
83 class ThreadContext;
84
85
86 struct ArticulationInternalTendonConstraint
87 {
88 Cm::UnAlignedSpatialVector row0; //24 24
89 Cm::UnAlignedSpatialVector row1; //24 48
90
91 Cm::UnAlignedSpatialVector deltaVB; //24 72
92
93 PxU32 linkID0; //4 74
94 PxU32 linkID1; //4 78
95 PxReal accumulatedLength; //4 82 //accumulate distance for spatial tendon, accumualate joint pose for fixed tendon
96 PxReal biasCoefficient; //4 94
97 PxReal velMultiplier; //4 98
98 PxReal impulseMultiplier; //4 102
99 PxReal appliedForce; //4 106
100 PxReal recipResponse; //4 110
101 PxReal deltaVA; //4 114
102 PxReal limitBiasCoefficient;
103 PxReal limitImpulseMultiplier;
104 PxReal limitAppliedForce;
105 PxReal restDistance;
106 PxReal lowLimit;
107 PxReal highLimit;
108 PxReal velImpulseMultiplier;
109 PxReal limitVelImpulseMultiplier;
110 };
111
112 struct ArticulationInternalConstraintBase
113 {
114 //Common/shared directional info between, frictions and drives
115 Cm::UnAlignedSpatialVector row0; //24 24
116 Cm::UnAlignedSpatialVector row1; //24 48
117 Cm::UnAlignedSpatialVector deltaVA; //24 72
118 Cm::UnAlignedSpatialVector deltaVB; //24 96
119
120 //Response information
121 PxReal recipResponse; //4 100
122 PxReal response; //4 104
123 };
124
125 struct ArticulationInternalLimit
126 {
127 //Initial error
128 PxReal errorLow; //4 4
129 PxReal errorHigh; //4 8
130 PxReal lowImpulse; //4 12 changed
131 PxReal highImpulse; //4 16 changed
132 };
133
134
135 struct ArticulationInternalConstraint : public ArticulationInternalConstraintBase
136 {
137 //Joint spring drive info
138 PxReal driveTargetVel; //4 128
139 PxReal driveInitialBias; //4 132
140 PxReal driveBiasCoefficient; //4 132
141 PxReal driveVelMultiplier; //4 140
142 PxReal driveImpulseMultiplier; //4 148
143 PxReal maxDriveForce; //4 152
144 PxReal driveForce; //4 156
145
146 PxReal maxFrictionForce; //4 160
147 PxReal frictionForce; //4 164
148 PxReal frictionForceCoefficient; //4 168
149
150 bool isLinearConstraint; //1 169
151 PxU8 padding[7]; //11 176
152 };
153
154 //linkID can be PxU32. However, each thread is going to read 16 bytes so we just keep ArticulationSensor 16 byte align.
155 //if not, newArticulationsLaunch kernel will fail to read the sensor data correctly
156 struct ArticulationSensor
157 {
158 PxTransform mRelativePose; //28 28
159 PxU16 mLinkID; //02 30
160 PxU16 mFlags; //02 32
161 };
162
163 struct PX_ALIGN_PREFIX(16) JointSpaceSpatialZ
164 {
165 PxReal mVals [6][4];
166
167 PxReal dot(Cm::SpatialVectorF& v, PxU32 id)
168 {
169 return v.top.x * mVals[0][id] + v.top.y * mVals[1][id] + v.top.z * mVals[2][id]
170 + v.bottom.x * mVals[3][id] + v.bottom.y * mVals[4][id] + v.bottom.z * mVals[5][id];
171 }
172 }
173 PX_ALIGN_SUFFIX(16);
174
175 class ArticulationData
176 {
177 public:
178
179 ArticulationData() :
180 mPathToRootElements(NULL), mNumPathToRootElements(0), mLinksData(NULL), mJointData(NULL), mJointTranData(NULL),
181 mSpatialTendons(NULL), mNumSpatialTendons(0), mNumTotalAttachments(0),
182 mFixedTendons(NULL), mNumFixedTendons(0), mSensors(NULL), mSensorForces(NULL),
183 mNbSensors(0), mDt(0.f), mDofs(0xffffffff),
184 mDataDirty(true)
185 {
186 mRootPreMotionVelocity = Cm::SpatialVectorF::Zero();
187 }
188
189 ~ArticulationData();
190
191 PX_FORCE_INLINE void init();
192 void resizeLinkData(const PxU32 linkCount);
193 void resizeJointData(const PxU32 dofs);
194
195 PX_FORCE_INLINE PxReal* getJointAccelerations() { return mJointAcceleration.begin(); }
196 PX_FORCE_INLINE const PxReal* getJointAccelerations() const { return mJointAcceleration.begin(); }
197 PX_FORCE_INLINE PxReal* getJointVelocities() { return mJointVelocity.begin(); }
198 PX_FORCE_INLINE const PxReal* getJointVelocities() const { return mJointVelocity.begin(); }
199 PX_FORCE_INLINE PxReal* getJointNewVelocities() { return mJointNewVelocity.begin(); }
200 PX_FORCE_INLINE const PxReal* getJointNewVelocities() const { return mJointNewVelocity.begin(); }
201 PX_FORCE_INLINE PxReal* getJointPositions() { return mJointPosition.begin(); }
202 PX_FORCE_INLINE const PxReal* getJointPositions() const { return mJointPosition.begin(); }
203 PX_FORCE_INLINE PxReal* getJointForces() { return mJointForce.begin(); }
204 PX_FORCE_INLINE const PxReal* getJointForces() const { return mJointForce.begin(); }
205 PX_FORCE_INLINE PxReal* getJointConstraintForces() { return mJointConstraintForces.begin(); }
206 PX_FORCE_INLINE const PxReal* getJointConstraintForces() const { return mJointConstraintForces.begin(); }
207 //PX_FORCE_INLINE PxReal* getJointFrictionForces() { return mJointFrictionForce.begin(); }
208
209 PX_FORCE_INLINE ArticulationInternalConstraint& getInternalConstraint(const PxU32 dofId) { return mInternalConstraints[dofId]; }
210 PX_FORCE_INLINE const ArticulationInternalConstraint& getInternalConstraint(const PxU32 dofId) const { return mInternalConstraints[dofId]; }
211
212 PX_FORCE_INLINE Cm::SpatialVectorF* getMotionVelocities() { return mMotionVelocities.begin(); }
213 PX_FORCE_INLINE Cm::SpatialVectorF* getMotionAccelerations() { return mMotionAccelerations.begin(); }
214 PX_FORCE_INLINE const Cm::SpatialVectorF* getMotionAccelerations() const { return mMotionAccelerations.begin(); }
215 PX_FORCE_INLINE Cm::SpatialVectorF* getCorioliseVectors() { return mCorioliseVectors.begin(); }
216 PX_FORCE_INLINE Cm::SpatialVectorF* getSpatialZAVectors() { return mZAForces.begin(); }
217 PX_FORCE_INLINE Cm::SpatialVectorF* getTransmittedForces() { return mJointTransmittedForce.begin(); }
218
219 PX_FORCE_INLINE Cm::SpatialVectorF* getPosIterMotionVelocities() { return mPosIterMotionVelocities.begin(); }
220 PX_FORCE_INLINE const Cm::SpatialVectorF* getPosIterMotionVelocities() const { return mPosIterMotionVelocities.begin(); }
221 PX_FORCE_INLINE PxReal* getPosIterJointVelocities() { return mPosIterJointVelocities.begin(); }
222
223 PX_FORCE_INLINE Cm::SpatialVectorF& getPosIterMotionVelocity(const PxU32 index) { return mPosIterMotionVelocities[index]; }
224 PX_FORCE_INLINE const Cm::SpatialVectorF& getMotionVelocity(const PxU32 index) const { return mMotionVelocities[index]; }
225 PX_FORCE_INLINE const Cm::SpatialVectorF& getMotionAcceleration(const PxU32 index) const { return mMotionAccelerations[index]; }
226 PX_FORCE_INLINE const Cm::SpatialVectorF& getCorioliseVector(const PxU32 index) const { return mCorioliseVectors[index]; }
227 PX_FORCE_INLINE const Cm::SpatialVectorF& getSpatialZAVector(const PxU32 index) const { return mZAForces[index]; }
228 PX_FORCE_INLINE const Cm::SpatialVectorF& getTransmittedForce(const PxU32 index) const { return mJointTransmittedForce[index]; }
229
230 PX_FORCE_INLINE Cm::SpatialVectorF& getMotionVelocity(const PxU32 index) { return mMotionVelocities[index]; }
231 PX_FORCE_INLINE Cm::SpatialVectorF& getMotionAcceleration(const PxU32 index) { return mMotionAccelerations[index]; }
232 PX_FORCE_INLINE Cm::SpatialVectorF& getCorioliseVector(const PxU32 index) { return mCorioliseVectors[index]; }
233 PX_FORCE_INLINE Cm::SpatialVectorF& getSpatialZAVector(const PxU32 index) { return mZAForces[index]; }
234 PX_FORCE_INLINE Cm::SpatialVectorF& getTransmittedForce(const PxU32 index) { return mJointTransmittedForce[index]; }
235
236 //PX_FORCE_INLINE Dy::SpatialMatrix* getTempSpatialMatrix() { mTempSpatialMatrix.begin(); }
237
238 PX_FORCE_INLINE PxTransform& getPreTransform(const PxU32 index) { return mPreTransform[index]; }
239 PX_FORCE_INLINE const PxTransform& getPreTransform(const PxU32 index) const { return mPreTransform[index]; }
240// PX_FORCE_INLINE void setPreTransform(const PxU32 index, const PxTransform& t){ mPreTransform[index] = t; }
241 PX_FORCE_INLINE PxTransform* getPreTransform() { return mPreTransform.begin(); }
242
243 PX_FORCE_INLINE const Cm::SpatialVectorF& getDeltaMotionVector(const PxU32 index) const { return mDeltaMotionVector[index]; }
244 PX_FORCE_INLINE void setDeltaMotionVector(const PxU32 index, const Cm::SpatialVectorF& vec) { mDeltaMotionVector[index] = vec; }
245 PX_FORCE_INLINE Cm::SpatialVectorF* getDeltaMotionVector() { return mDeltaMotionVector.begin(); }
246
247 PX_FORCE_INLINE ArticulationLink* getLinks() const { return mLinks; }
248 PX_FORCE_INLINE PxU32 getLinkCount() const { return mLinkCount; }
249 PX_FORCE_INLINE ArticulationLink& getLink(PxU32 index) const { return mLinks[index]; }
250
251 PX_FORCE_INLINE ArticulationSpatialTendon** getSpatialTendons() const { return mSpatialTendons; }
252 PX_FORCE_INLINE PxU32 getSpatialTendonCount() const { return mNumSpatialTendons; }
253 PX_FORCE_INLINE ArticulationSpatialTendon* getSpatialTendon(PxU32 index) const { return mSpatialTendons[index]; }
254
255 PX_FORCE_INLINE ArticulationFixedTendon** getFixedTendons() const { return mFixedTendons; }
256 PX_FORCE_INLINE PxU32 getFixedTendonCount() const { return mNumFixedTendons; }
257 PX_FORCE_INLINE ArticulationFixedTendon* getFixedTendon(PxU32 index) const { return mFixedTendons[index]; }
258
259 PX_FORCE_INLINE ArticulationSensor** getSensors() const { return mSensors; }
260 PX_FORCE_INLINE PxU32 getSensorCount() const { return mNbSensors; }
261
262
263 PX_FORCE_INLINE ArticulationLinkData* getLinkData() const { return mLinksData; }
264 ArticulationLinkData& getLinkData(PxU32 index) const;
265
266 PX_FORCE_INLINE ArticulationJointCoreData* getJointData() const { return mJointData; }
267 PX_FORCE_INLINE ArticulationJointCoreData& getJointData(PxU32 index) const { return mJointData[index]; }
268
269 PX_FORCE_INLINE ArticulationJointTargetData* getJointTranData() const { return mJointTranData; }
270 PX_FORCE_INLINE ArticulationJointTargetData& getJointTranData(PxU32 index) const { return mJointTranData[index]; }
271 // PT: PX-1399
272 PX_FORCE_INLINE PxArticulationFlags getArticulationFlags() const { return *mFlags; }
273
274 PX_FORCE_INLINE Cm::SpatialVector* getExternalAccelerations() { return mExternalAcceleration; }
275
276 PX_FORCE_INLINE Cm::SpatialVector& getExternalAcceleration(const PxU32 linkID) { return mExternalAcceleration[linkID]; }
277 PX_FORCE_INLINE const Cm::SpatialVector& getExternalAcceleration(const PxU32 linkID) const { return mExternalAcceleration[linkID]; }
278
279 PX_FORCE_INLINE PxReal getDt() const { return mDt; }
280 PX_FORCE_INLINE void setDt(const PxReal dt) { mDt = dt; }
281
282 PX_FORCE_INLINE bool getDataDirty() const { return mDataDirty; }
283 PX_FORCE_INLINE void setDataDirty(const bool dirty) { mDataDirty = dirty; }
284
285 PX_FORCE_INLINE PxU32 getDofs() const { return mDofs; }
286 PX_FORCE_INLINE void setDofs(const PxU32 dof) { mDofs = dof; }
287
288 PX_FORCE_INLINE FeatherstoneArticulation* getArticulation() { return mArticulation; }
289 PX_FORCE_INLINE void setArticulation(FeatherstoneArticulation* articulation) { mArticulation = articulation; }
290
291 PX_FORCE_INLINE const SpatialMatrix& getBaseInvSpatialArticulatedInertiaW() const { return mBaseInvSpatialArticulatedInertiaW; }
292
293 PX_FORCE_INLINE PxTransform* getAccumulatedPoses() { return mAccumulatedPoses.begin(); }
294 PX_FORCE_INLINE const PxTransform* getAccumulatedPoses() const { return mAccumulatedPoses.begin(); }
295
296 PX_FORCE_INLINE Cm::SpatialVectorF* getJointSpaceJacobians() { return mJointSpaceJacobians.begin(); }
297 PX_FORCE_INLINE const Cm::SpatialVectorF* getJointSpaceJacobians() const { return mJointSpaceJacobians.begin(); }
298
299 PX_FORCE_INLINE JointSpaceSpatialZ* getJointSpaceDeltaV() { return mJointSpaceDeltaVMatrix.begin(); }
300 PX_FORCE_INLINE const JointSpaceSpatialZ* getJointSpaceDeltaV() const { return mJointSpaceDeltaVMatrix.begin(); }
301
302 PX_FORCE_INLINE Cm::SpatialVectorF* getJointSpaceResponse() { return mJointSpaceResponseMatrix.begin(); }
303 PX_FORCE_INLINE const Cm::SpatialVectorF* getJointSpaceResponse() const { return mJointSpaceResponseMatrix.begin(); }
304
305 PX_FORCE_INLINE SpatialImpulseResponseMatrix* getRootResponseMatrix() { return mRootResponseMatrix.begin(); }
306 PX_FORCE_INLINE const SpatialImpulseResponseMatrix* getRootResponseMatrix() const { return mRootResponseMatrix.begin(); }
307
308 PX_FORCE_INLINE const Cm::SpatialVectorF& getRootDeferredZ() const { return mRootDeferredZ; }
309 PX_FORCE_INLINE Cm::SpatialVectorF& getRootDeferredZ() { return mRootDeferredZ; }
310
311
312
313 PX_FORCE_INLINE SpatialImpulseResponseMatrix* getImpulseResponseMatrixWorld() { return mResponseMatrixW.begin(); }
314
315 PX_FORCE_INLINE const SpatialImpulseResponseMatrix* getImpulseResponseMatrixWorld() const { return mResponseMatrixW.begin(); }
316
317 PX_FORCE_INLINE const SpatialMatrix& getWorldSpatialArticulatedInertia(const PxU32 linkID) const { return mWorldSpatialArticulatedInertia[linkID]; }
318
319
320 PX_FORCE_INLINE const InvStIs& getInvStIs(const PxU32 linkID) const { return mInvStIs[linkID]; }
321
322 PX_FORCE_INLINE const Cm::UnAlignedSpatialVector& getMotionMatrix(const PxU32 dofId) const { return mMotionMatrix[dofId]; }
323 PX_FORCE_INLINE const Cm::UnAlignedSpatialVector& getWorldMotionMatrix(const PxU32 dofId) const { return mWorldMotionMatrix[dofId]; }
324
325 PX_FORCE_INLINE Cm::UnAlignedSpatialVector& getJointAxis(const PxU32 dofId) { return mJointAxis[dofId]; }
326 PX_FORCE_INLINE const Cm::UnAlignedSpatialVector& getJointAxis(const PxU32 dofId) const { return mJointAxis[dofId]; }
327
328 PX_FORCE_INLINE const PxVec3& getRw(const PxU32 linkID) const { return mRw[linkID]; }
329
330 PX_FORCE_INLINE const Cm::SpatialVectorF& getIsW(const PxU32 dofId) const { return mIsW[dofId]; }
331
332 PX_FORCE_INLINE const Cm::SpatialVectorF& getWorldIsInvD(const PxU32 dofId) const { return mIsInvDW[dofId]; }
333 PX_FORCE_INLINE PxReal* getDeferredQstZ() { return mDeferredQstZ.begin(); }
334
335 PX_FORCE_INLINE PxReal* getQstZic() { return qstZIc.begin(); }
336
337 PX_FORCE_INLINE Cm::SpatialVectorF& getSolverSpatialForce(const PxU32 linkID) { return mSolverSpatialForces[linkID]; }
338 PX_FORCE_INLINE PxSpatialForce* getSensorForces() { return mSensorForces; }
339 PX_FORCE_INLINE void setRootPreMotionVelocity(const Cm::UnAlignedSpatialVector& vel) { mRootPreMotionVelocity.top = vel.top; mRootPreMotionVelocity.bottom = vel.bottom; }
340
341 PX_FORCE_INLINE PxU32* getPathToRootElements() const { return mPathToRootElements; }
342 PX_FORCE_INLINE PxU32 getPathToRootElementCount() const { return mNumPathToRootElements; }
343
344 private:
345 Cm::SpatialVectorF mRootPreMotionVelocity;
346 Cm::SpatialVectorF mRootDeferredZ;
347 PxArray<PxReal> mJointAcceleration; // joint acceleration
348 PxArray<PxReal> mJointInternalAcceleration; //joint internal force acceleration
349 PxArray<PxReal> mJointVelocity; // joint velocity
350 PxArray<PxReal> mJointNewVelocity; // joint velocity due to contacts
351 PxArray<PxReal> mJointPosition; // joint position
352 PxArray<PxReal> mJointForce; // joint force
353 //Ps::Array<PxReal> mJointFrictionForce; // joint friction force
354
355 PxArray<PxReal> mPosIterJointVelocities; //joint delta velocity after postion iternation before velocity iteration
356 PxArray<Cm::SpatialVectorF> mPosIterMotionVelocities; //link motion velocites after position iteration before velocity iteration
357 PxArray<Cm::SpatialVectorF> mMotionVelocities; //link motion velocites
358 PxArray<Cm::SpatialVectorF> mSolverSpatialForces;
359 PxArray<Cm::SpatialVectorF> mMotionAccelerations; //link motion accelerations
360 PxArray<Cm::SpatialVectorF> mMotionAccelerationsInternal; //link motion accelerations
361 PxArray<Cm::SpatialVectorF> mCorioliseVectors; //link coriolise vector
362 PxArray<Cm::SpatialVectorF> mZAInternalForces; //link internal spatial forces
363 PxArray<Cm::SpatialVectorF> mZAForces; //link spatial zero acceleration force/ spatial articulated force
364 PxArray<Cm::SpatialVectorF> mJointTransmittedForce;
365 PxArray<ArticulationInternalConstraint> mInternalConstraints;
366 PxArray<ArticulationInternalLimit> mInternalLimits;
367 PxArray<ArticulationInternalTendonConstraint> mInternalSpatialTendonConstraints;
368 PxArray<ArticulationInternalTendonConstraint> mInternalFixedTendonConstraints;
369
370
371 PxArray<PxReal> mDeferredQstZ;
372
373 PxArray<PxReal> mJointConstraintForces;
374
375 PxArray<Cm::SpatialVectorF> mDeltaMotionVector; //this is for TGS solver
376 PxArray<PxTransform> mPreTransform; //this is the previous transform list for links
377 PxArray<SpatialImpulseResponseMatrix> mResponseMatrixW;
378 PxArray<Cm::SpatialVectorF> mJointSpaceJacobians;
379 PxArray<JointSpaceSpatialZ> mJointSpaceDeltaVMatrix;
380 PxArray<Cm::SpatialVectorF> mJointSpaceResponseMatrix;
381 PxArray<Cm::SpatialVectorF> mPropagationAccelerator;
382 PxArray<SpatialImpulseResponseMatrix> mRootResponseMatrix;
383 PxArray<SpatialMatrix> mWorldSpatialArticulatedInertia;
384 PxArray<PxMat33> mWorldIsolatedSpatialArticulatedInertia;
385 PxArray<PxReal> mMasses;
386 PxArray<InvStIs> mInvStIs;
387 PxArray<Cm::SpatialVectorF> mIsW;
388 PxArray<PxReal> qstZIc;//jointForce - stZIc
389 PxArray<PxReal> qstZIntIc;
390 PxArray<Cm::UnAlignedSpatialVector> mJointAxis;
391 PxArray<Cm::UnAlignedSpatialVector> mMotionMatrix;
392 PxArray<Cm::UnAlignedSpatialVector> mWorldMotionMatrix;
393 PxArray<Cm::SpatialVectorF> mIsInvDW;
394 PxArray<PxVec3> mRw;
395
396 PxArray<PxU32> mNbStatic1DConstraints;
397 PxArray<PxU32> mNbStaticContactConstraints;
398
399 PxArray<PxU32> mStatic1DConstraintStartIndex;
400 PxArray<PxU32> mStaticContactConstraintStartIndex;
401
402 PxArray<PxQuat> mRelativeQuat;
403
404 ArticulationLink* mLinks;
405 PxU32 mLinkCount;
406 PxU32* mPathToRootElements;
407 PxU32 mNumPathToRootElements;
408 ArticulationLinkData* mLinksData;
409 ArticulationJointCoreData* mJointData;
410 ArticulationJointTargetData* mJointTranData;
411 ArticulationSpatialTendon** mSpatialTendons;
412 PxU32 mNumSpatialTendons;
413 PxU32 mNumTotalAttachments;
414 ArticulationFixedTendon** mFixedTendons;
415 PxU32 mNumFixedTendons;
416 ArticulationSensor** mSensors;
417 PxSpatialForce* mSensorForces;
418 PxU32 mNbSensors;
419 PxReal mDt;
420 PxU32 mDofs;
421 const PxArticulationFlags* mFlags; // PT: PX-1399
422 Cm::SpatialVector* mExternalAcceleration;
423 bool mDataDirty; //this means we need to call commonInit()
424 bool mJointDirty; //this means joint delta velocity has been changed by contacts so we need to update joint velocity/joint acceleration
425 FeatherstoneArticulation* mArticulation;
426
427 PxArray<PxTransform> mAccumulatedPoses;
428 PxArray<PxQuat> mDeltaQ;
429
430 SpatialMatrix mBaseInvSpatialArticulatedInertiaW;
431
432 PxReal mInvSumMass;
433 PxVec3 mCOM;
434
435 friend class FeatherstoneArticulation;
436 };
437
438
439 void ArticulationData::init()
440 {
441 //zero delta motion vector for TGS solver
442 PxMemZero(getDeltaMotionVector(), sizeof(Cm::SpatialVectorF) * mLinkCount);
443 PxMemZero(getPosIterMotionVelocities(), sizeof(Cm::SpatialVectorF) * mLinkCount);
444 mJointDirty = false;
445 }
446
447 struct ScratchData
448 {
449 public:
450 ScratchData()
451 {
452 motionVelocities = NULL;
453 motionAccelerations = NULL;
454 coriolisVectors = NULL;
455 spatialZAVectors = NULL;
456 externalAccels = NULL;
457 compositeSpatialInertias = NULL;
458
459 jointVelocities = NULL;
460 jointAccelerations = NULL;
461 jointForces = NULL;
462 jointPositions = NULL;
463 jointFrictionForces = NULL;
464 }
465
466 Cm::SpatialVectorF* motionVelocities;
467 Cm::SpatialVectorF* motionAccelerations;
468 Cm::SpatialVectorF* coriolisVectors;
469 Cm::SpatialVectorF* spatialZAVectors;
470 Cm::SpatialVector* externalAccels;
471 Dy::SpatialMatrix* compositeSpatialInertias;
472
473 PxReal* jointVelocities;
474 PxReal* jointAccelerations;
475 PxReal* jointForces;
476 PxReal* jointPositions;
477 PxReal* jointFrictionForces;
478 };
479
480 struct InternalConstraintSolverData
481 {
482 const PxReal dt;
483 const PxReal invDt;
484 const PxReal elapsedTime;
485 const PxReal erp;
486 Cm::SpatialVectorF* impulses;
487 Cm::SpatialVectorF* deltaV;
488 const bool velocityIteration;
489 const bool isTGS;
490 PxU32 dofId;
491 PxU32 complexId;
492 PxU32 limitId;
493 PxU32 articId;
494
495 InternalConstraintSolverData(const PxReal dt_, const PxReal invDt_, const PxReal elapsedTime_,
496 const PxReal erp_, Cm::SpatialVectorF* impulses_, Cm::SpatialVectorF* deltaV_,
497 bool velocityIteration_, bool isTGS_) : dt(dt_), invDt(invDt_), elapsedTime(elapsedTime_),
498 erp(erp_), impulses(impulses_), deltaV(deltaV_), velocityIteration(velocityIteration_),
499 isTGS(isTGS_), dofId(0), complexId(0), limitId(0)
500 {
501 }
502
503 PX_NOCOPY(InternalConstraintSolverData)
504 };
505
506 struct FixedTendonSolveData
507 {
508 ArticulationLink* links;
509 ArticulationTendonJoint* tendonJoints;
510 PxReal rootVel;
511 PxReal rootImp;
512 PxReal erp;
513 PxReal error;
514 PxReal limitError;
515 };
516
517#if PX_VC
518#pragma warning(push)
519#pragma warning( disable : 4324 ) // Padding was added at the end of a structure because of a __declspec(align) value.
520#endif
521
522 //Articulation dirty flag - used to tag which properties of the articulation are dirty. Used only to transfer selected data to the GPU...
523 struct ArticulationDirtyFlag
524 {
525 enum Enum
526 {
527 eDIRTY_JOINTS = 1 << 0,
528 eDIRTY_POSITIONS = 1 << 1,
529 eDIRTY_VELOCITIES = 1 << 2,
530 eDIRTY_ACCELERATIONS = 1 << 3,
531 eDIRTY_FORCES = 1 << 4,
532 eDIRTY_ROOT_TRANSFORM = 1 << 5,
533 eDIRTY_ROOT_VELOCITIES = 1 << 6,
534 eDIRTY_LINKS = 1 << 7,
535 eIN_DIRTY_LIST = 1 << 8,
536 eDIRTY_WAKECOUNTER = 1 << 9,
537 eDIRTY_EXT_ACCEL = 1 << 10,
538 eDIRTY_LINK_FORCE = 1 << 11,
539 eDIRTY_LINK_TORQUE = 1 << 12,
540 eDIRTY_JOINT_TARGET_VEL = 1 << 13,
541 eDIRTY_JOINT_TARGET_POS = 1 << 14,
542 ePENDING_INSERTION = 1 << 15,
543 eDIRTY_SPATIAL_TENDON = 1 << 16,
544 eDIRTY_SPATIAL_TENDON_ATTACHMENT = 1 << 17,
545 eDIRTY_FIXED_TENDON = 1 << 18,
546 eDIRTY_FIXED_TENDON_JOINT = 1 << 19,
547 eDIRTY_SENSOR = 1 << 20,
548 eDIRTY_VELOCITY_LIMITS = 1 << 21,
549 eDIRTY_DOFS = (eDIRTY_POSITIONS | eDIRTY_VELOCITIES | eDIRTY_ACCELERATIONS | eDIRTY_FORCES),
550 eALL = (1<<21)-1
551 };
552 };
553
554 PX_INLINE PX_CUDA_CALLABLE void computeArticJacobianAxes(PxVec3 row[3], const PxQuat& qa, const PxQuat& qb)
555 {
556 // Compute jacobian matrix for (qa* qb) [[* means conjugate in this expr]]
557 // d/dt (qa* qb) = 1/2 L(qa*) R(qb) (omega_b - omega_a)
558 // result is L(qa*) R(qb), where L(q) and R(q) are left/right q multiply matrix
559
560 const PxReal wa = qa.w, wb = qb.w;
561 const PxVec3 va(qa.x, qa.y, qa.z), vb(qb.x, qb.y, qb.z);
562
563 const PxVec3 c = vb*wa + va*wb;
564 const PxReal d0 = wa*wb;
565 const PxReal d1 = va.dot(vb);
566 const PxReal d = d0 - d1;
567
568 row[0] = (va * vb.x + vb * va.x + PxVec3(d, c.z, -c.y)) * 0.5f;
569 row[1] = (va * vb.y + vb * va.y + PxVec3(-c.z, d, c.x)) * 0.5f;
570 row[2] = (va * vb.z + vb * va.z + PxVec3(c.y, -c.x, d)) * 0.5f;
571
572 if ((d0 + d1) != 0.0f) // check if relative rotation is 180 degrees which can lead to singular matrix
573 return;
574 else
575 {
576 row[0].x += PX_EPS_F32;
577 row[1].y += PX_EPS_F32;
578 row[2].z += PX_EPS_F32;
579 }
580 }
581
582 PX_CUDA_CALLABLE PX_FORCE_INLINE float compAng(PxReal swingYZ, PxReal swingW)
583 {
584 return 4.0f * PxAtan2(swingYZ, 1.0f + swingW); // tan (t/2) = sin(t)/(1+cos t), so this is the quarter angle
585 }
586
587
588 PX_ALIGN_PREFIX(64)
589 class FeatherstoneArticulation
590 {
591 PX_NOCOPY(FeatherstoneArticulation)
592 public:
593 // public interface
594
595 explicit FeatherstoneArticulation(void*);
596 ~FeatherstoneArticulation();
597
598 // get data sizes for allocation at higher levels
599 void getDataSizes(PxU32 linkCount, PxU32& solverDataSize, PxU32& totalSize, PxU32& scratchSize);
600
601 bool resize(const PxU32 linkCount);
602
603 void assignTendons(const PxU32 /*nbTendons*/, Dy::ArticulationSpatialTendon** /*tendons*/);
604
605 void assignTendons(const PxU32 /*nbTendons*/, Dy::ArticulationFixedTendon** /*tendons*/);
606
607 void assignSensors(const PxU32 nbSensors, Dy::ArticulationSensor** sensors, PxSpatialForce* sensorForces);
608
609 PxU32 getDofs();
610
611 PxU32 getDof(const PxU32 linkID);
612
613 bool applyCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag, bool& shouldWake);
614
615 void copyInternalStateToCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag);
616
617 void packJointData(const PxReal* maximum, PxReal* reduced);
618
619 void unpackJointData(const PxReal* reduced, PxReal* maximum);
620
621 void initializeCommonData();
622
623 //gravity as input, joint force as output
624 void getGeneralizedGravityForce(const PxVec3& gravity, PxArticulationCache& cache);
625
626 //joint velocity as input, generalised force(coriolis and centrigugal force) as output
627 void getCoriolisAndCentrifugalForce(PxArticulationCache& cache);
628
629 //external force as input, joint force as output
630 void getGeneralizedExternalForce(PxArticulationCache& /*cache*/);
631
632 //joint force as input, joint acceleration as output
633 void getJointAcceleration(const PxVec3& gravity, PxArticulationCache& cache);
634
635 //joint acceleration as input, joint force as out
636 void getJointForce(PxArticulationCache& cache);
637
638 void getDenseJacobian(PxArticulationCache& cache, PxU32 & nRows, PxU32 & nCols);
639
640 //These two functions are for closed loop system
641 void getKMatrix(ArticulationJointCore* loopJoint, const PxU32 parentIndex, const PxU32 childIndex, PxArticulationCache& cache);
642
643 void getCoefficientMatrix(const PxReal dt, const PxU32 linkID, const PxContactJoint* contactJoints, const PxU32 nbContacts, PxArticulationCache& cache);
644
645 void getCoefficientMatrixWithLoopJoints(ArticulationLoopConstraint* lConstraints, const PxU32 nbJoints, PxArticulationCache& cache);
646
647 bool getLambda(ArticulationLoopConstraint* lConstraints, const PxU32 nbJoints, PxArticulationCache& cache, PxArticulationCache& rollBackCache,
648 const PxReal* jointTorque, const PxVec3& gravity, const PxU32 maxIter, const PxReal invLengthScale);
649
650 void getGeneralizedMassMatrix(PxArticulationCache& cache);
651
652 void getGeneralizedMassMatrixCRB(PxArticulationCache& cache);
653
654 bool storeStaticConstraint(const PxSolverConstraintDesc& desc);
655
656 bool willStoreStaticConstraint() { return DY_STATIC_CONTACTS_IN_INTERNAL_SOLVER; }
657
658 void setRootLinearVelocity(const PxVec3& velocity);
659 void setRootAngularVelocity(const PxVec3& velocity);
660 void teleportRootLink();
661
662 void getImpulseResponse(
663 PxU32 linkID,
664 Cm::SpatialVectorF* Z,
665 const Cm::SpatialVector& impulse,
666 Cm::SpatialVector& deltaV) const;
667
668 void getImpulseResponse(
669 PxU32 linkID,
670 Cm::SpatialVectorV* /*Z*/,
671 const Cm::SpatialVectorV& impulse,
672 Cm::SpatialVectorV& deltaV) const;
673
674 void getImpulseSelfResponse(
675 PxU32 linkID0,
676 PxU32 linkID1,
677 Cm::SpatialVectorF* Z,
678 const Cm::SpatialVector& impulse0,
679 const Cm::SpatialVector& impulse1,
680 Cm::SpatialVector& deltaV0,
681 Cm::SpatialVector& deltaV1) const;
682
683 Cm::SpatialVectorV getLinkVelocity(const PxU32 linkID) const;
684
685 Cm::SpatialVector getLinkScalarVelocity(const PxU32 linkID) const;
686
687 Cm::SpatialVectorV getLinkMotionVector(const PxU32 linkID) const;
688
689 //this is called by island gen to determine whether the articulation should be awake or sleep
690 Cm::SpatialVector getMotionVelocity(const PxU32 linkID) const;
691
692 Cm::SpatialVector getMotionAcceleration(const PxU32 linkID) const;
693
694 void fillIndexType(const PxU32 linkId, PxU8& indexType);
695
696 PxReal getLinkMaxPenBias(const PxU32 linkID) const;
697
698 PxReal getCfm(const PxU32 linkID) const;
699
700 static PxU32 computeUnconstrainedVelocities(
701 const ArticulationSolverDesc& desc,
702 PxReal dt,
703 PxU32& acCount,
704 const PxVec3& gravity,
705 Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV, const PxReal invLengthScale);
706
707 static void computeUnconstrainedVelocitiesTGS(
708 const ArticulationSolverDesc& desc,
709 PxReal dt, const PxVec3& gravity,
710 PxU64 contextID, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV,
711 const PxReal invLengthScale);
712
713 static PxU32 setupSolverConstraintsTGS(const ArticulationSolverDesc& articDesc,
714 PxReal dt,
715 PxReal invDt,
716 PxReal totalDt,
717 const PxReal biasCoefficient,
718 PxU32& acCount,
719 Cm::SpatialVectorF* Z);
720
721 static void saveVelocity(const ArticulationSolverDesc& d, Cm::SpatialVectorF* deltaV);
722
723 static void saveVelocityTGS(const ArticulationSolverDesc& d, PxReal invDtF32);
724
725 static void updateBodies(const ArticulationSolverDesc& desc, Cm::SpatialVectorF* tempDeltaV, PxReal dt);
726
727 static void updateBodiesTGS(const ArticulationSolverDesc& desc, Cm::SpatialVectorF* tempDeltaV, PxReal dt);
728
729 static void updateBodies(FeatherstoneArticulation* articulation, Cm::SpatialVectorF* tempDeltaV, PxReal dt, bool integrateJointPosition);
730
731 static void recordDeltaMotion(const ArticulationSolverDesc& desc, const PxReal dt, Cm::SpatialVectorF* deltaV, const PxReal totalInvDt);
732
733 static void deltaMotionToMotionVelocity(const ArticulationSolverDesc& desc, PxReal invDt);
734
735 void pxcFsApplyImpulse(PxU32 linkID, aos::Vec3V linear,
736 aos::Vec3V angular, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
737
738 void pxcFsApplyImpulses(PxU32 linkID, const aos::Vec3V& linear,
739 const aos::Vec3V& angular, PxU32 linkID2, const aos::Vec3V& linear2,
740 const aos::Vec3V& angular2, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
741
742 void pxcFsApplyImpulses(Cm::SpatialVectorF* Z);
743
744 Cm::SpatialVectorV pxcFsGetVelocity(PxU32 linkID);
745
746 void pxcFsGetVelocities(PxU32 linkID, PxU32 linkID1, Cm::SpatialVectorV& v0, Cm::SpatialVectorV& v1);
747
748 Cm::SpatialVectorV pxcFsGetVelocityTGS(PxU32 linkID);
749
750 const PxTransform& getCurrentTransform(PxU32 linkID) const;
751
752 const PxQuat& getDeltaQ(PxU32 linkID) const;
753
754 //Applies a set of N impulses, all in local space and updates the links' motion and joint velocities
755 void applyImpulses(Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
756 void getDeltaV(Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
757
758 //This method calculate the velocity change due to collision/constraint impulse, record joint velocity and acceleration
759 static Cm::SpatialVectorF propagateVelocityW(const PxVec3& c2p, const Dy::SpatialMatrix& spatialInertia,
760 const InvStIs& invStIs, const Cm::UnAlignedSpatialVector* motionMatrix, const Cm::SpatialVectorF& Z,
761 PxReal* jointVelocity, const Cm::SpatialVectorF& hDeltaV, const PxU32 dofCount);
762
763 static Cm::SpatialVectorF propagateAccelerationW(const PxVec3& c2p,
764 const InvStIs& invStIs, const Cm::UnAlignedSpatialVector* motionMatrix,
765 PxReal* jointVelocity, const Cm::SpatialVectorF& pAcceleration, const PxU32 dofCount, const Cm::SpatialVectorF* IsW, PxReal* qstZIc);
766
767 static void propagateAccelerationW(const PxVec3& c2p,
768 const InvStIs& invStIs, PxReal* jointVelocity, const Cm::SpatialVectorF& pAcceleration,
769 const PxU32 dofCount, const Cm::SpatialVectorF* IsW);
770
771 static Cm::SpatialVectorF propagateAccelerationW(const PxVec3& c2p,
772 const InvStIs& invStIs, const Cm::UnAlignedSpatialVector* motionMatrix,
773 const Cm::SpatialVectorF& pAcceleration, const PxU32 dofCount, const Cm::SpatialVectorF* IsW, PxReal* qstZIc);
774
775 static Cm::SpatialVectorF propagateAccelerationW(const PxVec3& c2p,
776 const InvStIs& invStIs, const Cm::UnAlignedSpatialVector* motionMatrix,
777 PxReal* jointVelocity, const Cm::SpatialVectorF& pAcceleration, Cm::SpatialVectorF& Z, const PxU32 dofCount, const Cm::SpatialVectorF* IsW);
778
779 //This method calculate the velocity change due to collision/constraint impulse
780 static Cm::SpatialVectorF propagateVelocityTestImpulseW(const PxVec3& c2p, const Dy::SpatialMatrix& spatialInertia, const InvStIs& invStIs,
781 const Cm::UnAlignedSpatialVector* motionMatrix, const Cm::SpatialVectorF& Z, const Cm::SpatialVectorF& hDeltaV,
782 const PxU32 dofCount);
783
784
786 //static Cm::SpatialVectorF propagateImpulse(const IsInvD& isInvD, const SpatialTransform& childToParent,
787 // const SpatialSubspaceMatrix& motionMatrix, const Cm::SpatialVectorF& Z);
788
789 static Cm::SpatialVectorF propagateImpulseW(const Cm::SpatialVectorF* isInvD, const PxVec3& childToParent,
790 const Cm::UnAlignedSpatialVector* motionMatrix, const Cm::SpatialVectorF& Z, const PxU32 dofCount);
791
792 static Cm::SpatialVectorF propagateImpulseW(const Cm::SpatialVectorF* isInvD, const PxVec3& childToParent,
793 const Cm::UnAlignedSpatialVector* motionMatrix, const Cm::SpatialVectorF& Z, const PxU32 dofCount, PxReal* qstZ);
794
795 bool applyCacheToDest(ArticulationData& data, PxArticulationCache& cache,
796 PxReal* jVelocities, PxReal* jAcceleration, PxReal* jPosition, PxReal* jointForce,
797 const PxArticulationCacheFlags flag, bool& shouldWake);
798
799 PX_FORCE_INLINE ArticulationData& getArticulationData() { return mArticulationData; }
800 PX_FORCE_INLINE const ArticulationData& getArticulationData() const { return mArticulationData; }
801
802 PX_FORCE_INLINE void setGpuDirtyFlag(ArticulationDirtyFlag::Enum flag)
803 {
804 mGPUDirtyFlags |= flag;
805 }
806 //void setGpuRemapId(const PxU32 id) { mGpuRemapId = id; }
807 //PxU32 getGpuRemapId() { return mGpuRemapId; }
808
809 static PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF translateSpatialVector(const PxVec3& offset, const Cm::SpatialVectorF& vec)
810 {
811 return Cm::SpatialVectorF(vec.top, vec.bottom + offset.cross(vec.top));
812 }
813
814 static PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::UnAlignedSpatialVector translateSpatialVector(const PxVec3& offset, const Cm::UnAlignedSpatialVector& vec)
815 {
816 return Cm::UnAlignedSpatialVector(vec.top, vec.bottom + offset.cross(vec.top));
817 }
818
819 static PX_FORCE_INLINE PxMat33 constructSkewSymmetricMatrix(const PxVec3 r)
820 {
821 return PxMat33(PxVec3(0.0f, r.z, -r.y),
822 PxVec3(-r.z, 0.0f, r.x),
823 PxVec3(r.y, -r.x, 0.0f));
824 }
825
826 bool raiseGPUDirtyFlag(ArticulationDirtyFlag::Enum flag)
827 {
828 bool nothingRaised = !(mGPUDirtyFlags);
829 mGPUDirtyFlags |= flag;
830 return nothingRaised;
831 }
832
833 void clearGPUDirtyFlags()
834 {
835 mGPUDirtyFlags = 0;
836 }
837
838 public:
839 void constraintPrep(ArticulationLoopConstraint* lConstraints, const PxU32 nbJoints,
840 Cm::SpatialVectorF* Z, PxSolverConstraintPrepDesc& prepDesc, PxSolverBody& sBody,
841 PxSolverBodyData& sBodyData, PxSolverConstraintDesc* desc, PxConstraintAllocator& allocator);
842
843 void updateArticulation(ScratchData& scratchData,
844 const PxVec3& gravity,
845 Cm::SpatialVectorF* Z,
846 Cm::SpatialVectorF* DeltaV,
847 const PxReal invLengthScale);
848
849 void computeUnconstrainedVelocitiesInternal(
850 const PxVec3& gravity,
851 Cm::SpatialVectorF* Z, Cm::SpatialVectorF* DeltaV, const PxReal invLengthScale);
852
853 //copy joint data from fromJointData to toJointData
854 void copyJointData(ArticulationData& data, PxReal* toJointData, const PxReal* fromJointData);
855
856 PxU32 computeDofs();
857 //this function calculates motion subspace matrix(s) for all tree joint
858 void jcalc(ArticulationData& data, bool forceUpdate = false);
859
860 //this function calculates loop joint constraint subspace matrix(s) and active force
861 //subspace matrix
862 void jcalcLoopJointSubspace(ArticulationJointCore* joint, ArticulationJointCoreData& jointDatum, SpatialSubspaceMatrix& T,
863 const Cm::UnAlignedSpatialVector* jointAxis);
864
865 void computeSpatialInertia(ArticulationData& data);
866
867 //compute zero acceleration force
868 void computeZ(const ArticulationData& data, const PxVec3& gravity, ScratchData& scratchData);
869 void computeZD(const ArticulationData& data, const PxVec3& gravity, ScratchData& scratchData);
870
871 void solveInternalConstraints(const PxReal dt, const PxReal invDt, Cm::SpatialVectorF* impulses, Cm::SpatialVectorF* DeltaV,
872 bool velocityIteration, bool isTGS, const PxReal elapsedTime, const PxReal biasCoefficient);
873
874
875 void solveInternalJointConstraints(const PxReal dt, const PxReal invDt, Cm::SpatialVectorF* impulses, Cm::SpatialVectorF* DeltaV,
876 bool velocityIteration, bool isTGS, const PxReal elapsedTime, const PxReal biasCoefficient);
877
878 Cm::SpatialVectorF solveInternalJointConstraintRecursive(InternalConstraintSolverData& data, const PxU32 linkID,
879 const Cm::SpatialVectorF& parentDeltaV);
880
881 void solveInternalSpatialTendonConstraints(bool isTGS);
882
883 void solveInternalFixedTendonConstraints(bool isTGS);
884
885 void writebackInternalConstraints(bool isTGS);
886
887 void concludeInternalConstraints(bool isTGS);
888
889 //compute coriolis force
890 void computeC(ArticulationData& data, ScratchData& scratchData);
891
892 //compute relative transform child to parent
893 void computeRelativeTransformC2P(ArticulationData& data);
894 //compute relative transform child to base
895 void computeRelativeTransformC2B(ArticulationData& data);
896
897 void computeLinkVelocities(ArticulationData& data, ScratchData& scratchData);
898
899
939 static void computeLinkStates(
940 const PxF32 dt, const PxReal invLengthScale, const PxVec3& gravity,
941 const bool fixBase,
942 const PxU32 linkCount,
943 const PxTransform* accumulatedPoses, const Cm::SpatialVector* externalAccels, const PxVec3* rws, const Cm::UnAlignedSpatialVector* worldMotionMatrices,
944 const Dy::ArticulationJointCoreData* jointCoreData,
945 Dy::ArticulationLinkData *linkData, Dy::ArticulationLink* links, Cm::SpatialVectorF* motionAccelerations,
946 Cm::SpatialVectorF* motionVelocities, Cm::SpatialVectorF* spatialZAForces, Cm::SpatialVectorF* spatialZAInternal, Cm::SpatialVectorF* coriolisVectors,
947 PxMat33* worldIsolatedSpatialArticulatedInertias, PxF32* linkMasses, Dy::SpatialMatrix* worldSpatialArticulatedInertias,
948 const PxU32 jointDofCount,
949 PxReal* jointVelocities,
950 Cm::SpatialVectorF& rootPreMotionVelocity, PxVec3& com, PxF32& invSumMass);
951
952 void initLinks(ArticulationData& data, const PxVec3& gravity,
953 ScratchData& scratchData, Cm::SpatialVectorF* tZ, Cm::SpatialVectorF* tDeltaV);
954
955 void computeIs(ArticulationJointCoreData& jointDatum, ArticulationJointTargetData& jointTarget, const PxU32 linkID);
956 static SpatialMatrix computePropagateSpatialInertia_ZA_ZIc(const PxU8 jointType, const ArticulationJointTargetData& jointTarget, const ArticulationJointCoreData& jointDatum,
957 const SpatialMatrix& articulatedInertia, const Cm::SpatialVectorF* linkIs, InvStIs& invStIs, Cm::SpatialVectorF* isInvD, const Cm::UnAlignedSpatialVector* motionMatrix,
958 const PxReal* jF, const Cm::SpatialVectorF& Z, const Cm::SpatialVectorF& ZIntIc, Cm::SpatialVectorF& ZA, Cm::SpatialVectorF& ZInt, PxReal* qstZ,
959 PxReal* qstZIntIc);
960
961 static SpatialMatrix computePropagateSpatialInertia_ZA_ZIc_NonSeparated(const PxU8 jointType, const ArticulationJointTargetData& jointTarget, const ArticulationJointCoreData& jointDatum,
962 const SpatialMatrix& articulatedInertia, const Cm::SpatialVectorF* linkIs, InvStIs& invStIs, Cm::SpatialVectorF* isInvD, const Cm::UnAlignedSpatialVector* motionMatrix,
963 const PxReal* jF, const Cm::SpatialVectorF& Z, Cm::SpatialVectorF& ZA, PxReal* qstZIc);
964
965 static SpatialMatrix computePropagateSpatialInertia(const PxU8 jointType, ArticulationJointCoreData& jointDatum,
966 const SpatialMatrix& articulatedInertia, const Cm::SpatialVectorF* linkIs, InvStIs& invStIs, Cm::SpatialVectorF* isInvD,
967 const Cm::UnAlignedSpatialVector* motionMatrix);
968
969 static void transformInertia(const SpatialTransform& sTod, SpatialMatrix& inertia);
970
971 static void translateInertia(const PxMat33& offset, SpatialMatrix& inertia);
972
973 static PxMat33 translateInertia(const PxMat33& inertia, const PxReal mass, const PxVec3& t);
974
975 void computeArticulatedSpatialInertiaAndZ(ArticulationData& data, ScratchData& scratchData);
976 void computeArticulatedSpatialInertiaAndZ_NonSeparated(ArticulationData& data, ScratchData& scratchData);
977
978 void computeArticulatedSpatialInertia(ArticulationData& data);
979
980 void computeArticulatedResponseMatrix(ArticulationData& data);
981
982 void computeJointSpaceJacobians(ArticulationData& data);
983
984 void computeArticulatedSpatialZ(ArticulationData& data, ScratchData& scratchData);
985
986 /*void computeJointAcceleration(ArticulationLinkData& linkDatum, ArticulationJointCoreData& jointDatum,
987 const Cm::SpatialVectorF& pMotionAcceleration, PxReal* jointAcceleration, const PxU32 linkID);*/
988
989 void computeJointAccelerationW(ArticulationJointCoreData& jointDatum,
990 const Cm::SpatialVectorF& pMotionAcceleration, PxReal* jointAcceleration, const Cm::SpatialVectorF* IsW, const PxU32 linkID,
991 const PxReal* qstZIc);
992
993 //compute joint acceleration, joint velocity and link acceleration, velocity based
994 //on spatial force and spatial articulated inertia tensor
995 void computeLinkAcceleration(ArticulationData& data, ScratchData& scratchData, bool doIC);
996
997 void computeLinkInternalAcceleration(ArticulationData& data, ScratchData& scratchData);
998
999 //void computeTempLinkAcceleration(ArticulationData& data, ScratchData& scratchData);
1000 void computeJointTransmittedFrictionForce(ArticulationData& data, ScratchData& scratchData,
1001 Cm::SpatialVectorF* Z, Cm::SpatialVectorF* DeltaV);
1002
1003 static Cm::SpatialVectorF getDeltaVWithDeltaJV(const bool fixBase, const PxU32 linkID,
1004 const ArticulationData& data, Cm::SpatialVectorF* Z,
1005 PxReal* jointVelocities);
1006
1007 static Cm::SpatialVectorF getDeltaV(const bool fixBase, const PxU32 linkID,
1008 const ArticulationData& data, Cm::SpatialVectorF* Z);
1009
1010 //impulse need to be in the linkID space
1011 static void getZ(const PxU32 linkID, const ArticulationData& data,
1012 Cm::SpatialVectorF* Z, const Cm::SpatialVectorF& impulse);
1013
1014 //This method use in impulse self response. The input impulse is in the link space
1015 static Cm::SpatialVectorF getImpulseResponseW(
1016 const PxU32 linkID,
1017 const ArticulationData& data,
1018 const Cm::SpatialVectorF& impulse);
1019
1020 //This method use in impulse self response. The input impulse is in the link space
1021 static Cm::SpatialVectorF getImpulseResponseWithJ(
1022 const PxU32 linkID,
1023 const bool fixBase,
1024 const ArticulationData& data,
1025 Cm::SpatialVectorF* Z,
1026 const Cm::SpatialVectorF& impulse,
1027 PxReal* jointVelocities);
1028
1029 void getImpulseSelfResponseInv(const bool fixBase,
1030 PxU32 linkID0,
1031 PxU32 linkID1,
1032 Cm::SpatialVectorF* Z,
1033 const Cm::SpatialVector& impulse0,
1034 const Cm::SpatialVector& impulse1,
1035 Cm::SpatialVector& deltaV0,
1036 Cm::SpatialVector& deltaV1,
1037 PxReal* jointVelocities);
1038
1039 void getImpulseResponseSlowInv(Dy::ArticulationLink* links,
1040 const ArticulationData& data,
1041 PxU32 linkID0_,
1042 const Cm::SpatialVector& impulse0,
1043 Cm::SpatialVector& deltaV0,
1044 PxU32 linkID1_,
1045 const Cm::SpatialVector& impulse1,
1046 Cm::SpatialVector& deltaV1,
1047 PxReal* jointVelocities,
1048 Cm::SpatialVectorF* Z);
1049
1050 Cm::SpatialVectorF getImpulseResponseInv(const bool fixBase,
1051 const PxU32 linkID, Cm::SpatialVectorF* Z,
1052 const Cm::SpatialVector& impulse,
1053 PxReal* jointVelocites);
1054
1055 void inverseDynamic(ArticulationData& data, const PxVec3& gravity,
1056 ScratchData& scratchData, bool computeCoriolis);
1057
1058 void inverseDynamicFloatingBase(ArticulationData& data, const PxVec3& gravity,
1059 ScratchData& scratchData, bool computeCoriolis);
1060
1061 //compute link body force with motion velocity and acceleration
1062 void computeZAForceInv(ArticulationData& data, ScratchData& scratchData);
1063 void initCompositeSpatialInertia(ArticulationData& data, Dy::SpatialMatrix* compositeSpatialInertia);
1064 void computeCompositeSpatialInertiaAndZAForceInv(ArticulationData& data, ScratchData& scratchData);
1065
1066 void computeRelativeGeneralizedForceInv(ArticulationData& data, ScratchData& scratchData);
1067
1068 //provided joint velocity and joint acceleartion, compute link acceleration
1069 void computeLinkAccelerationInv(ArticulationData& data, ScratchData& scratchData);
1070
1071 void computeGeneralizedForceInv(ArticulationData& data, ScratchData& scratchData);
1072
1073 void calculateMassMatrixColInv(ScratchData& scratchData);
1074
1075 void calculateHFixBase(PxArticulationCache& cache);
1076
1077 void calculateHFloatingBase(PxArticulationCache& cache);
1078
1079 //joint limits
1080 void enforcePrismaticLimits(PxReal& jPosition, ArticulationJointCore* joint);
1081
1082
1083 public:
1084
1085 PX_FORCE_INLINE void addBody()
1086 {
1087 mAcceleration.pushBack(Cm::SpatialVector(PxVec3(0.f), PxVec3(0.f)));
1088 mUpdateSolverData = true;
1089 }
1090
1091 PX_FORCE_INLINE void removeBody()
1092 {
1093 mUpdateSolverData = true;
1094 }
1095
1096 PX_FORCE_INLINE bool updateSolverData() { return mUpdateSolverData; }
1097
1098 PX_FORCE_INLINE PxU32 getMaxDepth() const { return mMaxDepth; }
1099 PX_FORCE_INLINE void setMaxDepth(const PxU32 depth) { mMaxDepth = depth; }
1100
1101 // solver methods
1102 PX_FORCE_INLINE PxU32 getBodyCount() const { return mSolverDesc.linkCount; }
1103 PX_FORCE_INLINE void getSolverDesc(ArticulationSolverDesc& d) const { d = mSolverDesc; }
1104 PX_FORCE_INLINE ArticulationSolverDesc& getSolverDesc() { return mSolverDesc; }
1105
1106 PX_FORCE_INLINE ArticulationCore* getCore() { return mSolverDesc.core; }
1107 PX_FORCE_INLINE PxU16 getIterationCounts() const { return mSolverDesc.core->solverIterationCounts; }
1108
1109 PX_FORCE_INLINE void* getUserData() const { return mUserData; }
1110
1111 PX_FORCE_INLINE void setDyContext(Dy::Context* context) { mContext = context; }
1112
1113 void setupLinks(PxU32 nbLinks, Dy::ArticulationLink* links);
1114 void allocatePathToRootElements(const PxU32 totalPathToRootElements);
1115 void initPathToRoot();
1116
1117 static void getImpulseSelfResponse(ArticulationLink* links,
1118 Cm::SpatialVectorF* Z,
1119 ArticulationData& data,
1120 PxU32 linkID0,
1121 const Cm::SpatialVectorV& impulse0,
1122 Cm::SpatialVectorV& deltaV0,
1123 PxU32 linkID1,
1124 const Cm::SpatialVectorV& impulse1,
1125 Cm::SpatialVectorV& deltaV1);
1126
1127 static void getImpulseResponseSlow(Dy::ArticulationLink* links,
1128 ArticulationData& data,
1129 PxU32 linkID0_,
1130 const Cm::SpatialVector& impulse0,
1131 Cm::SpatialVector& deltaV0,
1132 PxU32 linkID1_,
1133 const Cm::SpatialVector& impulse1,
1134 Cm::SpatialVector& deltaV1,
1135 Cm::SpatialVectorF* Z);
1136
1137 PxU32 setupSolverConstraints(
1138 ArticulationLink* links,
1139 const PxU32 linkCount,
1140 const bool fixBase,
1141 ArticulationData& data,
1142 Cm::SpatialVectorF* Z,
1143 PxU32& acCount);
1144
1145 void setupInternalConstraints(
1146 ArticulationLink* links,
1147 const PxU32 linkCount,
1148 const bool fixBase,
1149 ArticulationData& data,
1150 Cm::SpatialVectorF* Z,
1151 PxReal stepDt,
1152 PxReal dt,
1153 PxReal invDt,
1154 PxReal erp,
1155 bool isTGSSolver);
1156
1157 void setupInternalConstraintsRecursive(
1158 ArticulationLink* links,
1159 const PxU32 linkCount,
1160 const bool fixBase,
1161 ArticulationData& data,
1162 Cm::SpatialVectorF* Z,
1163 const PxReal stepDt,
1164 const PxReal dt,
1165 const PxReal invDt,
1166 const PxReal erp,
1167 const bool isTGSSolver,
1168 const PxU32 linkID,
1169 const PxReal maxForceScale);
1170
1171 void setupInternalSpatialTendonConstraintsRecursive(
1172 ArticulationLink* links,
1173 ArticulationAttachment* attachments,
1174 const PxU32 attachmentCount,
1175 const PxVec3& parentAttachmentPoint,
1176 const bool fixBase,
1177 ArticulationData& data,
1178 Cm::SpatialVectorF* Z,
1179 const PxReal stepDt,
1180 const bool isTGSSolver,
1181 const PxU32 attachmentID,
1182 const PxReal stiffness,
1183 const PxReal damping,
1184 const PxReal limitStiffness,
1185 const PxReal err,
1186 const PxU32 startLink,
1187 const PxVec3& startAxis,
1188 const PxVec3& startRaXn);
1189
1190
1191 void setupInternalFixedTendonConstraintsRecursive(
1192 ArticulationLink* links,
1193 ArticulationTendonJoint* tendonJoints,
1194 const bool fixBase,
1195 ArticulationData& data,
1196 Cm::SpatialVectorF* Z,
1197 const PxReal stepDt,
1198 const bool isTGSSolver,
1199 const PxU32 tendonJointID,
1200 const PxReal stiffness,
1201 const PxReal damping,
1202 const PxReal limitStiffness,
1203 const PxU32 startLink,
1204 const PxVec3& startAxis,
1205 const PxVec3& startRaXn);
1206
1207
1208 void updateSpatialTendonConstraintsRecursive(ArticulationAttachment* attachments, ArticulationData& data, const PxU32 attachmentID, const PxReal accumErr,
1209 const PxVec3& parentAttachmentPoint);
1210
1211 //void updateFixedTendonConstraintsRecursive(ArticulationLink* links, ArticulationTendonJoint* tendonJoint, ArticulationData& data, const PxU32 tendonJointID, const PxReal accumErr);
1212
1213 PxVec3 calculateFixedTendonVelocityAndPositionRecursive(FixedTendonSolveData& solveData,
1214 const Cm::SpatialVectorF& parentV, const Cm::SpatialVectorF& parentDeltaV, const PxU32 tendonJointID);
1215
1216 Cm::SpatialVectorF solveFixedTendonConstraintsRecursive(FixedTendonSolveData& solveData,
1217 const PxU32 tendonJointID);
1218
1219 void prepareStaticConstraints(const PxReal dt, const PxReal invDt, PxsContactManagerOutputIterator& outputs,
1220 Dy::ThreadContext& threadContext, PxReal correlationDist, PxReal bounceThreshold, PxReal frictionOffsetThreshold,
1221 PxReal ccdMaxSeparation, PxSolverBodyData* solverBodyData, PxsConstraintBlockManager& blockManager,
1222 Dy::ConstraintWriteback* constraintWritebackPool);
1223
1224 void prepareStaticConstraintsTGS(const PxReal stepDt, const PxReal totalDt, const PxReal invStepDt, const PxReal invTotalDt,
1225 PxsContactManagerOutputIterator& outputs, Dy::ThreadContext& threadContext, PxReal correlationDist, PxReal bounceThreshold,
1226 PxReal frictionOffsetThreshold, PxTGSSolverBodyData* solverBodyData,
1227 PxTGSSolverBodyTxInertia* txInertia, PxsConstraintBlockManager& blockManager, Dy::ConstraintWriteback* constraintWritebackPool,
1228 const PxReal biasCoefficient, const PxReal lengthScale);
1229
1230
1231 //integration
1232 void propagateLinksDown(ArticulationData& data, PxReal* jointVelocities, PxReal* jointPositions,
1233 Cm::SpatialVectorF* motionVelocities);
1234
1235 void updateJointProperties(
1236 PxReal* jointNewVelocities,
1237 PxReal* jointVelocities,
1238 PxReal* jointAccelerations);
1239
1240 void recomputeAccelerations(const PxReal dt);
1241 Cm::SpatialVector recomputeAcceleration(const PxU32 linkID, const PxReal dt) const;
1242
1243 void computeAndEnforceJointPositions(ArticulationData& data);
1244
1245 //update link position based on joint position provided by the cache
1246 void teleportLinks(ArticulationData& data);
1247
1248 void computeLinkVelocities(ArticulationData& data);
1249
1250 PxU8* allocateScratchSpatialData(PxcScratchAllocator* allocator,
1251 const PxU32 linkCount, ScratchData& scratchData, bool fallBackToHeap = false);
1252
1253 //This method calculate the velocity change from parent to child using parent current motion velocity
1254 PxTransform propagateTransform(const PxU32 linkID, ArticulationLink* links, ArticulationJointCoreData& jointDatum,
1255 Cm::SpatialVectorF* motionVelocities, const PxReal dt, const PxTransform& pBody2World, const PxTransform& currentTransform,
1256 PxReal* jointVelocity, PxReal* jointPosition, const Cm::UnAlignedSpatialVector* motionMatrix,
1257 const Cm::UnAlignedSpatialVector* worldMotionMatrix);
1258
1259 static void updateRootBody(const Cm::SpatialVectorF& motionVelocity,
1260 const PxTransform& preTransform, ArticulationData& data, const PxReal dt);
1261
1262 //These variables are used in the constraint partition
1263 PxU16 maxSolverFrictionProgress;
1264 PxU16 maxSolverNormalProgress;
1265 PxU32 solverProgress;
1266 PxU16 mArticulationIndex;
1267 PxU8 numTotalConstraints;
1268
1269 void* mUserData;
1270 Dy::Context* mContext;
1271 ArticulationSolverDesc mSolverDesc;
1272
1273 PxArray<Cm::SpatialVector> mAcceleration; // supplied by Sc-layer to feed into articulations
1274
1275 bool mUpdateSolverData;
1276 PxU32 mMaxDepth;
1277
1278 ArticulationData mArticulationData;
1279
1280 PxArray<PxSolverConstraintDesc> mStaticContactConstraints;
1281 PxArray<PxSolverConstraintDesc> mStatic1DConstraints;
1282 PxU32 mGPUDirtyFlags;
1283
1284
1285 } PX_ALIGN_SUFFIX(64);
1286
1287#if PX_VC
1288#pragma warning(pop)
1289#endif
1290
1291 void PxvRegisterArticulationsReducedCoordinate();
1292
1293
1294} //namespace Dy
1295
1296}
1297
1298#endif
context
Definition base.h:1940
physx::Cm::SpatialVector
Definition CmSpatialVector.h:46
physx::Dy::ArticulationData
Definition DyFeatherstoneArticulation.h:176
physx::Dy::ArticulationFixedTendon
Definition DyArticulationTendon.h:139
physx::Dy::ArticulationJointCoreData
Definition DyFeatherstoneArticulationJointData.h:50
physx::Dy::ArticulationLinkData
Definition DyFeatherstoneArticulationLink.h:47
physx::Dy::ArticulationSpatialTendon
Definition DyArticulationTendon.h:82
physx::Dy::ArticulationTendonJoint
Definition DyArticulationTendon.h:125
physx::Dy::Context
Definition DyContext.h:76
physx::Dy::FeatherstoneArticulation
Definition DyFeatherstoneArticulation.h:590
physx::Dy::ThreadContext
Definition DyThreadContext.h:65
physx::PxArray
Definition PxArray.h:53
physx::PxArray::begin
PX_FORCE_INLINE ConstIterator begin() const
Definition PxArray.h:176
physx::PxArticulationCache
Data structure used to read and write internal articulation data.
Definition PxArticulationReducedCoordinate.h:99
physx::PxConstraintAllocator
Definition PxSolverDefs.h:227
physx::PxContactJoint
a joint that maintains an upper or lower bound (or both) on the distance between two points on differ...
Definition PxContactJoint.h:95
physx::PxMat33
3x3 matrix class
Definition PxMat33.h:91
physx::PxQuat
This is a quaternion class. For more information on quaternion mathematics consult a mathematics sour...
Definition PxQuat.h:50
physx::PxQuat::x
float x
Definition PxQuat.h:395
physx::PxTransform
class representing a rigid euclidean transform as a quaternion and a vector
Definition PxTransform.h:49
physx::PxVec3
3 Element vector class.
Definition PxVec3.h:50
physx::PxcScratchAllocator
Definition PxcScratchAllocator.h:42
physx::PxsConstraintBlockManager
Definition PxcConstraintBlockStream.h:40
physx::PxsContactManagerOutputIterator
Definition PxvNphaseImplementationContext.h:81
PX_FORCE_INLINE
#define PX_FORCE_INLINE
Definition PxPreprocessor.h:335
PX_INLINE
#define PX_INLINE
Definition PxPreprocessor.h:320
physx
Sorts an array of objects in ascending order, assuming that the predicate implements the < operator:
Definition PxBoxController.h:39
physx::PxAtan2
PX_CUDA_CALLABLE PX_FORCE_INLINE float PxAtan2(float x, float y)
Arctangent of (x/y) with correct sign. Returns angle between -PI and PI in radians Unit: Radians.
Definition PxMath.h:302
physx::PxMemZero
PX_FORCE_INLINE void * PxMemZero(void *dest, PxU32 count)
Sets the bytes of the provided buffer to zero.
Definition PxMemory.h:53
physx::Cm::SpatialVectorF
Definition CmSpatialVector.h:134
physx::Cm::SpatialVectorV
Definition CmSpatialVector.h:483
physx::Cm::UnAlignedSpatialVector
Definition CmSpatialVector.h:310
physx::Dy::ArticulationAttachment
Definition DyArticulationTendon.h:52
physx::Dy::ArticulationCore
Definition DyArticulationCore.h:39
physx::Dy::ArticulationDirtyFlag
Definition DyFeatherstoneArticulation.h:524
physx::Dy::ArticulationInternalConstraintBase
Definition DyFeatherstoneArticulation.h:113
physx::Dy::ArticulationInternalConstraint
Definition DyFeatherstoneArticulation.h:136
physx::Dy::ArticulationInternalLimit
Definition DyFeatherstoneArticulation.h:126
physx::Dy::ArticulationInternalTendonConstraint
Definition DyFeatherstoneArticulation.h:87
physx::Dy::ArticulationJointCore
Definition DyArticulationJointCore.h:44
physx::Dy::ArticulationJointTargetData
Definition DyFeatherstoneArticulationJointData.h:190
physx::Dy::ArticulationLoopConstraint
Definition DyVArticulation.h:74
physx::Dy::ArticulationSensor
Definition DyFeatherstoneArticulation.h:157
physx::Dy::ArticulationSolverDesc
Definition DyVArticulation.h:102
physx::Dy::ConstraintWriteback
Definition DyConstraintWriteBack.h:43
physx::Dy::FixedTendonSolveData
Definition DyFeatherstoneArticulation.h:507
physx::Dy::InternalConstraintSolverData
Definition DyFeatherstoneArticulation.h:481
physx::Dy::InvStIs
Definition DyFeatherstoneArticulationUtils.h:227
physx::Dy::ScratchData
Definition DyFeatherstoneArticulation.h:448
physx::Dy::SpatialImpulseResponseMatrix
Definition DyFeatherstoneArticulationUtils.h:634
physx::Dy::SpatialMatrix
Definition DyFeatherstoneArticulationUtils.h:237
physx::Dy::SpatialSubspaceMatrix
Definition DyFeatherstoneArticulationUtils.h:45
physx::Dy::SpatialTransform
Definition DyFeatherstoneArticulationUtils.h:119
physx::PxSolverBodyData
Struct that the solver uses to store the state and other properties of a body.
Definition PxSolverDefs.h:74
physx::PxSolverBody
Struct that the solver uses to store velocity updates for a body.
Definition PxSolverDefs.h:55
physx::PxSolverConstraintDesc
Constraint descriptor used inside the solver.
Definition PxSolverDefs.h:108
physx::PxSolverConstraintPrepDesc
Data structure used for preparing constraints before solving them.
Definition PxSolverDefs.h:178
physx::PxSpatialForce
Data structure to represent spatial forces.
Definition PxArticulationReducedCoordinate.h:53
physx::PxTGSSolverBodyData
Definition PxSolverDefs.h:446
physx::PxTGSSolverBodyTxInertia
Definition PxSolverDefs.h:440
physx::aos::Vec3V
Definition PxVecMathAoSScalar.h:77