Runtime/Engine/Private/PhysicsEngine/BodyInstance.cpp
void FBodyInstance::UpdateMassProperties()
{
UPhysicalMaterial* PhysMat = GetSimplePhysicalMaterial();
#if WITH_PHYSX
if (FPhysicsInterface::IsValid(ActorHandle) && FPhysicsInterface::IsRigidBody(ActorHandle))
{
FPhysicsCommand::ExecuteWrite(ActorHandle, [&](FPhysicsActorHandle& Actor)
{
check(FPhysicsInterface::IsValid(Actor));
if (GetNumSimShapes_AssumesLocked(Actor) > 0)
{
const int32 NumShapes = FPhysicsInterface::GetNumShapes(Actor);
TArray<FPhysicsShapeHandle> Shapes;
Shapes.AddUninitialized(NumShapes);
FPhysicsInterface::GetAllShapes_AssumedLocked(Actor, Shapes);
// Ignore trimeshes & shapes which don't contribute to the mass
for (int32 ShapeIdx = Shapes.Num() - 1; ShapeIdx >= 0; --ShapeIdx)
{
const FPhysicsShapeHandle& Shape = Shapes[ShapeIdx];
const FKShapeElem* ShapeElem = FChaosUserData::Get<FKShapeElem>(FPhysicsInterface::GetUserData(Shape));
bool bIsTriangleMesh = FPhysicsInterface::GetShapeType(Shape) == ECollisionShapeType::Trimesh;
bool bHasNoMass = ShapeElem && !ShapeElem->GetContributeToMass();
if (bIsTriangleMesh || bHasNoMass)
{
Shapes.RemoveAtSwap(ShapeIdx);
}
}
#if WITH_CHAOS
Chaos::TMassProperties<float, 3> TotalMassProperties;
#elif PHYSICS_INTERFACE_PHYSX
PxMassProperties TotalMassProperties;
#endif
if (ShapeToBodiesMap.IsValid() && ShapeToBodiesMap->Num() > 0)
{
struct FWeldedBatch
{
TArray<FPhysicsShapeHandle> Shapes;
FTransform RelTM;
};
//If we have welded children we must compute the mass properties of each individual body first and then combine them all together
TMap<FBodyInstance*, FWeldedBatch> BodyToShapes;
for (const FPhysicsShapeHandle& Shape : Shapes) //sort all welded children by their original bodies
{
if (FWeldInfo* WeldInfo = ShapeToBodiesMap->Find(Shape))
{
FWeldedBatch* WeldedBatch = BodyToShapes.Find(WeldInfo->ChildBI);
if (!WeldedBatch)
{
WeldedBatch = &BodyToShapes.Add(WeldInfo->ChildBI);
WeldedBatch->RelTM = WeldInfo->RelativeTM;
}
WeldedBatch->Shapes.Add(Shape);
}
else
{
//no weld info so shape really belongs to this body
FWeldedBatch* WeldedBatch = BodyToShapes.Find(this);
if (!WeldedBatch)
{
WeldedBatch = &BodyToShapes.Add(this);
WeldedBatch->RelTM = FTransform::Identity;
}
WeldedBatch->Shapes.Add(Shape);
}
}
#if WITH_CHAOS
TArray<Chaos::TMassProperties<float, 3>> SubMassProperties;
for (auto BodyShapesItr : BodyToShapes)
{
const FBodyInstance* OwningBI = BodyShapesItr.Key;
const FWeldedBatch& WeldedBatch = BodyShapesItr.Value;
FTransform MassModifierTransform = WeldedBatch.RelTM;
MassModifierTransform.SetScale3D(MassModifierTransform.GetScale3D() * Scale3D); //Ensure that any scaling that is done on the component is passed into the mass frame modifiers
Chaos::TMassProperties<float, 3> BodyMassProperties = BodyUtils::ComputeMassProperties(OwningBI, WeldedBatch.Shapes, MassModifierTransform);
SubMassProperties.Add(BodyMassProperties);
}
TotalMassProperties = Chaos::Combine(SubMassProperties);
#elif PHYSICS_INTERFACE_PHYSX
TArray<PxMassProperties> SubMassProperties;
TArray<PxTransform> MassTMs;
for (auto BodyShapesItr : BodyToShapes)
{
const FBodyInstance* OwningBI = BodyShapesItr.Key;
const FWeldedBatch& WeldedBatch = BodyShapesItr.Value;
FTransform MassModifierTransform = WeldedBatch.RelTM;
MassModifierTransform.SetScale3D(MassModifierTransform.GetScale3D() * Scale3D); //Ensure that any scaling that is done on the component is passed into the mass frame modifiers
PxMassProperties BodyMassProperties = BodyUtils::ComputeMassProperties(OwningBI, WeldedBatch.Shapes, MassModifierTransform);
SubMassProperties.Add(BodyMassProperties);
MassTMs.Add(PxTransform(PxIdentity));
}
TotalMassProperties = PxMassProperties::sum(SubMassProperties.GetData(), MassTMs.GetData(), SubMassProperties.Num());
#endif
}
else
{
// If we have no shapes that affect mass we cannot compute the mass properties in a meaningful way.
if (Shapes.Num())
{
//No children welded so just get this body's mass properties
FTransform MassModifierTransform(FQuat::Identity, FVector(0.f, 0.f, 0.f), Scale3D); //Ensure that any scaling that is done on the component is passed into the mass frame modifiers
TotalMassProperties = BodyUtils::ComputeMassProperties(this, Shapes, MassModifierTransform);
}
}
// #PHYS2 Refactor out PxMassProperties (Our own impl?)
#if WITH_CHAOS
// Only set mass properties if inertia tensor is valid. TODO Remove this once we track down cause of empty tensors.
const float InertiaTensorTrace = (TotalMassProperties.InertiaTensor.M[0][0] + TotalMassProperties.InertiaTensor.M[1][1] + TotalMassProperties.InertiaTensor.M[2][2]) / 3;
if (CHAOS_ENSURE(InertiaTensorTrace > SMALL_NUMBER))
{
const Chaos::TRotation<float, 3> Rotation = Chaos::TransformToLocalSpace(TotalMassProperties.InertiaTensor);
const FVector MassSpaceInertiaTensor(TotalMassProperties.InertiaTensor.M[0][0], TotalMassProperties.InertiaTensor.M[1][1], TotalMassProperties.InertiaTensor.M[2][2]);
FPhysicsInterface::SetMassSpaceInertiaTensor_AssumesLocked(Actor, MassSpaceInertiaTensor);
FPhysicsInterface::SetMass_AssumesLocked(Actor, TotalMassProperties.Mass);
FTransform Com(Rotation, TotalMassProperties.CenterOfMass);
FPhysicsInterface::SetComLocalPose_AssumesLocked(Actor, Com);
}
#else
PxQuat MassOrientation;
const FVector MassSpaceInertiaTensor = P2UVector(PxMassProperties::getMassSpaceInertia(TotalMassProperties.inertiaTensor, MassOrientation));
FPhysicsInterface::SetMassSpaceInertiaTensor_AssumesLocked(Actor, MassSpaceInertiaTensor);
FPhysicsInterface::SetMass_AssumesLocked(Actor, TotalMassProperties.mass);
FTransform Com(P2UQuat(MassOrientation), P2UVector(TotalMassProperties.centerOfMass));
FPhysicsInterface::SetComLocalPose_AssumesLocked(Actor, Com);
#endif
}
});
}
//Let anyone who cares about mass properties know they've been updated
if (BodyInstanceDelegates.IsValid())
{
BodyInstanceDelegates->OnRecalculatedMassProperties.Broadcast(this);
}
#endif
}namespace BodyUtils
{
/** Computes and adds the mass properties (inertia, com, etc...) based on the mass settings of the body instance. */
PxMassProperties ComputeMassProperties(const FBodyInstance* OwningBodyInstance, TArray<FPhysicsShapeHandle> Shapes, const FTransform& MassModifierTransform, const bool bUnused)
{
// physical material - nothing can weigh less than hydrogen (0.09 kg/m^3)
float DensityKGPerCubicUU = 1.0f;
float RaiseMassToPower = 0.75f;
if (UPhysicalMaterial* PhysMat = OwningBodyInstance->GetSimplePhysicalMaterial())
{
DensityKGPerCubicUU = FMath::Max(KgPerM3ToKgPerCm3(0.09f), gPerCm3ToKgPerCm3(PhysMat->Density));
RaiseMassToPower = PhysMat->RaiseMassToPower;
}
PxMassProperties MassProps;
FPhysicsInterface::CalculateMassPropertiesFromShapeCollection(MassProps, Shapes, DensityKGPerCubicUU);
float OldMass = MassProps.mass;
float NewMass = 0.f;
if (OwningBodyInstance->bOverrideMass == false)
{
float UsePow = FMath::Clamp<float>(RaiseMassToPower, KINDA_SMALL_NUMBER, 1.f);
NewMass = FMath::Pow(OldMass, UsePow);
// Apply user-defined mass scaling.
NewMass = FMath::Max(OwningBodyInstance->MassScale * NewMass, 0.001f); //min weight of 1g
}
else
{
NewMass = FMath::Max(OwningBodyInstance->GetMassOverride(), 0.001f); //min weight of 1g
}
check(NewMass > 0.f);
float MassRatio = NewMass / OldMass;
PxMassProperties FinalMassProps = MassProps * MassRatio;
FinalMassProps.centerOfMass += U2PVector(MassModifierTransform.TransformVector(OwningBodyInstance->COMNudge));
FinalMassProps.inertiaTensor = PxMassProperties::scaleInertia(FinalMassProps.inertiaTensor, PxQuat(PxIdentity), U2PVector(OwningBodyInstance->InertiaTensorScale));
return FinalMassProps;
}
#endif
}// #PHYS2 Want this gone eventually - need a better solution for mass properties
void FPhysicsInterface_PhysX::CalculateMassPropertiesFromShapeCollection(PxMassProperties& OutProperties, const TArray<FPhysicsShapeHandle>& InShapes, float InDensityKGPerCM)
{
TArray<PxShape*> PShapes;
PShapes.Reserve(InShapes.Num());
for(const FPhysicsShapeHandle& Shape : InShapes)
{
PShapes.Add(Shape.IsValid() ? Shape.Shape : nullptr);
}
OutProperties = PxRigidBodyExt::computeMassPropertiesFromShapes(PShapes.GetData(), PShapes.Num()) * InDensityKGPerCM;
}PhysX-3.4/ExtRigidBodyExt.cpp at master · NVIDIAGameWorks/PhysX-3.4 · GitHub
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);
}
PxMassProperties::PxMassProperties ( const PxGeometry & geometry ) [inline]
Compute mass properties based on a provided geometry structure.
This constructor assumes the geometry has a density of 1. Mass and inertia tensor scale linearly with density.
Parameters:
[in] geometry The geometry to compute the mass properties for. Supported geometry types are: sphere, box, capsule and convex mesh.
References PxConvexMeshGeometry::convexMesh, PxMat33::createDiagonal(), PxGeometryType::eBOX, PxGeometryType::eCAPSULE, PxGeometryType::eCONVEXMESH, PxGeometryType::eGEOMETRY_COUNT, PxGeometryType::eHEIGHTFIELD, PxGeometryType::eINVALID, PxGeometryType::ePLANE, PxGeometryType::eSPHERE, PxGeometryType::eTRIANGLEMESH, PxConvexMesh::getMassInformation(), PxGeometry::getType(), PxBoxGeometry::halfExtents, PxCapsuleGeometry::halfHeight, PxVec3::multiply(), PX_ASSERT, PxIsFinite(), PxPi, PxCapsuleGeometry::radius, PxSphereGeometry::radius, PxQuat::rotate(), PxQuat::rotateInv(), PxMeshScale::rotation, PxMeshScale::scale, PxConvexMeshGeometry::scale, PxVec3::x, PxVec3::y, and PxVec3::z. PxMassProperties(const PxGeometry& geometry)
{
switch (geometry.getType())
{
case PxGeometryType::eSPHERE:
{
const PxSphereGeometry& s = static_cast<const PxSphereGeometry&>(geometry);
mass = (4.0f / 3.0f) * PxPi * s.radius * s.radius * s.radius;
inertiaTensor = PxMat33::createDiagonal(PxVec3(2.0f / 5.0f * mass * s.radius * s.radius));
centerOfMass = PxVec3(0.0f);
}
break;
case PxGeometryType::eBOX:
{
const PxBoxGeometry& b = static_cast<const PxBoxGeometry&>(geometry);
mass = b.halfExtents.x * b.halfExtents.y * b.halfExtents.z * 8.0f;
PxVec3 d2 = b.halfExtents.multiply(b.halfExtents);
inertiaTensor = PxMat33::createDiagonal(PxVec3(d2.y + d2.z, d2.x + d2.z, d2.x + d2.y)) * (mass * 1.0f / 3.0f);
centerOfMass = PxVec3(0.0f);
}
break;
case PxGeometryType::eCAPSULE:
{
const PxCapsuleGeometry& c = static_cast<const PxCapsuleGeometry&>(geometry);
PxReal r = c.radius, h = c.halfHeight;
mass = ((4.0f / 3.0f) * r + 2 * c.halfHeight) * PxPi * r * r;
PxReal a = r*r*r * (8.0f / 15.0f) + h*r*r * (3.0f / 2.0f) + h*h*r * (4.0f / 3.0f) + h*h*h * (2.0f / 3.0f);
PxReal b = r*r*r * (8.0f / 15.0f) + h*r*r;
inertiaTensor = PxMat33::createDiagonal(PxVec3(b, a, a) * PxPi * r * r);
centerOfMass = PxVec3(0.0f);
}
break;
case PxGeometryType::eCONVEXMESH:
{
const PxConvexMeshGeometry& c = static_cast<const PxConvexMeshGeometry&>(geometry);
PxVec3 unscaledCoM;
PxMat33 unscaledInertiaTensorNonCOM; // inertia tensor of convex mesh in mesh local space
PxMat33 unscaledInertiaTensorCOM;
PxReal unscaledMass;
c.convexMesh->getMassInformation(unscaledMass, unscaledInertiaTensorNonCOM, unscaledCoM);
// inertia tensor relative to center of mass
unscaledInertiaTensorCOM[0][0] = unscaledInertiaTensorNonCOM[0][0] - unscaledMass*PxReal((unscaledCoM.y*unscaledCoM.y+unscaledCoM.z*unscaledCoM.z));
unscaledInertiaTensorCOM[1][1] = unscaledInertiaTensorNonCOM[1][1] - unscaledMass*PxReal((unscaledCoM.z*unscaledCoM.z+unscaledCoM.x*unscaledCoM.x));
unscaledInertiaTensorCOM[2][2] = unscaledInertiaTensorNonCOM[2][2] - unscaledMass*PxReal((unscaledCoM.x*unscaledCoM.x+unscaledCoM.y*unscaledCoM.y));
unscaledInertiaTensorCOM[0][1] = unscaledInertiaTensorCOM[1][0] = (unscaledInertiaTensorNonCOM[0][1] + unscaledMass*PxReal(unscaledCoM.x*unscaledCoM.y));
unscaledInertiaTensorCOM[1][2] = unscaledInertiaTensorCOM[2][1] = (unscaledInertiaTensorNonCOM[1][2] + unscaledMass*PxReal(unscaledCoM.y*unscaledCoM.z));
unscaledInertiaTensorCOM[0][2] = unscaledInertiaTensorCOM[2][0] = (unscaledInertiaTensorNonCOM[0][2] + unscaledMass*PxReal(unscaledCoM.z*unscaledCoM.x));
const PxMeshScale& s = c.scale;
mass = unscaledMass * s.scale.x * s.scale.y * s.scale.z;
centerOfMass = s.rotation.rotate(s.scale.multiply(s.rotation.rotateInv(unscaledCoM)));
inertiaTensor = scaleInertia(unscaledInertiaTensorCOM, s.rotation, s.scale);
}
break;
case PxGeometryType::eHEIGHTFIELD:
case PxGeometryType::ePLANE:
case PxGeometryType::eTRIANGLEMESH:
case PxGeometryType::eINVALID:
case PxGeometryType::eGEOMETRY_COUNT:
{
*this = PxMassProperties();
}
break;
}
PX_ASSERT(inertiaTensor.column0.isFinite() && inertiaTensor.column1.isFinite() && inertiaTensor.column2.isFinite());
PX_ASSERT(centerOfMass.isFinite());
PX_ASSERT(PxIsFinite(mass));
}void Gu::ConvexMesh::getMassInformation(PxReal& mass, PxMat33& localInertia, PxVec3& localCenterOfMass) const
{
mass = Gu::ConvexMesh::getMass();
localInertia = Gu::ConvexMesh::getInertia();
localCenterOfMass = Gu::ConvexMesh::getHull().mCenterOfMass;
}class ConvexMeshBuilder
{
public:
ConvexMeshBuilder(const bool buildGRBData);
~ConvexMeshBuilder();
// loads the computed or given convex hull from descriptor.
// the descriptor does contain polygons directly, triangles are not allowed
bool build(const PxConvexMeshDesc&, PxU32 gaussMapVertexLimit, bool validateOnly = false, ConvexHullLib* hullLib = NULL);
// save the convex mesh into stream
bool save(PxOutputStream& stream, bool platformMismatch) const;
// copy the convex mesh into internal convex mesh, which can be directly used then
bool copy(Gu::ConvexHullData& convexData, PxU32& nb);
// loads the convex mesh from given polygons
bool loadConvexHull(const PxConvexMeshDesc&, ConvexHullLib* hullLib);
// computed hull polygons from given triangles
bool computeHullPolygons(const PxU32& nbVerts,const PxVec3* verts, const PxU32& nbTriangles, const PxU32* triangles, PxAllocatorCallback& inAllocator,
PxU32& outNbVerts, PxVec3*& outVertices, PxU32& nbIndices, PxU32*& indices, PxU32& nbPolygons, PxHullPolygon*& polygons);
// compute big convex data
bool computeGaussMaps();
// compute mass, inertia tensor
void computeMassInfo(bool lowerPrecision);
// TEST_INTERNAL_OBJECTS
// internal objects
void computeInternalObjects();
//~TEST_INTERNAL_OBJECTS
// return computed mass
PxReal getMass() const { return mMass; }
// return computed inertia tensor
const PxMat33& getInertia() const { return mInertia; }
// return big convex data
BigConvexData* getBigConvexData() const { return mBigConvexData; }
// set big convex data
void setBigConvexData(BigConvexData* data) { mBigConvexData = data; }
mutable ConvexPolygonsBuilder hullBuilder;
protected:
Gu::ConvexHullData mHullData;
BigConvexData* mBigConvexData; //!< optional, only for large meshes! PT: redundant with ptr in chull data? Could also be end of other buffer
PxReal mMass; //this is mass assuming a unit density that can be scaled by instances!
PxMat33 mInertia; //in local space of mesh!
};
https://github.com/NVIDIAGameWorks/PhysX-3.4/blob/5e42a5f112351a223c19c17bb331e6c55037b8eb/PhysX_3.4/Source/PhysXCooking/src/convex/ConvexMeshBuilder.cppvoid ConvexMeshBuilder::computeMassInfo(bool lowerPrecision)
{
if(mMass <= 0.0f) //not yet computed.
{
PxIntegrals integrals;
PxConvexMeshDesc meshDesc;
meshDesc.points.count = mHullData.mNbHullVertices;
meshDesc.points.data = hullBuilder.mHullDataHullVertices;
meshDesc.points.stride = sizeof(PxVec3);
meshDesc.polygons.data = hullBuilder.mHullDataPolygons;
meshDesc.polygons.stride = sizeof(Gu::HullPolygonData);
meshDesc.polygons.count = hullBuilder.mHull->mNbPolygons;
meshDesc.indices.data = hullBuilder.mHullDataVertexData8;
// using the centroid of the convex for the volume integration solved accuracy issues in cases where the inertia tensor
// ended up close to not being positive definite and after a few further transforms the diagonalized inertia tensor ended
// up with negative values.
PxVec3 mean(0.0f);
for(PxU32 i=0; i < mHullData.mNbHullVertices; i++)
mean += hullBuilder.mHullDataHullVertices[i];
mean *= (1.0f / mHullData.mNbHullVertices);
bool status = lowerPrecision ?
computeVolumeIntegralsEberlySIMD(meshDesc, 1.0f, integrals, mean) : computeVolumeIntegralsEberly(meshDesc, 1.0f, integrals, mean);
if(status)
{
integrals.getOriginInertia(reinterpret_cast<PxMat33&>(mInertia));
mHullData.mCenterOfMass = integrals.COM;
//note: the mass will be negative for an inside-out mesh!
if(mInertia.column0.isFinite() && mInertia.column1.isFinite() && mInertia.column2.isFinite()
&& mHullData.mCenterOfMass.isFinite() && PxIsFinite(PxReal(integrals.mass)))
{
if (integrals.mass < 0)
{
Ps::getFoundation().error(PX_WARN, "Gu::ConvexMesh: Mesh has a negative volume! Is it open or do (some) faces have reversed winding? (Taking absolute value.)");
integrals.mass = -integrals.mass;
mInertia = -mInertia;
}
mMass = PxReal(integrals.mass); //set mass to valid value.
return;
}
}
Ps::getFoundation().error(PxErrorCode::eINTERNAL_ERROR, __FILE__, __LINE__, "Gu::ConvexMesh: Error computing mesh mass properties!\n");
}
} bool VolumeIntegratorEberly::computeVolumeIntegrals(PxIntegrals& ir, const PxVec3& origin)
{
const PxF64 mult[10] = {1.0/6.0,1.0/24.0,1.0/24.0,1.0/24.0,1.0/60.0,1.0/60.0,1.0/60.0,1.0/120.0,1.0/120.0,1.0/120.0};
PxF64 intg[10] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; // order: 1, x, y, z, x^2, y^2, z^2, xy, yz, zx
const PxVec3* hullVerts = static_cast<const PxVec3*> (mDesc.points.data);
for (PxU32 i = 0; i < mDesc.polygons.count; i++)
{
const Gu::HullPolygonData& polygon = (static_cast<const Gu::HullPolygonData*> (mDesc.polygons.data))[i];
const PxU8* Data = static_cast<const PxU8*>(mDesc.indices.data) + polygon.mVRef8;
const PxU32 NbVerts = polygon.mNbVerts;
for (PxU32 j = 0; j < NbVerts - 2; j++)
{
const PxVec3 p0 = hullVerts[Data[0]] - origin;
PxVec3 p1 = hullVerts[Data[(j + 1) % NbVerts]] - origin;
PxVec3 p2 = hullVerts[Data[(j + 2) % NbVerts]] - origin;
PxVec3 cp = (p1 - p0).cross(p2 - p0);
if(cp.dot(polygon.mPlane.n) < 0)
{
cp = -cp;
Ps::swap(p1,p2);
}
PxF64 x0 = PxF64(p0.x); PxF64 y0 = PxF64(p0.y); PxF64 z0 = PxF64(p0.z);
PxF64 x1 = PxF64(p1.x); PxF64 y1 = PxF64(p1.y); PxF64 z1 = PxF64(p1.z);
PxF64 x2 = PxF64(p2.x); PxF64 y2 = PxF64(p2.y); PxF64 z2 = PxF64(p2.z);
// get edges and cross product of edges
PxF64 d0 = PxF64(cp.x); PxF64 d1 = PxF64(cp.y); PxF64 d2 = PxF64(cp.z);
// compute integral terms
PxF64 f1x; PxF64 f2x; PxF64 f3x; PxF64 g0x; PxF64 g1x; PxF64 g2x;
PxF64 f1y; PxF64 f2y; PxF64 f3y; PxF64 g0y; PxF64 g1y; PxF64 g2y;
PxF64 f1z; PxF64 f2z; PxF64 f3z; PxF64 g0z; PxF64 g1z; PxF64 g2z;
subexpressions(x0, x1, x2, f1x, f2x, f3x, g0x, g1x, g2x);
subexpressions(y0, y1, y2, f1y, f2y, f3y, g0y, g1y, g2y);
subexpressions(z0, z1, z2, f1z, f2z, f3z, g0z, g1z, g2z);
// update integrals
intg[0] += d0*f1x;
intg[1] += d0*f2x; intg[2] += d1*f2y; intg[3] += d2*f2z;
intg[4] += d0*f3x; intg[5] += d1*f3y; intg[6] += d2*f3z;
intg[7] += d0*(y0*g0x + y1*g1x + y2*g2x);
intg[8] += d1*(z0*g0y + z1*g1y + z2*g2y);
intg[9] += d2*(x0*g0z + x1*g1z + x2*g2z);
}
}
for (PxU32 i = 0; i < 10; i++)
{
intg[i] *= mult[i];
}
ir.mass = mMass = intg[0];
// center of mass
ir.COM.x = PxReal(intg[1]/mMass);
ir.COM.y = PxReal(intg[2]/mMass);
ir.COM.z = PxReal(intg[3]/mMass);
// inertia tensor relative to the provided origin parameter
ir.inertiaTensor[0][0] = intg[5]+intg[6];
ir.inertiaTensor[1][1] = intg[4]+intg[6];
ir.inertiaTensor[2][2] = intg[4]+intg[5];
ir.inertiaTensor[0][1] = ir.inertiaTensor[1][0] = -intg[7];
ir.inertiaTensor[1][2] = ir.inertiaTensor[2][1] = -intg[8];
ir.inertiaTensor[0][2] = ir.inertiaTensor[2][0] = -intg[9];
// inertia tensor relative to center of mass
ir.COMInertiaTensor[0][0] = ir.inertiaTensor[0][0] -mMass*PxF64((ir.COM.y*ir.COM.y+ir.COM.z*ir.COM.z));
ir.COMInertiaTensor[1][1] = ir.inertiaTensor[1][1] -mMass*PxF64((ir.COM.z*ir.COM.z+ir.COM.x*ir.COM.x));
ir.COMInertiaTensor[2][2] = ir.inertiaTensor[2][2] -mMass*PxF64((ir.COM.x*ir.COM.x+ir.COM.y*ir.COM.y));
ir.COMInertiaTensor[0][1] = ir.COMInertiaTensor[1][0] = (ir.inertiaTensor[0][1] +mMass*PxF64(ir.COM.x*ir.COM.y));
ir.COMInertiaTensor[1][2] = ir.COMInertiaTensor[2][1] = (ir.inertiaTensor[1][2] +mMass*PxF64(ir.COM.y*ir.COM.z));
ir.COMInertiaTensor[0][2] = ir.COMInertiaTensor[2][0] = (ir.inertiaTensor[0][2] +mMass*PxF64(ir.COM.z*ir.COM.x));
// inertia tensor relative to (0,0,0)
if (!origin.isZero())
{
PxVec3 sum = ir.COM + origin;
ir.inertiaTensor[0][0] -= mMass*PxF64((ir.COM.y*ir.COM.y+ir.COM.z*ir.COM.z) - (sum.y*sum.y+sum.z*sum.z));
ir.inertiaTensor[1][1] -= mMass*PxF64((ir.COM.z*ir.COM.z+ir.COM.x*ir.COM.x) - (sum.z*sum.z+sum.x*sum.x));
ir.inertiaTensor[2][2] -= mMass*PxF64((ir.COM.x*ir.COM.x+ir.COM.y*ir.COM.y) - (sum.x*sum.x+sum.y*sum.y));
ir.inertiaTensor[0][1] = ir.inertiaTensor[1][0] = ir.inertiaTensor[0][1] + mMass*PxF64((ir.COM.x*ir.COM.y) - (sum.x*sum.y));
ir.inertiaTensor[1][2] = ir.inertiaTensor[2][1] = ir.inertiaTensor[1][2] + mMass*PxF64((ir.COM.y*ir.COM.z) - (sum.y*sum.z));
ir.inertiaTensor[0][2] = ir.inertiaTensor[2][0] = ir.inertiaTensor[0][2] + mMass*PxF64((ir.COM.z*ir.COM.x) - (sum.z*sum.x));
ir.COM = sum;
}
return true;
}
} // namespace
// Wrapper
bool computeVolumeIntegrals(const PxSimpleTriangleMesh& mesh, PxReal density, PxIntegrals& integrals)
{
VolumeIntegrator v(mesh, PxF64(density));
return v.computeVolumeIntegrals(integrals);
}
// Wrapper
bool computeVolumeIntegralsEberly(const PxConvexMeshDesc& mesh, PxReal density, PxIntegrals& integrals, const PxVec3& origin)
{
VolumeIntegratorEberly v(mesh, PxF64(density));
v.computeVolumeIntegrals(integrals, origin);
return true;
}