00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #ifndef PX_PHYSICS_EXTENSIONS_MASS_PROPERTIES_H
00032 #define PX_PHYSICS_EXTENSIONS_MASS_PROPERTIES_H
00033
00037 #include "PxPhysXConfig.h"
00038 #include "foundation/PxMath.h"
00039 #include "foundation/PxMathUtils.h"
00040 #include "foundation/PxVec3.h"
00041 #include "foundation/PxMat33.h"
00042 #include "foundation/PxQuat.h"
00043 #include "foundation/PxTransform.h"
00044 #include "geometry/PxGeometry.h"
00045 #include "geometry/PxBoxGeometry.h"
00046 #include "geometry/PxSphereGeometry.h"
00047 #include "geometry/PxCapsuleGeometry.h"
00048 #include "geometry/PxConvexMeshGeometry.h"
00049 #include "geometry/PxConvexMesh.h"
00050
00051 #if !PX_DOXYGEN
00052 namespace physx
00053 {
00054 #endif
00055
00063 class PxMassProperties
00064 {
00065 public:
00069 PX_FORCE_INLINE PxMassProperties() : inertiaTensor(PxIdentity), centerOfMass(0.0f), mass(1.0f) {}
00070
00074 PX_FORCE_INLINE PxMassProperties(const PxReal m, const PxMat33& inertiaT, const PxVec3& com) : inertiaTensor(inertiaT), centerOfMass(com), mass(m) {}
00075
00083 PxMassProperties(const PxGeometry& geometry)
00084 {
00085 switch (geometry.getType())
00086 {
00087 case PxGeometryType::eSPHERE:
00088 {
00089 const PxSphereGeometry& s = static_cast<const PxSphereGeometry&>(geometry);
00090 mass = (4.0f / 3.0f) * PxPi * s.radius * s.radius * s.radius;
00091 inertiaTensor = PxMat33::createDiagonal(PxVec3(2.0f / 5.0f * mass * s.radius * s.radius));
00092 centerOfMass = PxVec3(0.0f);
00093 }
00094 break;
00095
00096 case PxGeometryType::eBOX:
00097 {
00098 const PxBoxGeometry& b = static_cast<const PxBoxGeometry&>(geometry);
00099 mass = b.halfExtents.x * b.halfExtents.y * b.halfExtents.z * 8.0f;
00100 PxVec3 d2 = b.halfExtents.multiply(b.halfExtents);
00101 inertiaTensor = PxMat33::createDiagonal(PxVec3(d2.y + d2.z, d2.x + d2.z, d2.x + d2.y)) * (mass * 2.0f / 3.0f);
00102 centerOfMass = PxVec3(0.0f);
00103 }
00104 break;
00105
00106 case PxGeometryType::eCAPSULE:
00107 {
00108 const PxCapsuleGeometry& c = static_cast<const PxCapsuleGeometry&>(geometry);
00109 PxReal r = c.radius, h = c.halfHeight;
00110 mass = ((4.0f / 3.0f) * r + 2 * c.halfHeight) * PxPi * r * r;
00111
00112 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);
00113 PxReal b = r*r*r * (8.0f / 15.0f) + h*r*r;
00114 inertiaTensor = PxMat33::createDiagonal(PxVec3(b, a, a) * PxPi * r * r);
00115 centerOfMass = PxVec3(0.0f);
00116 }
00117 break;
00118
00119 case PxGeometryType::eCONVEXMESH:
00120 {
00121 const PxConvexMeshGeometry& c = static_cast<const PxConvexMeshGeometry&>(geometry);
00122 PxVec3 unscaledCoM;
00123 PxMat33 unscaledInertiaTensor;
00124 PxReal unscaledMass;
00125 c.convexMesh->getMassInformation(unscaledMass, unscaledInertiaTensor, unscaledCoM);
00126
00127 const PxMeshScale& s = c.scale;
00128 mass = unscaledMass * s.scale.x * s.scale.y * s.scale.z;
00129 centerOfMass = s.rotation.rotate(s.scale.multiply(s.rotation.rotateInv(unscaledCoM)));
00130 inertiaTensor = scaleInertia(unscaledInertiaTensor, s.rotation, s.scale);
00131 }
00132 break;
00133
00134 case PxGeometryType::eHEIGHTFIELD:
00135 case PxGeometryType::ePLANE:
00136 case PxGeometryType::eTRIANGLEMESH:
00137 case PxGeometryType::eINVALID:
00138 case PxGeometryType::eGEOMETRY_COUNT:
00139 {
00140 *this = PxMassProperties();
00141 }
00142 break;
00143 }
00144
00145 PX_ASSERT(inertiaTensor.column0.isFinite() && inertiaTensor.column1.isFinite() && inertiaTensor.column2.isFinite());
00146 PX_ASSERT(centerOfMass.isFinite());
00147 PX_ASSERT(PxIsFinite(mass));
00148 }
00149
00156 PX_FORCE_INLINE PxMassProperties operator*(const PxReal scale) const
00157 {
00158 PX_ASSERT(PxIsFinite(scale));
00159
00160 return PxMassProperties(mass * scale, inertiaTensor * scale, centerOfMass);
00161 }
00162
00168 PX_FORCE_INLINE void translate(const PxVec3& t)
00169 {
00170 PX_ASSERT(t.isFinite());
00171
00172 inertiaTensor = translateInertia(inertiaTensor, mass, t);
00173 centerOfMass += t;
00174
00175 PX_ASSERT(inertiaTensor.column0.isFinite() && inertiaTensor.column1.isFinite() && inertiaTensor.column2.isFinite());
00176 PX_ASSERT(centerOfMass.isFinite());
00177 }
00178
00186 PX_FORCE_INLINE static PxVec3 getMassSpaceInertia(const PxMat33& inertia, PxQuat& massFrame)
00187 {
00188 PX_ASSERT(inertia.column0.isFinite() && inertia.column1.isFinite() && inertia.column2.isFinite());
00189
00190 PxVec3 diagT = PxDiagonalize(inertia, massFrame);
00191 PX_ASSERT(diagT.isFinite());
00192 PX_ASSERT(massFrame.isFinite());
00193 return diagT;
00194 }
00195
00204 PX_FORCE_INLINE static PxMat33 translateInertia(const PxMat33& inertia, const PxReal mass, const PxVec3& t)
00205 {
00206 PX_ASSERT(inertia.column0.isFinite() && inertia.column1.isFinite() && inertia.column2.isFinite());
00207 PX_ASSERT(PxIsFinite(mass));
00208 PX_ASSERT(t.isFinite());
00209
00210 PxMat33 s( PxVec3(0,t.z,-t.y),
00211 PxVec3(-t.z,0,t.x),
00212 PxVec3(t.y,-t.x,0) );
00213
00214 PxMat33 translatedIT = s.getTranspose() * s * mass + inertia;
00215 PX_ASSERT(translatedIT.column0.isFinite() && translatedIT.column1.isFinite() && translatedIT.column2.isFinite());
00216 return translatedIT;
00217 }
00218
00226 PX_FORCE_INLINE static PxMat33 rotateInertia(const PxMat33& inertia, const PxQuat& q)
00227 {
00228 PX_ASSERT(inertia.column0.isFinite() && inertia.column1.isFinite() && inertia.column2.isFinite());
00229 PX_ASSERT(q.isUnit());
00230
00231 PxMat33 m(q);
00232 PxMat33 rotatedIT = m * inertia * m.getTranspose();
00233 PX_ASSERT(rotatedIT.column0.isFinite() && rotatedIT.column1.isFinite() && rotatedIT.column2.isFinite());
00234 return rotatedIT;
00235 }
00236
00245 static PxMat33 scaleInertia(const PxMat33& inertia, const PxQuat& scaleRotation, const PxVec3& scale)
00246 {
00247 PX_ASSERT(inertia.column0.isFinite() && inertia.column1.isFinite() && inertia.column2.isFinite());
00248 PX_ASSERT(scaleRotation.isUnit());
00249 PX_ASSERT(scale.isFinite());
00250
00251 PxMat33 localInertiaT = rotateInertia(inertia, scaleRotation);
00252 PxVec3 diagonal(localInertiaT[0][0], localInertiaT[1][1], localInertiaT[2][2]);
00253
00254 PxVec3 xyz2 = PxVec3(diagonal.dot(PxVec3(0.5f))) - diagonal;
00255 PxVec3 scaledxyz2 = xyz2.multiply(scale).multiply(scale);
00256
00257 PxReal xx = scaledxyz2.y + scaledxyz2.z,
00258 yy = scaledxyz2.z + scaledxyz2.x,
00259 zz = scaledxyz2.x + scaledxyz2.y;
00260
00261 PxReal xy = localInertiaT[0][1] * scale.x * scale.y,
00262 xz = localInertiaT[0][2] * scale.x * scale.z,
00263 yz = localInertiaT[1][2] * scale.y * scale.z;
00264
00265 PxMat33 scaledInertia( PxVec3(xx, xy, xz),
00266 PxVec3(xy, yy, yz),
00267 PxVec3(xz, yz, zz));
00268
00269 PxMat33 scaledIT = rotateInertia(scaledInertia * (scale.x * scale.y * scale.z), scaleRotation.getConjugate());
00270 PX_ASSERT(scaledIT.column0.isFinite() && scaledIT.column1.isFinite() && scaledIT.column2.isFinite());
00271 return scaledIT;
00272 }
00273
00282 static PxMassProperties sum(const PxMassProperties* props, const PxTransform* transforms, const PxU32 count)
00283 {
00284 PxReal combinedMass = 0.0f;
00285 PxVec3 combinedCoM(0.0f);
00286 PxMat33 combinedInertiaT = PxMat33(PxZero);
00287
00288 for(PxU32 i = 0; i < count; i++)
00289 {
00290 PX_ASSERT(props[i].inertiaTensor.column0.isFinite() && props[i].inertiaTensor.column1.isFinite() && props[i].inertiaTensor.column2.isFinite());
00291 PX_ASSERT(props[i].centerOfMass.isFinite());
00292 PX_ASSERT(PxIsFinite(props[i].mass));
00293
00294 combinedMass += props[i].mass;
00295 const PxVec3 comTm = transforms[i].transform(props[i].centerOfMass);
00296 combinedCoM += comTm * props[i].mass;
00297 }
00298
00299 combinedCoM /= combinedMass;
00300
00301 for(PxU32 i = 0; i < count; i++)
00302 {
00303 const PxVec3 comTm = transforms[i].transform(props[i].centerOfMass);
00304 combinedInertiaT += translateInertia(rotateInertia(props[i].inertiaTensor, transforms[i].q), props[i].mass, combinedCoM - comTm);
00305 }
00306
00307 PX_ASSERT(combinedInertiaT.column0.isFinite() && combinedInertiaT.column1.isFinite() && combinedInertiaT.column2.isFinite());
00308 PX_ASSERT(combinedCoM.isFinite());
00309 PX_ASSERT(PxIsFinite(combinedMass));
00310
00311 return PxMassProperties(combinedMass, combinedInertiaT, combinedCoM);
00312 }
00313
00314
00315 PxMat33 inertiaTensor;
00316 PxVec3 centerOfMass;
00317 PxReal mass;
00318 };
00319
00320 #if !PX_DOXYGEN
00321 }
00322 #endif
00323
00325 #endif
