645 lines
24 KiB
C++
645 lines
24 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 "geometry/PxBoxGeometry.h"
|
|
#include "geometry/PxSphereGeometry.h"
|
|
#include "geometry/PxCapsuleGeometry.h"
|
|
#include "geometry/PxPlaneGeometry.h"
|
|
#include "geometry/PxConvexMeshGeometry.h"
|
|
#include "geometry/PxTriangleMeshGeometry.h"
|
|
#include "geometry/PxHeightFieldGeometry.h"
|
|
#include "geometry/PxGeometryHelpers.h"
|
|
#include "geometry/PxConvexMesh.h"
|
|
#include "extensions/PxRigidBodyExt.h"
|
|
#include "extensions/PxShapeExt.h"
|
|
#include "extensions/PxMassProperties.h"
|
|
#include "PxShape.h"
|
|
#include "PxScene.h"
|
|
#include "PxBatchQuery.h"
|
|
#include "PxRigidDynamic.h"
|
|
#include "PxRigidStatic.h"
|
|
|
|
#include "ExtInertiaTensor.h"
|
|
#include "PsAllocator.h"
|
|
#include "PsFoundation.h"
|
|
|
|
#include "CmUtils.h"
|
|
|
|
using namespace physx;
|
|
using namespace Cm;
|
|
|
|
static bool computeMassAndDiagInertia(Ext::InertiaTensorComputer& inertiaComp,
|
|
PxVec3& diagTensor, PxQuat& orient, PxReal& massOut, PxVec3& coM, bool lockCOM, const PxRigidBody& body, const char* errorStr)
|
|
{
|
|
// The inertia tensor and center of mass is relative to the actor at this point. Transform to the
|
|
// body frame directly if CoM is specified, else use computed center of mass
|
|
if (lockCOM)
|
|
{
|
|
inertiaComp.translate(-coM); // base the tensor on user's desired center of mass.
|
|
}
|
|
else
|
|
{
|
|
//get center of mass - has to be done BEFORE centering.
|
|
coM = inertiaComp.getCenterOfMass();
|
|
|
|
//the computed result now needs to be centered around the computed center of mass:
|
|
inertiaComp.center();
|
|
}
|
|
// The inertia matrix is now based on the body's center of mass desc.massLocalPose.p
|
|
|
|
massOut = inertiaComp.getMass();
|
|
diagTensor = PxDiagonalize(inertiaComp.getInertia(), orient);
|
|
|
|
if ((diagTensor.x > 0.0f) && (diagTensor.y > 0.0f) && (diagTensor.z > 0.0f))
|
|
return true;
|
|
else
|
|
{
|
|
Ps::getFoundation().error(PxErrorCode::eDEBUG_WARNING, __FILE__, __LINE__,
|
|
"%s: inertia tensor has negative components (ill-conditioned input expected). Approximation for inertia tensor will be used instead.", errorStr);
|
|
|
|
// keep center of mass but use the AABB as a crude approximation for the inertia tensor
|
|
PxBounds3 bounds = body.getWorldBounds();
|
|
PxTransform pose = body.getGlobalPose();
|
|
bounds = PxBounds3::transformFast(pose.getInverse(), bounds);
|
|
Ext::InertiaTensorComputer it(false);
|
|
it.setBox(bounds.getExtents());
|
|
it.scaleDensity(massOut / it.getMass());
|
|
PxMat33 inertia = it.getInertia();
|
|
diagTensor = PxVec3(inertia.column0.x, inertia.column1.y, inertia.column2.z);
|
|
orient = PxQuat(PxIdentity);
|
|
|
|
return true;
|
|
}
|
|
}
|
|
|
|
static bool computeMassAndInertia(bool multipleMassOrDensity, PxRigidBody& body, const PxReal* densities, const PxReal* masses, PxU32 densityOrMassCount, bool includeNonSimShapes, Ext::InertiaTensorComputer& computer)
|
|
{
|
|
PX_ASSERT(!densities || !masses);
|
|
PX_ASSERT((densities || masses) && (densityOrMassCount > 0));
|
|
|
|
Ext::InertiaTensorComputer inertiaComp(true);
|
|
|
|
Ps::InlineArray<PxShape*, 16> shapes("PxShape*"); shapes.resize(body.getNbShapes());
|
|
|
|
body.getShapes(shapes.begin(), shapes.size());
|
|
|
|
PxU32 validShapeIndex = 0;
|
|
PxReal currentMassOrDensity;
|
|
const PxReal* massOrDensityArray;
|
|
if (densities)
|
|
{
|
|
massOrDensityArray = densities;
|
|
currentMassOrDensity = densities[0];
|
|
}
|
|
else
|
|
{
|
|
massOrDensityArray = masses;
|
|
currentMassOrDensity = masses[0];
|
|
}
|
|
if (!PxIsFinite(currentMassOrDensity))
|
|
{
|
|
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
|
|
"computeMassAndInertia: Provided mass or density has no valid value");
|
|
return false;
|
|
}
|
|
|
|
for(PxU32 i=0; i < shapes.size(); i++)
|
|
{
|
|
if ((!(shapes[i]->getFlags() & PxShapeFlag::eSIMULATION_SHAPE)) && (!includeNonSimShapes))
|
|
continue;
|
|
|
|
if (multipleMassOrDensity)
|
|
{
|
|
if (validShapeIndex < densityOrMassCount)
|
|
{
|
|
currentMassOrDensity = massOrDensityArray[validShapeIndex];
|
|
|
|
if (!PxIsFinite(currentMassOrDensity))
|
|
{
|
|
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
|
|
"computeMassAndInertia: Provided mass or density has no valid value");
|
|
return false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
|
|
"computeMassAndInertia: Not enough mass/density values provided for all (simulation) shapes");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
Ext::InertiaTensorComputer it(false);
|
|
|
|
switch(shapes[i]->getGeometryType())
|
|
{
|
|
case PxGeometryType::eSPHERE :
|
|
{
|
|
PxSphereGeometry g;
|
|
bool ok = shapes[i]->getSphereGeometry(g);
|
|
PX_ASSERT(ok);
|
|
PX_UNUSED(ok);
|
|
PxTransform temp(shapes[i]->getLocalPose());
|
|
|
|
it.setSphere(g.radius, &temp);
|
|
}
|
|
break;
|
|
|
|
case PxGeometryType::eBOX :
|
|
{
|
|
PxBoxGeometry g;
|
|
bool ok = shapes[i]->getBoxGeometry(g);
|
|
PX_ASSERT(ok);
|
|
PX_UNUSED(ok);
|
|
PxTransform temp(shapes[i]->getLocalPose());
|
|
|
|
it.setBox(g.halfExtents, &temp);
|
|
}
|
|
break;
|
|
|
|
case PxGeometryType::eCAPSULE :
|
|
{
|
|
PxCapsuleGeometry g;
|
|
bool ok = shapes[i]->getCapsuleGeometry(g);
|
|
PX_ASSERT(ok);
|
|
PX_UNUSED(ok);
|
|
PxTransform temp(shapes[i]->getLocalPose());
|
|
|
|
it.setCapsule(0, g.radius, g.halfHeight, &temp);
|
|
}
|
|
break;
|
|
|
|
case PxGeometryType::eCONVEXMESH :
|
|
{
|
|
PxConvexMeshGeometry g;
|
|
bool ok = shapes[i]->getConvexMeshGeometry(g);
|
|
PX_ASSERT(ok);
|
|
PX_UNUSED(ok);
|
|
PxConvexMesh& convMesh = *g.convexMesh;
|
|
|
|
PxReal convMass;
|
|
PxMat33 convInertia;
|
|
PxVec3 convCoM;
|
|
convMesh.getMassInformation(convMass, reinterpret_cast<PxMat33&>(convInertia), convCoM);
|
|
|
|
if (!g.scale.isIdentity())
|
|
{
|
|
//scale the mass properties
|
|
convMass *= (g.scale.scale.x * g.scale.scale.y * g.scale.scale.z);
|
|
convCoM = g.scale.rotation.rotateInv(g.scale.scale.multiply(g.scale.rotation.rotate(convCoM)));
|
|
convInertia = PxMassProperties::scaleInertia(convInertia, g.scale.rotation, g.scale.scale);
|
|
}
|
|
|
|
it = Ext::InertiaTensorComputer(convInertia, convCoM, convMass);
|
|
it.transform(shapes[i]->getLocalPose());
|
|
}
|
|
break;
|
|
case PxGeometryType::eHEIGHTFIELD:
|
|
case PxGeometryType::ePLANE:
|
|
case PxGeometryType::eTRIANGLEMESH:
|
|
case PxGeometryType::eINVALID:
|
|
case PxGeometryType::eGEOMETRY_COUNT:
|
|
{
|
|
|
|
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
|
|
"computeMassAndInertia: Dynamic actor with illegal collision shapes");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
if (densities)
|
|
it.scaleDensity(currentMassOrDensity);
|
|
else if (multipleMassOrDensity) // mass per shape -> need to scale density per shape
|
|
it.scaleDensity(currentMassOrDensity / it.getMass());
|
|
|
|
inertiaComp.add(it);
|
|
|
|
validShapeIndex++;
|
|
}
|
|
|
|
if (validShapeIndex && masses && (!multipleMassOrDensity)) // at least one simulation shape and single mass for all shapes -> scale density at the end
|
|
{
|
|
inertiaComp.scaleDensity(currentMassOrDensity / inertiaComp.getMass());
|
|
}
|
|
|
|
computer = inertiaComp;
|
|
return true;
|
|
}
|
|
|
|
static bool updateMassAndInertia(bool multipleMassOrDensity, PxRigidBody& body, const PxReal* densities, PxU32 densityCount, const PxVec3* massLocalPose, bool includeNonSimShapes)
|
|
{
|
|
bool success;
|
|
|
|
// default values in case there were no shapes
|
|
PxReal massOut = 1.0f;
|
|
PxVec3 diagTensor(1.f,1.f,1.f);
|
|
PxQuat orient = PxQuat(PxIdentity);
|
|
bool lockCom = massLocalPose != NULL;
|
|
PxVec3 com = lockCom ? *massLocalPose : PxVec3(0);
|
|
const char* errorStr = "PxRigidBodyExt::updateMassAndInertia";
|
|
|
|
if (densities && densityCount)
|
|
{
|
|
Ext::InertiaTensorComputer inertiaComp(true);
|
|
if(computeMassAndInertia(multipleMassOrDensity, body, densities, NULL, densityCount, includeNonSimShapes, inertiaComp))
|
|
{
|
|
if(inertiaComp.getMass()!=0 && computeMassAndDiagInertia(inertiaComp, diagTensor, orient, massOut, com, lockCom, body, errorStr))
|
|
success = true;
|
|
else
|
|
success = false; // body with no shapes provided or computeMassAndDiagInertia() failed
|
|
}
|
|
else
|
|
{
|
|
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
|
|
"%s: Mass and inertia computation failed, setting mass to 1 and inertia to (1,1,1)", errorStr);
|
|
|
|
success = false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
|
|
"%s: No density specified, setting mass to 1 and inertia to (1,1,1)", errorStr);
|
|
|
|
success = false;
|
|
}
|
|
|
|
PX_ASSERT(orient.isFinite());
|
|
PX_ASSERT(diagTensor.isFinite());
|
|
PX_ASSERT(PxIsFinite(massOut));
|
|
|
|
body.setMass(massOut);
|
|
body.setMassSpaceInertiaTensor(diagTensor);
|
|
body.setCMassLocalPose(PxTransform(com, orient));
|
|
|
|
return success;
|
|
}
|
|
|
|
bool PxRigidBodyExt::updateMassAndInertia(PxRigidBody& body, const PxReal* densities, PxU32 densityCount, const PxVec3* massLocalPose, bool includeNonSimShapes)
|
|
{
|
|
return ::updateMassAndInertia(true, body, densities, densityCount, massLocalPose, includeNonSimShapes);
|
|
}
|
|
|
|
bool PxRigidBodyExt::updateMassAndInertia(PxRigidBody& body, PxReal density, const PxVec3* massLocalPose, bool includeNonSimShapes)
|
|
{
|
|
return ::updateMassAndInertia(false, body, &density, 1, massLocalPose, includeNonSimShapes);
|
|
}
|
|
|
|
static bool setMassAndUpdateInertia(bool multipleMassOrDensity, PxRigidBody& body, const PxReal* masses, PxU32 massCount, const PxVec3* massLocalPose, bool includeNonSimShapes)
|
|
{
|
|
bool success;
|
|
|
|
// default values in case there were no shapes
|
|
PxReal massOut = 1.0f;
|
|
PxVec3 diagTensor(1.0f,1.0f,1.0f);
|
|
PxQuat orient = PxQuat(PxIdentity);
|
|
bool lockCom = massLocalPose != NULL;
|
|
PxVec3 com = lockCom ? *massLocalPose : PxVec3(0);
|
|
const char* errorStr = "PxRigidBodyExt::setMassAndUpdateInertia";
|
|
|
|
if(masses && massCount)
|
|
{
|
|
Ext::InertiaTensorComputer inertiaComp(true);
|
|
if(computeMassAndInertia(multipleMassOrDensity, body, NULL, masses, massCount, includeNonSimShapes, inertiaComp))
|
|
{
|
|
success = true;
|
|
|
|
if (inertiaComp.getMass()!=0 && !computeMassAndDiagInertia(inertiaComp, diagTensor, orient, massOut, com, lockCom, body, errorStr))
|
|
success = false; // computeMassAndDiagInertia() failed (mass zero?)
|
|
|
|
if (massCount == 1)
|
|
massOut = masses[0]; // to cover special case where body has no simulation shape
|
|
}
|
|
else
|
|
{
|
|
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
|
|
"%s: Mass and inertia computation failed, setting mass to 1 and inertia to (1,1,1)", errorStr);
|
|
|
|
success = false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
|
|
"%s: No mass specified, setting mass to 1 and inertia to (1,1,1)", errorStr);
|
|
success = false;
|
|
}
|
|
|
|
PX_ASSERT(orient.isFinite());
|
|
PX_ASSERT(diagTensor.isFinite());
|
|
|
|
body.setMass(massOut);
|
|
body.setMassSpaceInertiaTensor(diagTensor);
|
|
body.setCMassLocalPose(PxTransform(com, orient));
|
|
|
|
return success;
|
|
}
|
|
|
|
bool PxRigidBodyExt::setMassAndUpdateInertia(PxRigidBody& body, const PxReal* masses, PxU32 massCount, const PxVec3* massLocalPose, bool includeNonSimShapes)
|
|
{
|
|
return ::setMassAndUpdateInertia(true, body, masses, massCount, massLocalPose, includeNonSimShapes);
|
|
}
|
|
|
|
bool PxRigidBodyExt::setMassAndUpdateInertia(PxRigidBody& body, PxReal mass, const PxVec3* massLocalPose, bool includeNonSimShapes)
|
|
{
|
|
return ::setMassAndUpdateInertia(false, body, &mass, 1, massLocalPose, includeNonSimShapes);
|
|
}
|
|
|
|
PxMassProperties PxRigidBodyExt::computeMassPropertiesFromShapes(const PxShape* const* shapes, PxU32 shapeCount)
|
|
{
|
|
Ps::InlineArray<PxMassProperties, 16> massProps;
|
|
massProps.reserve(shapeCount);
|
|
Ps::InlineArray<PxTransform, 16> localTransforms;
|
|
localTransforms.reserve(shapeCount);
|
|
|
|
for(PxU32 shapeIdx=0; shapeIdx < shapeCount; shapeIdx++)
|
|
{
|
|
const PxShape* shape = shapes[shapeIdx];
|
|
PxMassProperties mp(shape->getGeometry().any());
|
|
massProps.pushBack(mp);
|
|
localTransforms.pushBack(shape->getLocalPose());
|
|
}
|
|
|
|
return PxMassProperties::sum(massProps.begin(), localTransforms.begin(), shapeCount);
|
|
}
|
|
|
|
PX_INLINE void addForceAtPosInternal(PxRigidBody& body, const PxVec3& force, const PxVec3& pos, PxForceMode::Enum mode, bool wakeup)
|
|
{
|
|
if(mode == PxForceMode::eACCELERATION || mode == PxForceMode::eVELOCITY_CHANGE)
|
|
{
|
|
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
|
|
"PxRigidBodyExt::addForce methods do not support eACCELERATION or eVELOCITY_CHANGE modes");
|
|
return;
|
|
}
|
|
|
|
const PxTransform globalPose = body.getGlobalPose();
|
|
const PxVec3 centerOfMass = globalPose.transform(body.getCMassLocalPose().p);
|
|
|
|
const PxVec3 torque = (pos - centerOfMass).cross(force);
|
|
body.addForce(force, mode, wakeup);
|
|
body.addTorque(torque, mode, wakeup);
|
|
}
|
|
|
|
void PxRigidBodyExt::addForceAtPos(PxRigidBody& body, const PxVec3& force, const PxVec3& pos, PxForceMode::Enum mode, bool wakeup)
|
|
{
|
|
addForceAtPosInternal(body, force, pos, mode, wakeup);
|
|
}
|
|
|
|
void PxRigidBodyExt::addForceAtLocalPos(PxRigidBody& body, const PxVec3& force, const PxVec3& pos, PxForceMode::Enum mode, bool wakeup)
|
|
{
|
|
//transform pos to world space
|
|
const PxVec3 globalForcePos = body.getGlobalPose().transform(pos);
|
|
|
|
addForceAtPosInternal(body, force, globalForcePos, mode, wakeup);
|
|
}
|
|
|
|
void PxRigidBodyExt::addLocalForceAtPos(PxRigidBody& body, const PxVec3& force, const PxVec3& pos, PxForceMode::Enum mode, bool wakeup)
|
|
{
|
|
const PxVec3 globalForce = body.getGlobalPose().rotate(force);
|
|
|
|
addForceAtPosInternal(body, globalForce, pos, mode, wakeup);
|
|
}
|
|
|
|
void PxRigidBodyExt::addLocalForceAtLocalPos(PxRigidBody& body, const PxVec3& force, const PxVec3& pos, PxForceMode::Enum mode, bool wakeup)
|
|
{
|
|
const PxTransform globalPose = body.getGlobalPose();
|
|
const PxVec3 globalForcePos = globalPose.transform(pos);
|
|
const PxVec3 globalForce = globalPose.rotate(force);
|
|
|
|
addForceAtPosInternal(body, globalForce, globalForcePos, mode, wakeup);
|
|
}
|
|
|
|
PX_INLINE PxVec3 getVelocityAtPosInternal(const PxRigidBody& body, const PxVec3& point)
|
|
{
|
|
PxVec3 velocity = body.getLinearVelocity();
|
|
velocity += body.getAngularVelocity().cross(point);
|
|
|
|
return velocity;
|
|
}
|
|
|
|
PxVec3 PxRigidBodyExt::getVelocityAtPos(const PxRigidBody& body, const PxVec3& point)
|
|
{
|
|
const PxTransform globalPose = body.getGlobalPose();
|
|
const PxVec3 centerOfMass = globalPose.transform(body.getCMassLocalPose().p);
|
|
const PxVec3 rpoint = point - centerOfMass;
|
|
|
|
return getVelocityAtPosInternal(body, rpoint);
|
|
}
|
|
|
|
PxVec3 PxRigidBodyExt::getLocalVelocityAtLocalPos(const PxRigidBody& body, const PxVec3& point)
|
|
{
|
|
const PxTransform globalPose = body.getGlobalPose();
|
|
const PxVec3 centerOfMass = globalPose.transform(body.getCMassLocalPose().p);
|
|
const PxVec3 rpoint = globalPose.transform(point) - centerOfMass;
|
|
|
|
return getVelocityAtPosInternal(body, rpoint);
|
|
}
|
|
|
|
PxVec3 PxRigidBodyExt::getVelocityAtOffset(const PxRigidBody& body, const PxVec3& point)
|
|
{
|
|
const PxTransform globalPose = body.getGlobalPose();
|
|
const PxVec3 centerOfMass = globalPose.rotate(body.getCMassLocalPose().p);
|
|
const PxVec3 rpoint = point - centerOfMass;
|
|
|
|
return getVelocityAtPosInternal(body, rpoint);
|
|
}
|
|
|
|
void PxRigidBodyExt::computeVelocityDeltaFromImpulse(const PxRigidBody& body, const PxTransform& globalPose, const PxVec3& point, const PxVec3& impulse, const PxReal invMassScale,
|
|
const PxReal invInertiaScale, PxVec3& linearVelocityChange, PxVec3& angularVelocityChange)
|
|
{
|
|
const PxVec3 centerOfMass = globalPose.transform(body.getCMassLocalPose().p);
|
|
const PxReal invMass = body.getInvMass() * invMassScale;
|
|
const PxVec3 invInertiaMS = body.getMassSpaceInvInertiaTensor() * invInertiaScale;
|
|
|
|
PxMat33 invInertia;
|
|
transformInertiaTensor(invInertiaMS, PxMat33(globalPose.q), invInertia);
|
|
linearVelocityChange = impulse * invMass;
|
|
const PxVec3 rXI = (point - centerOfMass).cross(impulse);
|
|
angularVelocityChange = invInertia * rXI;
|
|
}
|
|
|
|
void PxRigidBodyExt::computeLinearAngularImpulse(const PxRigidBody& body, const PxTransform& globalPose, const PxVec3& point, const PxVec3& impulse, const PxReal invMassScale,
|
|
const PxReal invInertiaScale, PxVec3& linearImpulse, PxVec3& angularImpulse)
|
|
{
|
|
const PxVec3 centerOfMass = globalPose.transform(body.getCMassLocalPose().p);
|
|
linearImpulse = impulse * invMassScale;
|
|
angularImpulse = (point - centerOfMass).cross(impulse) * invInertiaScale;
|
|
}
|
|
|
|
|
|
|
|
//=================================================================================
|
|
// Single closest hit compound sweep
|
|
bool PxRigidBodyExt::linearSweepSingle(
|
|
PxRigidBody& body, PxScene& scene, const PxVec3& unitDir, const PxReal distance,
|
|
PxHitFlags outputFlags, PxSweepHit& closestHit, PxU32& shapeIndex,
|
|
const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall,
|
|
const PxQueryCache* cache, const PxReal inflation)
|
|
{
|
|
shapeIndex = 0xFFFFffff;
|
|
PxReal closestDist = distance;
|
|
PxU32 nbShapes = body.getNbShapes();
|
|
for(PxU32 i=0; i < nbShapes; i++)
|
|
{
|
|
PxShape* shape = NULL;
|
|
body.getShapes(&shape, 1, i);
|
|
PX_ASSERT(shape != NULL);
|
|
PxTransform pose = PxShapeExt::getGlobalPose(*shape, body);
|
|
PxQueryFilterData fd;
|
|
fd.flags = filterData.flags;
|
|
PxU32 or4 = (filterData.data.word0 | filterData.data.word1 | filterData.data.word2 | filterData.data.word3);
|
|
fd.data = or4 ? filterData.data : shape->getQueryFilterData();
|
|
PxGeometryHolder anyGeom = shape->getGeometry();
|
|
|
|
PxSweepBuffer subHit; // touching hits are not allowed to be returned from the filters
|
|
scene.sweep(anyGeom.any(), pose, unitDir, distance, subHit, outputFlags, fd, filterCall, cache, inflation);
|
|
if (subHit.hasBlock && subHit.block.distance < closestDist)
|
|
{
|
|
closestDist = subHit.block.distance;
|
|
closestHit = subHit.block;
|
|
shapeIndex = i;
|
|
}
|
|
}
|
|
|
|
return (shapeIndex != 0xFFFFffff);
|
|
}
|
|
|
|
//=================================================================================
|
|
// Multiple hits compound sweep
|
|
// AP: we might be able to improve the return results API but no time for it in 3.3
|
|
PxU32 PxRigidBodyExt::linearSweepMultiple(
|
|
PxRigidBody& body, PxScene& scene, const PxVec3& unitDir, const PxReal distance, PxHitFlags outputFlags,
|
|
PxSweepHit* hitBuffer, PxU32* hitShapeIndices, PxU32 hitBufferSize, PxSweepHit& block, PxI32& blockingHitShapeIndex,
|
|
bool& overflow, const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall,
|
|
const PxQueryCache* cache, const PxReal inflation)
|
|
{
|
|
overflow = false;
|
|
blockingHitShapeIndex = -1;
|
|
|
|
for (PxU32 i = 0; i < hitBufferSize; i++)
|
|
hitShapeIndices[i] = 0xFFFFffff;
|
|
|
|
PxI32 sumNbResults = 0;
|
|
|
|
PxU32 nbShapes = body.getNbShapes();
|
|
PxF32 shrunkMaxDistance = distance;
|
|
for(PxU32 i=0; i < nbShapes; i++)
|
|
{
|
|
PxShape* shape = NULL;
|
|
body.getShapes(&shape, 1, i);
|
|
PX_ASSERT(shape != NULL);
|
|
PxTransform pose = PxShapeExt::getGlobalPose(*shape, body);
|
|
PxQueryFilterData fd;
|
|
fd.flags = filterData.flags;
|
|
PxU32 or4 = (filterData.data.word0 | filterData.data.word1 | filterData.data.word2 | filterData.data.word3);
|
|
fd.data = or4 ? filterData.data : shape->getQueryFilterData();
|
|
PxGeometryHolder anyGeom = shape->getGeometry();
|
|
|
|
PxU32 bufSizeLeft = hitBufferSize-sumNbResults;
|
|
PxSweepHit extraHit;
|
|
PxSweepBuffer buffer(bufSizeLeft == 0 ? &extraHit : hitBuffer+sumNbResults, bufSizeLeft == 0 ? 1 : hitBufferSize-sumNbResults);
|
|
scene.sweep(anyGeom.any(), pose, unitDir, shrunkMaxDistance, buffer, outputFlags, fd, filterCall, cache, inflation);
|
|
|
|
// Check and abort on overflow. Assume overflow if result count is bufSize.
|
|
PxU32 nbNewResults = buffer.getNbTouches();
|
|
overflow |= (nbNewResults >= bufSizeLeft);
|
|
if (bufSizeLeft == 0) // this is for when we used the extraHit buffer
|
|
nbNewResults = 0;
|
|
|
|
// set hitShapeIndices for each new non-blocking hit
|
|
for (PxU32 j = 0; j < nbNewResults; j++)
|
|
if (sumNbResults + PxU32(j) < hitBufferSize)
|
|
hitShapeIndices[sumNbResults+j] = i;
|
|
|
|
if (buffer.hasBlock) // there's a blocking hit in the most recent sweepMultiple results
|
|
{
|
|
// overwrite the return result blocking hit with the new blocking hit if under
|
|
if (blockingHitShapeIndex == -1 || buffer.block.distance < block.distance)
|
|
{
|
|
blockingHitShapeIndex = PxI32(i);
|
|
block = buffer.block;
|
|
}
|
|
|
|
// Remove all the old touching hits below the new maxDist
|
|
// sumNbResults is not updated yet at this point
|
|
// and represents the count accumulated so far excluding the very last query
|
|
PxI32 nbNewResultsSigned = PxI32(nbNewResults); // need a signed version, see nbNewResultsSigned-- below
|
|
for (PxI32 j = sumNbResults-1; j >= 0; j--) // iterate over "old" hits (up to shapeIndex-1)
|
|
if (buffer.block.distance < hitBuffer[j].distance)
|
|
{
|
|
// overwrite with last "new" hit
|
|
PxI32 sourceIndex = PxI32(sumNbResults)+nbNewResultsSigned-1; PX_ASSERT(sourceIndex >= j);
|
|
hitBuffer[j] = hitBuffer[sourceIndex];
|
|
hitShapeIndices[j] = hitShapeIndices[sourceIndex];
|
|
nbNewResultsSigned--; // can get negative, that means we are shifting the last results array
|
|
}
|
|
|
|
sumNbResults += nbNewResultsSigned;
|
|
} else // if there was no new blocking hit we don't need to do anything special, simply append all results to touch array
|
|
sumNbResults += nbNewResults;
|
|
|
|
PX_ASSERT(sumNbResults >= 0 && sumNbResults <= PxI32(hitBufferSize));
|
|
}
|
|
|
|
return PxU32(sumNbResults);
|
|
}
|
|
|
|
void PxRigidBodyExt::computeVelocityDeltaFromImpulse(const PxRigidBody& body, const PxVec3& impulsiveForce, const PxVec3& impulsiveTorque, PxVec3& deltaLinearVelocity, PxVec3& deltaAngularVelocity)
|
|
{
|
|
{
|
|
const PxF32 recipMass = body.getInvMass();
|
|
deltaLinearVelocity = impulsiveForce*recipMass;
|
|
}
|
|
|
|
{
|
|
const PxTransform globalPose = body.getGlobalPose();
|
|
const PxTransform cmLocalPose = body.getCMassLocalPose();
|
|
const PxTransform body2World = globalPose*cmLocalPose;
|
|
PxMat33 M(body2World.q);
|
|
|
|
const PxVec3 recipInertiaBodySpace = body.getMassSpaceInvInertiaTensor();
|
|
|
|
PxMat33 recipInertiaWorldSpace;
|
|
const float axx = recipInertiaBodySpace.x*M(0,0), axy = recipInertiaBodySpace.x*M(1,0), axz = recipInertiaBodySpace.x*M(2,0);
|
|
const float byx = recipInertiaBodySpace.y*M(0,1), byy = recipInertiaBodySpace.y*M(1,1), byz = recipInertiaBodySpace.y*M(2,1);
|
|
const float czx = recipInertiaBodySpace.z*M(0,2), czy = recipInertiaBodySpace.z*M(1,2), czz = recipInertiaBodySpace.z*M(2,2);
|
|
recipInertiaWorldSpace(0,0) = axx*M(0,0) + byx*M(0,1) + czx*M(0,2);
|
|
recipInertiaWorldSpace(1,1) = axy*M(1,0) + byy*M(1,1) + czy*M(1,2);
|
|
recipInertiaWorldSpace(2,2) = axz*M(2,0) + byz*M(2,1) + czz*M(2,2);
|
|
recipInertiaWorldSpace(0,1) = recipInertiaWorldSpace(1,0) = axx*M(1,0) + byx*M(1,1) + czx*M(1,2);
|
|
recipInertiaWorldSpace(0,2) = recipInertiaWorldSpace(2,0) = axx*M(2,0) + byx*M(2,1) + czx*M(2,2);
|
|
recipInertiaWorldSpace(1,2) = recipInertiaWorldSpace(2,1) = axy*M(2,0) + byy*M(2,1) + czy*M(2,2);
|
|
|
|
deltaAngularVelocity = recipInertiaWorldSpace*(impulsiveTorque);
|
|
}
|
|
}
|
|
|
|
|