237 lines
8.9 KiB
C++
237 lines
8.9 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-2025 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 "DySleep.h"
|
|
|
|
using namespace physx;
|
|
|
|
// PT: TODO: refactor this, parts of the two codepaths are very similar
|
|
|
|
static PX_FORCE_INLINE PxReal updateWakeCounter(PxsRigidBody* originalBody, PxReal dt, bool enableStabilization, const Cm::SpatialVector& motionVelocity, PxIntBool hasStaticTouch)
|
|
{
|
|
PxsBodyCore& bodyCore = originalBody->getCore();
|
|
|
|
// update the body's sleep state and
|
|
const PxReal wakeCounterResetTime = 20.0f*0.02f;
|
|
|
|
PxReal wc = bodyCore.wakeCounter;
|
|
|
|
if (enableStabilization)
|
|
{
|
|
const PxTransform& body2World = bodyCore.body2World;
|
|
|
|
// calculate normalized energy: kinetic energy divided by mass
|
|
|
|
const PxVec3& t = bodyCore.inverseInertia;
|
|
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);
|
|
|
|
const PxVec3& sleepLinVelAcc = motionVelocity.linear;
|
|
const PxVec3 sleepAngVelAcc = body2World.q.rotateInv(motionVelocity.angular);
|
|
|
|
// scale threshold by cluster factor (more contacts => higher sleep threshold)
|
|
//const PxReal clusterFactor = PxReal(1u + getNumUniqueInteractions());
|
|
|
|
PxReal invMass = bodyCore.inverseMass;
|
|
if (invMass == 0.0f)
|
|
invMass = 1.0f;
|
|
|
|
const PxReal angular = sleepAngVelAcc.multiply(sleepAngVelAcc).dot(inertia) * invMass;
|
|
const PxReal linear = sleepLinVelAcc.magnitudeSquared();
|
|
const PxReal frameNormalizedEnergy = 0.5f * (angular + linear);
|
|
|
|
const PxReal cf = hasStaticTouch ? PxReal(PxMin(10u, bodyCore.numCountedInteractions)) : 0.0f;
|
|
const PxReal freezeThresh = cf*bodyCore.freezeThreshold;
|
|
|
|
originalBody->mFreezeCount = PxMax(originalBody->mFreezeCount - dt, 0.0f);
|
|
bool settled = true;
|
|
|
|
PxReal accelScale = PxMin(1.0f, originalBody->mAccelScale + dt);
|
|
|
|
if (frameNormalizedEnergy >= freezeThresh)
|
|
{
|
|
settled = false;
|
|
originalBody->mFreezeCount = PXD_FREEZE_INTERVAL;
|
|
}
|
|
|
|
if (!hasStaticTouch)
|
|
{
|
|
accelScale = 1.0f;
|
|
settled = false;
|
|
}
|
|
|
|
bool freeze = false;
|
|
if (settled)
|
|
{
|
|
//Dampen bodies that are just about to go to sleep
|
|
if (cf > 1.0f)
|
|
{
|
|
const PxReal sleepDamping = PXD_SLEEP_DAMPING;
|
|
const PxReal sleepDampingTimesDT = sleepDamping*dt;
|
|
const PxReal d = 1.0f - sleepDampingTimesDT;
|
|
bodyCore.linearVelocity = bodyCore.linearVelocity * d;
|
|
bodyCore.angularVelocity = bodyCore.angularVelocity * d;
|
|
accelScale = accelScale * 0.75f + 0.25f*PXD_FREEZE_SCALE;
|
|
}
|
|
freeze = originalBody->mFreezeCount == 0.0f && frameNormalizedEnergy < (bodyCore.freezeThreshold * PXD_FREEZE_TOLERANCE);
|
|
}
|
|
|
|
originalBody->mAccelScale = accelScale;
|
|
|
|
const PxU32 wasFrozen = originalBody->mInternalFlags & PxsRigidBody::eFROZEN;
|
|
PxU16 flags;
|
|
if(freeze)
|
|
{
|
|
//current flag isn't frozen but freeze flag raise so we need to raise the frozen flag in this frame
|
|
flags = PxU16(PxsRigidBody::eFROZEN);
|
|
if(!wasFrozen)
|
|
flags |= PxsRigidBody::eFREEZE_THIS_FRAME;
|
|
bodyCore.body2World = originalBody->getLastCCDTransform();
|
|
}
|
|
else
|
|
{
|
|
flags = 0;
|
|
if(wasFrozen)
|
|
flags |= PxsRigidBody::eUNFREEZE_THIS_FRAME;
|
|
}
|
|
originalBody->mInternalFlags = flags;
|
|
|
|
/*KS: New algorithm for sleeping when using stabilization:
|
|
* Energy *this frame* must be higher than sleep threshold and accumulated energy over previous frames
|
|
* must be higher than clusterFactor*energyThreshold.
|
|
*/
|
|
if (wc < wakeCounterResetTime * 0.5f || wc < dt)
|
|
{
|
|
//Accumulate energy
|
|
originalBody->mSleepLinVelAcc += sleepLinVelAcc;
|
|
originalBody->mSleepAngVelAcc += sleepAngVelAcc;
|
|
|
|
//If energy this frame is high
|
|
if (frameNormalizedEnergy >= bodyCore.sleepThreshold)
|
|
{
|
|
//Compute energy over sleep preparation time
|
|
const PxReal sleepAngular = originalBody->mSleepAngVelAcc.multiply(originalBody->mSleepAngVelAcc).dot(inertia) * invMass;
|
|
const PxReal sleepLinear = originalBody->mSleepLinVelAcc.magnitudeSquared();
|
|
const PxReal normalizedEnergy = 0.5f * (sleepAngular + sleepLinear);
|
|
const PxReal sleepClusterFactor = PxReal(1u + bodyCore.numCountedInteractions);
|
|
// scale threshold by cluster factor (more contacts => higher sleep threshold)
|
|
const PxReal threshold = sleepClusterFactor*bodyCore.sleepThreshold;
|
|
|
|
//If energy over sleep preparation time is high
|
|
if (normalizedEnergy >= threshold)
|
|
{
|
|
//Wake up
|
|
//PX_ASSERT(isActive());
|
|
originalBody->resetSleepFilter();
|
|
|
|
const float factor = bodyCore.sleepThreshold == 0.0f ? 2.0f : PxMin(normalizedEnergy / threshold, 2.0f);
|
|
PxReal oldWc = wc;
|
|
wc = factor * 0.5f * wakeCounterResetTime + dt * (sleepClusterFactor - 1.0f);
|
|
bodyCore.solverWakeCounter = 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(bodyCore.nodeIndex);
|
|
|
|
if (oldWc == 0.0f)
|
|
originalBody->mInternalFlags |= PxsRigidBody::eACTIVATE_THIS_FRAME;
|
|
|
|
return wc;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (wc < wakeCounterResetTime * 0.5f || wc < dt)
|
|
{
|
|
const PxTransform& body2World = bodyCore.body2World;
|
|
|
|
// calculate normalized energy: kinetic energy divided by mass
|
|
const PxVec3& t = bodyCore.inverseInertia;
|
|
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);
|
|
|
|
const PxVec3& sleepLinVelAcc = motionVelocity.linear;
|
|
const PxVec3 sleepAngVelAcc = body2World.q.rotateInv(motionVelocity.angular);
|
|
|
|
originalBody->mSleepLinVelAcc += sleepLinVelAcc;
|
|
originalBody->mSleepAngVelAcc += sleepAngVelAcc;
|
|
|
|
PxReal invMass = bodyCore.inverseMass;
|
|
if (invMass == 0.0f)
|
|
invMass = 1.0f;
|
|
|
|
const PxReal angular = originalBody->mSleepAngVelAcc.multiply(originalBody->mSleepAngVelAcc).dot(inertia) * invMass;
|
|
const PxReal linear = originalBody->mSleepLinVelAcc.magnitudeSquared();
|
|
const PxReal normalizedEnergy = 0.5f * (angular + linear);
|
|
|
|
// scale threshold by cluster factor (more contacts => higher sleep threshold)
|
|
const PxReal clusterFactor = PxReal(1 + bodyCore.numCountedInteractions);
|
|
const PxReal threshold = clusterFactor*bodyCore.sleepThreshold;
|
|
|
|
if (normalizedEnergy >= threshold)
|
|
{
|
|
//PX_ASSERT(isActive());
|
|
originalBody->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);
|
|
bodyCore.solverWakeCounter = wc;
|
|
PxU16 flags = 0;
|
|
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
|
|
{
|
|
flags |= PxsRigidBody::eACTIVATE_THIS_FRAME;
|
|
//notifyNotReadyForSleeping(bodyCore.nodeIndex);
|
|
}
|
|
|
|
originalBody->mInternalFlags = flags;
|
|
|
|
return wc;
|
|
}
|
|
}
|
|
}
|
|
|
|
wc = PxMax(wc - dt, 0.0f);
|
|
bodyCore.solverWakeCounter = wc;
|
|
return wc;
|
|
}
|
|
|
|
void Dy::sleepCheck(PxsRigidBody* originalBody, PxReal dt, bool enableStabilization, const Cm::SpatialVector& motionVelocity, PxIntBool hasStaticTouch)
|
|
{
|
|
const PxReal wc = updateWakeCounter(originalBody, dt, enableStabilization, motionVelocity, hasStaticTouch);
|
|
if(wc == 0.0f)
|
|
{
|
|
//PxsBodyCore& bodyCore = originalBody->getCore();
|
|
originalBody->mInternalFlags |= PxsRigidBody::eDEACTIVATE_THIS_FRAME;
|
|
// notifyReadyForSleeping(bodyCore.nodeIndex);
|
|
originalBody->resetSleepFilter();
|
|
}
|
|
}
|