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 "ScBodySim.h"
31#include "ScShapeSim.h"
32#include "ScScene.h"
33#include "ScArticulationSim.h"
34#include "PxsContext.h"
35#include "PxsSimpleIslandManager.h"
36#include "PxsSimulationController.h"
37
38using namespace physx;
39using namespace physx::Dy;
40using namespace Sc;
41
42#define PX_FREEZE_INTERVAL 1.5f
43#define PX_FREE_EXIT_THRESHOLD 4.f
44#define PX_FREEZE_TOLERANCE 0.25f
45
46#define PX_SLEEP_DAMPING 0.5f
47#define PX_FREEZE_SCALE 0.9f
48
49BodySim::BodySim(Scene& scene, BodyCore& core, bool compound) :
50 RigidSim (scene, core),
51 mLLBody (&core.getCore(), PX_FREEZE_INTERVAL),
52 mNodeIndex (IG_INVALID_NODE),
53 mInternalFlags (0),
54 mVelModState (VMF_GRAVITY_DIRTY),
55 mActiveListIndex (SC_NOT_IN_SCENE_INDEX),
56 mActiveCompoundListIndex(SC_NOT_IN_SCENE_INDEX),
57 mArticulation (NULL),
58 mConstraintGroup (NULL)
59{
60 core.getCore().numCountedInteractions = 0;
61 core.getCore().numBodyInteractions = 0;
62 core.getCore().disableGravity = core.getActorFlags() & PxActorFlag::eDISABLE_GRAVITY;
63 if(core.getFlags() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
64 mLLBody.mInternalFlags |= PxsRigidBody::eSPECULATIVE_CCD;
65
66 //If a body pending insertion was given a force/torque then it will have
67 //the dirty flags stored in a separate structure. Copy them across
68 //so we can use them now that the BodySim is constructed.
69 SimStateData* simStateData = core.getSimStateData(isKinematic: false);
70 bool hasPendingForce = false;
71 if(simStateData)
72 {
73 VelocityMod* velmod = simStateData->getVelocityModData();
74 hasPendingForce = (velmod->flags != 0) &&
75 (!velmod->getLinearVelModPerSec().isZero() || !velmod->getAngularVelModPerSec().isZero() ||
76 !velmod->getLinearVelModPerStep().isZero() || !velmod->getAngularVelModPerStep().isZero());
77 mVelModState = velmod->flags;
78 velmod->flags = 0;
79 }
80
81 // PT: don't read the core ptr we just wrote, use input param
82 // PT: at time of writing we get a big L2 here because even though bodycore has been prefetched, the wake counter is 160 bytes away
83 const bool isAwake = (core.getWakeCounter() > 0) ||
84 (!core.getLinearVelocity().isZero()) ||
85 (!core.getAngularVelocity().isZero()) ||
86 hasPendingForce;
87
88 const bool isKine = isKinematic();
89
90 IG::SimpleIslandManager* simpleIslandManager = scene.getSimpleIslandManager();
91 if (!isArticulationLink())
92 {
93 mNodeIndex = simpleIslandManager->addRigidBody(body: &mLLBody, isKinematic: isKine, isActive: isAwake);
94 }
95 else
96 {
97 if(mArticulation)
98 {
99 const ArticulationLinkHandle articLinkhandle = mArticulation->getLinkHandle(body&: *this);
100 IG::NodeIndex index = mArticulation->getIslandNodeIndex();
101 mNodeIndex.setIndices(index: index.index(), articLinkId: articLinkhandle & (DY_ARTICULATION_MAX_SIZE-1));
102 }
103 }
104
105 //If a user add force or torque before the body is inserted into the scene,
106 //this logic will make sure pre solver stage apply external force/torque to the body
107 if(hasPendingForce && !isArticulationLink())
108 scene.getVelocityModifyMap().growAndSet(index: mNodeIndex.index());
109
110 PX_ASSERT(mActiveListIndex == SC_NOT_IN_SCENE_INDEX);
111
112 // A.B. need to set the compound rigid flag early enough, so that we add the rigid into
113 // active list and do not create the shape bounds
114 if(compound)
115 raiseInternalFlag(flag: BF_IS_COMPOUND_RIGID);
116
117 setActive(active: isAwake, infoFlag: ActorSim::AS_PART_OF_CREATION);
118
119 if (isAwake)
120 {
121 scene.addToActiveBodyList(actor&: *this);
122 PX_ASSERT(isActive());
123 }
124 else
125 {
126 mActiveListIndex = SC_NOT_IN_ACTIVE_LIST_INDEX;
127 mActiveCompoundListIndex = SC_NOT_IN_ACTIVE_LIST_INDEX;
128 PX_ASSERT(!isActive());
129
130 simpleIslandManager->deactivateNode(index: mNodeIndex);
131 }
132
133 if (isKine)
134 {
135 initKinematicStateBase(core, asPartOfCreation: true);
136
137 const SimStateData* kd = core.getSimStateData(isKinematic: true);
138 if (!kd)
139 {
140 core.setupSimStateData(simStateDataPool: scene.getSimStateDataPool(), isKinematic: true, targetValid: false);
141 notifyPutToSleep(); // sleep state of kinematics is fully controlled by the simulation controller not the island manager
142 }
143 else
144 {
145 PX_ASSERT(kd->isKine());
146 PX_ASSERT(kd->getKinematicData()->targetValid); // the only reason for the kinematic data to exist at that point already is if the target has been set
147 PX_ASSERT(isAwake); // the expectation is that setting a target also sets the wake counter to a positive value
148 postSetKinematicTarget();
149 }
150 }
151}
152
153BodySim::~BodySim()
154{
155 Scene& scene = getScene();
156 const bool active = isActive();
157
158 getBodyCore().tearDownSimStateData(simStateDataPool: scene.getSimStateDataPool(), isKinematic: isKinematic() ? true : false);
159
160 PX_ASSERT(!readInternalFlag(BF_ON_DEATHROW)); // Before 3.0 it could happen that destroy could get called twice. Assert to make sure this is fixed.
161 raiseInternalFlag(flag: BF_ON_DEATHROW);
162
163 scene.removeBody(*this);
164 PX_ASSERT(!getConstraintGroup()); // Removing from scene should erase constraint group node if it existed
165
166 if(mArticulation)
167 mArticulation->removeBody(sim&: *this);
168
169 //Articulations are represented by a single node, so they must only be removed by the articulation and not the links!
170 if(mArticulation == NULL && mNodeIndex.articulationLinkId() == 0) //If it wasn't an articulation link, then we can remove it
171 scene.getSimpleIslandManager()->removeNode(index: mNodeIndex);
172
173 PX_ASSERT(mActiveListIndex != SC_NOT_IN_SCENE_INDEX);
174
175 if (active)
176 scene.removeFromActiveBodyList(actor&: *this);
177
178 mActiveListIndex = SC_NOT_IN_SCENE_INDEX;
179 mActiveCompoundListIndex = SC_NOT_IN_SCENE_INDEX;
180
181 mCore.setSim(NULL);
182}
183
184void BodySim::updateCached(Cm::BitMapPinned* shapeChangedMap)
185{
186 if(!(mLLBody.mInternalFlags & PxsRigidBody::eFROZEN))
187 {
188 ElementSim* current = getElements_();
189 while(current)
190 {
191 static_cast<ShapeSim*>(current)->updateCached(transformCacheFlags: 0, shapeChangedMap);
192 current = current->mNextInActor;
193 }
194 }
195}
196
197void BodySim::updateCached(PxsTransformCache& transformCache, Bp::BoundsArray& boundsArray)
198{
199 PX_ASSERT(!(mLLBody.mInternalFlags & PxsRigidBody::eFROZEN)); // PT: should not be called otherwise
200
201 ElementSim* current = getElements_();
202 while(current)
203 {
204 static_cast<ShapeSim*>(current)->updateCached(transformCache, boundsArray);
205 current = current->mNextInActor;
206 }
207}
208
209void BodySim::updateContactDistance(PxReal* contactDistance, const PxReal dt, Bp::BoundsArray& boundsArray)
210{
211 if (getLowLevelBody().getCore().mFlags & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD
212 && !(getLowLevelBody().mInternalFlags & PxsRigidBody::eFROZEN))
213 {
214 const PxVec3 linVel = getLowLevelBody().getLinearVelocity();
215 const PxVec3 aVel = getLowLevelBody().getAngularVelocity();
216 const PxReal inflation = linVel.magnitude() * dt;
217
218 ElementSim* current = getElements_();
219 while(current)
220 {
221 static_cast<ShapeSim*>(current)->updateContactDistance(contactDistance, inflation, angVel: aVel, dt, boundsArray);
222 current = current->mNextInActor;
223 }
224 }
225}
226
227//--------------------------------------------------------------
228//
229// BodyCore interface implementation
230//
231//--------------------------------------------------------------
232
233void BodySim::notifyAddSpatialAcceleration()
234{
235 //The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to
236 //the expense of querying the simStateData for the velmod values.
237 raiseVelocityModFlag(f: VMF_ACC_DIRTY);
238
239 if(!isArticulationLink())
240 getScene().getVelocityModifyMap().growAndSet(index: getNodeIndex().index());
241}
242
243void BodySim::notifyClearSpatialAcceleration()
244{
245 //The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to
246 //the expense of querying the simStateData for the velmod values.
247 raiseVelocityModFlag(f: VMF_ACC_DIRTY);
248 if (!isArticulationLink())
249 getScene().getVelocityModifyMap().growAndSet(index: getNodeIndex().index());
250}
251
252void BodySim::notifyAddSpatialVelocity()
253{
254 //The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to
255 //the expense of querying the simStateData for the velmod values.
256 raiseVelocityModFlag(f: VMF_VEL_DIRTY);
257 if (!isArticulationLink())
258 getScene().getVelocityModifyMap().growAndSet(index: getNodeIndex().index());
259}
260
261void BodySim::notifyClearSpatialVelocity()
262{
263 //The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to
264 //the expense of querying the simStateData for the velmod values.
265 raiseVelocityModFlag(f: VMF_VEL_DIRTY);
266 if (!isArticulationLink())
267 getScene().getVelocityModifyMap().growAndSet(index: getNodeIndex().index());
268}
269
270void BodySim::postActorFlagChange(PxU32 oldFlags, PxU32 newFlags)
271{
272 // PT: don't convert to bool if not needed
273 const PxU32 wasWeightless = oldFlags & PxActorFlag::eDISABLE_GRAVITY;
274 const PxU32 isWeightless = newFlags & PxActorFlag::eDISABLE_GRAVITY;
275
276 if (isWeightless != wasWeightless)
277 {
278 if (mVelModState == 0)
279 raiseVelocityModFlag(f: VMF_GRAVITY_DIRTY);
280
281 getBodyCore().getCore().disableGravity = isWeightless!=0;
282 }
283}
284
285void BodySim::postBody2WorldChange()
286{
287 mLLBody.saveLastCCDTransform();
288 notifyShapesOfTransformChange();
289}
290
291void BodySim::postSetWakeCounter(PxReal t, bool forceWakeUp)
292{
293 if ((t > 0.0f) || forceWakeUp)
294 notifyNotReadyForSleeping();
295 else
296 {
297 const bool readyForSleep = checkSleepReadinessBesidesWakeCounter();
298 if (readyForSleep)
299 notifyReadyForSleeping();
300 }
301}
302
303void BodySim::postSetKinematicTarget()
304{
305 PX_ASSERT(getBodyCore().getSimStateData(true));
306 PX_ASSERT(getBodyCore().getSimStateData(true)->isKine());
307 PX_ASSERT(getBodyCore().getSimStateData(true)->getKinematicData()->targetValid);
308
309 raiseInternalFlag(flag: BF_KINEMATIC_MOVED); // Important to set this here already because trigger interactions need to have this information when being activated.
310 clearInternalFlag(flag: BF_KINEMATIC_SURFACE_VELOCITY);
311}
312
313static void updateBPGroup(ElementSim* current)
314{
315 while(current)
316 {
317 static_cast<ShapeSim*>(current)->updateBPGroup();
318 current = current->mNextInActor;
319 }
320}
321
322void BodySim::postSwitchToKinematic()
323{
324 initKinematicStateBase(getBodyCore(), asPartOfCreation: false);
325
326 // - interactions need to get refiltered to make sure that kinematic-kinematic and kinematic-static pairs get suppressed
327 // - unlike postSwitchToDynamic(), constraint interactions are not marked dirty here because a transition to kinematic will put the object asleep which in turn
328 // triggers onDeactivate_() on the constraint pairs that are active. If such pairs get deactivated, they will get removed from the list of active breakable
329 // constraints automatically.
330 setActorsInteractionsDirty(flag: InteractionDirtyFlag::eBODY_KINEMATIC, NULL, interactionFlag: InteractionFlag::eFILTERABLE);
331
332 getScene().getSimpleIslandManager()->setKinematic(mNodeIndex);
333
334 //
335 updateBPGroup(current: getElements_());
336}
337
338void BodySim::postSwitchToDynamic()
339{
340 mScene.getSimpleIslandManager()->setDynamic(mNodeIndex);
341
342 setForcesToDefaults(true);
343
344 if(getConstraintGroup())
345 getConstraintGroup()->markForProjectionTreeRebuild(mScene.getProjectionManager());
346
347 // - interactions need to get refiltered to make sure that former kinematic-kinematic and kinematic-static pairs get enabled
348 // - switching from kinematic to dynamic does not change the sleep state of the body. The constraint interactions are marked dirty
349 // to check later whether they need to be activated plus potentially tracked for constraint break testing. This special treatment
350 // is necessary because constraints between two kinematic bodies are considered inactive, no matter whether one of the kinematics
351 // is active (has a target) or not.
352 setActorsInteractionsDirty(flag: InteractionDirtyFlag::eBODY_KINEMATIC, NULL, interactionFlag: InteractionFlag::eFILTERABLE | InteractionFlag::eCONSTRAINT);
353
354 clearInternalFlag(flag: BF_KINEMATIC_MOVE_FLAGS);
355
356 if(isActive())
357 mScene.swapInActiveBodyList(body&: *this);
358
359 //
360 updateBPGroup(current: getElements_());
361}
362
363void BodySim::postPosePreviewChange(const PxU32 posePreviewFlag)
364{
365 if (isActive())
366 {
367 if (posePreviewFlag & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW)
368 getScene().addToPosePreviewList(b&: *this);
369 else
370 getScene().removeFromPosePreviewList(b&: *this);
371 }
372 else
373 PX_ASSERT(!getScene().isInPosePreviewList(*this));
374}
375
376//--------------------------------------------------------------
377//
378// Sleeping
379//
380//--------------------------------------------------------------
381
382void BodySim::activate()
383{
384 // Activate body
385 {
386 PX_ASSERT((!isKinematic()) || notInScene() || readInternalFlag(InternalFlags(BF_KINEMATIC_MOVED | BF_KINEMATIC_SURFACE_VELOCITY))); // kinematics should only get activated when a target is set.
387 // exception: object gets newly added, then the state change will happen later
388 if(!isArticulationLink())
389 {
390 mLLBody.mInternalFlags &= (~PxsRigidBody::eFROZEN);
391 // Put in list of activated bodies. The list gets cleared at the end of a sim step after the sleep callbacks have been fired.
392 getScene().onBodyWakeUp(body: this);
393 }
394
395 BodyCore& core = getBodyCore();
396 if(core.getFlags() & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW)
397 {
398 PX_ASSERT(!getScene().isInPosePreviewList(*this));
399 getScene().addToPosePreviewList(b&: *this);
400 }
401 createSqBounds();
402 }
403
404 // Activate interactions
405 {
406 const PxU32 nbInteractions = getActorInteractionCount();
407
408 for(PxU32 i=0; i<nbInteractions; ++i)
409 {
410 Ps::prefetchLine(ptr: mInteractions[PxMin(a: i+1,b: nbInteractions-1)]);
411 Interaction* interaction = mInteractions[i];
412
413 const bool isNotIGControlled = interaction->getType() != InteractionType::eOVERLAP &&
414 interaction->getType() != InteractionType::eMARKER;
415
416 if(!interaction->readInteractionFlag(flag: InteractionFlag::eIS_ACTIVE) && (isNotIGControlled))
417 {
418 const bool proceed = activateInteraction(interaction, NULL);
419
420 if (proceed && (interaction->getType() < InteractionType::eTRACKED_IN_SCENE_COUNT))
421 getScene().notifyInteractionActivated(interaction);
422 }
423 }
424 }
425
426 //set speculative CCD bit map if speculative CCD flag is on
427 {
428 BodyCore& core = getBodyCore();
429 if (core.getFlags() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
430 {
431 if (isArticulationLink())
432 {
433 if (getNodeIndex().isValid())
434 getScene().setSpeculativeCCDArticulationLink(getNodeIndex().index());
435 }
436 else
437 getScene().setSpeculativeCCDRigidBody(getNodeIndex().index());
438 }
439 }
440}
441
442void BodySim::deactivate()
443{
444 // Deactivate interactions
445 {
446 const PxU32 nbInteractions = getActorInteractionCount();
447
448 for(PxU32 i=0; i<nbInteractions; ++i)
449 {
450 Ps::prefetchLine(ptr: mInteractions[PxMin(a: i+1,b: nbInteractions-1)]);
451 Interaction* interaction = mInteractions[i];
452
453 const bool isNotIGControlled = interaction->getType() != InteractionType::eOVERLAP &&
454 interaction->getType() != InteractionType::eMARKER;
455
456 if (interaction->readInteractionFlag(flag: InteractionFlag::eIS_ACTIVE) && isNotIGControlled)
457 {
458 const bool proceed = deactivateInteraction(interaction);
459 if (proceed && (interaction->getType() < InteractionType::eTRACKED_IN_SCENE_COUNT))
460 getScene().notifyInteractionDeactivated(interaction);
461 }
462 }
463 }
464
465 // Deactivate body
466 {
467 PX_ASSERT((!isKinematic()) || notInScene() || !readInternalFlag(BF_KINEMATIC_MOVED)); // kinematics should only get deactivated when no target is set.
468 // exception: object gets newly added, then the state change will happen later
469 BodyCore& core = getBodyCore();
470 if(!readInternalFlag(flag: BF_ON_DEATHROW))
471 {
472 // Set velocity to 0.
473 // Note: this is also fine if the method gets called because the user puts something to sleep (this behavior is documented in the API)
474 PX_ASSERT(core.getWakeCounter() == 0.0f);
475 const PxVec3 zero(0.0f);
476 core.setLinearVelocityInternal(zero);
477 core.setAngularVelocityInternal(zero);
478
479 setForcesToDefaults(!core.getCore().disableGravity);
480 }
481
482 if(!isArticulationLink()) // Articulations have their own sleep logic.
483 getScene().onBodySleep(body: this);
484
485 if(core.getFlags() & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW)
486 {
487 PX_ASSERT(getScene().isInPosePreviewList(*this));
488 getScene().removeFromPosePreviewList(b&: *this);
489 }
490 destroySqBounds();
491 }
492
493 // reset speculative CCD bit map if speculative CCD flag is on
494 {
495 BodyCore& core = getBodyCore();
496 if (core.getFlags() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
497 {
498 if (isArticulationLink())
499 {
500 if (getNodeIndex().isValid())
501 getScene().resetSpeculativeCCDArticulationLink(index: getNodeIndex().index());
502 }
503 else
504 getScene().resetSpeculativeCCDRigidBody(index: getNodeIndex().index());
505 }
506 }
507}
508
509void BodySim::setActive(bool active, PxU32 infoFlag)
510{
511 PX_ASSERT(!active || isDynamicRigid()); // Currently there should be no need to activate an actor that does not take part in island generation
512
513 const PxU32 asPartOfCreation = infoFlag & ActorSim::AS_PART_OF_CREATION;
514 if (asPartOfCreation || isActive() != active)
515 {
516 PX_ASSERT(!asPartOfCreation || (getActorInteractionCount() == 0)); // On creation or destruction there should be no interactions
517
518 if (active)
519 {
520 if (!asPartOfCreation)
521 {
522 // Inactive => Active
523 getScene().addToActiveBodyList(actor&: *this);
524 }
525
526 activate();
527
528 PX_ASSERT(asPartOfCreation || isActive());
529 }
530 else
531 {
532 if (!asPartOfCreation)
533 {
534 // Active => Inactive
535 getScene().removeFromActiveBodyList(actor&: *this);
536 }
537
538 deactivate();
539
540 PX_ASSERT(asPartOfCreation || (!isActive()));
541 }
542 }
543}
544
545void BodySim::wakeUp()
546{
547 setActive(active: true);
548 notifyWakeUp(wakeUpInIslandGen: true);
549}
550
551void BodySim::putToSleep()
552{
553 PX_ASSERT(getBodyCore().getWakeCounter() == 0.0f);
554 PX_ASSERT(getBodyCore().getLinearVelocity().isZero());
555 PX_ASSERT(getBodyCore().getAngularVelocity().isZero());
556#ifdef _DEBUG
557 // pending forces should have been cleared at this point
558 const SimStateData* sd = getBodyCore().getSimStateData(isKinematic: false);
559 if (sd)
560 {
561 const VelocityMod* vm = sd->getVelocityModData();
562 PX_ASSERT(vm->linearPerSec.isZero() && vm->linearPerStep.isZero() && vm->angularPerSec.isZero() && vm->angularPerStep.isZero());
563 }
564#endif
565
566 setActive(active: false);
567 notifyPutToSleep();
568
569 clearInternalFlag(flag: InternalFlags(BF_KINEMATIC_SETTLING | BF_KINEMATIC_SETTLING_2)); // putToSleep is used when a kinematic gets removed from the scene while the sim is running and then gets re-inserted immediately.
570 // We can move this code when we look into the open task of making buffered re-insertion more consistent with the non-buffered case.
571}
572
573void BodySim::internalWakeUp(PxReal wakeCounterValue)
574{
575 if(mArticulation)
576 mArticulation->internalWakeUp(wakeCounter: wakeCounterValue);
577 else
578 internalWakeUpBase(wakeCounterValue);
579}
580
581void BodySim::internalWakeUpArticulationLink(PxReal wakeCounterValue)
582{
583 PX_ASSERT(mArticulation);
584 internalWakeUpBase(wakeCounterValue);
585}
586
587void BodySim::internalWakeUpBase(PxReal wakeCounterValue) //this one can only increase the wake counter, not decrease it, so it can't be used to put things to sleep!
588{
589 if ((!isKinematic()) && (getBodyCore().getWakeCounter() < wakeCounterValue))
590 {
591 PX_ASSERT(wakeCounterValue > 0.0f);
592 getBodyCore().setWakeCounterFromSim(wakeCounterValue);
593
594 //we need to update the gpu body sim because we reset the wake counter for the body core
595 mScene.getSimulationController()->updateDynamic(isArticulationLink: isArticulationLink(), mNodeIndex);
596 setActive(active: true);
597 notifyWakeUp(wakeUpInIslandGen: false);
598 mLLBody.mInternalFlags &= (~PxsRigidBody::eFROZEN);
599 }
600}
601
602void BodySim::notifyReadyForSleeping()
603{
604 if(mArticulation == NULL)
605 getScene().getSimpleIslandManager()->deactivateNode(index: mNodeIndex);
606}
607
608void BodySim::notifyNotReadyForSleeping()
609{
610 getScene().getSimpleIslandManager()->activateNode(index: mNodeIndex);
611}
612
613void BodySim::notifyWakeUp(bool /*wakeUpInIslandGen*/)
614{
615 getScene().getSimpleIslandManager()->activateNode(index: mNodeIndex);
616}
617
618void BodySim::notifyPutToSleep()
619{
620 getScene().getSimpleIslandManager()->putNodeToSleep(index: mNodeIndex);
621}
622
623void BodySim::resetSleepFilter()
624{
625 mLLBody.sleepAngVelAcc = PxVec3(0.0f);
626 mLLBody.sleepLinVelAcc = PxVec3(0.0f);
627}
628
629//This function will be called by CPU sleepCheck code
630PxReal BodySim::updateWakeCounter(PxReal dt, PxReal energyThreshold, const Cm::SpatialVector& motionVelocity)
631{
632 // update the body's sleep state and
633 BodyCore& core = getBodyCore();
634
635 const PxReal wakeCounterResetTime = ScInternalWakeCounterResetValue;
636
637 PxReal wc = core.getWakeCounter();
638
639 {
640 PxVec3 bcSleepLinVelAcc = mLLBody.sleepLinVelAcc;
641 PxVec3 bcSleepAngVelAcc = mLLBody.sleepAngVelAcc;
642
643 if(wc < wakeCounterResetTime * 0.5f || wc < dt)
644 {
645 const PxTransform& body2World = getBody2World();
646
647 // calculate normalized energy: kinetic energy divided by mass
648 const PxVec3 t = core.getInverseInertia();
649 const PxVec3 inertia(t.x > 0.0f ? 1.0f/t.x : 1.0f, t.y > 0.0f ? 1.0f/t.y : 1.0f, t.z > 0.0f ? 1.0f/t.z : 1.0f);
650
651 PxVec3 sleepLinVelAcc =motionVelocity.linear;
652 PxVec3 sleepAngVelAcc = body2World.q.rotateInv(v: motionVelocity.angular);
653
654 bcSleepLinVelAcc += sleepLinVelAcc;
655 bcSleepAngVelAcc += sleepAngVelAcc;
656
657 PxReal invMass = core.getInverseMass();
658 if(invMass == 0.0f)
659 invMass = 1.0f;
660
661 const PxReal angular = bcSleepAngVelAcc.multiply(a: bcSleepAngVelAcc).dot(v: inertia) * invMass;
662 const PxReal linear = bcSleepLinVelAcc.magnitudeSquared();
663 PxReal normalizedEnergy = 0.5f * (angular + linear);
664
665 // scale threshold by cluster factor (more contacts => higher sleep threshold)
666 const PxReal clusterFactor = PxReal(1 + getNumCountedInteractions());
667 const PxReal threshold = clusterFactor*energyThreshold;
668
669 if (normalizedEnergy >= threshold)
670 {
671 PX_ASSERT(isActive());
672 resetSleepFilter();
673 const float factor = threshold == 0.0f ? 2.0f : PxMin(a: normalizedEnergy/threshold, b: 2.0f);
674 PxReal oldWc = wc;
675 wc = factor * 0.5f * wakeCounterResetTime + dt * (clusterFactor - 1.0f);
676 core.setWakeCounterFromSim(wc);
677 if (oldWc == 0.0f) // for the case where a sleeping body got activated by the system (not the user) AND got processed by the solver as well
678 notifyNotReadyForSleeping();
679
680 return wc;
681 }
682 }
683
684 mLLBody.sleepLinVelAcc = bcSleepLinVelAcc;
685 mLLBody.sleepAngVelAcc = bcSleepAngVelAcc;
686 }
687
688 wc = PxMax(a: wc-dt, b: 0.0f);
689 core.setWakeCounterFromSim(wc);
690 return wc;
691}
692
693//--------------------------------------------------------------
694//
695// Kinematics
696//
697//--------------------------------------------------------------
698
699PX_FORCE_INLINE void BodySim::initKinematicStateBase(BodyCore&, bool asPartOfCreation)
700{
701 PX_ASSERT(!readInternalFlag(BF_KINEMATIC_MOVED));
702
703 if (!asPartOfCreation && isActive())
704 getScene().swapInActiveBodyList(body&: *this);
705
706 //mLLBody.setAccelerationV(Cm::SpatialVector::zero());
707
708 // Need to be before setting setRigidBodyFlag::KINEMATIC
709
710 if (getConstraintGroup())
711 getConstraintGroup()->markForProjectionTreeRebuild(getScene().getProjectionManager());
712}
713
714void BodySim::calculateKinematicVelocity(PxReal oneOverDt)
715{
716 PX_ASSERT(isKinematic());
717
718 /*------------------------------------------------\
719 | kinematic bodies are moved directly by the user and are not influenced by external forces
720 | we simply determine the distance moved since the last simulation frame and
721 | assign the appropriate delta to the velocity. This vel will be used to shove dynamic
722 | objects in the solver.
723 | We have to do this like so in a delayed way, because when the user sets the target pos the dt is not
724 | yet known.
725 \------------------------------------------------*/
726 PX_ASSERT(isActive());
727
728 BodyCore& core = getBodyCore();
729
730 if (readInternalFlag(flag: BF_KINEMATIC_MOVED))
731 {
732 clearInternalFlag(flag: InternalFlags(BF_KINEMATIC_SETTLING | BF_KINEMATIC_SETTLING_2));
733 const SimStateData* kData = core.getSimStateData(isKinematic: true);
734 PX_ASSERT(kData);
735 PX_ASSERT(kData->isKine());
736 PX_ASSERT(kData->getKinematicData()->targetValid);
737 PxVec3 linVelLL, angVelLL;
738 const PxTransform targetPose = kData->getKinematicData()->targetPose;
739 const PxTransform& currBody2World = getBody2World();
740
741 //the kinematic target pose is now the target of the body (CoM) and not the actor.
742
743 PxVec3 deltaPos = targetPose.p;
744 deltaPos -= currBody2World.p;
745 linVelLL = deltaPos * oneOverDt;
746
747 PxQuat q = targetPose.q * currBody2World.q.getConjugate();
748
749 if (q.w < 0) //shortest angle.
750 q = -q;
751
752 PxReal angle;
753 PxVec3 axis;
754 q.toRadiansAndUnitAxis(angle, axis);
755 angVelLL = axis * angle * oneOverDt;
756
757 core.getCore().linearVelocity = linVelLL;
758 core.getCore().angularVelocity = angVelLL;
759
760 // Moving a kinematic should trigger a wakeUp call on a higher level.
761 PX_ASSERT(core.getWakeCounter()>0);
762 PX_ASSERT(isActive());
763
764 }
765 else if (!readInternalFlag(flag: BF_KINEMATIC_SURFACE_VELOCITY))
766 {
767 core.setLinearVelocity(PxVec3(0));
768 core.setAngularVelocity(PxVec3(0));
769 }
770}
771
772void BodySim::updateKinematicPose()
773{
774 /*------------------------------------------------\
775 | kinematic bodies are moved directly by the user and are not influenced by external forces
776 | we simply determine the distance moved since the last simulation frame and
777 | assign the appropriate delta to the velocity. This vel will be used to shove dynamic
778 | objects in the solver.
779 | We have to do this like so in a delayed way, because when the user sets the target pos the dt is not
780 | yet known.
781 \------------------------------------------------*/
782
783 PX_ASSERT(isKinematic());
784 PX_ASSERT(isActive());
785
786 BodyCore& core = getBodyCore();
787
788 if (readInternalFlag(flag: BF_KINEMATIC_MOVED))
789 {
790 clearInternalFlag(flag: InternalFlags(BF_KINEMATIC_SETTLING | BF_KINEMATIC_SETTLING_2));
791 const SimStateData* kData = core.getSimStateData(isKinematic: true);
792 PX_ASSERT(kData);
793 PX_ASSERT(kData->isKine());
794 PX_ASSERT(kData->getKinematicData()->targetValid);
795
796 const PxTransform targetPose = kData->getKinematicData()->targetPose;
797 getBodyCore().getCore().body2World = targetPose;
798 }
799}
800
801bool BodySim::deactivateKinematic()
802{
803 BodyCore& core = getBodyCore();
804 if(readInternalFlag(flag: BF_KINEMATIC_SETTLING_2))
805 {
806 clearInternalFlag(flag: BF_KINEMATIC_SETTLING_2);
807 core.setWakeCounterFromSim(0); // For sleeping objects the wake counter must be 0. This needs to hold for kinematics too.
808 notifyReadyForSleeping();
809 notifyPutToSleep();
810 setActive(active: false);
811 return true;
812 }
813 else if (readInternalFlag(flag: BF_KINEMATIC_SETTLING))
814 {
815 clearInternalFlag(flag: BF_KINEMATIC_SETTLING);
816 raiseInternalFlag(flag: BF_KINEMATIC_SETTLING_2);
817 }
818 else if (!readInternalFlag(flag: BF_KINEMATIC_SURFACE_VELOCITY))
819 {
820 clearInternalFlag(flag: BF_KINEMATIC_MOVED);
821 raiseInternalFlag(flag: BF_KINEMATIC_SETTLING);
822 }
823 return false;
824}
825
826//--------------------------------------------------------------
827//
828// Miscellaneous
829//
830//--------------------------------------------------------------
831
832void BodySim::updateForces(PxReal dt, PxsRigidBody** updatedBodySims, PxU32* updatedBodyNodeIndices, PxU32& index, Cm::SpatialVector* acceleration, const bool useAccelerations, bool simUsesAdaptiveForce)
833{
834 PxVec3 linAcc(0.0f), angAcc(0.0f);
835
836 const bool accDirty = readVelocityModFlag(f: VMF_ACC_DIRTY);
837 const bool velDirty = readVelocityModFlag(f: VMF_VEL_DIRTY);
838
839 BodyCore& bodyCore = getBodyCore();
840 SimStateData* simStateData = NULL;
841
842 //if we change the logic like this, which means we don't need to have two seperate variables in the pxgbodysim to represent linAcc and angAcc. However, this
843 //means angAcc will be always 0
844 if( (accDirty || velDirty) && ((simStateData = bodyCore.getSimStateData(isKinematic: false)) != NULL) )
845 {
846 VelocityMod* velmod = simStateData->getVelocityModData();
847
848 //we don't have support for articulation yet
849 if (updatedBodySims)
850 {
851 updatedBodySims[index] = &getLowLevelBody();
852 updatedBodyNodeIndices[index++] = getNodeIndex().index();
853 }
854
855 if(velDirty)
856 {
857 linAcc = velmod->getLinearVelModPerStep();
858 angAcc = velmod->getAngularVelModPerStep();
859
860 if (useAccelerations)
861 {
862 acceleration->linear = linAcc / dt;
863 acceleration->angular = angAcc / dt;
864
865 }
866 else
867 {
868 getBodyCore().updateVelocities(linearVelModPerStep: linAcc, angularVelModPerStep: angAcc);
869 }
870 }
871
872 if (accDirty)
873 {
874 linAcc = velmod->getLinearVelModPerSec();
875 angAcc = velmod->getAngularVelModPerSec();
876
877 if (acceleration)
878 {
879 acceleration->linear = linAcc;
880 acceleration->angular = angAcc;
881 }
882 else
883 {
884 PxReal scale = dt;
885 if (simUsesAdaptiveForce)
886 {
887 if (getScene().getSimpleIslandManager()->getAccurateIslandSim().getIslandStaticTouchCount(nodeIndex: mNodeIndex) != 0)
888 {
889 scale *= mLLBody.accelScale;
890 }
891 }
892 getBodyCore().updateVelocities(linearVelModPerStep: linAcc*scale, angularVelModPerStep: angAcc*scale);
893 }
894 }
895 }
896
897 setForcesToDefaults(readVelocityModFlag(f: VMF_ACC_DIRTY));
898}
899
900void BodySim::onConstraintDetach()
901{
902 PX_ASSERT(readInternalFlag(BF_HAS_CONSTRAINTS));
903
904 PxU32 size = getActorInteractionCount();
905 Interaction** interactions = getActorInteractions();
906 unregisterCountedInteraction();
907
908 while(size--)
909 {
910 const Interaction* interaction = *interactions++;
911 if(interaction->getType() == InteractionType::eCONSTRAINTSHADER)
912 return;
913 }
914
915 clearInternalFlag(flag: BF_HAS_CONSTRAINTS); // There are no other constraint interactions left
916}
917
918void BodySim::setArticulation(ArticulationSim* a, PxReal wakeCounter, bool asleep, PxU32 bodyIndex)
919{
920 mArticulation = a;
921 if(a)
922 {
923 IG::NodeIndex index = mArticulation->getIslandNodeIndex();
924 mNodeIndex.setIndices(index: index.index(), articLinkId: bodyIndex);
925 getBodyCore().setWakeCounterFromSim(wakeCounter);
926
927 if (getFlagsFast() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
928 getScene().setSpeculativeCCDArticulationLink(mNodeIndex.index());
929
930 if (!asleep)
931 {
932 setActive(active: true);
933 notifyWakeUp(false);
934 }
935 else
936 {
937 notifyReadyForSleeping();
938 notifyPutToSleep();
939 setActive(active: false);
940 }
941 }
942 else
943 {
944 //Setting a 1 in the articulation ID to avoid returning the node Index to the node index
945 //manager
946 mNodeIndex.setIndices(IG_INVALID_NODE, articLinkId: 1);
947 }
948}
949
950void BodySim::createSqBounds()
951{
952 if(!isActive() || usingSqKinematicTarget() || readInternalFlag(flag: BF_IS_COMPOUND_RIGID))
953 return;
954
955 PX_ASSERT(!isFrozen());
956
957 ElementSim* current = getElements_();
958 while(current)
959 {
960 static_cast<ShapeSim*>(current)->createSqBounds();
961 current = current->mNextInActor;
962 }
963}
964
965void BodySim::destroySqBounds()
966{
967 ElementSim* current = getElements_();
968 while(current)
969 {
970 static_cast<ShapeSim*>(current)->destroySqBounds();
971 current = current->mNextInActor;
972 }
973}
974
975void BodySim::freezeTransforms(Cm::BitMapPinned* shapeChangedMap)
976{
977 ElementSim* current = getElements_();
978 while(current)
979 {
980 ShapeSim* sim = static_cast<ShapeSim*>(current);
981 sim->updateCached(transformCacheFlags: PxsTransformFlag::eFROZEN, shapeChangedMap);
982 sim->destroySqBounds();
983 current = current->mNextInActor;
984 }
985}
986
987void BodySim::disableCompound()
988{
989 if(isActive())
990 getScene().removeFromActiveCompoundBodyList(actor&: *this);
991 clearInternalFlag(flag: BF_IS_COMPOUND_RIGID);
992}
993
994

source code of qtquick3dphysics/src/3rdparty/PhysX/source/simulationcontroller/src/ScBodySim.cpp