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 PXDV_ARTICULATION_H
32#define PXDV_ARTICULATION_H
33
34#include "foundation/PxVec3.h"
35#include "foundation/PxQuat.h"
36#include "foundation/PxTransform.h"
37#include "PsVecMath.h"
38#include "PsUtilities.h"
39#include "CmUtils.h"
40#include "CmSpatialVector.h"
41#include "PxArticulationJoint.h"
42#include "PxArticulation.h"
43#include "foundation/PxMemory.h"
44#include "DyArticulationCore.h"
45#include "DyArticulationJointCore.h"
46
47namespace physx
48{
49 struct PxsBodyCore;
50 class PxsConstraintBlockManager;
51 class PxsContactManagerOutputIterator;
52 struct PxSolverConstraintDesc;
53 struct PxSolverBodyData;
54 class PxContactJoint;
55 struct PxTGSSolverBodyData;
56 struct PxTGSSolverBodyTxInertia;
57 struct PxSolverConstraintDesc;
58
59 namespace Dy
60 {
61
62 struct SpatialSubspaceMatrix;
63
64 struct ConstraintWriteback;
65 class ThreadContext;
66
67 static const size_t DY_ARTICULATION_MAX_SIZE = 64;
68 class ArticulationJointCoreData;
69 struct Constraint;
70 class Context;
71
72 struct ArticulationJointCore : public ArticulationJointCoreBase
73 {
74 //= ATTENTION! =====================================================================================
75 // Changing the data layout of this class breaks the binary serialization format. See comments for
76 // PX_BINARY_SERIAL_VERSION. If a modification is required, please adjust the getBinaryMetaData
77 // function. If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
78 // accordingly.
79 //==================================================================================================
80
81 // drive model
82 PxQuat targetPosition;
83 PxVec3 targetVelocity;
84
85 PxReal spring;//old
86 PxReal damping;//old
87
88 PxReal internalCompliance;//old
89 PxReal externalCompliance;//old
90
91 // limit model
92
93 PxReal swingLimitContactDistance;//old
94
95 PxReal tangentialStiffness;//old
96 PxReal tangentialDamping;//old
97
98 bool swingLimited;//old
99 bool twistLimited;//old
100
101 PxU8 driveType; //both
102
103 PxReal twistLimitContactDistance; //old
104
105 PxReal tanQSwingY;//old
106 PxReal tanQSwingZ;//old
107 PxReal tanQSwingPad;//old
108 PxReal tanQTwistHigh;//old
109 PxReal tanQTwistLow;//old
110 PxReal tanQTwistPad;//old
111
112 ArticulationJointCore()
113 {
114 //Cm::markSerializedMem(this, sizeof(ArticulationJointCore));
115 parentPose = PxTransform(PxIdentity);
116 childPose = PxTransform(PxIdentity);
117 internalCompliance = 0.0f;
118 externalCompliance = 0.0f;
119 swingLimitContactDistance = 0.05f;
120 twistLimitContactDistance = 0.05f;
121
122 driveType = PxArticulationJointDriveType::eTARGET;
123
124 jointType = PxArticulationJointType::eFIX;
125
126 for(PxU32 i=0; i<PxArticulationAxis::eCOUNT; i++)
127 motion[i] = PxArticulationMotion::eLOCKED;
128
129 dirtyFlag = ArticulationJointCoreDirtyFlag::eMOTION;
130
131 jointOffset = 0;
132 }
133
134 ArticulationJointCore(const PxTransform& parentFrame, const PxTransform& childFrame)
135 {
136 parentPose = parentFrame;
137 childPose = childFrame;
138
139 //the old articulation is eTARGET
140 driveType = PxArticulationJointDriveType::eTARGET;
141
142 spring = 0.0f;
143 damping = 0.0f;
144
145 internalCompliance = 1.0f;
146 externalCompliance = 1.0f;
147
148 for(PxU32 i=0; i<PxArticulationAxis::eCOUNT; i++)
149 {
150 limits[i].low = 0.f;
151 limits[i].high = 0.f;
152 drives[i].stiffness = 0.f;
153 drives[i].damping = 0.f;
154 drives[i].maxForce = 0.f;
155 targetP[i] = 0.f;
156 targetV[i] = 0.f;
157 motion[i] = PxArticulationMotion::eLOCKED;
158 }
159
160 const PxReal swingYLimit = PxPi / 4.0f;
161 const PxReal swingZLimit = PxPi / 4.0f;
162
163 limits[PxArticulationAxis::eSWING1].low = swingYLimit;
164 limits[PxArticulationAxis::eSWING2].low = swingZLimit;
165
166 swingLimitContactDistance = 0.05f;
167 swingLimited = false;
168 tangentialStiffness = 0.0f;
169 tangentialDamping = 0.0f;
170
171 const PxReal twistLimitLow = -PxPi / 4.0f;
172 const PxReal twistLimitHigh = PxPi / 4.0f;
173 limits[PxArticulationAxis::eTWIST].low = twistLimitLow;
174 limits[PxArticulationAxis::eTWIST].high = twistLimitHigh;
175 twistLimitContactDistance = 0.05f;
176 twistLimited = false;
177
178 tanQSwingY = PxTan(a: swingYLimit / 4.0f);
179 tanQSwingZ = PxTan(a: swingZLimit / 4.0f);
180 tanQSwingPad = PxTan(a: swingLimitContactDistance / 4.0f);
181
182 tanQTwistHigh = PxTan(a: twistLimitHigh / 4.0f);
183 tanQTwistLow = PxTan(a: twistLimitLow / 4.0f);
184 tanQTwistPad = PxTan(a: twistLimitContactDistance / 4.0f);
185
186 frictionCoefficient = 0.f;
187
188 dirtyFlag = ArticulationJointCoreDirtyFlag::eMOTION;
189
190 jointOffset = 0;
191 }
192
193 void setJointPose(ArticulationJointCoreData& jointDatum, SpatialSubspaceMatrix& motionMatrix, bool forceUpdate,
194 PxQuat& relativeRot);
195
196 // PX_SERIALIZATION
197 ArticulationJointCore(const PxEMPTY&) : ArticulationJointCoreBase(PxEmpty) {}
198 //~PX_SERIALIZATION
199 };
200
201 struct ArticulationLoopConstraint
202 {
203 public:
204 PxU32 linkIndex0;
205 PxU32 linkIndex1;
206 Dy::Constraint* constraint;
207 };
208
209#define DY_ARTICULATION_LINK_NONE 0xffffffff
210
211 typedef PxU64 ArticulationBitField;
212
213 struct ArticulationLink
214 {
215 ArticulationBitField children; // child bitmap
216 ArticulationBitField pathToRoot; // path to root, including link and root
217 PxsBodyCore* bodyCore;
218 ArticulationJointCore* inboundJoint;
219 PxU32 parent;
220 };
221
222 typedef size_t ArticulationLinkHandle;
223
224 class ArticulationV;
225
226 struct ArticulationSolverDesc
227 {
228 void initData(const ArticulationCore* core_, const PxArticulationFlags* flags_)
229 {
230 articulation = NULL;
231 links = NULL;
232 motionVelocity = NULL;
233 acceleration = NULL;
234 poses = NULL;
235 deltaQ = NULL;
236 externalLoads = NULL;
237 internalLoads = NULL;
238 core = core_;
239 flags = flags_;
240 scratchMemory = NULL;
241 totalDataSize = 0;
242 solverDataSize = 0;
243 linkCount = 0;
244 numInternalConstraints = 0;
245 scratchMemorySize = 0;
246 }
247
248 ArticulationV* articulation;
249 ArticulationLink* links;
250 Cm::SpatialVectorV* motionVelocity;
251 Cm::SpatialVector* acceleration;
252 PxTransform* poses;
253 PxQuat* deltaQ;
254 physx::shdfnd::aos::Mat33V* externalLoads;
255 physx::shdfnd::aos::Mat33V* internalLoads;
256 const ArticulationCore* core;
257 const PxArticulationFlags* flags; // PT: PX-1399
258 char* scratchMemory;
259 PxU16 totalDataSize;
260 PxU16 solverDataSize;
261 PxU8 linkCount;
262 PxU8 numInternalConstraints;
263 PxU16 scratchMemorySize;
264 };
265
266 struct PxcFsScratchAllocator
267 {
268 char* base;
269 size_t size;
270 size_t taken;
271 PxcFsScratchAllocator(char* p, size_t s) : base(p), size(s), taken(0) {}
272
273 template<typename T>
274 static size_t sizeof16()
275 {
276 return (sizeof(T) + 15)&~15;
277 }
278
279 template<class T> T* alloc(PxU32 count)
280 {
281 size_t s = sizeof16<T>();
282 PX_ASSERT(taken + s*count <= size);
283 T* result = reinterpret_cast<T*>(base + taken);
284 taken += s*count;
285 return result;
286 }
287 };
288
289 static const size_t DY_ARTICULATION_IDMASK = DY_ARTICULATION_MAX_SIZE - 1;
290
291#if PX_VC
292#pragma warning(push)
293#pragma warning( disable : 4324 ) // Padding was added at the end of a structure because of a __declspec(align) value.
294#endif
295 PX_ALIGN_PREFIX(64)
296 class ArticulationV
297 {
298 PX_NOCOPY(ArticulationV)
299 public:
300
301 enum Enum
302 {
303 eReducedCoordinate = 0,
304 eMaximumCoordinate = 1
305 };
306
307 ArticulationV(void* userData, Enum type) :
308 mUserData (userData),
309 mContext (NULL),
310 mType (type),
311 mUpdateSolverData (true),
312 mDirty (false),
313 mMaxDepth (0)
314 {}
315
316 virtual ~ArticulationV() {}
317
318 virtual void onUpdateSolverDesc() {}
319
320 virtual bool resize(const PxU32 linkCount);
321
322 virtual void addBody()
323 {
324 mAcceleration.pushBack(a: Cm::SpatialVector(PxVec3(0.f), PxVec3(0.f)));
325 mUpdateSolverData = true;
326 }
327
328 virtual void removeBody()
329 {
330 mUpdateSolverData = true;
331 }
332
333 PX_FORCE_INLINE bool updateSolverData() { return mUpdateSolverData; }
334
335 PX_FORCE_INLINE void setDirty(const bool dirty) { mDirty = dirty; }
336 PX_FORCE_INLINE bool getDirty() const { return mDirty; }
337
338 PX_FORCE_INLINE PxU32 getMaxDepth() const { return mMaxDepth; }
339 PX_FORCE_INLINE void setMaxDepth(const PxU32 depth) { mMaxDepth = depth; }
340
341 // solver methods
342 PX_FORCE_INLINE PxU32 getBodyCount() const { return mSolverDesc.linkCount; }
343 PX_FORCE_INLINE PxU32 getSolverDataSize() const { return mSolverDesc.solverDataSize; }
344 PX_FORCE_INLINE PxU32 getTotalDataSize() const { return mSolverDesc.totalDataSize; }
345 PX_FORCE_INLINE void getSolverDesc(ArticulationSolverDesc& d) const { d = mSolverDesc; }
346 PX_FORCE_INLINE ArticulationSolverDesc& getSolverDesc() { return mSolverDesc; }
347
348 PX_FORCE_INLINE const ArticulationCore* getCore() const { return mSolverDesc.core; }
349 PX_FORCE_INLINE PxU16 getIterationCounts() const { return mSolverDesc.core->solverIterationCounts; }
350
351 PX_FORCE_INLINE void* getUserData() const { return mUserData; }
352
353 PX_FORCE_INLINE PxU32 getType() const { return mType; }
354
355 PX_FORCE_INLINE void setDyContext(Dy::Context* context) { mContext = context; }
356
357 // get data sizes for allocation at higher levels
358 virtual void getDataSizes(PxU32 linkCount, PxU32& solverDataSize, PxU32& totalSize, PxU32& scratchSize) = 0;
359
360 virtual PxU32 getDofs() { return 0; }
361
362 virtual PxU32 getDof(const PxU32 /*linkID*/) { return 0; }
363
364 virtual bool applyCache(PxArticulationCache& /*cache*/, const PxArticulationCacheFlags /*flag*/) {return false;}
365
366 virtual void copyInternalStateToCache(PxArticulationCache&/* cache*/, const PxArticulationCacheFlags /*flag*/) {}
367
368 virtual void packJointData(const PxReal* /*maximum*/, PxReal* /*reduced*/) {}
369
370 virtual void unpackJointData(const PxReal* /*reduced*/, PxReal* /*maximum*/) {}
371
372 virtual void initializeCommonData() {}
373
374 virtual void getGeneralizedGravityForce(const PxVec3& /*gravity*/, PxArticulationCache& /*cache*/) {}
375
376 virtual void getCoriolisAndCentrifugalForce(PxArticulationCache& /*cache*/) {}
377
378 virtual void getGeneralizedExternalForce(PxArticulationCache& /*cache*/) {}
379
380 virtual void getJointAcceleration(const PxVec3& /*gravity*/, PxArticulationCache& /*cache*/){}
381
382 virtual void getJointForce(PxArticulationCache& /*cache*/){}
383
384 virtual void getCoefficientMatrix(const PxReal /*dt*/, const PxU32 /*linkID*/, const PxContactJoint* /*joints*/, const PxU32 /*nbContacts*/, PxArticulationCache& /*cache*/){}
385
386 virtual void getDenseJacobian(PxArticulationCache& /*cache*/, PxU32&, PxU32&) {}
387
388 virtual void getCoefficientMatrixWithLoopJoints(ArticulationLoopConstraint* /*lConstraints*/, const PxU32 /*nbJoints*/, PxArticulationCache& /*cache*/) {}
389
390 virtual bool getLambda( ArticulationLoopConstraint* /*lConstraints*/, const PxU32 /*nbJoints*/,
391 PxArticulationCache& /*cache*/, PxArticulationCache& /*initialState*/, const PxReal* /*jointTorque*/,
392 const PxVec3& /*gravity*/, const PxU32 /*maxIter*/) { return false; }
393
394 virtual void getGeneralizedMassMatrix(PxArticulationCache& /*cache*/){}
395 virtual void getGeneralizedMassMatrixCRB(PxArticulationCache& /*cache*/){}
396
397 virtual void teleportRootLink(){}
398
399 virtual void getImpulseResponse( PxU32 linkID,
400 Cm::SpatialVectorF* Z,
401 const Cm::SpatialVector& impulse,
402 Cm::SpatialVector& deltaV) const = 0;
403
404 virtual void getImpulseResponse(
405 PxU32 linkID,
406 Cm::SpatialVectorV* Z,
407 const Cm::SpatialVectorV& impulse,
408 Cm::SpatialVectorV& deltaV) const = 0;
409
410 virtual void getImpulseSelfResponse(
411 PxU32 linkID0,
412 PxU32 linkID1,
413 Cm::SpatialVectorF* Z,
414 const Cm::SpatialVector& impulse0,
415 const Cm::SpatialVector& impulse1,
416 Cm::SpatialVector& deltaV0,
417 Cm::SpatialVector& deltaV1) const = 0;
418
419 virtual Cm::SpatialVectorV getLinkVelocity(const PxU32 linkID) const = 0;
420
421 virtual Cm::SpatialVectorV getLinkMotionVector(const PxU32 linkID) const = 0;
422
423 virtual PxReal getLinkMaxPenBias(const PxU32 linkID) const = 0;
424
425 virtual void pxcFsApplyImpulse( PxU32 linkID, Ps::aos::Vec3V linear,
426 Ps::aos::Vec3V angular, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV) = 0;
427
428 virtual void pxcFsApplyImpulses( PxU32 linkID, const Ps::aos::Vec3V& linear,
429 const Ps::aos::Vec3V& angular, PxU32 linkID2, const Ps::aos::Vec3V& linear2,
430 const Ps::aos::Vec3V& angular2, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV) = 0;
431
432 virtual void solveInternalConstraints(const PxReal dt, const PxReal invDt, Cm::SpatialVectorF* impulses, Cm::SpatialVectorF* DeltaV,
433 bool velIteration, bool isTGS, const PxReal elapsedTime) = 0;
434
435 virtual void writebackInternalConstraints(bool isTGS) = 0;
436
437 virtual void concludeInternalConstraints(bool isTGS) = 0;
438
439 virtual void prepareStaticConstraints(const PxReal /*dt*/, const PxReal /*invDt*/, PxsContactManagerOutputIterator& /*outputs*/,
440 ThreadContext& /*threadContext*/, PxReal /*correlationDist*/, PxReal /*bounceThreshold*/, PxReal /*frictionOffsetThreshold*/, PxReal /*solverOffsetSlop*/,
441 PxReal /*ccdMaxSeparation*/, PxSolverBodyData* /*solverBodyData*/, PxsConstraintBlockManager& /*blockManager*/,
442 Dy::ConstraintWriteback* /*constraintWritebackPool*/) {}
443
444 virtual void prepareStaticConstraintsTGS(const PxReal /*stepDt*/, const PxReal /*totalDt*/, const PxReal /*invStepDt*/, const PxReal /*invTotalDt*/,
445 PxsContactManagerOutputIterator& /*outputs*/, ThreadContext& /*threadContext*/, PxReal /*correlationDist*/, PxReal /*bounceThreshold*/,
446 PxReal /*frictionOffsetThreshold*/, PxTGSSolverBodyData* /*solverBodyData*/,
447 PxTGSSolverBodyTxInertia* /*txInertia*/, PxsConstraintBlockManager& /*blockManager*/, Dy::ConstraintWriteback* /*constraintWritebackPool*/,
448 const PxU32 /*nbSubsteps*/, const PxReal /*lengthScale*/) {}
449
450 virtual void pxcFsGetVelocities(PxU32 linkID, PxU32 linkID1, Cm::SpatialVectorV& v0, Cm::SpatialVectorV& v1) = 0;
451
452 virtual Cm::SpatialVectorV pxcFsGetVelocity(PxU32 linkID) = 0;
453
454 virtual Cm::SpatialVectorV pxcFsGetVelocityTGS(PxU32 linkID) = 0;
455
456 virtual const PxTransform& getCurrentTransform(PxU32 linkID) const= 0;
457
458 virtual const PxQuat& getDeltaQ(PxU32 linkID) const = 0;
459
460 virtual bool storeStaticConstraint(const PxSolverConstraintDesc& /*desc*/) { return false; }
461
462 virtual bool willStoreStaticConstraint() { return false; }
463
464 //this is called by island gen to determine whether the articulation should be awake or sleep
465 virtual Cm::SpatialVector getMotionVelocity(const PxU32 linkID) const = 0;
466
467 virtual Cm::SpatialVector getMotionAcceleration(const PxU32 linkID) const = 0;
468
469 void setupLinks(PxU32 nbLinks, Dy::ArticulationLink* links)
470 {
471 //if this is needed, we need to re-allocated the link data
472 resize(linkCount: nbLinks);
473
474 getSolverDesc().links = links;
475 getSolverDesc().linkCount = Ps::to8(value: nbLinks);
476
477 //if this is needed, we need to re-allocated the joint data
478 onUpdateSolverDesc();
479 }
480 virtual void fillIndexedManager(const PxU32 linkId, Dy::ArticulationLinkHandle& handle, PxU8& indexType) = 0;
481
482 //These variables are used in the constraint partition
483 PxU16 maxSolverFrictionProgress;
484 PxU16 maxSolverNormalProgress;
485 PxU32 solverProgress;
486 PxU8 numTotalConstraints;
487 protected:
488 void* mUserData;
489 Dy::Context* mContext;
490 PxU32 mType;
491 ArticulationSolverDesc mSolverDesc;
492
493 Ps::Array<Cm::SpatialVector> mAcceleration; // supplied by Sc-layer to feed into articulations
494
495 bool mUpdateSolverData;
496 bool mDirty; //any of links update configulations, the boolean will be set to true
497 PxU32 mMaxDepth;
498
499 } PX_ALIGN_SUFFIX(64);
500
501#if PX_VC
502#pragma warning(pop)
503#endif
504
505 PX_FORCE_INLINE ArticulationV* getArticulation(ArticulationLinkHandle handle)
506 {
507 return reinterpret_cast<ArticulationV*>(handle & ~DY_ARTICULATION_IDMASK);
508 }
509
510 PX_FORCE_INLINE bool isArticulationRootLink(ArticulationLinkHandle handle)
511 {
512 return !(handle & DY_ARTICULATION_IDMASK);
513 }
514
515 PX_FORCE_INLINE PxU32 getLinkIndex(ArticulationLinkHandle handle)
516 {
517 return PxU32(handle&DY_ARTICULATION_IDMASK);
518 }
519 }
520
521}
522
523#endif
524

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