347 lines
17 KiB
C++
347 lines
17 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-2021 NVIDIA Corporation. All rights reserved.
|
|
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
|
|
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
|
|
|
|
|
|
#ifndef PX_PHYSICS_SCP_BODYSIM
|
|
#define PX_PHYSICS_SCP_BODYSIM
|
|
|
|
#include "PsUtilities.h"
|
|
#include "PsIntrinsics.h"
|
|
#include "ScRigidSim.h"
|
|
#include "PxvDynamics.h"
|
|
#include "ScBodyCore.h"
|
|
#include "ScSimStateData.h"
|
|
#include "ScConstraintGroupNode.h"
|
|
#include "PxRigidDynamic.h"
|
|
#include "DyArticulation.h"
|
|
#include "PxsRigidBody.h"
|
|
#include "PxsSimpleIslandManager.h"
|
|
|
|
namespace physx
|
|
{
|
|
namespace Bp
|
|
{
|
|
class BoundsArray;
|
|
}
|
|
class PxsTransformCache;
|
|
class PxsSimulationController;
|
|
namespace Sc
|
|
{
|
|
#define SC_NOT_IN_SCENE_INDEX 0xffffffff // the body is not in the scene yet
|
|
#define SC_NOT_IN_ACTIVE_LIST_INDEX 0xfffffffe // the body is in the scene but not in the active list
|
|
|
|
class Scene;
|
|
class ArticulationSim;
|
|
|
|
static const PxReal ScInternalWakeCounterResetValue = 20.0f*0.02f;
|
|
|
|
#if PX_VC
|
|
#pragma warning(push)
|
|
#pragma warning( disable : 4324 ) // Padding was added at the end of a structure because of a __declspec(align) value.
|
|
#endif
|
|
|
|
class BodySim : public RigidSim
|
|
{
|
|
public:
|
|
enum InternalFlags
|
|
{
|
|
//BF_DISABLE_GRAVITY = 1 << 0, // Don't apply the scene's gravity
|
|
|
|
BF_HAS_STATIC_TOUCH = 1 << 1, // Set when a body is part of an island with static contacts. Needed to be able to recalculate adaptive force if this changes
|
|
BF_KINEMATIC_MOVED = 1 << 2, // Set when the kinematic was moved
|
|
|
|
BF_ON_DEATHROW = 1 << 3, // Set when the body is destroyed
|
|
|
|
BF_IS_IN_SLEEP_LIST = 1 << 4, // Set when the body is added to the list of bodies which were put to sleep
|
|
BF_IS_IN_WAKEUP_LIST = 1 << 5, // Set when the body is added to the list of bodies which were woken up
|
|
BF_SLEEP_NOTIFY = 1 << 6, // A sleep notification should be sent for this body (and not a wakeup event, even if the body is part of the woken list as well)
|
|
BF_WAKEUP_NOTIFY = 1 << 7, // A wake up notification should be sent for this body (and not a sleep event, even if the body is part of the sleep list as well)
|
|
|
|
BF_HAS_CONSTRAINTS = 1 << 8, // Set if the body has one or more constraints
|
|
BF_KINEMATIC_SETTLING = 1 << 9, // Set when the body was moved kinematically last frame
|
|
BF_KINEMATIC_SETTLING_2 = 1 << 10,
|
|
BF_KINEMATIC_MOVE_FLAGS = BF_KINEMATIC_MOVED | BF_KINEMATIC_SETTLING | BF_KINEMATIC_SETTLING_2, //Used to clear kinematic masks in 1 call
|
|
BF_KINEMATIC_SURFACE_VELOCITY = 1 << 11, //Set when the application calls setKinematicVelocity. Actor remains awake until application calls clearKinematicVelocity.
|
|
BF_IS_COMPOUND_RIGID = 1 << 12 // Set when the body is a compound actor, we dont want to set the sq bounds
|
|
|
|
// PT: WARNING: flags stored on 16-bits now.
|
|
};
|
|
|
|
public:
|
|
BodySim(Scene&, BodyCore&, bool);
|
|
virtual ~BodySim();
|
|
|
|
void notifyAddSpatialAcceleration();
|
|
void notifyClearSpatialAcceleration();
|
|
void notifyAddSpatialVelocity();
|
|
void notifyClearSpatialVelocity();
|
|
void updateCached(Cm::BitMapPinned* shapeChangedMap);
|
|
void updateCached(PxsTransformCache& transformCache, Bp::BoundsArray& boundsArray);
|
|
void updateContactDistance(PxReal* contactDistance, const PxReal dt, Bp::BoundsArray& boundsArray);
|
|
|
|
// hooks for actions in body core when it's attached to a sim object. Generally
|
|
// we get called after the attribute changed.
|
|
|
|
virtual void postActorFlagChange(PxU32 oldFlags, PxU32 newFlags);
|
|
void postBody2WorldChange();
|
|
void postSetWakeCounter(PxReal t, bool forceWakeUp);
|
|
void postSetKinematicTarget();
|
|
void postSwitchToKinematic();
|
|
void postSwitchToDynamic();
|
|
void postPosePreviewChange(const PxU32 posePreviewFlag); // called when PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW changes
|
|
|
|
PX_FORCE_INLINE const PxTransform& getBody2World() const { return getBodyCore().getCore().body2World; }
|
|
PX_FORCE_INLINE const PxTransform& getBody2Actor() const { return getBodyCore().getCore().getBody2Actor(); }
|
|
PX_FORCE_INLINE const PxsRigidBody& getLowLevelBody() const { return mLLBody; }
|
|
PX_FORCE_INLINE PxsRigidBody& getLowLevelBody() { return mLLBody; }
|
|
void wakeUp(); // note: for user API call purposes only, i.e., use from BodyCore. For simulation internal purposes there is internalWakeUp().
|
|
void putToSleep();
|
|
|
|
void disableCompound();
|
|
|
|
static PxU32 getRigidBodyOffset() { return PxU32(PX_OFFSET_OF_RT(BodySim, mLLBody));}
|
|
|
|
private:
|
|
void activate();
|
|
void deactivate();
|
|
|
|
//---------------------------------------------------------------------------------
|
|
// Constraint projection
|
|
//---------------------------------------------------------------------------------
|
|
public:
|
|
PX_FORCE_INLINE ConstraintGroupNode* getConstraintGroup() { return mConstraintGroup; }
|
|
PX_FORCE_INLINE void setConstraintGroup(ConstraintGroupNode* node) { mConstraintGroup = node; }
|
|
|
|
//// A list of active projection trees in the scene might be better
|
|
//PX_FORCE_INLINE void projectPose() { PX_ASSERT(mConstraintGroup); ConstraintGroupNode::projectPose(*mConstraintGroup); }
|
|
|
|
//---------------------------------------------------------------------------------
|
|
// Kinematics
|
|
//---------------------------------------------------------------------------------
|
|
public:
|
|
PX_FORCE_INLINE bool isKinematic() const { return getBodyCore().getFlags() & PxRigidBodyFlag::eKINEMATIC; }
|
|
PX_FORCE_INLINE bool isArticulationLink() const { return getActorType() == PxActorType::eARTICULATION_LINK; }
|
|
PX_FORCE_INLINE bool hasForcedKinematicNotif() const
|
|
{
|
|
return getBodyCore().getFlags() & (PxRigidBodyFlag::eFORCE_KINE_KINE_NOTIFICATIONS|PxRigidBodyFlag::eFORCE_STATIC_KINE_NOTIFICATIONS);
|
|
}
|
|
void calculateKinematicVelocity(PxReal oneOverDt);
|
|
void updateKinematicPose();
|
|
bool deactivateKinematic();
|
|
private:
|
|
PX_FORCE_INLINE void initKinematicStateBase(BodyCore&, bool asPartOfCreation);
|
|
|
|
//---------------------------------------------------------------------------------
|
|
// Sleeping
|
|
//---------------------------------------------------------------------------------
|
|
public:
|
|
PX_FORCE_INLINE bool isActive() const { return (mActiveListIndex < SC_NOT_IN_ACTIVE_LIST_INDEX); }
|
|
void setActive(bool active, PxU32 infoFlag=0); // see ActivityChangeInfoFlag
|
|
|
|
PX_FORCE_INLINE PxU32 getActiveListIndex() const { return mActiveListIndex; } // if the body is active, the index is smaller than SC_NOT_IN_ACTIVE_LIST_INDEX
|
|
PX_FORCE_INLINE void setActiveListIndex(PxU32 index) { mActiveListIndex = index; }
|
|
|
|
PX_FORCE_INLINE PxU32 getActiveCompoundListIndex() const { return mActiveCompoundListIndex; } // if the body is active and is compound, the index is smaller than SC_NOT_IN_ACTIVE_LIST_INDEX
|
|
PX_FORCE_INLINE void setActiveCompoundListIndex(PxU32 index) { mActiveCompoundListIndex = index; }
|
|
|
|
void internalWakeUp(PxReal wakeCounterValue=ScInternalWakeCounterResetValue);
|
|
void internalWakeUpArticulationLink(PxReal wakeCounterValue); // called by ArticulationSim to wake up this link
|
|
|
|
PxReal updateWakeCounter(PxReal dt, PxReal energyThreshold, const Cm::SpatialVector& motionVelocity);
|
|
|
|
void resetSleepFilter();
|
|
void notifyReadyForSleeping(); // inform the sleep island generation system that the body is ready for sleeping
|
|
void notifyNotReadyForSleeping(); // inform the sleep island generation system that the body is not ready for sleeping
|
|
PX_FORCE_INLINE bool checkSleepReadinessBesidesWakeCounter(); // for API triggered changes to test sleep readiness
|
|
|
|
PX_FORCE_INLINE void registerCountedInteraction() { mLLBody.getCore().numCountedInteractions++; PX_ASSERT(mLLBody.getCore().numCountedInteractions); }
|
|
PX_FORCE_INLINE void unregisterCountedInteraction() { PX_ASSERT(mLLBody.getCore().numCountedInteractions); mLLBody.getCore().numCountedInteractions--;}
|
|
PX_FORCE_INLINE PxU32 getNumCountedInteractions() const { return mLLBody.getCore().numCountedInteractions; }
|
|
|
|
PX_FORCE_INLINE Ps::IntBool isFrozen() const { return Ps::IntBool(mLLBody.mInternalFlags & PxsRigidBody::eFROZEN); }
|
|
private:
|
|
PX_FORCE_INLINE void notifyWakeUp(bool wakeUpInIslandGen = false); // inform the sleep island generation system that the object got woken up
|
|
PX_FORCE_INLINE void notifyPutToSleep(); // inform the sleep island generation system that the object was put to sleep
|
|
PX_FORCE_INLINE void internalWakeUpBase(PxReal wakeCounterValue);
|
|
|
|
//---------------------------------------------------------------------------------
|
|
// External velocity changes
|
|
//---------------------------------------------------------------------------------
|
|
public:
|
|
void updateForces(PxReal dt, PxsRigidBody** updatedBodySims, PxU32* updatedBodyNodeIndices,
|
|
PxU32& index, Cm::SpatialVector* acceleration, const bool useAcceleration, bool simUsesAdaptiveForce);
|
|
private:
|
|
PX_FORCE_INLINE void raiseVelocityModFlag(VelocityModFlags f) { mVelModState |= f; }
|
|
PX_FORCE_INLINE void clearVelocityModFlag(VelocityModFlags f) { mVelModState &= ~f; }
|
|
PX_FORCE_INLINE bool readVelocityModFlag(VelocityModFlags f) { return (mVelModState & f) != 0; }
|
|
PX_FORCE_INLINE void setForcesToDefaults(bool enableGravity);
|
|
|
|
//---------------------------------------------------------------------------------
|
|
// Miscellaneous
|
|
//---------------------------------------------------------------------------------
|
|
public:
|
|
PX_FORCE_INLINE PxU16 getInternalFlag() const { return mInternalFlags; }
|
|
PX_FORCE_INLINE PxU16 readInternalFlag(InternalFlags flag) const { return PxU16(mInternalFlags & flag); }
|
|
PX_FORCE_INLINE void raiseInternalFlag(InternalFlags flag) { mInternalFlags |= flag; }
|
|
PX_FORCE_INLINE void clearInternalFlag(InternalFlags flag) { mInternalFlags &= ~flag; }
|
|
PX_FORCE_INLINE PxU32 getFlagsFast() const { return getBodyCore().getFlags(); }
|
|
|
|
PX_FORCE_INLINE void incrementBodyConstraintCounter() { mLLBody.mCore->numBodyInteractions++; }
|
|
PX_FORCE_INLINE void decrementBodyConstraintCounter() { PX_ASSERT(mLLBody.mCore->numBodyInteractions>0); mLLBody.mCore->numBodyInteractions--; }
|
|
|
|
PX_FORCE_INLINE BodyCore& getBodyCore() const { return static_cast<BodyCore&>(getRigidCore()); }
|
|
|
|
PX_INLINE ArticulationSim* getArticulation() const { return mArticulation; }
|
|
void setArticulation(ArticulationSim* a, PxReal wakeCounter, bool asleep, PxU32 bodyIndex);
|
|
|
|
PX_FORCE_INLINE IG::NodeIndex getNodeIndex() const { return mNodeIndex; }
|
|
|
|
PX_FORCE_INLINE void onConstraintAttach() { raiseInternalFlag(BF_HAS_CONSTRAINTS); registerCountedInteraction(); }
|
|
void onConstraintDetach();
|
|
|
|
PX_FORCE_INLINE void onOriginShift(const PxVec3& shift) { mLLBody.mLastTransform.p -= shift; }
|
|
|
|
PX_FORCE_INLINE bool notInScene() const { return mActiveListIndex == SC_NOT_IN_SCENE_INDEX; }
|
|
|
|
PX_FORCE_INLINE bool usingSqKinematicTarget() const
|
|
{
|
|
PxU32 ktFlags(PxRigidBodyFlag::eUSE_KINEMATIC_TARGET_FOR_SCENE_QUERIES | PxRigidBodyFlag::eKINEMATIC);
|
|
return (getFlagsFast()&ktFlags) == ktFlags;
|
|
}
|
|
|
|
PX_FORCE_INLINE PxU32 getNbShapes() const { return mElementCount; }
|
|
|
|
void createSqBounds();
|
|
void destroySqBounds();
|
|
void freezeTransforms(Cm::BitMapPinned* shapeChangedMap);
|
|
void invalidateSqBounds();
|
|
private:
|
|
|
|
//---------------------------------------------------------------------------------
|
|
// Base body
|
|
//---------------------------------------------------------------------------------
|
|
PxsRigidBody mLLBody;
|
|
|
|
//---------------------------------------------------------------------------------
|
|
// Island manager
|
|
//---------------------------------------------------------------------------------
|
|
IG::NodeIndex mNodeIndex;
|
|
|
|
//---------------------------------------------------------------------------------
|
|
// External velocity changes
|
|
//---------------------------------------------------------------------------------
|
|
// VelocityMod data allocated on the fly when the user applies velocity changes
|
|
// which need to be accumulated.
|
|
// VelMod dirty flags stored in BodySim so we can save ourselves the expense of looking at
|
|
// the separate velmod data if no forces have been set.
|
|
PxU16 mInternalFlags;
|
|
PxU8 mVelModState;
|
|
|
|
//---------------------------------------------------------------------------------
|
|
// Sleeping
|
|
//---------------------------------------------------------------------------------
|
|
PxU32 mActiveListIndex; // Used by Scene to track active bodies
|
|
PxU32 mActiveCompoundListIndex; // Used by Scene to track active compound bodies
|
|
|
|
//---------------------------------------------------------------------------------
|
|
// Articulation
|
|
//---------------------------------------------------------------------------------
|
|
ArticulationSim* mArticulation; // NULL if not in an articulation
|
|
|
|
//---------------------------------------------------------------------------------
|
|
// Joints & joint groups
|
|
//---------------------------------------------------------------------------------
|
|
|
|
// This is a tree data structure that gives us the projection order of joints in which this body is the tree root.
|
|
// note: the link of the root body is not necces. the root link due to the re-rooting of the articulation!
|
|
ConstraintGroupNode* mConstraintGroup;
|
|
};
|
|
|
|
#if PX_VC
|
|
#pragma warning(pop)
|
|
#endif
|
|
|
|
} // namespace Sc
|
|
|
|
PX_FORCE_INLINE void Sc::BodySim::setForcesToDefaults(bool enableGravity)
|
|
{
|
|
if (!(mLLBody.mCore->mFlags & PxRigidBodyFlag::eRETAIN_ACCELERATIONS))
|
|
{
|
|
SimStateData* simStateData = getBodyCore().getSimStateData(false);
|
|
if(simStateData)
|
|
{
|
|
VelocityMod* velmod = simStateData->getVelocityModData();
|
|
velmod->clear();
|
|
}
|
|
|
|
if (enableGravity)
|
|
mVelModState = VMF_GRAVITY_DIRTY; // We want to keep the gravity flag to make sure the acceleration gets changed to gravity-only
|
|
// in the next step (unless the application adds new forces of course)
|
|
else
|
|
mVelModState = 0;
|
|
}
|
|
else
|
|
{
|
|
SimStateData* simStateData = getBodyCore().getSimStateData(false);
|
|
if (simStateData)
|
|
{
|
|
VelocityMod* velmod = simStateData->getVelocityModData();
|
|
velmod->clearPerStep();
|
|
}
|
|
|
|
mVelModState &= (~(VMF_VEL_DIRTY));
|
|
}
|
|
}
|
|
|
|
PX_FORCE_INLINE bool Sc::BodySim::checkSleepReadinessBesidesWakeCounter()
|
|
{
|
|
const BodyCore& bodyCore = getBodyCore();
|
|
const SimStateData* simStateData = bodyCore.getSimStateData(false);
|
|
const VelocityMod* velmod = simStateData ? simStateData->getVelocityModData() : NULL;
|
|
|
|
bool readyForSleep = bodyCore.getLinearVelocity().isZero() && bodyCore.getAngularVelocity().isZero();
|
|
if (readVelocityModFlag(VMF_ACC_DIRTY))
|
|
{
|
|
readyForSleep = readyForSleep && (!velmod || velmod->getLinearVelModPerSec().isZero());
|
|
readyForSleep = readyForSleep && (!velmod || velmod->getAngularVelModPerSec().isZero());
|
|
}
|
|
if (readVelocityModFlag(VMF_VEL_DIRTY))
|
|
{
|
|
readyForSleep = readyForSleep && (!velmod || velmod->getLinearVelModPerStep().isZero());
|
|
readyForSleep = readyForSleep && (!velmod || velmod->getAngularVelModPerStep().isZero());
|
|
}
|
|
|
|
return readyForSleep;
|
|
}
|
|
|
|
|
|
}
|
|
|
|
#endif
|