feat(physics): wire physx sdk into build
This commit is contained in:
236
engine/third_party/physx/source/lowleveldynamics/src/DySleep.cpp
vendored
Normal file
236
engine/third_party/physx/source/lowleveldynamics/src/DySleep.cpp
vendored
Normal file
@@ -0,0 +1,236 @@
|
||||
// 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();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user