PxMassProperties.h
Go to the documentation of this 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-2017 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-2017 NVIDIA Corporation, 2701 San Tomas Expressway, Santa Clara, CA 95050 U.S.A. All rights reserved. www.nvidia.com