feat(physics): wire physx sdk into build
This commit is contained in:
435
engine/third_party/physx/source/gpusolver/src/CUDA/integration.cuh
vendored
Normal file
435
engine/third_party/physx/source/gpusolver/src/CUDA/integration.cuh
vendored
Normal file
@@ -0,0 +1,435 @@
|
||||
// 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 "foundation/PxSimpleTypes.h"
|
||||
#include "PxgBodySim.h"
|
||||
#include "PxgSolverBody.h"
|
||||
#include "PxvDynamics.h"
|
||||
#include "PxsRigidBody.h"
|
||||
#include "PxgSolverKernelIndices.h"
|
||||
#include "DySleepingConfigulation.h"
|
||||
#include "stdio.h"
|
||||
|
||||
using namespace physx;
|
||||
|
||||
static __device__ void updateWakeCounter(bool& freeze, float4& solverBodyLinVel, float4& solverBodyAngVel, const PxAlignedTransform& body2World,
|
||||
PxgBodySim& bodySim, PxgSolverBodySleepData& sleepData,
|
||||
const float4& inverseInertia, const PxVec3& linearMotionVel, const PxVec3& angularMotionVel, const float invertedMass, const float dt,
|
||||
const float invDt, const bool enableStabilization, const bool hasStaticTouch, PxU32 numCountedInteractions,
|
||||
PxU32 nodeIndex)
|
||||
{
|
||||
// update the body's sleep state and
|
||||
PxReal wakeCounterResetTime = 20.0f*0.02f;
|
||||
|
||||
float4 freezeThresholdX_wakeCounterY_sleepThresholdZ_bodySimIndex = bodySim.freezeThresholdX_wakeCounterY_sleepThresholdZ_bodySimIndex;
|
||||
float4 sleepLinVelAccXYZ_freezeCountW = bodySim.sleepLinVelAccXYZ_freezeCountW;
|
||||
float4 sleepAngVelAccXYZ_accelScaleW = bodySim.sleepAngVelAccXYZ_accelScaleW;
|
||||
|
||||
PxReal wc = freezeThresholdX_wakeCounterY_sleepThresholdZ_bodySimIndex.y; //wakeCounter;
|
||||
|
||||
PxU32 flags = bodySim.internalFlags & (PxsRigidBody::eDISABLE_GRAVITY_GPU | PxsRigidBody::eFROZEN | PxsRigidBody::eENABLE_GYROSCOPIC | PxsRigidBody::eRETAIN_ACCELERATION);
|
||||
PxReal freezeCount = sleepLinVelAccXYZ_freezeCountW.w;
|
||||
PxReal accelScale = sleepAngVelAccXYZ_accelScaleW.w;
|
||||
|
||||
bool alreadyUpdateWC = false;
|
||||
PxVec3 sleepLinVelAcc(0.f), sleepAngVelAcc(0.f);
|
||||
|
||||
{
|
||||
if (enableStabilization)
|
||||
{
|
||||
const PxU32 maxCountedInteractions = 10u; //KS - arbitrary limit to make sure that
|
||||
//bool freeze = false;
|
||||
//const PxAlignedTransform& body2World = solverBodyData.body2World;
|
||||
|
||||
// calculate normalized energy: kinetic energy divided by mass
|
||||
const PxVec3 inertia(inverseInertia.x > 0.f ? 1.0f / inverseInertia.x : 1.f, inverseInertia.y > 0.f ? 1.0f / inverseInertia.y : 1.f, inverseInertia.z > 0.f ? 1.0f / inverseInertia.z : 1.f);
|
||||
sleepLinVelAcc = linearMotionVel;
|
||||
sleepAngVelAcc = angularMotionVel;
|
||||
|
||||
// scale threshold by cluster factor (more contacts => higher sleep threshold)
|
||||
const PxU32 clusterFactor = PxMin(numCountedInteractions, maxCountedInteractions);
|
||||
|
||||
PxReal invMass = invertedMass;// intialVel_invMass.w;
|
||||
if (invMass == 0.f)
|
||||
invMass = 1.f;
|
||||
|
||||
const PxReal angular = sleepAngVelAcc.multiply(sleepAngVelAcc).dot(inertia) * invMass;
|
||||
const PxReal linear = sleepLinVelAcc.magnitudeSquared();
|
||||
PxReal frameNormalizedEnergy = 0.5f * (angular + linear);
|
||||
|
||||
const PxReal cf = hasStaticTouch ? clusterFactor : 0.f;
|
||||
const PxReal freezeThresh = cf * freezeThresholdX_wakeCounterY_sleepThresholdZ_bodySimIndex.x;// solverBodySleepData.freezeThreshold;
|
||||
freezeCount = PxMax(freezeCount - dt, 0.0f);
|
||||
|
||||
bool settled = true;
|
||||
|
||||
accelScale = PxMin(1.f, accelScale + dt);
|
||||
|
||||
if (frameNormalizedEnergy >= freezeThresh)
|
||||
{
|
||||
settled = false;
|
||||
|
||||
freezeCount = PXD_FREEZE_INTERVAL;
|
||||
}
|
||||
|
||||
if (!hasStaticTouch)
|
||||
{
|
||||
accelScale = 1.f;
|
||||
settled = false;
|
||||
}
|
||||
|
||||
if (settled)
|
||||
{
|
||||
//Dampen bodies that are just about to go to sleep
|
||||
if (cf > 1)
|
||||
{
|
||||
const PxReal d = 1.0f - (PXD_SLEEP_DAMPING * dt);
|
||||
|
||||
solverBodyLinVel = solverBodyLinVel * d;
|
||||
solverBodyAngVel = solverBodyAngVel * d;
|
||||
accelScale = accelScale * 0.75f + 0.25f*PXD_FREEZE_SCALE;
|
||||
}
|
||||
|
||||
freeze = freezeCount == 0.f && frameNormalizedEnergy < (freezeThresholdX_wakeCounterY_sleepThresholdZ_bodySimIndex.x * PXD_FREEZE_TOLERANCE);
|
||||
}
|
||||
|
||||
if (freeze)
|
||||
{
|
||||
//current flag isn't frozen but freeze flag raise so we need to raise the frozen flag in this frame
|
||||
|
||||
bool wasNotFrozen = (flags & PxsRigidBody::eFROZEN) == 0;
|
||||
flags |= PxsRigidBody::eFROZEN;
|
||||
if (wasNotFrozen)
|
||||
{
|
||||
flags |= PxsRigidBody::eFREEZE_THIS_FRAME;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
bool wasFrozen = (flags & PxsRigidBody::eFROZEN) != 0;
|
||||
flags &= (PxsRigidBody::eDISABLE_GRAVITY_GPU | PxsRigidBody::eENABLE_GYROSCOPIC | PxsRigidBody::eRETAIN_ACCELERATION);
|
||||
if (wasFrozen)
|
||||
{
|
||||
flags |= PxsRigidBody::eUNFREEZE_THIS_FRAME;
|
||||
}
|
||||
}
|
||||
|
||||
/*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
|
||||
sleepLinVelAcc.x += sleepLinVelAccXYZ_freezeCountW.x;
|
||||
sleepLinVelAcc.y += sleepLinVelAccXYZ_freezeCountW.y;
|
||||
sleepLinVelAcc.z += sleepLinVelAccXYZ_freezeCountW.z;
|
||||
|
||||
sleepAngVelAcc.x += sleepAngVelAccXYZ_accelScaleW.x;
|
||||
sleepAngVelAcc.y += sleepAngVelAccXYZ_accelScaleW.y;
|
||||
sleepAngVelAcc.z += sleepAngVelAccXYZ_accelScaleW.z;
|
||||
|
||||
//If energy this frame is high
|
||||
const PxReal sleepThreshold = freezeThresholdX_wakeCounterY_sleepThresholdZ_bodySimIndex.z;
|
||||
|
||||
if (frameNormalizedEnergy >= sleepThreshold)
|
||||
{
|
||||
//Compute energy over sleep preparation time
|
||||
|
||||
const PxReal sleepAngular = sleepAngVelAcc.multiply(sleepAngVelAcc).dot(inertia) * invMass;
|
||||
const PxReal sleepLinear = sleepLinVelAcc.magnitudeSquared();
|
||||
|
||||
PxReal normalizedEnergy = 0.5f * (sleepAngular + sleepLinear);
|
||||
PxReal sleepClusterFactor = clusterFactor + 1.f;
|
||||
|
||||
// scale threshold by cluster factor (more contacts => higher sleep threshold)
|
||||
const PxReal threshold = sleepClusterFactor * sleepThreshold;
|
||||
|
||||
//If energy over sleep preparation time is high
|
||||
if (normalizedEnergy >= threshold)
|
||||
{
|
||||
//Wake up
|
||||
//assert(isActive());
|
||||
|
||||
sleepAngVelAcc = PxVec3(0);
|
||||
sleepLinVelAcc = PxVec3(0);
|
||||
|
||||
const float factor = sleepThreshold == 0.f ? 2.0f : PxMin(normalizedEnergy / threshold, 2.0f);
|
||||
PxReal oldWc = wc;
|
||||
wc = factor * 0.5f * wakeCounterResetTime + dt * (sleepClusterFactor - 1.0f);
|
||||
|
||||
//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)
|
||||
flags |= PxsRigidBody::eACTIVATE_THIS_FRAME;
|
||||
|
||||
alreadyUpdateWC = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (wc < wakeCounterResetTime * 0.5f || wc < dt)
|
||||
{
|
||||
//const PxAlignedTransform& body2World = solverBodyData.body2World;
|
||||
|
||||
// calculate normalized energy: kinetic energy divided by mass
|
||||
const PxVec3 inertia(inverseInertia.x > 0.f ? 1.0f / inverseInertia.x : 1.f, inverseInertia.y > 0.f ? 1.0f / inverseInertia.y : 1.f, inverseInertia.z > 0.f ? 1.0f / inverseInertia.z : 1.f);
|
||||
|
||||
sleepLinVelAcc = linearMotionVel;// originalBody->mAcceleration.linear;
|
||||
sleepAngVelAcc = body2World.q.rotateInv(angularMotionVel);// originalBody->mAcceleration.angular;
|
||||
|
||||
sleepLinVelAcc.x += sleepLinVelAccXYZ_freezeCountW.x;
|
||||
sleepLinVelAcc.y += sleepLinVelAccXYZ_freezeCountW.y;
|
||||
sleepLinVelAcc.z += sleepLinVelAccXYZ_freezeCountW.z;
|
||||
|
||||
sleepAngVelAcc.x += sleepAngVelAccXYZ_accelScaleW.x;
|
||||
sleepAngVelAcc.y += sleepAngVelAccXYZ_accelScaleW.y;
|
||||
sleepAngVelAcc.z += sleepAngVelAccXYZ_accelScaleW.z;
|
||||
|
||||
|
||||
PxReal invMass = invertedMass;
|
||||
if (invMass == 0.f)
|
||||
invMass = 1.f;
|
||||
|
||||
const PxReal angular = sleepAngVelAcc.multiply(sleepAngVelAcc).dot(inertia) * invMass;
|
||||
const PxReal linear = sleepLinVelAcc.magnitudeSquared();
|
||||
PxReal normalizedEnergy = 0.5f * (angular + linear);
|
||||
|
||||
// scale threshold by cluster factor (more contacts => higher sleep threshold)
|
||||
const PxReal clusterFactor = PxReal(1 + numCountedInteractions);
|
||||
|
||||
const PxReal sleepThreshold = freezeThresholdX_wakeCounterY_sleepThresholdZ_bodySimIndex.z;
|
||||
|
||||
const PxReal threshold = clusterFactor * sleepThreshold;
|
||||
|
||||
if (normalizedEnergy >= threshold)
|
||||
{
|
||||
//assert(isActive());
|
||||
sleepLinVelAcc = PxVec3(0);
|
||||
sleepAngVelAcc = PxVec3(0);
|
||||
const float factor = threshold == 0.f ? 2.0f : PxMin(normalizedEnergy / threshold, 2.0f);
|
||||
PxReal oldWc = wc;
|
||||
wc = factor * 0.5f * wakeCounterResetTime + dt * (clusterFactor - 1.0f);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
alreadyUpdateWC = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(!alreadyUpdateWC)
|
||||
wc = PxMax(wc - dt, 0.0f);
|
||||
|
||||
bool wakeCounterZero = (wc == 0.0f);
|
||||
|
||||
if (wakeCounterZero)
|
||||
{
|
||||
flags |= PxsRigidBody::eDEACTIVATE_THIS_FRAME;
|
||||
sleepLinVelAcc = PxVec3(0);
|
||||
sleepAngVelAcc = PxVec3(0);
|
||||
}
|
||||
|
||||
bodySim.internalFlags = flags;
|
||||
bodySim.freezeThresholdX_wakeCounterY_sleepThresholdZ_bodySimIndex.y = wc;
|
||||
bodySim.sleepLinVelAccXYZ_freezeCountW = make_float4(sleepLinVelAcc.x, sleepLinVelAcc.y, sleepLinVelAcc.z, freezeCount);
|
||||
bodySim.sleepAngVelAccXYZ_accelScaleW = make_float4(sleepAngVelAcc.x, sleepAngVelAcc.y, sleepAngVelAcc.z, accelScale);
|
||||
|
||||
if (!(flags & PxsRigidBody::eRETAIN_ACCELERATION))
|
||||
{
|
||||
bodySim.externalLinearAcceleration = make_float4(0.f, 0.f, 0.f, 0.f);
|
||||
bodySim.externalAngularAcceleration = make_float4(0.f, 0.f, 0.f, 0.f);
|
||||
}
|
||||
|
||||
sleepData.internalFlags = flags;
|
||||
sleepData.wakeCounter = wc;
|
||||
}
|
||||
|
||||
static __device__ bool sleepCheck(float4& solverBodyLinVel, float4& solverBodyAngVel, const PxAlignedTransform& body2World, PxgBodySim& bodySim, PxgSolverBodySleepData& sleepData,
|
||||
const float4& inverseInertia, const PxVec3& linearMotionVel, const PxVec3& angularMotionVel, const float invertedMass, const float dt, const float invDt, const bool enableStabilization,
|
||||
const bool hasStaticTouch, const PxU32 numCountedInteractions, PxU32 nodeIndex)
|
||||
{
|
||||
bool freeze = false;
|
||||
updateWakeCounter(freeze, solverBodyLinVel, solverBodyAngVel, body2World, bodySim, sleepData, inverseInertia, linearMotionVel, angularMotionVel, invertedMass, dt,
|
||||
invDt, enableStabilization, hasStaticTouch, numCountedInteractions, nodeIndex);
|
||||
|
||||
return freeze;
|
||||
}
|
||||
|
||||
static __device__ void integrateCore(const float4 motionLinVelXYZW, const float4 motionAngVelXYZW, const float4& inverseInertia, float4& solverBodyLinVel,
|
||||
float4& solverBodyAngVel, PxAlignedTransform& body2World, const PxgSolverBodyData& solverBodyData, PxgBodySim& bodySim, PxgSolverBodySleepData& sleepData,
|
||||
const PxMat33& sqrtInvInertia, const float dt, const float invDt, const bool enableStabilization, const bool hasStaticTouch, const PxU32 numCountedInteractions,
|
||||
const PxU32 nodeIndex)
|
||||
{
|
||||
// Integrate linear part
|
||||
const float4 initialLinVelXYZ_invMassW = solverBodyData.initialLinVelXYZ_invMassW;
|
||||
const float4 initialAngVelXYZ_penBiasClamp = solverBodyData.initialAngVelXYZ_penBiasClamp;
|
||||
//ML: solverBodyData.initialLinVelocity store the PxsBodyCore (original )linearVelocity and angularVelocity
|
||||
const PxVec3 initialLinVel(initialLinVelXYZ_invMassW.x, initialLinVelXYZ_invMassW.y, initialLinVelXYZ_invMassW.z);
|
||||
const PxVec3 initialAngVel(initialAngVelXYZ_penBiasClamp.x, initialAngVelXYZ_penBiasClamp.y, initialAngVelXYZ_penBiasClamp.z);
|
||||
|
||||
PxU32 lockFlags = bodySim.lockFlags;
|
||||
|
||||
//update body lin and ang velocity
|
||||
PxVec3 bodyLinearVelocity(solverBodyLinVel.x, solverBodyLinVel.y, solverBodyLinVel.z);
|
||||
PxVec3 bodyAngVelocity(solverBodyAngVel.x, solverBodyAngVel.y, solverBodyAngVel.z);
|
||||
|
||||
#ifndef IS_TGS_SOLVER
|
||||
bodyLinearVelocity = initialLinVel + bodyLinearVelocity;
|
||||
bodyAngVelocity = initialAngVel + sqrtInvInertia * bodyAngVelocity;
|
||||
#else
|
||||
bodyAngVelocity = sqrtInvInertia * bodyAngVelocity;
|
||||
#endif
|
||||
|
||||
solverBodyLinVel = make_float4(lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_X ? 0.f : bodyLinearVelocity.x,
|
||||
lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_Y ? 0.f : bodyLinearVelocity.y,
|
||||
lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_Z ? 0.f : bodyLinearVelocity.z, 0.f);
|
||||
solverBodyAngVel = make_float4(lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_X ? 0.f : bodyAngVelocity.x,
|
||||
lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_Y ? 0.f : bodyAngVelocity.y,
|
||||
lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_Z ? 0.f : bodyAngVelocity.z, 0.f);
|
||||
|
||||
//we need to perform sleep check here to decide whether we want to update body2World transform for the body
|
||||
const PxVec3 motionLinVel(lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_X ? 0.f : motionLinVelXYZW.x,
|
||||
lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_Y ? 0.f : motionLinVelXYZW.y,
|
||||
lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_Z ? 0.f : motionLinVelXYZW.z);
|
||||
const PxVec3 motionAngVel(lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_X ? 0.f : motionAngVelXYZW.x,
|
||||
lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_Y ? 0.f : motionAngVelXYZW.y,
|
||||
lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_Z ? 0.f : motionAngVelXYZW.z);
|
||||
|
||||
#ifndef IS_TGS_SOLVER
|
||||
PxVec3 linearMotionVel = initialLinVel + motionLinVel;
|
||||
PxVec3 angularMotionVel = initialAngVel + sqrtInvInertia * motionAngVel;
|
||||
|
||||
//printf("%i: DeltaLinVel = (%f, %f, %f)\n", nodeIndex, motionLinVel.x, motionLinVel.y, motionLinVel.z);
|
||||
#else
|
||||
PxVec3 linearMotionVel = motionLinVel;
|
||||
PxVec3 angularMotionVel = sqrtInvInertia * motionAngVel;
|
||||
#endif
|
||||
|
||||
// Integrate the rotation using closed form quaternion integrator
|
||||
PxReal w = angularMotionVel.magnitudeSquared();
|
||||
w = PxSqrt(w);
|
||||
const PxReal maxW = 1e+7f; //Should be about sqrt(PX_MAX_REAL/2) or smaller
|
||||
if (w > maxW)
|
||||
{
|
||||
angularMotionVel = angularMotionVel.getNormalized() * maxW;
|
||||
w = maxW;
|
||||
}
|
||||
|
||||
const bool freeze = sleepCheck(solverBodyLinVel, solverBodyAngVel, body2World, bodySim, sleepData, inverseInertia, linearMotionVel, angularMotionVel, initialLinVelXYZ_invMassW.w,
|
||||
dt, invDt, enableStabilization, hasStaticTouch, numCountedInteractions, nodeIndex);
|
||||
|
||||
if (!freeze)
|
||||
{
|
||||
PxVec3 delta = linearMotionVel * dt;
|
||||
body2World.p.x += delta.x; body2World.p.y += delta.y; body2World.p.z += delta.z;
|
||||
|
||||
if (w != 0.0f)
|
||||
{
|
||||
const PxReal v = dt * w * 0.5f;
|
||||
PxReal s, q;
|
||||
//s = sin(v);
|
||||
//q = cos(v);
|
||||
__sincosf(v, &s, &q);
|
||||
s /= w;
|
||||
|
||||
const PxVec3 pqr = angularMotionVel * s;
|
||||
const PxAlignedQuat quatVel(pqr.x, pqr.y, pqr.z, 0);
|
||||
PxAlignedQuat result = quatVel * body2World.q;
|
||||
|
||||
result += body2World.q * q;
|
||||
|
||||
//ML: solverBodyData store the current transform for PxsBodyCore
|
||||
body2World.q = result.getNormalized();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static __device__ void integrateCoreTGS(const float4 motionLinVelXYZW, const float4 motionAngVelXYZW, const float4& inverseInertia, float4& solverBodyLinVel,
|
||||
float4& solverBodyAngVel, PxAlignedTransform& body2World, const PxTransform& deltaBody2World, const PxgSolverBodyData& solverBodyData, PxgBodySim& bodySim, PxgSolverBodySleepData& sleepData,
|
||||
const PxMat33& sqrtInvInertia, const float dt, const float invDt, const bool enableStabilization, const bool hasStaticTouch, const PxU32 numCountedInteractions,
|
||||
const PxU32 nodeIndex)
|
||||
{
|
||||
const PxU32 lockFlags = bodySim.lockFlags;
|
||||
|
||||
//KS - TODO - optimize this away
|
||||
const float4 initialLinVelXYZ_invMassW = solverBodyData.initialLinVelXYZ_invMassW;
|
||||
|
||||
//update body lin and ang velocity
|
||||
PxVec3 bodyLinearVelocity(solverBodyLinVel.x, solverBodyLinVel.y, solverBodyLinVel.z);
|
||||
PxVec3 bodyAngVelocity(solverBodyAngVel.x, solverBodyAngVel.y, solverBodyAngVel.z);
|
||||
bodyAngVelocity = sqrtInvInertia * bodyAngVelocity;
|
||||
|
||||
solverBodyLinVel = make_float4(lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_X ? 0.f : bodyLinearVelocity.x,
|
||||
lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_Y ? 0.f : bodyLinearVelocity.y,
|
||||
lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_Z ? 0.f : bodyLinearVelocity.z, 0.f);
|
||||
solverBodyAngVel = make_float4(lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_X ? 0.f : bodyAngVelocity.x,
|
||||
lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_Y ? 0.f : bodyAngVelocity.y,
|
||||
lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_Z ? 0.f : bodyAngVelocity.z, 0.f);
|
||||
|
||||
//we need to perform sleep check here to decide whether we want to update body2World transform for the body
|
||||
const PxVec3 motionLinVel(lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_X ? 0.f : motionLinVelXYZW.x,
|
||||
lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_Y ? 0.f : motionLinVelXYZW.y,
|
||||
lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_Z ? 0.f : motionLinVelXYZW.z);
|
||||
const PxVec3 motionAngVel(lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_X ? 0.f : motionAngVelXYZW.x,
|
||||
lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_Y ? 0.f : motionAngVelXYZW.y,
|
||||
lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_Z ? 0.f : motionAngVelXYZW.z);
|
||||
|
||||
PxVec3 linearMotionVel = motionLinVel;
|
||||
PxVec3 angularMotionVel = sqrtInvInertia * motionAngVel;
|
||||
|
||||
// Integrate the rotation using closed form quaternion integrator
|
||||
PxReal w = angularMotionVel.magnitudeSquared();
|
||||
w = PxSqrt(w);
|
||||
const PxReal maxW = 1e+7f; //Should be about sqrt(PX_MAX_REAL/2) or smaller
|
||||
if (w > maxW)
|
||||
{
|
||||
angularMotionVel = angularMotionVel.getNormalized() * maxW;
|
||||
w = maxW;
|
||||
}
|
||||
|
||||
const bool freeze = sleepCheck(solverBodyLinVel, solverBodyAngVel, body2World, bodySim, sleepData, inverseInertia, linearMotionVel, angularMotionVel, initialLinVelXYZ_invMassW.w,
|
||||
dt, invDt, enableStabilization, hasStaticTouch, numCountedInteractions, nodeIndex);
|
||||
|
||||
if (!freeze)
|
||||
{
|
||||
//printf("DeltaP = (%f, %f, %f)\n", deltaBody2World.p.x, deltaBody2World.p.y, deltaBody2World.p.z);
|
||||
body2World.p.x += deltaBody2World.p.x; body2World.p.y += deltaBody2World.p.y; body2World.p.z += deltaBody2World.p.z;
|
||||
PxQuat q(body2World.q.q.x, body2World.q.q.y, body2World.q.q.z, body2World.q.q.w);
|
||||
q = (deltaBody2World.q * q).getNormalized();
|
||||
|
||||
body2World.q.q.x = q.x; body2World.q.q.y = q.y; body2World.q.q.z = q.z; body2World.q.q.w = q.w;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user