PxMassProperties.h

Go to the documentation of this file.
00001 //
00002 // Redistribution and use in source and binary forms, with or without
00003 // modification, are permitted provided that the following conditions
00004 // are met:
00005 //  * Redistributions of source code must retain the above copyright
00006 //    notice, this list of conditions and the following disclaimer.
00007 //  * Redistributions in binary form must reproduce the above copyright
00008 //    notice, this list of conditions and the following disclaimer in the
00009 //    documentation and/or other materials provided with the distribution.
00010 //  * Neither the name of NVIDIA CORPORATION nor the names of its
00011 //    contributors may be used to endorse or promote products derived
00012 //    from this software without specific prior written permission.
00013 //
00014 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY
00015 // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00016 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
00017 // PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
00018 // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
00019 // EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
00020 // PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
00021 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
00022 // OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00023 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00024 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00025 //
00026 // Copyright (c) 2008-2018 NVIDIA Corporation. All rights reserved.
00027 // Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
00028 // Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
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; // inertia tensor of convex mesh in mesh local space
00124                 PxMat33 unscaledInertiaTensorCOM;
00125                 PxReal unscaledMass;
00126                 c.convexMesh->getMassInformation(unscaledMass, unscaledInertiaTensorNonCOM, unscaledCoM);               
00127 
00128                 // inertia tensor relative to center of mass
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); // rotate inertia into scaling frame
00261         PxVec3 diagonal(localInertiaT[0][0], localInertiaT[1][1], localInertiaT[2][2]);
00262 
00263         PxVec3 xyz2 = PxVec3(diagonal.dot(PxVec3(0.5f))) - diagonal; // original x^2, y^2, z^2
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 } // namespace physx
00331 #endif
00332 
00334 #endif


Copyright © 2008-2018 NVIDIA Corporation, 2701 San Tomas Expressway, Santa Clara, CA 95050 U.S.A. All rights reserved. www.nvidia.com