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 * 1.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 unscaledInertiaTensorNonCOM;
00124 PxMat33 unscaledInertiaTensorCOM;
00125 PxReal unscaledMass;
00126 c.convexMesh->getMassInformation(unscaledMass, unscaledInertiaTensorNonCOM, unscaledCoM);
00127
00128
00129 unscaledInertiaTensorCOM[0][0] = unscaledInertiaTensorNonCOM[0][0] - unscaledMass*PxReal((unscaledCoM.y*unscaledCoM.y+unscaledCoM.z*unscaledCoM.z));
00130 unscaledInertiaTensorCOM[1][1] = unscaledInertiaTensorNonCOM[1][1] - unscaledMass*PxReal((unscaledCoM.z*unscaledCoM.z+unscaledCoM.x*unscaledCoM.x));
00131 unscaledInertiaTensorCOM[2][2] = unscaledInertiaTensorNonCOM[2][2] - unscaledMass*PxReal((unscaledCoM.x*unscaledCoM.x+unscaledCoM.y*unscaledCoM.y));
00132 unscaledInertiaTensorCOM[0][1] = unscaledInertiaTensorCOM[1][0] = (unscaledInertiaTensorNonCOM[0][1] + unscaledMass*PxReal(unscaledCoM.x*unscaledCoM.y));
00133 unscaledInertiaTensorCOM[1][2] = unscaledInertiaTensorCOM[2][1] = (unscaledInertiaTensorNonCOM[1][2] + unscaledMass*PxReal(unscaledCoM.y*unscaledCoM.z));
00134 unscaledInertiaTensorCOM[0][2] = unscaledInertiaTensorCOM[2][0] = (unscaledInertiaTensorNonCOM[0][2] + unscaledMass*PxReal(unscaledCoM.z*unscaledCoM.x));
00135
00136 const PxMeshScale& s = c.scale;
00137 mass = unscaledMass * s.scale.x * s.scale.y * s.scale.z;
00138 centerOfMass = s.rotation.rotate(s.scale.multiply(s.rotation.rotateInv(unscaledCoM)));
00139 inertiaTensor = scaleInertia(unscaledInertiaTensorCOM, s.rotation, s.scale);
00140 }
00141 break;
00142
00143 case PxGeometryType::eHEIGHTFIELD:
00144 case PxGeometryType::ePLANE:
00145 case PxGeometryType::eTRIANGLEMESH:
00146 case PxGeometryType::eINVALID:
00147 case PxGeometryType::eGEOMETRY_COUNT:
00148 {
00149 *this = PxMassProperties();
00150 }
00151 break;
00152 }
00153
00154 PX_ASSERT(inertiaTensor.column0.isFinite() && inertiaTensor.column1.isFinite() && inertiaTensor.column2.isFinite());
00155 PX_ASSERT(centerOfMass.isFinite());
00156 PX_ASSERT(PxIsFinite(mass));
00157 }
00158
00165 PX_FORCE_INLINE PxMassProperties operator*(const PxReal scale) const
00166 {
00167 PX_ASSERT(PxIsFinite(scale));
00168
00169 return PxMassProperties(mass * scale, inertiaTensor * scale, centerOfMass);
00170 }
00171
00177 PX_FORCE_INLINE void translate(const PxVec3& t)
00178 {
00179 PX_ASSERT(t.isFinite());
00180
00181 inertiaTensor = translateInertia(inertiaTensor, mass, t);
00182 centerOfMass += t;
00183
00184 PX_ASSERT(inertiaTensor.column0.isFinite() && inertiaTensor.column1.isFinite() && inertiaTensor.column2.isFinite());
00185 PX_ASSERT(centerOfMass.isFinite());
00186 }
00187
00195 PX_FORCE_INLINE static PxVec3 getMassSpaceInertia(const PxMat33& inertia, PxQuat& massFrame)
00196 {
00197 PX_ASSERT(inertia.column0.isFinite() && inertia.column1.isFinite() && inertia.column2.isFinite());
00198
00199 PxVec3 diagT = PxDiagonalize(inertia, massFrame);
00200 PX_ASSERT(diagT.isFinite());
00201 PX_ASSERT(massFrame.isFinite());
00202 return diagT;
00203 }
00204
00213 PX_FORCE_INLINE static PxMat33 translateInertia(const PxMat33& inertia, const PxReal mass, const PxVec3& t)
00214 {
00215 PX_ASSERT(inertia.column0.isFinite() && inertia.column1.isFinite() && inertia.column2.isFinite());
00216 PX_ASSERT(PxIsFinite(mass));
00217 PX_ASSERT(t.isFinite());
00218
00219 PxMat33 s( PxVec3(0,t.z,-t.y),
00220 PxVec3(-t.z,0,t.x),
00221 PxVec3(t.y,-t.x,0) );
00222
00223 PxMat33 translatedIT = s.getTranspose() * s * mass + inertia;
00224 PX_ASSERT(translatedIT.column0.isFinite() && translatedIT.column1.isFinite() && translatedIT.column2.isFinite());
00225 return translatedIT;
00226 }
00227
00235 PX_FORCE_INLINE static PxMat33 rotateInertia(const PxMat33& inertia, const PxQuat& q)
00236 {
00237 PX_ASSERT(inertia.column0.isFinite() && inertia.column1.isFinite() && inertia.column2.isFinite());
00238 PX_ASSERT(q.isUnit());
00239
00240 PxMat33 m(q);
00241 PxMat33 rotatedIT = m * inertia * m.getTranspose();
00242 PX_ASSERT(rotatedIT.column0.isFinite() && rotatedIT.column1.isFinite() && rotatedIT.column2.isFinite());
00243 return rotatedIT;
00244 }
00245
00254 static PxMat33 scaleInertia(const PxMat33& inertia, const PxQuat& scaleRotation, const PxVec3& scale)
00255 {
00256 PX_ASSERT(inertia.column0.isFinite() && inertia.column1.isFinite() && inertia.column2.isFinite());
00257 PX_ASSERT(scaleRotation.isUnit());
00258 PX_ASSERT(scale.isFinite());
00259
00260 PxMat33 localInertiaT = rotateInertia(inertia, scaleRotation);
00261 PxVec3 diagonal(localInertiaT[0][0], localInertiaT[1][1], localInertiaT[2][2]);
00262
00263 PxVec3 xyz2 = PxVec3(diagonal.dot(PxVec3(0.5f))) - diagonal;
00264 PxVec3 scaledxyz2 = xyz2.multiply(scale).multiply(scale);
00265
00266 PxReal xx = scaledxyz2.y + scaledxyz2.z,
00267 yy = scaledxyz2.z + scaledxyz2.x,
00268 zz = scaledxyz2.x + scaledxyz2.y;
00269
00270 PxReal xy = localInertiaT[0][1] * scale.x * scale.y,
00271 xz = localInertiaT[0][2] * scale.x * scale.z,
00272 yz = localInertiaT[1][2] * scale.y * scale.z;
00273
00274 PxMat33 scaledInertia( PxVec3(xx, xy, xz),
00275 PxVec3(xy, yy, yz),
00276 PxVec3(xz, yz, zz));
00277
00278 PxMat33 scaledIT = rotateInertia(scaledInertia * (scale.x * scale.y * scale.z), scaleRotation.getConjugate());
00279 PX_ASSERT(scaledIT.column0.isFinite() && scaledIT.column1.isFinite() && scaledIT.column2.isFinite());
00280 return scaledIT;
00281 }
00282
00291 static PxMassProperties sum(const PxMassProperties* props, const PxTransform* transforms, const PxU32 count)
00292 {
00293 PxReal combinedMass = 0.0f;
00294 PxVec3 combinedCoM(0.0f);
00295 PxMat33 combinedInertiaT = PxMat33(PxZero);
00296
00297 for(PxU32 i = 0; i < count; i++)
00298 {
00299 PX_ASSERT(props[i].inertiaTensor.column0.isFinite() && props[i].inertiaTensor.column1.isFinite() && props[i].inertiaTensor.column2.isFinite());
00300 PX_ASSERT(props[i].centerOfMass.isFinite());
00301 PX_ASSERT(PxIsFinite(props[i].mass));
00302
00303 combinedMass += props[i].mass;
00304 const PxVec3 comTm = transforms[i].transform(props[i].centerOfMass);
00305 combinedCoM += comTm * props[i].mass;
00306 }
00307
00308 combinedCoM /= combinedMass;
00309
00310 for(PxU32 i = 0; i < count; i++)
00311 {
00312 const PxVec3 comTm = transforms[i].transform(props[i].centerOfMass);
00313 combinedInertiaT += translateInertia(rotateInertia(props[i].inertiaTensor, transforms[i].q), props[i].mass, combinedCoM - comTm);
00314 }
00315
00316 PX_ASSERT(combinedInertiaT.column0.isFinite() && combinedInertiaT.column1.isFinite() && combinedInertiaT.column2.isFinite());
00317 PX_ASSERT(combinedCoM.isFinite());
00318 PX_ASSERT(PxIsFinite(combinedMass));
00319
00320 return PxMassProperties(combinedMass, combinedInertiaT, combinedCoM);
00321 }
00322
00323
00324 PxMat33 inertiaTensor;
00325 PxVec3 centerOfMass;
00326 PxReal mass;
00327 };
00328
00329 #if !PX_DOXYGEN
00330 }
00331 #endif
00332
00334 #endif
