NVIDIA(R) PhysX(R) SDK 3.4 API Reference: PxMassProperties.h Source File

00001 // This code contains NVIDIA Confidential Information and is disclosed to you
+00002 // under a form of NVIDIA software license agreement provided separately to you.
+00003 //
+00004 // Notice
+00005 // NVIDIA Corporation and its licensors retain all intellectual property and
+00006 // proprietary rights in and to this software and related documentation and
+00007 // any modifications thereto. Any use, reproduction, disclosure, or
+00008 // distribution of this software and related documentation without an express
+00009 // license agreement from NVIDIA Corporation is strictly prohibited.
+00010 //
+00011 // ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+00012 // NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+00013 // THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+00014 // MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+00015 //
+00016 // Information and code furnished is believed to be accurate and reliable.
+00017 // However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+00018 // information or for any infringement of patents or other rights of third parties that may
+00019 // result from its use. No license is granted by implication or otherwise under any patent
+00020 // or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+00021 // This code supersedes and replaces all information previously supplied.
+00022 // NVIDIA Corporation products are not authorized for use as critical
+00023 // components in life support devices or systems without express written approval of
+00024 // NVIDIA Corporation.
+00025 //
+00026 // Copyright (c) 2008-2016 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 * 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); // rotate inertia into scaling frame
+00252         PxVec3 diagonal(localInertiaT[0][0], localInertiaT[1][1], localInertiaT[2][2]);
+00253 
+00254         PxVec3 xyz2 = PxVec3(diagonal.dot(PxVec3(0.5f))) - diagonal; // original x^2, y^2, z^2
+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 } // namespace physx
+00322 #endif
+00323 
+00325 #endif
+