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#include "NpArticulationReducedCoordinate.h"
31
32#include "CmPhysXCommon.h"
33#include "DyFeatherstoneArticulation.h"
34#include "ScArticulationSim.h"
35#include "ScConstraintSim.h"
36
37#include "PsAlignedMalloc.h"
38#include "PsFoundation.h"
39#include "PsPool.h"
40
41#include "PxPvdDataStream.h"
42#include "extensions/PxJoint.h"
43
44using namespace physx;
45
46class LLReducedArticulationPool : public Ps::Pool<Dy::FeatherstoneArticulation, Ps::AlignedAllocator<Dy::DY_ARTICULATION_MAX_SIZE> >
47{
48public:
49 LLReducedArticulationPool() {}
50};
51
52// PX_SERIALIZATION
53NpArticulationReducedCoordinate* NpArticulationReducedCoordinate::createObject(PxU8*& address, PxDeserializationContext& context)
54{
55 NpArticulationReducedCoordinate* obj = new (address) NpArticulationReducedCoordinate(PxBaseFlag::eIS_RELEASABLE);
56 address += sizeof(NpArticulationReducedCoordinate);
57 obj->importExtraData(context);
58 obj->resolveReferences(context);
59 return obj;
60}
61
62void NpArticulationReducedCoordinate::preExportDataReset()
63{
64 //for now, no support for loop joint serialization
65 Ps::Array<PxJoint*> emptyLoopJoints;
66 PxMemCopy(dest: &mLoopJoints, src: &emptyLoopJoints, count: sizeof(Ps::Array<PxJoint*>));
67}
68
69//~PX_SERIALIZATION
70
71NpArticulationReducedCoordinate::NpArticulationReducedCoordinate()
72 : NpArticulationTemplate(PxConcreteType::eARTICULATION_REDUCED_COORDINATE, PxBaseFlag::eOWNS_MEMORY | PxBaseFlag::eIS_RELEASABLE)
73{
74}
75
76void NpArticulationReducedCoordinate::setArticulationFlags(PxArticulationFlags flags)
77{
78 NP_WRITE_CHECK(mImpl.getOwnerScene());
79 mImpl.getScbArticulation().setArticulationFlags(flags);
80}
81
82void NpArticulationReducedCoordinate::setArticulationFlag(PxArticulationFlag::Enum flag, bool value)
83{
84 NP_WRITE_CHECK(mImpl.getOwnerScene());
85 PxArticulationFlags flags = mImpl.getScbArticulation().getArticulationFlags();
86
87 if(value)
88 flags |= flag;
89 else
90 flags &= (~flag);
91
92 mImpl.getScbArticulation().setArticulationFlags(flags);
93}
94
95PxArticulationFlags NpArticulationReducedCoordinate::getArticulationFlags() const
96{
97 NP_READ_CHECK(mImpl.getOwnerScene());
98 return mImpl.getScbArticulation().getArticulationFlags();
99}
100
101PxU32 NpArticulationReducedCoordinate::getDofs() const
102{
103 NP_READ_CHECK(mImpl.getOwnerScene());
104 return mImpl.mArticulation.getScArticulation().getDofs();
105}
106
107PxArticulationCache* NpArticulationReducedCoordinate::createCache() const
108{
109 PX_CHECK_AND_RETURN_NULL(mImpl.getAPIScene(), "PxArticulation::createCache: object must be in a scene");
110 NP_READ_CHECK(mImpl.getOwnerScene()); // doesn't modify the scene, only reads
111
112 PxArticulationCache* cache = mImpl.mArticulation.getScArticulation().createCache();
113 cache->version = mImpl.mCacheVersion;
114
115 return cache;
116}
117
118PxU32 NpArticulationReducedCoordinate::getCacheDataSize() const
119{
120 PX_CHECK_AND_RETURN_NULL(mImpl.getAPIScene(), "PxArticulation::getCacheDataSize: object must be in a scene");
121 NP_READ_CHECK(mImpl.getOwnerScene()); // doesn't modify the scene, only reads
122
123 return mImpl.mArticulation.getScArticulation().getCacheDataSize();
124}
125
126void NpArticulationReducedCoordinate::zeroCache(PxArticulationCache& cache)
127{
128 NP_READ_CHECK(mImpl.getOwnerScene()); // doesn't modify the scene, only reads
129 return mImpl.mArticulation.getScArticulation().zeroCache(cache);
130}
131
132void NpArticulationReducedCoordinate::applyCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag, bool autowake)
133{
134 PX_CHECK_AND_RETURN(mImpl.getAPIScene(), "PxArticulation::applyCache: object must be in a scene");
135
136 PX_CHECK_AND_RETURN(cache.version == mImpl.mCacheVersion, "PxArticulation::applyCache : cache is invalid, articulation configuration has changed! ")
137
138 //if we try to do a bulk op when sim is running, return with error
139 if (static_cast<NpScene*>(getScene())->getSimulationStage() != Sc::SimulationStage::eCOMPLETE)
140 {
141 Ps::getFoundation().error(PxErrorCode::eDEBUG_WARNING, __FILE__, __LINE__,
142 messageFmt: "NpArticulation::applyCache() not allowed while simulation is running.");
143 return;
144 }
145
146 mImpl.mArticulation.getScArticulation().applyCache(cache, flag);
147
148 if ((flag & PxArticulationCache::ePOSITION))
149 {
150 const PxU32 linkCount = mImpl.mArticulationLinks.size();
151
152 for (PxU32 i = 0; i < linkCount; ++i)
153 {
154 NpArticulationLink* link = mImpl.mArticulationLinks[i];
155 //in the lowlevel articulation, we have already updated bodyCore's body2World
156 const PxTransform internalPose = link->getScbBodyFast().getScBody().getBody2World();
157 link->getScbBodyFast().setBody2World(p: internalPose, asPartOfBody2ActorChange: false);
158 }
159 }
160
161 if ((flag & PxArticulationCache::ePOSITION) || (flag & PxArticulationCache::eVELOCITY))
162 {
163 const PxU32 linkCount = mImpl.mArticulationLinks.size();
164
165 for (PxU32 i = 0; i < linkCount; ++i)
166 {
167 NpArticulationLink* link = mImpl.mArticulationLinks[i];
168 //in the lowlevel articulation, we have already updated bodyCore's linear/angular velocity
169 const PxVec3 internalLinVel = link->getScbBodyFast().getScBody().getLinearVelocity();
170 const PxVec3 internalAngVel = link->getScbBodyFast().getScBody().getAngularVelocity();
171 link->getScbBodyFast().setLinearVelocity(internalLinVel);
172 link->getScbBodyFast().setAngularVelocity(internalAngVel);
173 }
174 }
175
176 mImpl.wakeUpInternal(forceWakeUp: false, autowake);
177}
178
179void NpArticulationReducedCoordinate::copyInternalStateToCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag) const
180{
181 PX_CHECK_AND_RETURN(mImpl.getAPIScene(), "PxArticulation::copyInternalStateToCache: object must be in a scene");
182
183 PX_CHECK_AND_RETURN(cache.version == mImpl.mCacheVersion, "PxArticulation::applyCache : cache is invalid, articulation configuration has changed! ")
184
185 mImpl.mArticulation.getScArticulation().copyInternalStateToCache(cache, flag);
186}
187
188void NpArticulationReducedCoordinate::releaseCache(PxArticulationCache& cache) const
189{
190 PX_CHECK_AND_RETURN(mImpl.getAPIScene(), "PxArticulation::releaseCache: object must be in a scene");
191 NP_READ_CHECK(mImpl.getOwnerScene()); // doesn't modify the scene, only reads
192
193 mImpl.mArticulation.getScArticulation().releaseCache(cache);
194}
195
196void NpArticulationReducedCoordinate::packJointData(const PxReal* maximum, PxReal* reduced) const
197{
198 PX_CHECK_AND_RETURN(mImpl.getAPIScene(), "PxArticulation::packJointData: object must be in a scene");
199 NP_READ_CHECK(mImpl.getOwnerScene());
200
201 mImpl.mArticulation.getScArticulation().packJointData(maximum, reduced);
202}
203
204void NpArticulationReducedCoordinate::unpackJointData(const PxReal* reduced, PxReal* maximum) const
205{
206 PX_CHECK_AND_RETURN(mImpl.getAPIScene(), "PxArticulation::unpackJointData: object must be in a scene");
207 NP_READ_CHECK(mImpl.getOwnerScene());
208
209 mImpl.mArticulation.getScArticulation().unpackJointData(reduced, maximum);
210}
211
212void NpArticulationReducedCoordinate::commonInit() const
213{
214 PX_CHECK_AND_RETURN(mImpl.getAPIScene(), "PxArticulation::commonInit: object must be in a scene");
215 NP_READ_CHECK(mImpl.getOwnerScene());
216
217 mImpl.mArticulation.getScArticulation().commonInit();
218}
219
220void NpArticulationReducedCoordinate::computeGeneralizedGravityForce(PxArticulationCache& cache) const
221{
222 PX_CHECK_AND_RETURN(mImpl.getAPIScene(), "PxArticulation::computeGeneralisedGravityForce: object must be in a scene");
223 NP_READ_CHECK(mImpl.getOwnerScene());
224
225 PX_CHECK_AND_RETURN(cache.version == mImpl.mCacheVersion, "PxArticulation::computeGeneralisedGravityForce : cache is invalid, articulation configuration has changed! ");
226
227 mImpl.mArticulation.getScArticulation().computeGeneralizedGravityForce(cache);
228}
229
230void NpArticulationReducedCoordinate::computeCoriolisAndCentrifugalForce(PxArticulationCache& cache) const
231{
232 PX_CHECK_AND_RETURN(mImpl.getAPIScene(), "PxArticulation::computeCoriolisAndCentrifugalForce: object must be in a scene");
233 NP_READ_CHECK(mImpl.getOwnerScene());
234
235 PX_CHECK_AND_RETURN(cache.version == mImpl.mCacheVersion, "PxArticulation::computeCoriolisAndCentrifugalForce : cache is invalid, articulation configuration has changed! ");
236
237 mImpl.mArticulation.getScArticulation().computeCoriolisAndCentrifugalForce(cache);
238}
239
240void NpArticulationReducedCoordinate::computeGeneralizedExternalForce(PxArticulationCache& cache) const
241{
242 PX_CHECK_AND_RETURN(mImpl.getAPIScene(), "PxArticulation::computeGeneralizedExternalForce: object must be in a scene");
243 NP_READ_CHECK(mImpl.getOwnerScene());
244
245 PX_CHECK_AND_RETURN(cache.version == mImpl.mCacheVersion, "PxArticulation::computeGeneralizedExternalForce : cache is invalid, articulation configuration has changed! ");
246
247 mImpl.mArticulation.getScArticulation().computeGeneralizedExternalForce(cache);
248}
249
250void NpArticulationReducedCoordinate::computeJointAcceleration(PxArticulationCache& cache) const
251{
252 PX_CHECK_AND_RETURN(mImpl.getAPIScene(), "PxArticulation::computeJointAcceleration: object must be in a scene");
253 NP_READ_CHECK(mImpl.getOwnerScene());
254
255 PX_CHECK_AND_RETURN(cache.version == mImpl.mCacheVersion, "PxArticulation::computeJointAcceleration : cache is invalid, articulation configuration has changed! ");
256
257 mImpl.mArticulation.getScArticulation().computeJointAcceleration(cache);
258}
259
260void NpArticulationReducedCoordinate::computeJointForce(PxArticulationCache& cache) const
261{
262 PX_CHECK_AND_RETURN(mImpl.getAPIScene(), "PxArticulation::computeJointForce: object must be in a scene");
263 NP_READ_CHECK(mImpl.getOwnerScene());
264
265 PX_CHECK_AND_RETURN(cache.version == mImpl.mCacheVersion, "PxArticulation::computeJointForce : cache is invalid, articulation configuration has changed! ");
266
267 mImpl.mArticulation.getScArticulation().computeJointForce(cache);
268}
269
270
271void NpArticulationReducedCoordinate::computeDenseJacobian(PxArticulationCache& cache, PxU32& nRows, PxU32& nCols) const
272{
273 PX_CHECK_AND_RETURN(mImpl.getAPIScene(), "PxArticulation::computeDenseJacobian: object must be in a scene");
274 NP_READ_CHECK(mImpl.getOwnerScene());
275
276 PX_CHECK_AND_RETURN(cache.version == mImpl.mCacheVersion, "PxArticulation::computeDenseJacobian : cache is invalid, articulation configuration has changed! ");
277
278 mImpl.mArticulation.getScArticulation().computeDenseJacobian(cache, nRows, nCols);
279}
280
281void NpArticulationReducedCoordinate::computeCoefficientMatrix(PxArticulationCache& cache) const
282{
283 PX_CHECK_AND_RETURN(mImpl.getAPIScene(), "PxArticulation::computeCoefficientMatrix: object must be in a scene");
284 NP_READ_CHECK(mImpl.getOwnerScene());
285
286 PX_CHECK_AND_RETURN(cache.version == mImpl.mCacheVersion, "PxArticulation::computeCoefficientMatrix : cache is invalid, articulation configuration has changed! ");
287
288 for (PxU32 i = 0; i < mLoopJoints.size(); ++i)
289 {
290 static_cast<NpConstraint*>(mLoopJoints[i]->getConstraint())->updateConstants();
291 }
292
293 mImpl.mArticulation.getScArticulation().computeCoefficientMatrix(cache);
294}
295
296bool NpArticulationReducedCoordinate::computeLambda(PxArticulationCache& cache, PxArticulationCache& initialState, const PxReal* const jointTorque, const PxU32 maxIter) const
297{
298 if (!mImpl.getAPIScene())
299 {
300 physx::shdfnd::getFoundation().error(physx::PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, messageFmt: "PxArticulation::computeLambda : object must be in a scened!");
301 return false;
302 }
303
304 NP_READ_CHECK(mImpl.getOwnerScene());
305
306 if (cache.version != mImpl.mCacheVersion)
307 {
308 physx::shdfnd::getFoundation().error(physx::PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, messageFmt: "PxArticulation::computeLambda : cache is invalid, articulation configuration has changed!");
309 return false;
310 }
311
312 return mImpl.mArticulation.getScArticulation().computeLambda(cache, rollBackCache&: initialState, jointTorque, gravity: getScene()->getGravity(), maxIter);
313}
314
315void NpArticulationReducedCoordinate::computeGeneralizedMassMatrix(PxArticulationCache& cache) const
316{
317 PX_CHECK_AND_RETURN(mImpl.getAPIScene(), "PxArticulation::computeGeneralizedMassMatrix: object must be in a scene");
318 NP_READ_CHECK(mImpl.getOwnerScene());
319
320 PX_CHECK_AND_RETURN(cache.version == mImpl.mCacheVersion, "PxArticulation::computeGeneralizedMassMatrix : cache is invalid, articulation configuration has changed! ");
321
322 mImpl.mArticulation.getScArticulation().computeGeneralizedMassMatrix(cache);
323}
324
325void NpArticulationReducedCoordinate::addLoopJoint(PxJoint* joint)
326{
327 NP_WRITE_CHECK(mImpl.getOwnerScene());
328
329#if PX_CHECKED
330
331 PxRigidActor* actor0;
332 PxRigidActor* actor1;
333
334 joint->getActors(actor0, actor1);
335
336 PxArticulationLink* link0 = NULL;
337 PxArticulationLink* link1 = NULL;
338
339 if(actor0)
340 link0 = actor0->is<PxArticulationLink>();
341
342 if(actor1)
343 link1 = actor1->is<PxArticulationLink>();
344
345 PX_CHECK_AND_RETURN((link0 || link1), "PxArticulation::addLoopJoint : at least one of the PxRigidActors need to be PxArticulationLink! ");
346
347
348 PxArticulationBase* base0 = NULL;
349 PxArticulationBase* base1 = NULL;
350 if (link0)
351 base0 = &link0->getArticulation();
352
353 if (link1)
354 base1 = &link1->getArticulation();
355
356 PX_CHECK_AND_RETURN((base0 == this || base1 == this), "PxArticulation::addLoopJoint : at least one of the PxArticulationLink belongs to this articulation! ");
357#endif
358
359 const PxU32 size = mLoopJoints.size();
360 if (size >= mLoopJoints.capacity())
361 {
362 mLoopJoints.reserve(capacity: size * 2 + 1);
363 }
364
365 mLoopJoints.pushBack(a: joint);
366
367 Scb::Articulation& scbArt = mImpl.getScbArticulation();
368 Sc::ArticulationSim* scArtSim = scbArt.getScArticulation().getSim();
369
370 NpConstraint* constraint = static_cast<NpConstraint*>(joint->getConstraint());
371 Sc::ConstraintSim* cSim = constraint->getScbConstraint().getScConstraint().getSim();
372 if(scArtSim)
373 scArtSim->addLoopConstraint(constraint: cSim);
374}
375
376void NpArticulationReducedCoordinate::removeLoopJoint(PxJoint* joint)
377{
378 NP_WRITE_CHECK(mImpl.getOwnerScene());
379
380 mLoopJoints.findAndReplaceWithLast(a: joint);
381
382 Scb::Articulation& scbArt = mImpl.getScbArticulation();
383 Sc::ArticulationSim* scArtSim = scbArt.getScArticulation().getSim();
384
385 NpConstraint* constraint = static_cast<NpConstraint*>(joint->getConstraint());
386 Sc::ConstraintSim* cSim = constraint->getScbConstraint().getScConstraint().getSim();
387 scArtSim->removeLoopConstraint(constraint: cSim);
388}
389
390PxU32 NpArticulationReducedCoordinate::getNbLoopJoints() const
391{
392 NP_READ_CHECK(mImpl.getOwnerScene());
393
394 return mLoopJoints.size();
395}
396
397PxU32 NpArticulationReducedCoordinate::getLoopJoints(PxJoint** userBuffer, PxU32 bufferSize, PxU32 startIndex) const
398{
399 NP_READ_CHECK(mImpl.getOwnerScene());
400
401 return Cm::getArrayOfPointers(userBuffer, bufferSize, startIndex, src: mLoopJoints.begin(), size: mLoopJoints.size());
402}
403
404PxU32 NpArticulationReducedCoordinate::getCoefficientMatrixSize() const
405{
406 NP_READ_CHECK(mImpl.getOwnerScene());
407
408 return mImpl.mArticulation.getScArticulation().getCoefficientMatrixSize();
409}
410
411void NpArticulationReducedCoordinate::teleportRootLink(const PxTransform& pose, bool autowake)
412{
413 PX_CHECK_AND_RETURN(mImpl.getAPIScene(), "PxArticulationReducedCoordinate::teleportRootLink: object must be in a scene");
414
415 PX_CHECK_AND_RETURN(pose.isValid(), "PxArticulationReducedCoordinate::teleportRootLink pose is not valid.");
416
417 NP_WRITE_CHECK(mImpl.getOwnerScene());
418
419 NpArticulationLink* root = mImpl.mArticulationLinks[0];
420
421 root->setGlobalPoseInternal(pose, autowake);
422}
423
424PxSpatialVelocity NpArticulationReducedCoordinate::getLinkVelocity(const PxU32 linkId)
425{
426 PX_CHECK_AND_RETURN_VAL(mImpl.getAPIScene(), "PxArticulationReducedCoordinate::getLinkVelocity: object must be in a scene", PxSpatialVelocity());
427 PX_CHECK_AND_RETURN_VAL(linkId < 64, "PxArticulationReducedCoordinate::getLinkVelocity index is not valid.", PxSpatialVelocity());
428
429 NP_READ_CHECK(mImpl.getOwnerScene());
430
431 return mImpl.mArticulation.getScArticulation().getLinkVelocity(linkId);
432}
433
434PxSpatialVelocity NpArticulationReducedCoordinate::getLinkAcceleration(const PxU32 linkId)
435{
436 PX_CHECK_AND_RETURN_VAL(mImpl.getAPIScene(), "PxArticulationReducedCoordinate::getLinkAcceleration: object must be in a scene", PxSpatialVelocity());
437 PX_CHECK_AND_RETURN_VAL(linkId < 64, "PxArticulationReducedCoordinate::getLinkAcceleration index is not valid.", PxSpatialVelocity());
438
439 NP_READ_CHECK(mImpl.getOwnerScene());
440
441 return mImpl.mArticulation.getScArticulation().getLinkAcceleration(linkId);
442}
443
444NpArticulationReducedCoordinate::~NpArticulationReducedCoordinate()
445{
446 NpFactory::getInstance().onArticulationRelease(this);
447}
448
449PxArticulationJointBase* NpArticulationReducedCoordinate::createArticulationJoint(PxArticulationLink& parent,
450 const PxTransform& parentFrame,
451 PxArticulationLink& child,
452 const PxTransform& childFrame)
453{
454 return NpFactory::getInstance().createNpArticulationJointRC(parent&: static_cast<NpArticulationLink&>(parent), parentFrame, child&: static_cast<NpArticulationLink&>(child), childFrame);
455}
456
457void NpArticulationReducedCoordinate::releaseArticulationJoint(PxArticulationJointBase* joint)
458{
459 NpFactory::getInstance().releaseArticulationJointRCToPool(articulationJoint&: *static_cast<NpArticulationJointReducedCoordinate*>(joint));
460}
461
462void PxArticulationImpl::recomputeLinkIDs()
463{
464 PX_CHECK_AND_RETURN(getAPIScene(), "PxArticulation::recomputeLinkIDs: object must be in a scene");
465 NP_WRITE_CHECK(getOwnerScene());
466
467 if (!mArticulation.isBuffering())
468 {
469 Sc::ArticulationSim* scArtSim = getScbArticulation().getScArticulation().getSim();
470
471 if (scArtSim)
472 {
473
474 physx::NpArticulationLink*const* links = getLinks();
475
476 const PxU32 nbLinks = getNbLinks();
477 for (PxU32 i = 1; i < nbLinks; ++i)
478 {
479 physx::NpArticulationLink* link = links[i];
480 PxU32 cHandle = scArtSim->findBodyIndex(body&: *link->getScbBodyFast().getScBody().getSim());
481 link->setLLIndex(cHandle);
482 }
483 }
484 }
485}
486
487
488

source code of qtquick3dphysics/src/3rdparty/PhysX/source/physx/src/NpArticulationReducedCoordinate.cpp