A fast 2D and 3D game engine — TypeScript SDK, C++/WebAssembly core, visual editor. Ship one project to web, desktop, WeChat MiniGames, playable ads, and native Android / iOS.
TypeScript 2d-game-engine3d3d-game-engineandroidcppecsgame-editorgame-enginegamedeviostypescriptwebassemblywebglwebgpuwechat-minigame
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// SPDX-FileCopyrightText: Copyright (c) 2008-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
// SPDX-License-Identifier: Apache-2.0
/*
* This code computes volume integrals needed to compute mass properties of polyhedral bodies.
* Based on public domain code by Brian Mirtich.
*/
#include "foundation/PxMemory.h"
#include "geometry/PxSimpleTriangleMesh.h"
#include "cooking/PxConvexMeshDesc.h"
#include "GuCookingVolumeIntegration.h"
#include "GuConvexMeshData.h"
#include "foundation/PxUtilities.h"
#include "foundation/PxVecMath.h"
// PT: code archeology: this initially came from ICE (IceVolumeIntegration.h/cpp). Consider revisiting.
namespace physx
{
using namespace aos;
namespace
{
class VolumeIntegrator
{
PX_NOCOPY(VolumeIntegrator)
public:
VolumeIntegrator(const PxSimpleTriangleMesh& mesh, PxF64 density) : mMass(0.0), mDensity(density), mMesh(mesh) {}
~VolumeIntegrator() {}
bool computeVolumeIntegrals(PxIntegrals& ir);
private:
struct Normal
{
PxVec3 normal;
PxF32 w;
};
struct Face
{
PxF64 Norm[3];
PxF64 w;
PxU32 Verts[3];
};
// Data structures
PxF64 mMass; //!< Mass
PxF64 mDensity; //!< Density
const PxSimpleTriangleMesh& mMesh;
PxU32 mA; //!< Alpha
PxU32 mB; //!< Beta
PxU32 mC; //!< Gamma
// Projection integrals
PxF64 mP1;
PxF64 mPa; //!< Pi Alpha
PxF64 mPb; //!< Pi Beta
PxF64 mPaa; //!< Pi Alpha^2
PxF64 mPab; //!< Pi AlphaBeta
PxF64 mPbb; //!< Pi Beta^2
PxF64 mPaaa; //!< Pi Alpha^3
PxF64 mPaab; //!< Pi Alpha^2Beta
PxF64 mPabb; //!< Pi AlphaBeta^2
PxF64 mPbbb; //!< Pi Beta^3
// Face integrals
PxF64 mFa; //!< FAlpha
PxF64 mFb; //!< FBeta
PxF64 mFc; //!< FGamma
PxF64 mFaa; //!< FAlpha^2
PxF64 mFbb; //!< FBeta^2
PxF64 mFcc; //!< FGamma^2
PxF64 mFaaa; //!< FAlpha^3
PxF64 mFbbb; //!< FBeta^3
PxF64 mFccc; //!< FGamma^3
PxF64 mFaab; //!< FAlpha^2Beta
PxF64 mFbbc; //!< FBeta^2Gamma
PxF64 mFcca; //!< FGamma^2Alpha
// The 10 volume integrals
PxF64 mT0; //!< ~Total mass
PxF64 mT1[3]; //!< Location of the center of mass
PxF64 mT2[3]; //!< Moments of inertia
PxF64 mTP[3]; //!< Products of inertia
// Internal methods
// bool Init();
PxVec3 computeCenterOfMass();
void computeInertiaTensor(PxF64* J);
void computeCOMInertiaTensor(PxF64* J);
void computeFaceNormal(Face & f, PxU32 * indices);
void computeProjectionIntegrals(const Face& f);
void computeFaceIntegrals(const Face& f);
};
#define X 0u
#define Y 1u
#define Z 2u
void VolumeIntegrator::computeFaceNormal(Face & f, PxU32 * indices)
{
const PxU8 * vertPointer = reinterpret_cast<const PxU8*>(mMesh.points.data);
const PxU32 stride = mMesh.points.stride;
//two edges
const PxVec3 d1 = (*reinterpret_cast<const PxVec3 *>(vertPointer + stride * indices[1] )) - (*reinterpret_cast<const PxVec3 *>(vertPointer + stride * indices[0] ));
const PxVec3 d2 = (*reinterpret_cast<const PxVec3 *>(vertPointer + stride * indices[2] )) - (*reinterpret_cast<const PxVec3 *>(vertPointer + stride * indices[1] ));
PxVec3 normal = d1.cross(d2);
normal.normalizeSafe();
f.w = - PxF64(normal.dot((*reinterpret_cast<const PxVec3 *>(vertPointer + stride * indices[0] )) ));
f.Norm[0] = PxF64(normal.x);
f.Norm[1] = PxF64(normal.y);
f.Norm[2] = PxF64(normal.z);
}
/**
* Computes volume integrals for a polyhedron by summing surface integrals over its faces.
* \param ir [out] a result structure.
* \return true if success
*/
bool VolumeIntegrator::computeVolumeIntegrals(PxIntegrals& ir)
{
// Clear all integrals
mT0 = mT1[X] = mT1[Y] = mT1[Z] = mT2[X] = mT2[Y] = mT2[Z] = mTP[X] = mTP[Y] = mTP[Z] = 0;
Face f;
const PxU8* trigPointer = reinterpret_cast<const PxU8*>(mMesh.triangles.data);
const PxU32 nbTris = mMesh.triangles.count;
const PxU32 stride = mMesh.triangles.stride;
for(PxU32 i=0;i<nbTris;i++, trigPointer += stride)
{
if (mMesh.flags & PxMeshFlag::e16_BIT_INDICES)
{
f.Verts[0] = (reinterpret_cast<const PxU16 *>(trigPointer))[0];
f.Verts[1] = (reinterpret_cast<const PxU16 *>(trigPointer))[1];
f.Verts[2] = (reinterpret_cast<const PxU16 *>(trigPointer))[2];
}
else
{
f.Verts[0] = (reinterpret_cast<const PxU32 *>(trigPointer)[0]);
f.Verts[1] = (reinterpret_cast<const PxU32 *>(trigPointer)[1]);
f.Verts[2] = (reinterpret_cast<const PxU32 *>(trigPointer)[2]);
}
if (mMesh.flags & PxMeshFlag::eFLIPNORMALS)
{
PxU32 t = f.Verts[1];
f.Verts[1] = f.Verts[2];
f.Verts[2] = t;
}
//compute face normal:
computeFaceNormal(f,f.Verts);
if(f.Norm[X] * f.Norm[X] + f.Norm[Y] * f.Norm[Y] + f.Norm[Z] * f.Norm[Z] < 1e-20)
continue;
// Compute alpha/beta/gamma as the right-handed permutation of (x,y,z) that maximizes |n|
const PxF64 nx = fabs(f.Norm[X]);
const PxF64 ny = fabs(f.Norm[Y]);
const PxF64 nz = fabs(f.Norm[Z]);
if (nx > ny && nx > nz) mC = X;
else mC = (ny > nz) ? Y : Z;
mA = (mC + 1) % 3;
mB = (mA + 1) % 3;
// Compute face contribution
computeFaceIntegrals(f);
// Update integrals
mT0 += f.Norm[X] * ((mA == X) ? mFa : ((mB == X) ? mFb : mFc));
mT1[mA] += f.Norm[mA] * mFaa;
mT1[mB] += f.Norm[mB] * mFbb;
mT1[mC] += f.Norm[mC] * mFcc;
mT2[mA] += f.Norm[mA] * mFaaa;
mT2[mB] += f.Norm[mB] * mFbbb;
mT2[mC] += f.Norm[mC] * mFccc;
mTP[mA] += f.Norm[mA] * mFaab;
mTP[mB] += f.Norm[mB] * mFbbc;
mTP[mC] += f.Norm[mC] * mFcca;
}
mT1[X] /= 2; mT1[Y] /= 2; mT1[Z] /= 2;
mT2[X] /= 3; mT2[Y] /= 3; mT2[Z] /= 3;
mTP[X] /= 2; mTP[Y] /= 2; mTP[Z] /= 2;
// Fill result structure
ir.COM = computeCenterOfMass();
computeInertiaTensor(reinterpret_cast<PxF64*>(ir.inertiaTensor));
computeCOMInertiaTensor(reinterpret_cast<PxF64*>(ir.COMInertiaTensor));
ir.mass = mMass;
return true;
}
/**
* Computes the center of mass.
* \return The center of mass.
*/
PxVec3 VolumeIntegrator::computeCenterOfMass()
{
// Compute center of mass
PxVec3 COM(0.0f);
if(mT0!=0.0)
{
COM.x = float(mT1[X] / mT0);
COM.y = float(mT1[Y] / mT0);
COM.z = float(mT1[Z] / mT0);
}
return COM;
}
/**
* Setups the inertia tensor relative to the origin.
* \param it [out] the returned inertia tensor.
*/
void VolumeIntegrator::computeInertiaTensor(PxF64* it)
{
PxF64 J[3][3];
// Compute inertia tensor
J[X][X] = mDensity * (mT2[Y] + mT2[Z]);
J[Y][Y] = mDensity * (mT2[Z] + mT2[X]);
J[Z][Z] = mDensity * (mT2[X] + mT2[Y]);
J[X][Y] = J[Y][X] = - mDensity * mTP[X];
J[Y][Z] = J[Z][Y] = - mDensity * mTP[Y];
J[Z][X] = J[X][Z] = - mDensity * mTP[Z];
PxMemCopy(it, J, 9*sizeof(PxF64));
}
/**
* Setups the inertia tensor relative to the COM.
* \param it [out] the returned inertia tensor.
*/
void VolumeIntegrator::computeCOMInertiaTensor(PxF64* it)
{
PxF64 J[3][3];
mMass = mDensity * mT0;
const PxVec3 COM = computeCenterOfMass();
const PxVec3 MassCOM(PxF32(mMass) * COM);
const PxVec3 MassCOM2(MassCOM.x * COM.x, MassCOM.y * COM.y, MassCOM.z * COM.z);
// Compute initial inertia tensor
computeInertiaTensor(reinterpret_cast<PxF64*>(J));
// Translate inertia tensor to center of mass
// Huyghens' theorem:
// Jx'x' = Jxx - m*(YG^2+ZG^2)
// Jy'y' = Jyy - m*(ZG^2+XG^2)
// Jz'z' = Jzz - m*(XG^2+YG^2)
// XG, YG, ZG = new origin
// YG^2+ZG^2 = dx^2
J[X][X] -= PxF64(MassCOM2.y + MassCOM2.z);
J[Y][Y] -= PxF64(MassCOM2.z + MassCOM2.x);
J[Z][Z] -= PxF64(MassCOM2.x + MassCOM2.y);
// Huyghens' theorem:
// Jx'y' = Jxy - m*XG*YG
// Jy'z' = Jyz - m*YG*ZG
// Jz'x' = Jzx - m*ZG*XG
// ### IS THE SIGN CORRECT ?
J[X][Y] = J[Y][X] += PxF64(MassCOM.x * COM.y);
J[Y][Z] = J[Z][Y] += PxF64(MassCOM.y * COM.z);
J[Z][X] = J[X][Z] += PxF64(MassCOM.z * COM.x);
PxMemCopy(it, J, 9*sizeof(PxF64));
}
/**
* Computes integrals over a face projection from the coordinates of the projections vertices.
* \param f [in] a face structure.
*/
void VolumeIntegrator::computeProjectionIntegrals(const Face& f)
{
mP1 = mPa = mPb = mPaa = mPab = mPbb = mPaaa = mPaab = mPabb = mPbbb = 0.0;
const PxU8* vertPointer = reinterpret_cast<const PxU8*>(mMesh.points.data);
const PxU32 stride = mMesh.points.stride;
for(PxU32 i=0;i<3;i++)
{
const PxVec3& p0 = *reinterpret_cast<const PxVec3 *>(vertPointer + stride * (f.Verts[i]) );
const PxVec3& p1 = *reinterpret_cast<const PxVec3 *>(vertPointer + stride * (f.Verts[(i+1) % 3]) );
const PxF64 a0 = PxF64(p0[mA]);
const PxF64 b0 = PxF64(p0[mB]);
const PxF64 a1 = PxF64(p1[mA]);
const PxF64 b1 = PxF64(p1[mB]);
const PxF64 da = a1 - a0; // DeltaA
const PxF64 db = b1 - b0; // DeltaB
const PxF64 a0_2 = a0 * a0; // Alpha0^2
const PxF64 a0_3 = a0_2 * a0; // ...
const PxF64 a0_4 = a0_3 * a0;
const PxF64 b0_2 = b0 * b0;
const PxF64 b0_3 = b0_2 * b0;
const PxF64 b0_4 = b0_3 * b0;
const PxF64 a1_2 = a1 * a1;
const PxF64 a1_3 = a1_2 * a1;
const PxF64 b1_2 = b1 * b1;
const PxF64 b1_3 = b1_2 * b1;
const PxF64 C1 = a1 + a0;
const PxF64 Ca = a1*C1 + a0_2;
const PxF64 Caa = a1*Ca + a0_3;
const PxF64 Caaa = a1*Caa + a0_4;
const PxF64 Cb = b1*(b1 + b0) + b0_2;
const PxF64 Cbb = b1*Cb + b0_3;
const PxF64 Cbbb = b1*Cbb + b0_4;
const PxF64 Cab = 3*a1_2 + 2*a1*a0 + a0_2;
const PxF64 Kab = a1_2 + 2*a1*a0 + 3*a0_2;
const PxF64 Caab = a0*Cab + 4*a1_3;
const PxF64 Kaab = a1*Kab + 4*a0_3;
const PxF64 Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3;
const PxF64 Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3;
mP1 += db*C1;
mPa += db*Ca;
mPaa += db*Caa;
mPaaa += db*Caaa;
mPb += da*Cb;
mPbb += da*Cbb;
mPbbb += da*Cbbb;
mPab += db*(b1*Cab + b0*Kab);
mPaab += db*(b1*Caab + b0*Kaab);
mPabb += da*(a1*Cabb + a0*Kabb);
}
mP1 /= 2.0;
mPa /= 6.0;
mPaa /= 12.0;
mPaaa /= 20.0;
mPb /= -6.0;
mPbb /= -12.0;
mPbbb /= -20.0;
mPab /= 24.0;
mPaab /= 60.0;
mPabb /= -60.0;
}
#define SQR(x) ((x)*(x)) //!< Returns x square
#define CUBE(x) ((x)*(x)*(x)) //!< Returns x cube
/**
* Computes surface integrals over a polyhedral face from the integrals over its projection.
* \param f [in] a face structure.
*/
void VolumeIntegrator::computeFaceIntegrals(const Face& f)
{
computeProjectionIntegrals(f);
const PxF64 w = f.w;
const PxF64* n = f.Norm;
const PxF64 k1 = 1 / n[mC];
const PxF64 k2 = k1 * k1;
const PxF64 k3 = k2 * k1;
const PxF64 k4 = k3 * k1;
mFa = k1 * mPa;
mFb = k1 * mPb;
mFc = -k2 * (n[mA]*mPa + n[mB]*mPb + w*mP1);
mFaa = k1 * mPaa;
mFbb = k1 * mPbb;
mFcc = k3 * (SQR(n[mA])*mPaa + 2*n[mA]*n[mB]*mPab + SQR(n[mB])*mPbb + w*(2*(n[mA]*mPa + n[mB]*mPb) + w*mP1));
mFaaa = k1 * mPaaa;
mFbbb = k1 * mPbbb;
mFccc = -k4 * (CUBE(n[mA])*mPaaa + 3*SQR(n[mA])*n[mB]*mPaab
+ 3*n[mA]*SQR(n[mB])*mPabb + CUBE(n[mB])*mPbbb
+ 3*w*(SQR(n[mA])*mPaa + 2*n[mA]*n[mB]*mPab + SQR(n[mB])*mPbb)
+ w*w*(3*(n[mA]*mPa + n[mB]*mPb) + w*mP1));
mFaab = k1 * mPaab;
mFbbc = -k2 * (n[mA]*mPabb + n[mB]*mPbbb + w*mPbb);
mFcca = k3 * (SQR(n[mA])*mPaaa + 2*n[mA]*n[mB]*mPaab + SQR(n[mB])*mPabb + w*(2*(n[mA]*mPaa + n[mB]*mPab) + w*mPa));
}
/*
* This code computes volume integrals needed to compute mass properties of polyhedral bodies.
* Based on public domain code by David Eberly.
*/
class VolumeIntegratorEberly
{
PX_NOCOPY(VolumeIntegratorEberly)
public:
VolumeIntegratorEberly(const PxConvexMeshDesc& desc, PxF64 density) : mDesc(desc), mMass(0), mMassR(0), mDensity(density) {}
~VolumeIntegratorEberly() {}
bool computeVolumeIntegralsSIMD(PxIntegrals& ir, const PxVec3& origin);
bool computeVolumeIntegrals(PxIntegrals& ir, const PxVec3& origin);
private:
const PxConvexMeshDesc& mDesc;
PxF64 mMass;
PxReal mMassR;
PxF64 mDensity;
};
PX_FORCE_INLINE void subexpressions(PxF64 w0, PxF64 w1, PxF64 w2, PxF64& f1, PxF64& f2, PxF64& f3, PxF64& g0, PxF64& g1, PxF64& g2)
{
PxF64 temp0 = w0 + w1;
f1 = temp0 + w2;
PxF64 temp1 = w0*w0;
PxF64 temp2 = temp1 + w1*temp0;
f2 = temp2 + w2*f1;
f3 = w0*temp1 + w1*temp2 + w2*f2;
g0 = f2 + w0*(f1 + w0);
g1 = f2 + w1*(f1 + w1);
g2 = f2 + w2*(f1 + w2);
}
PX_FORCE_INLINE void subexpressionsSIMD(const Vec4V& w0, const Vec4V& w1, const Vec4V& w2,
Vec4V& f1, Vec4V& f2, Vec4V& f3, Vec4V& g0, Vec4V& g1, Vec4V& g2)
{
const Vec4V temp0 = V4Add(w0, w1);
f1 = V4Add(temp0, w2);
const Vec4V temp1 = V4Mul(w0,w0);
const Vec4V temp2 = V4MulAdd(w1, temp0, temp1);
f2 = V4MulAdd(w2, f1, temp2);
// f3 = w0.multiply(temp1) + w1.multiply(temp2) + w2.multiply(f2);
const Vec4V ad0 = V4Mul(w0, temp1);
const Vec4V ad1 = V4MulAdd(w1, temp2, ad0);
f3 = V4MulAdd(w2, f2, ad1);
g0 = V4MulAdd(w0, V4Add(f1, w0), f2); // f2 + w0.multiply(f1 + w0);
g1 = V4MulAdd(w1, V4Add(f1, w1), f2); // f2 + w1.multiply(f1 + w1);
g2 = V4MulAdd(w2, V4Add(f1, w2), f2); // f2 + w2.multiply(f1 + w2);
}
/**
* Computes volume integrals for a polyhedron by summing surface integrals over its faces. SIMD version
* \param ir [out] a result structure.
* \param origin [in] the origin of the mesh vertices. All vertices will be shifted accordingly prior to computing the volume integrals.
Can improve accuracy, for example, if the centroid is used in the case of a convex mesh. Note: the returned inertia will not be relative to this origin but relative to (0,0,0).
* \return true if success
*/
bool VolumeIntegratorEberly::computeVolumeIntegralsSIMD(PxIntegrals& ir, const PxVec3& origin)
{
FloatV mult = FLoad(1.0f/6.0f);
const Vec4V multV = V4Load(1.0f/24.0f);
const Vec4V multV2 = V4Load(1.0f/60.0f);
const Vec4V multVV = V4Load(1.0f/120.0f);
// order: 1, x, y, z, x^2, y^2, z^2, xy, yz, zx
FloatV intg = FLoad(0.0f);
Vec4V intgV = V4Load(0.0f);
Vec4V intgV2 = V4Load(0.0f);
Vec4V intgVV = V4Load(0.0f);
const Vec4V originV = Vec4V_From_PxVec3_WUndefined(origin);
const FloatV zeroV = FLoad(0.0f);
const PxVec3* hullVerts = static_cast<const PxVec3*> (mDesc.points.data);
const Gu::HullPolygonData* hullPolygons = static_cast<const Gu::HullPolygonData*> (mDesc.polygons.data);
for (PxU32 i = 0; i < mDesc.polygons.count; i++)
{
const Gu::HullPolygonData& polygon = hullPolygons[i];
const PxU8* data = static_cast<const PxU8*>(mDesc.indices.data) + polygon.mVRef8;
const PxU32 nbVerts = polygon.mNbVerts;
PX_ASSERT(nbVerts > 2);
const Vec4V normalV = V4LoadU(&polygon.mPlane.n.x);
for (PxU32 j = 0; j < nbVerts - 2; j++)
{
// Should be safe to V4Load, we allocate one more vertex each time
const Vec4V vertex0 = V4LoadU(&hullVerts[data[0]].x);
const Vec4V vertex1 = V4LoadU(&hullVerts[data[j + 1]].x);
const Vec4V vertex2 = V4LoadU(&hullVerts[data[j + 2]].x);
const Vec4V p0 = V4Sub(vertex0, originV);
Vec4V p1 = V4Sub(vertex1, originV);
Vec4V p2 = V4Sub(vertex2, originV);
const Vec4V p0YZX = V4PermYZXW(p0);
const Vec4V p1YZX = V4PermYZXW(p1);
const Vec4V p2YZX = V4PermYZXW(p2);
// get edges and cross product of edges
Vec4V d = V4Cross(V4Sub(p1, p0), V4Sub(p2, p0)); // (p1 - p0).cross(p2 - p0);
const FloatV dist = V4Dot3(d, normalV);
//if(cp.dot(normalV) < 0)
if(FAllGrtr(zeroV, dist))
{
d = V4Neg(d);
Vec4V temp = p1;
p1 = p2;
p2 = temp;
}
// compute integral terms
Vec4V f1; Vec4V f2; Vec4V f3; Vec4V g0; Vec4V g1; Vec4V g2;
subexpressionsSIMD(p0, p1, p2, f1, f2, f3, g0, g1, g2);
// update integrals
intg = FScaleAdd(V4GetX(d), V4GetX(f1), intg); //intg += d.x*f1.x;
intgV = V4MulAdd(d, f2, intgV); // intgV +=d.multiply(f2);
intgV2 = V4MulAdd(d, f3, intgV2); // intgV2 += d.multiply(f3);
const Vec4V ad0 = V4Mul(p0YZX, g0);
const Vec4V ad1 = V4MulAdd(p1YZX, g1, ad0);
const Vec4V ad2 = V4MulAdd(p2YZX, g2, ad1);
intgVV = V4MulAdd(d, ad2, intgVV); //intgVV += d.multiply(p0YZX.multiply(g0) + p1YZX.multiply(g1) + p2YZX.multiply(g2));
}
}
intg = FMul(intg, mult); // intg *= mult;
intgV = V4Mul(intgV, multV);
intgV2 = V4Mul(intgV2, multV2);
intgVV = V4Mul(intgVV, multVV);
// center of mass ir.COM = intgV/mMassR;
const Vec4V comV = V4ScaleInv(intgV, intg);
// we rewrite the mass, but then we set it back
V4StoreU(comV, &ir.COM.x);
FStore(intg, &mMassR);
ir.mass = PxF64(mMassR); // = intg;
PxVec3 intg2;
V3StoreU(Vec3V_From_Vec4V(intgV2), intg2);
PxVec3 intVV;
V3StoreU(Vec3V_From_Vec4V(intgVV), intVV);
// inertia tensor relative to the provided origin parameter
ir.inertiaTensor[0][0] = PxF64(intg2.y + intg2.z);
ir.inertiaTensor[1][1] = PxF64(intg2.x + intg2.z);
ir.inertiaTensor[2][2] = PxF64(intg2.x + intg2.y);
ir.inertiaTensor[0][1] = ir.inertiaTensor[1][0] = PxF64(-intVV.x);
ir.inertiaTensor[1][2] = ir.inertiaTensor[2][1] = PxF64(-intVV.y);
ir.inertiaTensor[0][2] = ir.inertiaTensor[2][0] = PxF64(-intVV.z);
// inertia tensor relative to center of mass
ir.COMInertiaTensor[0][0] = ir.inertiaTensor[0][0] -PxF64(mMassR*(ir.COM.y*ir.COM.y+ir.COM.z*ir.COM.z));
ir.COMInertiaTensor[1][1] = ir.inertiaTensor[1][1] -PxF64(mMassR*(ir.COM.z*ir.COM.z+ir.COM.x*ir.COM.x));
ir.COMInertiaTensor[2][2] = ir.inertiaTensor[2][2] -PxF64(mMassR*(ir.COM.x*ir.COM.x+ir.COM.y*ir.COM.y));
ir.COMInertiaTensor[0][1] = ir.COMInertiaTensor[1][0] = (ir.inertiaTensor[0][1] +PxF64(mMassR*ir.COM.x*ir.COM.y));
ir.COMInertiaTensor[1][2] = ir.COMInertiaTensor[2][1] = (ir.inertiaTensor[1][2] +PxF64(mMassR*ir.COM.y*ir.COM.z));
ir.COMInertiaTensor[0][2] = ir.COMInertiaTensor[2][0] = (ir.inertiaTensor[0][2] +PxF64(mMassR*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] -= PxF64(mMassR*((ir.COM.y*ir.COM.y+ir.COM.z*ir.COM.z) - (sum.y*sum.y+sum.z*sum.z)));
ir.inertiaTensor[1][1] -= PxF64(mMassR*((ir.COM.z*ir.COM.z+ir.COM.x*ir.COM.x) - (sum.z*sum.z+sum.x*sum.x)));
ir.inertiaTensor[2][2] -= PxF64(mMassR*((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] + PxF64(mMassR*((ir.COM.x*ir.COM.y) - (sum.x*sum.y)));
ir.inertiaTensor[1][2] = ir.inertiaTensor[2][1] = ir.inertiaTensor[1][2] + PxF64(mMassR*((ir.COM.y*ir.COM.z) - (sum.y*sum.z)));
ir.inertiaTensor[0][2] = ir.inertiaTensor[2][0] = ir.inertiaTensor[0][2] + PxF64(mMassR*((ir.COM.z*ir.COM.x) - (sum.z*sum.x)));
ir.COM = sum;
}
return true;
}
/**
* Computes volume integrals for a polyhedron by summing surface integrals over its faces.
* \param ir [out] a result structure.
* \param origin [in] the origin of the mesh vertices. All vertices will be shifted accordingly prior to computing the volume integrals.
Can improve accuracy, for example, if the centroid is used in the case of a convex mesh. Note: the returned inertia will not be relative to this origin but relative to (0,0,0).
* \return true if success
*/
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;
PxSwap(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, bool useSimd)
{
VolumeIntegratorEberly v(mesh, PxF64(density));
if(useSimd)
return v.computeVolumeIntegralsSIMD(integrals, origin);
else
return v.computeVolumeIntegrals(integrals, origin);
}
}