994 lines
31 KiB
C++
994 lines
31 KiB
C++
//
|
|
// Redistribution and use in source and binary forms, with or without
|
|
// modification, are permitted provided that the following conditions
|
|
// are met:
|
|
// * Redistributions of source code must retain the above copyright
|
|
// notice, this list of conditions and the following disclaimer.
|
|
// * Redistributions in binary form must reproduce the above copyright
|
|
// notice, this list of conditions and the following disclaimer in the
|
|
// documentation and/or other materials provided with the distribution.
|
|
// * Neither the name of NVIDIA CORPORATION nor the names of its
|
|
// contributors may be used to endorse or promote products derived
|
|
// from this software without specific prior written permission.
|
|
//
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY
|
|
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
|
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
|
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
|
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
|
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
|
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
|
|
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
//
|
|
// Copyright (c) 2008-2019 NVIDIA Corporation. All rights reserved.
|
|
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
|
|
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
|
|
|
|
#include "ScBodySim.h"
|
|
#include "ScShapeSim.h"
|
|
#include "ScScene.h"
|
|
#include "ScArticulationSim.h"
|
|
#include "PxsContext.h"
|
|
#include "PxsSimpleIslandManager.h"
|
|
#include "PxsSimulationController.h"
|
|
|
|
using namespace physx;
|
|
using namespace physx::Dy;
|
|
using namespace Sc;
|
|
|
|
#define PX_FREEZE_INTERVAL 1.5f
|
|
#define PX_FREE_EXIT_THRESHOLD 4.f
|
|
#define PX_FREEZE_TOLERANCE 0.25f
|
|
|
|
#define PX_SLEEP_DAMPING 0.5f
|
|
#define PX_FREEZE_SCALE 0.9f
|
|
|
|
BodySim::BodySim(Scene& scene, BodyCore& core, bool compound) :
|
|
RigidSim (scene, core),
|
|
mLLBody (&core.getCore(), PX_FREEZE_INTERVAL),
|
|
mNodeIndex (IG_INVALID_NODE),
|
|
mInternalFlags (0),
|
|
mVelModState (VMF_GRAVITY_DIRTY),
|
|
mActiveListIndex (SC_NOT_IN_SCENE_INDEX),
|
|
mActiveCompoundListIndex(SC_NOT_IN_SCENE_INDEX),
|
|
mArticulation (NULL),
|
|
mConstraintGroup (NULL)
|
|
{
|
|
core.getCore().numCountedInteractions = 0;
|
|
core.getCore().numBodyInteractions = 0;
|
|
core.getCore().disableGravity = core.getActorFlags() & PxActorFlag::eDISABLE_GRAVITY;
|
|
if(core.getFlags() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
|
|
mLLBody.mInternalFlags |= PxsRigidBody::eSPECULATIVE_CCD;
|
|
|
|
//If a body pending insertion was given a force/torque then it will have
|
|
//the dirty flags stored in a separate structure. Copy them across
|
|
//so we can use them now that the BodySim is constructed.
|
|
SimStateData* simStateData = core.getSimStateData(false);
|
|
bool hasPendingForce = false;
|
|
if(simStateData)
|
|
{
|
|
VelocityMod* velmod = simStateData->getVelocityModData();
|
|
hasPendingForce = (velmod->flags != 0) &&
|
|
(!velmod->getLinearVelModPerSec().isZero() || !velmod->getAngularVelModPerSec().isZero() ||
|
|
!velmod->getLinearVelModPerStep().isZero() || !velmod->getAngularVelModPerStep().isZero());
|
|
mVelModState = velmod->flags;
|
|
velmod->flags = 0;
|
|
}
|
|
|
|
// PT: don't read the core ptr we just wrote, use input param
|
|
// 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
|
|
const bool isAwake = (core.getWakeCounter() > 0) ||
|
|
(!core.getLinearVelocity().isZero()) ||
|
|
(!core.getAngularVelocity().isZero()) ||
|
|
hasPendingForce;
|
|
|
|
const bool isKine = isKinematic();
|
|
|
|
IG::SimpleIslandManager* simpleIslandManager = scene.getSimpleIslandManager();
|
|
if (!isArticulationLink())
|
|
{
|
|
mNodeIndex = simpleIslandManager->addRigidBody(&mLLBody, isKine, isAwake);
|
|
}
|
|
else
|
|
{
|
|
if(mArticulation)
|
|
{
|
|
const ArticulationLinkHandle articLinkhandle = mArticulation->getLinkHandle(*this);
|
|
IG::NodeIndex index = mArticulation->getIslandNodeIndex();
|
|
mNodeIndex.setIndices(index.index(), articLinkhandle & (DY_ARTICULATION_MAX_SIZE-1));
|
|
}
|
|
}
|
|
|
|
//If a user add force or torque before the body is inserted into the scene,
|
|
//this logic will make sure pre solver stage apply external force/torque to the body
|
|
if(hasPendingForce && !isArticulationLink())
|
|
scene.getVelocityModifyMap().growAndSet(mNodeIndex.index());
|
|
|
|
PX_ASSERT(mActiveListIndex == SC_NOT_IN_SCENE_INDEX);
|
|
|
|
// A.B. need to set the compound rigid flag early enough, so that we add the rigid into
|
|
// active list and do not create the shape bounds
|
|
if(compound)
|
|
raiseInternalFlag(BF_IS_COMPOUND_RIGID);
|
|
|
|
setActive(isAwake, ActorSim::AS_PART_OF_CREATION);
|
|
|
|
if (isAwake)
|
|
{
|
|
scene.addToActiveBodyList(*this);
|
|
PX_ASSERT(isActive());
|
|
}
|
|
else
|
|
{
|
|
mActiveListIndex = SC_NOT_IN_ACTIVE_LIST_INDEX;
|
|
mActiveCompoundListIndex = SC_NOT_IN_ACTIVE_LIST_INDEX;
|
|
PX_ASSERT(!isActive());
|
|
|
|
simpleIslandManager->deactivateNode(mNodeIndex);
|
|
}
|
|
|
|
if (isKine)
|
|
{
|
|
initKinematicStateBase(core, true);
|
|
|
|
const SimStateData* kd = core.getSimStateData(true);
|
|
if (!kd)
|
|
{
|
|
core.setupSimStateData(scene.getSimStateDataPool(), true, false);
|
|
notifyPutToSleep(); // sleep state of kinematics is fully controlled by the simulation controller not the island manager
|
|
}
|
|
else
|
|
{
|
|
PX_ASSERT(kd->isKine());
|
|
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
|
|
PX_ASSERT(isAwake); // the expectation is that setting a target also sets the wake counter to a positive value
|
|
postSetKinematicTarget();
|
|
}
|
|
}
|
|
}
|
|
|
|
BodySim::~BodySim()
|
|
{
|
|
Scene& scene = getScene();
|
|
const bool active = isActive();
|
|
|
|
getBodyCore().tearDownSimStateData(scene.getSimStateDataPool(), isKinematic() ? true : false);
|
|
|
|
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.
|
|
raiseInternalFlag(BF_ON_DEATHROW);
|
|
|
|
scene.removeBody(*this);
|
|
PX_ASSERT(!getConstraintGroup()); // Removing from scene should erase constraint group node if it existed
|
|
|
|
if(mArticulation)
|
|
mArticulation->removeBody(*this);
|
|
|
|
//Articulations are represented by a single node, so they must only be removed by the articulation and not the links!
|
|
if(mArticulation == NULL && mNodeIndex.articulationLinkId() == 0) //If it wasn't an articulation link, then we can remove it
|
|
scene.getSimpleIslandManager()->removeNode(mNodeIndex);
|
|
|
|
PX_ASSERT(mActiveListIndex != SC_NOT_IN_SCENE_INDEX);
|
|
|
|
if (active)
|
|
scene.removeFromActiveBodyList(*this);
|
|
|
|
mActiveListIndex = SC_NOT_IN_SCENE_INDEX;
|
|
mActiveCompoundListIndex = SC_NOT_IN_SCENE_INDEX;
|
|
|
|
mCore.setSim(NULL);
|
|
}
|
|
|
|
void BodySim::updateCached(Cm::BitMapPinned* shapeChangedMap)
|
|
{
|
|
if(!(mLLBody.mInternalFlags & PxsRigidBody::eFROZEN))
|
|
{
|
|
ElementSim* current = getElements_();
|
|
while(current)
|
|
{
|
|
static_cast<ShapeSim*>(current)->updateCached(0, shapeChangedMap);
|
|
current = current->mNextInActor;
|
|
}
|
|
}
|
|
}
|
|
|
|
void BodySim::updateCached(PxsTransformCache& transformCache, Bp::BoundsArray& boundsArray)
|
|
{
|
|
PX_ASSERT(!(mLLBody.mInternalFlags & PxsRigidBody::eFROZEN)); // PT: should not be called otherwise
|
|
|
|
ElementSim* current = getElements_();
|
|
while(current)
|
|
{
|
|
static_cast<ShapeSim*>(current)->updateCached(transformCache, boundsArray);
|
|
current = current->mNextInActor;
|
|
}
|
|
}
|
|
|
|
void BodySim::updateContactDistance(PxReal* contactDistance, const PxReal dt, Bp::BoundsArray& boundsArray)
|
|
{
|
|
if (getLowLevelBody().getCore().mFlags & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD
|
|
&& !(getLowLevelBody().mInternalFlags & PxsRigidBody::eFROZEN))
|
|
{
|
|
const PxVec3 linVel = getLowLevelBody().getLinearVelocity();
|
|
const PxVec3 aVel = getLowLevelBody().getAngularVelocity();
|
|
const PxReal inflation = linVel.magnitude() * dt;
|
|
|
|
ElementSim* current = getElements_();
|
|
while(current)
|
|
{
|
|
static_cast<ShapeSim*>(current)->updateContactDistance(contactDistance, inflation, aVel, dt, boundsArray);
|
|
current = current->mNextInActor;
|
|
}
|
|
}
|
|
}
|
|
|
|
//--------------------------------------------------------------
|
|
//
|
|
// BodyCore interface implementation
|
|
//
|
|
//--------------------------------------------------------------
|
|
|
|
void BodySim::notifyAddSpatialAcceleration()
|
|
{
|
|
//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to
|
|
//the expense of querying the simStateData for the velmod values.
|
|
raiseVelocityModFlag(VMF_ACC_DIRTY);
|
|
|
|
if(!isArticulationLink())
|
|
getScene().getVelocityModifyMap().growAndSet(getNodeIndex().index());
|
|
}
|
|
|
|
void BodySim::notifyClearSpatialAcceleration()
|
|
{
|
|
//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to
|
|
//the expense of querying the simStateData for the velmod values.
|
|
raiseVelocityModFlag(VMF_ACC_DIRTY);
|
|
if (!isArticulationLink())
|
|
getScene().getVelocityModifyMap().growAndSet(getNodeIndex().index());
|
|
}
|
|
|
|
void BodySim::notifyAddSpatialVelocity()
|
|
{
|
|
//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to
|
|
//the expense of querying the simStateData for the velmod values.
|
|
raiseVelocityModFlag(VMF_VEL_DIRTY);
|
|
if (!isArticulationLink())
|
|
getScene().getVelocityModifyMap().growAndSet(getNodeIndex().index());
|
|
}
|
|
|
|
void BodySim::notifyClearSpatialVelocity()
|
|
{
|
|
//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to
|
|
//the expense of querying the simStateData for the velmod values.
|
|
raiseVelocityModFlag(VMF_VEL_DIRTY);
|
|
if (!isArticulationLink())
|
|
getScene().getVelocityModifyMap().growAndSet(getNodeIndex().index());
|
|
}
|
|
|
|
void BodySim::postActorFlagChange(PxU32 oldFlags, PxU32 newFlags)
|
|
{
|
|
// PT: don't convert to bool if not needed
|
|
const PxU32 wasWeightless = oldFlags & PxActorFlag::eDISABLE_GRAVITY;
|
|
const PxU32 isWeightless = newFlags & PxActorFlag::eDISABLE_GRAVITY;
|
|
|
|
if (isWeightless != wasWeightless)
|
|
{
|
|
if (mVelModState == 0)
|
|
raiseVelocityModFlag(VMF_GRAVITY_DIRTY);
|
|
|
|
getBodyCore().getCore().disableGravity = isWeightless!=0;
|
|
}
|
|
}
|
|
|
|
void BodySim::postBody2WorldChange()
|
|
{
|
|
mLLBody.saveLastCCDTransform();
|
|
notifyShapesOfTransformChange();
|
|
}
|
|
|
|
void BodySim::postSetWakeCounter(PxReal t, bool forceWakeUp)
|
|
{
|
|
if ((t > 0.0f) || forceWakeUp)
|
|
notifyNotReadyForSleeping();
|
|
else
|
|
{
|
|
const bool readyForSleep = checkSleepReadinessBesidesWakeCounter();
|
|
if (readyForSleep)
|
|
notifyReadyForSleeping();
|
|
}
|
|
}
|
|
|
|
void BodySim::postSetKinematicTarget()
|
|
{
|
|
PX_ASSERT(getBodyCore().getSimStateData(true));
|
|
PX_ASSERT(getBodyCore().getSimStateData(true)->isKine());
|
|
PX_ASSERT(getBodyCore().getSimStateData(true)->getKinematicData()->targetValid);
|
|
|
|
raiseInternalFlag(BF_KINEMATIC_MOVED); // Important to set this here already because trigger interactions need to have this information when being activated.
|
|
clearInternalFlag(BF_KINEMATIC_SURFACE_VELOCITY);
|
|
}
|
|
|
|
static void updateBPGroup(ElementSim* current)
|
|
{
|
|
while(current)
|
|
{
|
|
static_cast<ShapeSim*>(current)->updateBPGroup();
|
|
current = current->mNextInActor;
|
|
}
|
|
}
|
|
|
|
void BodySim::postSwitchToKinematic()
|
|
{
|
|
initKinematicStateBase(getBodyCore(), false);
|
|
|
|
// - interactions need to get refiltered to make sure that kinematic-kinematic and kinematic-static pairs get suppressed
|
|
// - unlike postSwitchToDynamic(), constraint interactions are not marked dirty here because a transition to kinematic will put the object asleep which in turn
|
|
// triggers onDeactivate_() on the constraint pairs that are active. If such pairs get deactivated, they will get removed from the list of active breakable
|
|
// constraints automatically.
|
|
setActorsInteractionsDirty(InteractionDirtyFlag::eBODY_KINEMATIC, NULL, InteractionFlag::eFILTERABLE);
|
|
|
|
getScene().getSimpleIslandManager()->setKinematic(mNodeIndex);
|
|
|
|
//
|
|
updateBPGroup(getElements_());
|
|
}
|
|
|
|
void BodySim::postSwitchToDynamic()
|
|
{
|
|
mScene.getSimpleIslandManager()->setDynamic(mNodeIndex);
|
|
|
|
setForcesToDefaults(true);
|
|
|
|
if(getConstraintGroup())
|
|
getConstraintGroup()->markForProjectionTreeRebuild(mScene.getProjectionManager());
|
|
|
|
// - interactions need to get refiltered to make sure that former kinematic-kinematic and kinematic-static pairs get enabled
|
|
// - switching from kinematic to dynamic does not change the sleep state of the body. The constraint interactions are marked dirty
|
|
// to check later whether they need to be activated plus potentially tracked for constraint break testing. This special treatment
|
|
// is necessary because constraints between two kinematic bodies are considered inactive, no matter whether one of the kinematics
|
|
// is active (has a target) or not.
|
|
setActorsInteractionsDirty(InteractionDirtyFlag::eBODY_KINEMATIC, NULL, InteractionFlag::eFILTERABLE | InteractionFlag::eCONSTRAINT);
|
|
|
|
clearInternalFlag(BF_KINEMATIC_MOVE_FLAGS);
|
|
|
|
if(isActive())
|
|
mScene.swapInActiveBodyList(*this);
|
|
|
|
//
|
|
updateBPGroup(getElements_());
|
|
}
|
|
|
|
void BodySim::postPosePreviewChange(const PxU32 posePreviewFlag)
|
|
{
|
|
if (isActive())
|
|
{
|
|
if (posePreviewFlag & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW)
|
|
getScene().addToPosePreviewList(*this);
|
|
else
|
|
getScene().removeFromPosePreviewList(*this);
|
|
}
|
|
else
|
|
PX_ASSERT(!getScene().isInPosePreviewList(*this));
|
|
}
|
|
|
|
//--------------------------------------------------------------
|
|
//
|
|
// Sleeping
|
|
//
|
|
//--------------------------------------------------------------
|
|
|
|
void BodySim::activate()
|
|
{
|
|
// Activate body
|
|
{
|
|
PX_ASSERT((!isKinematic()) || notInScene() || readInternalFlag(InternalFlags(BF_KINEMATIC_MOVED | BF_KINEMATIC_SURFACE_VELOCITY))); // kinematics should only get activated when a target is set.
|
|
// exception: object gets newly added, then the state change will happen later
|
|
if(!isArticulationLink())
|
|
{
|
|
mLLBody.mInternalFlags &= (~PxsRigidBody::eFROZEN);
|
|
// Put in list of activated bodies. The list gets cleared at the end of a sim step after the sleep callbacks have been fired.
|
|
getScene().onBodyWakeUp(this);
|
|
}
|
|
|
|
BodyCore& core = getBodyCore();
|
|
if(core.getFlags() & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW)
|
|
{
|
|
PX_ASSERT(!getScene().isInPosePreviewList(*this));
|
|
getScene().addToPosePreviewList(*this);
|
|
}
|
|
createSqBounds();
|
|
}
|
|
|
|
// Activate interactions
|
|
{
|
|
const PxU32 nbInteractions = getActorInteractionCount();
|
|
|
|
for(PxU32 i=0; i<nbInteractions; ++i)
|
|
{
|
|
Ps::prefetchLine(mInteractions[PxMin(i+1,nbInteractions-1)]);
|
|
Interaction* interaction = mInteractions[i];
|
|
|
|
const bool isNotIGControlled = interaction->getType() != InteractionType::eOVERLAP &&
|
|
interaction->getType() != InteractionType::eMARKER;
|
|
|
|
if(!interaction->readInteractionFlag(InteractionFlag::eIS_ACTIVE) && (isNotIGControlled))
|
|
{
|
|
const bool proceed = activateInteraction(interaction, NULL);
|
|
|
|
if (proceed && (interaction->getType() < InteractionType::eTRACKED_IN_SCENE_COUNT))
|
|
getScene().notifyInteractionActivated(interaction);
|
|
}
|
|
}
|
|
}
|
|
|
|
//set speculative CCD bit map if speculative CCD flag is on
|
|
{
|
|
BodyCore& core = getBodyCore();
|
|
if (core.getFlags() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
|
|
{
|
|
if (isArticulationLink())
|
|
{
|
|
if (getNodeIndex().isValid())
|
|
getScene().setSpeculativeCCDArticulationLink(getNodeIndex().index());
|
|
}
|
|
else
|
|
getScene().setSpeculativeCCDRigidBody(getNodeIndex().index());
|
|
}
|
|
}
|
|
}
|
|
|
|
void BodySim::deactivate()
|
|
{
|
|
// Deactivate interactions
|
|
{
|
|
const PxU32 nbInteractions = getActorInteractionCount();
|
|
|
|
for(PxU32 i=0; i<nbInteractions; ++i)
|
|
{
|
|
Ps::prefetchLine(mInteractions[PxMin(i+1,nbInteractions-1)]);
|
|
Interaction* interaction = mInteractions[i];
|
|
|
|
const bool isNotIGControlled = interaction->getType() != InteractionType::eOVERLAP &&
|
|
interaction->getType() != InteractionType::eMARKER;
|
|
|
|
if (interaction->readInteractionFlag(InteractionFlag::eIS_ACTIVE) && isNotIGControlled)
|
|
{
|
|
const bool proceed = deactivateInteraction(interaction);
|
|
if (proceed && (interaction->getType() < InteractionType::eTRACKED_IN_SCENE_COUNT))
|
|
getScene().notifyInteractionDeactivated(interaction);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Deactivate body
|
|
{
|
|
PX_ASSERT((!isKinematic()) || notInScene() || !readInternalFlag(BF_KINEMATIC_MOVED)); // kinematics should only get deactivated when no target is set.
|
|
// exception: object gets newly added, then the state change will happen later
|
|
BodyCore& core = getBodyCore();
|
|
if(!readInternalFlag(BF_ON_DEATHROW))
|
|
{
|
|
// Set velocity to 0.
|
|
// Note: this is also fine if the method gets called because the user puts something to sleep (this behavior is documented in the API)
|
|
PX_ASSERT(core.getWakeCounter() == 0.0f);
|
|
const PxVec3 zero(0.0f);
|
|
core.setLinearVelocityInternal(zero);
|
|
core.setAngularVelocityInternal(zero);
|
|
|
|
setForcesToDefaults(!core.getCore().disableGravity);
|
|
}
|
|
|
|
if(!isArticulationLink()) // Articulations have their own sleep logic.
|
|
getScene().onBodySleep(this);
|
|
|
|
if(core.getFlags() & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW)
|
|
{
|
|
PX_ASSERT(getScene().isInPosePreviewList(*this));
|
|
getScene().removeFromPosePreviewList(*this);
|
|
}
|
|
destroySqBounds();
|
|
}
|
|
|
|
// reset speculative CCD bit map if speculative CCD flag is on
|
|
{
|
|
BodyCore& core = getBodyCore();
|
|
if (core.getFlags() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
|
|
{
|
|
if (isArticulationLink())
|
|
{
|
|
if (getNodeIndex().isValid())
|
|
getScene().resetSpeculativeCCDArticulationLink(getNodeIndex().index());
|
|
}
|
|
else
|
|
getScene().resetSpeculativeCCDRigidBody(getNodeIndex().index());
|
|
}
|
|
}
|
|
}
|
|
|
|
void BodySim::setActive(bool active, PxU32 infoFlag)
|
|
{
|
|
PX_ASSERT(!active || isDynamicRigid()); // Currently there should be no need to activate an actor that does not take part in island generation
|
|
|
|
const PxU32 asPartOfCreation = infoFlag & ActorSim::AS_PART_OF_CREATION;
|
|
if (asPartOfCreation || isActive() != active)
|
|
{
|
|
PX_ASSERT(!asPartOfCreation || (getActorInteractionCount() == 0)); // On creation or destruction there should be no interactions
|
|
|
|
if (active)
|
|
{
|
|
if (!asPartOfCreation)
|
|
{
|
|
// Inactive => Active
|
|
getScene().addToActiveBodyList(*this);
|
|
}
|
|
|
|
activate();
|
|
|
|
PX_ASSERT(asPartOfCreation || isActive());
|
|
}
|
|
else
|
|
{
|
|
if (!asPartOfCreation)
|
|
{
|
|
// Active => Inactive
|
|
getScene().removeFromActiveBodyList(*this);
|
|
}
|
|
|
|
deactivate();
|
|
|
|
PX_ASSERT(asPartOfCreation || (!isActive()));
|
|
}
|
|
}
|
|
}
|
|
|
|
void BodySim::wakeUp()
|
|
{
|
|
setActive(true);
|
|
notifyWakeUp(true);
|
|
}
|
|
|
|
void BodySim::putToSleep()
|
|
{
|
|
PX_ASSERT(getBodyCore().getWakeCounter() == 0.0f);
|
|
PX_ASSERT(getBodyCore().getLinearVelocity().isZero());
|
|
PX_ASSERT(getBodyCore().getAngularVelocity().isZero());
|
|
#ifdef _DEBUG
|
|
// pending forces should have been cleared at this point
|
|
const SimStateData* sd = getBodyCore().getSimStateData(false);
|
|
if (sd)
|
|
{
|
|
const VelocityMod* vm = sd->getVelocityModData();
|
|
PX_ASSERT(vm->linearPerSec.isZero() && vm->linearPerStep.isZero() && vm->angularPerSec.isZero() && vm->angularPerStep.isZero());
|
|
}
|
|
#endif
|
|
|
|
setActive(false);
|
|
notifyPutToSleep();
|
|
|
|
clearInternalFlag(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.
|
|
// We can move this code when we look into the open task of making buffered re-insertion more consistent with the non-buffered case.
|
|
}
|
|
|
|
void BodySim::internalWakeUp(PxReal wakeCounterValue)
|
|
{
|
|
if(mArticulation)
|
|
mArticulation->internalWakeUp(wakeCounterValue);
|
|
else
|
|
internalWakeUpBase(wakeCounterValue);
|
|
}
|
|
|
|
void BodySim::internalWakeUpArticulationLink(PxReal wakeCounterValue)
|
|
{
|
|
PX_ASSERT(mArticulation);
|
|
internalWakeUpBase(wakeCounterValue);
|
|
}
|
|
|
|
void 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!
|
|
{
|
|
if ((!isKinematic()) && (getBodyCore().getWakeCounter() < wakeCounterValue))
|
|
{
|
|
PX_ASSERT(wakeCounterValue > 0.0f);
|
|
getBodyCore().setWakeCounterFromSim(wakeCounterValue);
|
|
|
|
//we need to update the gpu body sim because we reset the wake counter for the body core
|
|
mScene.getSimulationController()->updateDynamic(isArticulationLink(), mNodeIndex);
|
|
setActive(true);
|
|
notifyWakeUp(false);
|
|
mLLBody.mInternalFlags &= (~PxsRigidBody::eFROZEN);
|
|
}
|
|
}
|
|
|
|
void BodySim::notifyReadyForSleeping()
|
|
{
|
|
if(mArticulation == NULL)
|
|
getScene().getSimpleIslandManager()->deactivateNode(mNodeIndex);
|
|
}
|
|
|
|
void BodySim::notifyNotReadyForSleeping()
|
|
{
|
|
getScene().getSimpleIslandManager()->activateNode(mNodeIndex);
|
|
}
|
|
|
|
void BodySim::notifyWakeUp(bool /*wakeUpInIslandGen*/)
|
|
{
|
|
getScene().getSimpleIslandManager()->activateNode(mNodeIndex);
|
|
}
|
|
|
|
void BodySim::notifyPutToSleep()
|
|
{
|
|
getScene().getSimpleIslandManager()->putNodeToSleep(mNodeIndex);
|
|
}
|
|
|
|
void BodySim::resetSleepFilter()
|
|
{
|
|
mLLBody.sleepAngVelAcc = PxVec3(0.0f);
|
|
mLLBody.sleepLinVelAcc = PxVec3(0.0f);
|
|
}
|
|
|
|
//This function will be called by CPU sleepCheck code
|
|
PxReal BodySim::updateWakeCounter(PxReal dt, PxReal energyThreshold, const Cm::SpatialVector& motionVelocity)
|
|
{
|
|
// update the body's sleep state and
|
|
BodyCore& core = getBodyCore();
|
|
|
|
const PxReal wakeCounterResetTime = ScInternalWakeCounterResetValue;
|
|
|
|
PxReal wc = core.getWakeCounter();
|
|
|
|
{
|
|
PxVec3 bcSleepLinVelAcc = mLLBody.sleepLinVelAcc;
|
|
PxVec3 bcSleepAngVelAcc = mLLBody.sleepAngVelAcc;
|
|
|
|
if(wc < wakeCounterResetTime * 0.5f || wc < dt)
|
|
{
|
|
const PxTransform& body2World = getBody2World();
|
|
|
|
// calculate normalized energy: kinetic energy divided by mass
|
|
const PxVec3 t = core.getInverseInertia();
|
|
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);
|
|
|
|
PxVec3 sleepLinVelAcc =motionVelocity.linear;
|
|
PxVec3 sleepAngVelAcc = body2World.q.rotateInv(motionVelocity.angular);
|
|
|
|
bcSleepLinVelAcc += sleepLinVelAcc;
|
|
bcSleepAngVelAcc += sleepAngVelAcc;
|
|
|
|
PxReal invMass = core.getInverseMass();
|
|
if(invMass == 0.0f)
|
|
invMass = 1.0f;
|
|
|
|
const PxReal angular = bcSleepAngVelAcc.multiply(bcSleepAngVelAcc).dot(inertia) * invMass;
|
|
const PxReal linear = bcSleepLinVelAcc.magnitudeSquared();
|
|
PxReal normalizedEnergy = 0.5f * (angular + linear);
|
|
|
|
// scale threshold by cluster factor (more contacts => higher sleep threshold)
|
|
const PxReal clusterFactor = PxReal(1 + getNumCountedInteractions());
|
|
const PxReal threshold = clusterFactor*energyThreshold;
|
|
|
|
if (normalizedEnergy >= threshold)
|
|
{
|
|
PX_ASSERT(isActive());
|
|
resetSleepFilter();
|
|
const float factor = threshold == 0.0f ? 2.0f : PxMin(normalizedEnergy/threshold, 2.0f);
|
|
PxReal oldWc = wc;
|
|
wc = factor * 0.5f * wakeCounterResetTime + dt * (clusterFactor - 1.0f);
|
|
core.setWakeCounterFromSim(wc);
|
|
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
|
|
notifyNotReadyForSleeping();
|
|
|
|
return wc;
|
|
}
|
|
}
|
|
|
|
mLLBody.sleepLinVelAcc = bcSleepLinVelAcc;
|
|
mLLBody.sleepAngVelAcc = bcSleepAngVelAcc;
|
|
}
|
|
|
|
wc = PxMax(wc-dt, 0.0f);
|
|
core.setWakeCounterFromSim(wc);
|
|
return wc;
|
|
}
|
|
|
|
//--------------------------------------------------------------
|
|
//
|
|
// Kinematics
|
|
//
|
|
//--------------------------------------------------------------
|
|
|
|
PX_FORCE_INLINE void BodySim::initKinematicStateBase(BodyCore&, bool asPartOfCreation)
|
|
{
|
|
PX_ASSERT(!readInternalFlag(BF_KINEMATIC_MOVED));
|
|
|
|
if (!asPartOfCreation && isActive())
|
|
getScene().swapInActiveBodyList(*this);
|
|
|
|
//mLLBody.setAccelerationV(Cm::SpatialVector::zero());
|
|
|
|
// Need to be before setting setRigidBodyFlag::KINEMATIC
|
|
|
|
if (getConstraintGroup())
|
|
getConstraintGroup()->markForProjectionTreeRebuild(getScene().getProjectionManager());
|
|
}
|
|
|
|
void BodySim::calculateKinematicVelocity(PxReal oneOverDt)
|
|
{
|
|
PX_ASSERT(isKinematic());
|
|
|
|
/*------------------------------------------------\
|
|
| kinematic bodies are moved directly by the user and are not influenced by external forces
|
|
| we simply determine the distance moved since the last simulation frame and
|
|
| assign the appropriate delta to the velocity. This vel will be used to shove dynamic
|
|
| objects in the solver.
|
|
| We have to do this like so in a delayed way, because when the user sets the target pos the dt is not
|
|
| yet known.
|
|
\------------------------------------------------*/
|
|
PX_ASSERT(isActive());
|
|
|
|
BodyCore& core = getBodyCore();
|
|
|
|
if (readInternalFlag(BF_KINEMATIC_MOVED))
|
|
{
|
|
clearInternalFlag(InternalFlags(BF_KINEMATIC_SETTLING | BF_KINEMATIC_SETTLING_2));
|
|
const SimStateData* kData = core.getSimStateData(true);
|
|
PX_ASSERT(kData);
|
|
PX_ASSERT(kData->isKine());
|
|
PX_ASSERT(kData->getKinematicData()->targetValid);
|
|
PxVec3 linVelLL, angVelLL;
|
|
const PxTransform targetPose = kData->getKinematicData()->targetPose;
|
|
const PxTransform& currBody2World = getBody2World();
|
|
|
|
//the kinematic target pose is now the target of the body (CoM) and not the actor.
|
|
|
|
PxVec3 deltaPos = targetPose.p;
|
|
deltaPos -= currBody2World.p;
|
|
linVelLL = deltaPos * oneOverDt;
|
|
|
|
PxQuat q = targetPose.q * currBody2World.q.getConjugate();
|
|
|
|
if (q.w < 0) //shortest angle.
|
|
q = -q;
|
|
|
|
PxReal angle;
|
|
PxVec3 axis;
|
|
q.toRadiansAndUnitAxis(angle, axis);
|
|
angVelLL = axis * angle * oneOverDt;
|
|
|
|
core.getCore().linearVelocity = linVelLL;
|
|
core.getCore().angularVelocity = angVelLL;
|
|
|
|
// Moving a kinematic should trigger a wakeUp call on a higher level.
|
|
PX_ASSERT(core.getWakeCounter()>0);
|
|
PX_ASSERT(isActive());
|
|
|
|
}
|
|
else if (!readInternalFlag(BF_KINEMATIC_SURFACE_VELOCITY))
|
|
{
|
|
core.setLinearVelocity(PxVec3(0));
|
|
core.setAngularVelocity(PxVec3(0));
|
|
}
|
|
}
|
|
|
|
void BodySim::updateKinematicPose()
|
|
{
|
|
/*------------------------------------------------\
|
|
| kinematic bodies are moved directly by the user and are not influenced by external forces
|
|
| we simply determine the distance moved since the last simulation frame and
|
|
| assign the appropriate delta to the velocity. This vel will be used to shove dynamic
|
|
| objects in the solver.
|
|
| We have to do this like so in a delayed way, because when the user sets the target pos the dt is not
|
|
| yet known.
|
|
\------------------------------------------------*/
|
|
|
|
PX_ASSERT(isKinematic());
|
|
PX_ASSERT(isActive());
|
|
|
|
BodyCore& core = getBodyCore();
|
|
|
|
if (readInternalFlag(BF_KINEMATIC_MOVED))
|
|
{
|
|
clearInternalFlag(InternalFlags(BF_KINEMATIC_SETTLING | BF_KINEMATIC_SETTLING_2));
|
|
const SimStateData* kData = core.getSimStateData(true);
|
|
PX_ASSERT(kData);
|
|
PX_ASSERT(kData->isKine());
|
|
PX_ASSERT(kData->getKinematicData()->targetValid);
|
|
|
|
const PxTransform targetPose = kData->getKinematicData()->targetPose;
|
|
getBodyCore().getCore().body2World = targetPose;
|
|
}
|
|
}
|
|
|
|
bool BodySim::deactivateKinematic()
|
|
{
|
|
BodyCore& core = getBodyCore();
|
|
if(readInternalFlag(BF_KINEMATIC_SETTLING_2))
|
|
{
|
|
clearInternalFlag(BF_KINEMATIC_SETTLING_2);
|
|
core.setWakeCounterFromSim(0); // For sleeping objects the wake counter must be 0. This needs to hold for kinematics too.
|
|
notifyReadyForSleeping();
|
|
notifyPutToSleep();
|
|
setActive(false);
|
|
return true;
|
|
}
|
|
else if (readInternalFlag(BF_KINEMATIC_SETTLING))
|
|
{
|
|
clearInternalFlag(BF_KINEMATIC_SETTLING);
|
|
raiseInternalFlag(BF_KINEMATIC_SETTLING_2);
|
|
}
|
|
else if (!readInternalFlag(BF_KINEMATIC_SURFACE_VELOCITY))
|
|
{
|
|
clearInternalFlag(BF_KINEMATIC_MOVED);
|
|
raiseInternalFlag(BF_KINEMATIC_SETTLING);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
//--------------------------------------------------------------
|
|
//
|
|
// Miscellaneous
|
|
//
|
|
//--------------------------------------------------------------
|
|
|
|
void BodySim::updateForces(PxReal dt, PxsRigidBody** updatedBodySims, PxU32* updatedBodyNodeIndices, PxU32& index, Cm::SpatialVector* acceleration, const bool useAccelerations, bool simUsesAdaptiveForce)
|
|
{
|
|
PxVec3 linAcc(0.0f), angAcc(0.0f);
|
|
|
|
const bool accDirty = readVelocityModFlag(VMF_ACC_DIRTY);
|
|
const bool velDirty = readVelocityModFlag(VMF_VEL_DIRTY);
|
|
|
|
BodyCore& bodyCore = getBodyCore();
|
|
SimStateData* simStateData = NULL;
|
|
|
|
//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
|
|
//means angAcc will be always 0
|
|
if( (accDirty || velDirty) && ((simStateData = bodyCore.getSimStateData(false)) != NULL) )
|
|
{
|
|
VelocityMod* velmod = simStateData->getVelocityModData();
|
|
|
|
//we don't have support for articulation yet
|
|
if (updatedBodySims)
|
|
{
|
|
updatedBodySims[index] = &getLowLevelBody();
|
|
updatedBodyNodeIndices[index++] = getNodeIndex().index();
|
|
}
|
|
|
|
if(velDirty)
|
|
{
|
|
linAcc = velmod->getLinearVelModPerStep();
|
|
angAcc = velmod->getAngularVelModPerStep();
|
|
|
|
if (useAccelerations)
|
|
{
|
|
acceleration->linear = linAcc / dt;
|
|
acceleration->angular = angAcc / dt;
|
|
|
|
}
|
|
else
|
|
{
|
|
getBodyCore().updateVelocities(linAcc, angAcc);
|
|
}
|
|
}
|
|
|
|
if (accDirty)
|
|
{
|
|
linAcc = velmod->getLinearVelModPerSec();
|
|
angAcc = velmod->getAngularVelModPerSec();
|
|
|
|
if (acceleration)
|
|
{
|
|
acceleration->linear = linAcc;
|
|
acceleration->angular = angAcc;
|
|
}
|
|
else
|
|
{
|
|
PxReal scale = dt;
|
|
if (simUsesAdaptiveForce)
|
|
{
|
|
if (getScene().getSimpleIslandManager()->getAccurateIslandSim().getIslandStaticTouchCount(mNodeIndex) != 0)
|
|
{
|
|
scale *= mLLBody.accelScale;
|
|
}
|
|
}
|
|
getBodyCore().updateVelocities(linAcc*scale, angAcc*scale);
|
|
}
|
|
}
|
|
}
|
|
|
|
setForcesToDefaults(readVelocityModFlag(VMF_ACC_DIRTY));
|
|
}
|
|
|
|
void BodySim::onConstraintDetach()
|
|
{
|
|
PX_ASSERT(readInternalFlag(BF_HAS_CONSTRAINTS));
|
|
|
|
PxU32 size = getActorInteractionCount();
|
|
Interaction** interactions = getActorInteractions();
|
|
unregisterCountedInteraction();
|
|
|
|
while(size--)
|
|
{
|
|
const Interaction* interaction = *interactions++;
|
|
if(interaction->getType() == InteractionType::eCONSTRAINTSHADER)
|
|
return;
|
|
}
|
|
|
|
clearInternalFlag(BF_HAS_CONSTRAINTS); // There are no other constraint interactions left
|
|
}
|
|
|
|
void BodySim::setArticulation(ArticulationSim* a, PxReal wakeCounter, bool asleep, PxU32 bodyIndex)
|
|
{
|
|
mArticulation = a;
|
|
if(a)
|
|
{
|
|
IG::NodeIndex index = mArticulation->getIslandNodeIndex();
|
|
mNodeIndex.setIndices(index.index(), bodyIndex);
|
|
getBodyCore().setWakeCounterFromSim(wakeCounter);
|
|
|
|
if (getFlagsFast() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
|
|
getScene().setSpeculativeCCDArticulationLink(mNodeIndex.index());
|
|
|
|
if (!asleep)
|
|
{
|
|
setActive(true);
|
|
notifyWakeUp(false);
|
|
}
|
|
else
|
|
{
|
|
notifyReadyForSleeping();
|
|
notifyPutToSleep();
|
|
setActive(false);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//Setting a 1 in the articulation ID to avoid returning the node Index to the node index
|
|
//manager
|
|
mNodeIndex.setIndices(IG_INVALID_NODE, 1);
|
|
}
|
|
}
|
|
|
|
void BodySim::createSqBounds()
|
|
{
|
|
if(!isActive() || usingSqKinematicTarget() || readInternalFlag(BF_IS_COMPOUND_RIGID))
|
|
return;
|
|
|
|
PX_ASSERT(!isFrozen());
|
|
|
|
ElementSim* current = getElements_();
|
|
while(current)
|
|
{
|
|
static_cast<ShapeSim*>(current)->createSqBounds();
|
|
current = current->mNextInActor;
|
|
}
|
|
}
|
|
|
|
void BodySim::destroySqBounds()
|
|
{
|
|
ElementSim* current = getElements_();
|
|
while(current)
|
|
{
|
|
static_cast<ShapeSim*>(current)->destroySqBounds();
|
|
current = current->mNextInActor;
|
|
}
|
|
}
|
|
|
|
void BodySim::freezeTransforms(Cm::BitMapPinned* shapeChangedMap)
|
|
{
|
|
ElementSim* current = getElements_();
|
|
while(current)
|
|
{
|
|
ShapeSim* sim = static_cast<ShapeSim*>(current);
|
|
sim->updateCached(PxsTransformFlag::eFROZEN, shapeChangedMap);
|
|
sim->destroySqBounds();
|
|
current = current->mNextInActor;
|
|
}
|
|
}
|
|
|
|
void BodySim::disableCompound()
|
|
{
|
|
if(isActive())
|
|
getScene().removeFromActiveCompoundBodyList(*this);
|
|
clearInternalFlag(BF_IS_COMPOUND_RIGID);
|
|
}
|
|
|