1 | // |
2 | // Redistribution and use in source and binary forms, with or without |
3 | // modification, are permitted provided that the following conditions |
4 | // are met: |
5 | // * Redistributions of source code must retain the above copyright |
6 | // notice, this list of conditions and the following disclaimer. |
7 | // * Redistributions in binary form must reproduce the above copyright |
8 | // notice, this list of conditions and the following disclaimer in the |
9 | // documentation and/or other materials provided with the distribution. |
10 | // * Neither the name of NVIDIA CORPORATION nor the names of its |
11 | // contributors may be used to endorse or promote products derived |
12 | // from this software without specific prior written permission. |
13 | // |
14 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY |
15 | // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
16 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR |
17 | // PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR |
18 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, |
19 | // EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, |
20 | // PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR |
21 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY |
22 | // OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
24 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
25 | // |
26 | // Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved. |
27 | // Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved. |
28 | // Copyright (c) 2001-2004 NovodeX AG. All rights reserved. |
29 | |
30 | |
31 | #ifndef PX_PHYSICS_EXTENSIONS_MASS_PROPERTIES_H |
32 | #define PX_PHYSICS_EXTENSIONS_MASS_PROPERTIES_H |
33 | /** \addtogroup extensions |
34 | @{ |
35 | */ |
36 | |
37 | #include "PxPhysXConfig.h" |
38 | #include "foundation/PxMath.h" |
39 | #include "foundation/PxMathUtils.h" |
40 | #include "foundation/PxVec3.h" |
41 | #include "foundation/PxMat33.h" |
42 | #include "foundation/PxQuat.h" |
43 | #include "foundation/PxTransform.h" |
44 | #include "geometry/PxGeometry.h" |
45 | #include "geometry/PxBoxGeometry.h" |
46 | #include "geometry/PxSphereGeometry.h" |
47 | #include "geometry/PxCapsuleGeometry.h" |
48 | #include "geometry/PxConvexMeshGeometry.h" |
49 | #include "geometry/PxConvexMesh.h" |
50 | |
51 | #if !PX_DOXYGEN |
52 | namespace physx |
53 | { |
54 | #endif |
55 | |
56 | /** |
57 | \brief Utility class to compute and manipulate mass and inertia tensor properties. |
58 | |
59 | In most cases #PxRigidBodyExt::updateMassAndInertia(), #PxRigidBodyExt::setMassAndUpdateInertia() should be enough to |
60 | setup the mass properties of a rigid body. This utility class targets users that need to customize the mass properties |
61 | computation. |
62 | */ |
63 | class PxMassProperties |
64 | { |
65 | public: |
66 | /** |
67 | \brief Default constructor. |
68 | */ |
69 | PX_FORCE_INLINE PxMassProperties() : inertiaTensor(PxIdentity), centerOfMass(0.0f), mass(1.0f) {} |
70 | |
71 | /** |
72 | \brief Construct from individual elements. |
73 | */ |
74 | PX_FORCE_INLINE PxMassProperties(const PxReal m, const PxMat33& inertiaT, const PxVec3& com) : inertiaTensor(inertiaT), centerOfMass(com), mass(m) {} |
75 | |
76 | /** |
77 | \brief Compute mass properties based on a provided geometry structure. |
78 | |
79 | This constructor assumes the geometry has a density of 1. Mass and inertia tensor scale linearly with density. |
80 | |
81 | \param[in] geometry The geometry to compute the mass properties for. Supported geometry types are: sphere, box, capsule and convex mesh. |
82 | */ |
83 | PxMassProperties(const PxGeometry& geometry) |
84 | { |
85 | switch (geometry.getType()) |
86 | { |
87 | case PxGeometryType::eSPHERE: |
88 | { |
89 | const PxSphereGeometry& s = static_cast<const PxSphereGeometry&>(geometry); |
90 | mass = (4.0f / 3.0f) * PxPi * s.radius * s.radius * s.radius; |
91 | inertiaTensor = PxMat33::createDiagonal(d: PxVec3(2.0f / 5.0f * mass * s.radius * s.radius)); |
92 | centerOfMass = PxVec3(0.0f); |
93 | } |
94 | break; |
95 | |
96 | case PxGeometryType::eBOX: |
97 | { |
98 | const PxBoxGeometry& b = static_cast<const PxBoxGeometry&>(geometry); |
99 | mass = b.halfExtents.x * b.halfExtents.y * b.halfExtents.z * 8.0f; |
100 | PxVec3 d2 = b.halfExtents.multiply(a: b.halfExtents); |
101 | inertiaTensor = PxMat33::createDiagonal(d: PxVec3(d2.y + d2.z, d2.x + d2.z, d2.x + d2.y)) * (mass * 1.0f / 3.0f); |
102 | centerOfMass = PxVec3(0.0f); |
103 | } |
104 | break; |
105 | |
106 | case PxGeometryType::eCAPSULE: |
107 | { |
108 | const PxCapsuleGeometry& c = static_cast<const PxCapsuleGeometry&>(geometry); |
109 | PxReal r = c.radius, h = c.halfHeight; |
110 | mass = ((4.0f / 3.0f) * r + 2 * c.halfHeight) * PxPi * r * r; |
111 | |
112 | 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); |
113 | PxReal b = r*r*r * (8.0f / 15.0f) + h*r*r; |
114 | inertiaTensor = PxMat33::createDiagonal(d: PxVec3(b, a, a) * PxPi * r * r); |
115 | centerOfMass = PxVec3(0.0f); |
116 | } |
117 | break; |
118 | |
119 | case PxGeometryType::eCONVEXMESH: |
120 | { |
121 | const PxConvexMeshGeometry& c = static_cast<const PxConvexMeshGeometry&>(geometry); |
122 | PxVec3 unscaledCoM; |
123 | PxMat33 unscaledInertiaTensorNonCOM; // inertia tensor of convex mesh in mesh local space |
124 | PxMat33 unscaledInertiaTensorCOM; |
125 | PxReal unscaledMass; |
126 | c.convexMesh->getMassInformation(mass&: unscaledMass, localInertia&: unscaledInertiaTensorNonCOM, localCenterOfMass&: unscaledCoM); |
127 | |
128 | // inertia tensor relative to center of mass |
129 | unscaledInertiaTensorCOM[0][0] = unscaledInertiaTensorNonCOM[0][0] - unscaledMass*PxReal((unscaledCoM.y*unscaledCoM.y+unscaledCoM.z*unscaledCoM.z)); |
130 | unscaledInertiaTensorCOM[1][1] = unscaledInertiaTensorNonCOM[1][1] - unscaledMass*PxReal((unscaledCoM.z*unscaledCoM.z+unscaledCoM.x*unscaledCoM.x)); |
131 | unscaledInertiaTensorCOM[2][2] = unscaledInertiaTensorNonCOM[2][2] - unscaledMass*PxReal((unscaledCoM.x*unscaledCoM.x+unscaledCoM.y*unscaledCoM.y)); |
132 | unscaledInertiaTensorCOM[0][1] = unscaledInertiaTensorCOM[1][0] = (unscaledInertiaTensorNonCOM[0][1] + unscaledMass*PxReal(unscaledCoM.x*unscaledCoM.y)); |
133 | unscaledInertiaTensorCOM[1][2] = unscaledInertiaTensorCOM[2][1] = (unscaledInertiaTensorNonCOM[1][2] + unscaledMass*PxReal(unscaledCoM.y*unscaledCoM.z)); |
134 | unscaledInertiaTensorCOM[0][2] = unscaledInertiaTensorCOM[2][0] = (unscaledInertiaTensorNonCOM[0][2] + unscaledMass*PxReal(unscaledCoM.z*unscaledCoM.x)); |
135 | |
136 | const PxMeshScale& s = c.scale; |
137 | mass = unscaledMass * s.scale.x * s.scale.y * s.scale.z; |
138 | centerOfMass = s.rotation.rotate(v: s.scale.multiply(a: s.rotation.rotateInv(v: unscaledCoM))); |
139 | inertiaTensor = scaleInertia(inertia: unscaledInertiaTensorCOM, scaleRotation: s.rotation, scale: s.scale); |
140 | } |
141 | break; |
142 | |
143 | case PxGeometryType::eHEIGHTFIELD: |
144 | case PxGeometryType::ePLANE: |
145 | case PxGeometryType::eTRIANGLEMESH: |
146 | case PxGeometryType::eINVALID: |
147 | case PxGeometryType::eGEOMETRY_COUNT: |
148 | { |
149 | *this = PxMassProperties(); |
150 | } |
151 | break; |
152 | } |
153 | |
154 | PX_ASSERT(inertiaTensor.column0.isFinite() && inertiaTensor.column1.isFinite() && inertiaTensor.column2.isFinite()); |
155 | PX_ASSERT(centerOfMass.isFinite()); |
156 | PX_ASSERT(PxIsFinite(mass)); |
157 | } |
158 | |
159 | /** |
160 | \brief Scale mass properties. |
161 | |
162 | \param[in] scale The linear scaling factor to apply to the mass properties. |
163 | \return The scaled mass properties. |
164 | */ |
165 | PX_FORCE_INLINE PxMassProperties operator*(const PxReal scale) const |
166 | { |
167 | PX_ASSERT(PxIsFinite(scale)); |
168 | |
169 | return PxMassProperties(mass * scale, inertiaTensor * scale, centerOfMass); |
170 | } |
171 | |
172 | /** |
173 | \brief Translate the center of mass by a given vector and adjust the inertia tensor accordingly. |
174 | |
175 | \param[in] t The translation vector for the center of mass. |
176 | */ |
177 | PX_FORCE_INLINE void translate(const PxVec3& t) |
178 | { |
179 | PX_ASSERT(t.isFinite()); |
180 | |
181 | inertiaTensor = translateInertia(inertia: inertiaTensor, mass, t); |
182 | centerOfMass += t; |
183 | |
184 | PX_ASSERT(inertiaTensor.column0.isFinite() && inertiaTensor.column1.isFinite() && inertiaTensor.column2.isFinite()); |
185 | PX_ASSERT(centerOfMass.isFinite()); |
186 | } |
187 | |
188 | /** |
189 | \brief Get the entries of the diagonalized inertia tensor and the corresponding reference rotation. |
190 | |
191 | \param[in] inertia The inertia tensor to diagonalize. |
192 | \param[out] massFrame The frame the diagonalized tensor refers to. |
193 | \return The entries of the diagonalized inertia tensor. |
194 | */ |
195 | PX_FORCE_INLINE static PxVec3 getMassSpaceInertia(const PxMat33& inertia, PxQuat& massFrame) |
196 | { |
197 | PX_ASSERT(inertia.column0.isFinite() && inertia.column1.isFinite() && inertia.column2.isFinite()); |
198 | |
199 | PxVec3 diagT = PxDiagonalize(m: inertia, axes&: massFrame); |
200 | PX_ASSERT(diagT.isFinite()); |
201 | PX_ASSERT(massFrame.isFinite()); |
202 | return diagT; |
203 | } |
204 | |
205 | /** |
206 | \brief Translate an inertia tensor using the parallel axis theorem |
207 | |
208 | \param[in] inertia The inertia tensor to translate. |
209 | \param[in] mass The mass of the object. |
210 | \param[in] t The relative frame to translate the inertia tensor to. |
211 | \return The translated inertia tensor. |
212 | */ |
213 | PX_FORCE_INLINE static PxMat33 translateInertia(const PxMat33& inertia, const PxReal mass, const PxVec3& t) |
214 | { |
215 | PX_ASSERT(inertia.column0.isFinite() && inertia.column1.isFinite() && inertia.column2.isFinite()); |
216 | PX_ASSERT(PxIsFinite(mass)); |
217 | PX_ASSERT(t.isFinite()); |
218 | |
219 | PxMat33 s( PxVec3(0,t.z,-t.y), |
220 | PxVec3(-t.z,0,t.x), |
221 | PxVec3(t.y,-t.x,0) ); |
222 | |
223 | PxMat33 translatedIT = s.getTranspose() * s * mass + inertia; |
224 | PX_ASSERT(translatedIT.column0.isFinite() && translatedIT.column1.isFinite() && translatedIT.column2.isFinite()); |
225 | return translatedIT; |
226 | } |
227 | |
228 | /** |
229 | \brief Rotate an inertia tensor around the center of mass |
230 | |
231 | \param[in] inertia The inertia tensor to rotate. |
232 | \param[in] q The rotation to apply to the inertia tensor. |
233 | \return The rotated inertia tensor. |
234 | */ |
235 | PX_FORCE_INLINE static PxMat33 rotateInertia(const PxMat33& inertia, const PxQuat& q) |
236 | { |
237 | PX_ASSERT(inertia.column0.isFinite() && inertia.column1.isFinite() && inertia.column2.isFinite()); |
238 | PX_ASSERT(q.isUnit()); |
239 | |
240 | PxMat33 m(q); |
241 | PxMat33 rotatedIT = m * inertia * m.getTranspose(); |
242 | PX_ASSERT(rotatedIT.column0.isFinite() && rotatedIT.column1.isFinite() && rotatedIT.column2.isFinite()); |
243 | return rotatedIT; |
244 | } |
245 | |
246 | /** |
247 | \brief Non-uniform scaling of the inertia tensor |
248 | |
249 | \param[in] inertia The inertia tensor to scale. |
250 | \param[in] scaleRotation The frame of the provided scaling factors. |
251 | \param[in] scale The scaling factor for each axis (relative to the frame specified in scaleRotation). |
252 | \return The scaled inertia tensor. |
253 | */ |
254 | static PxMat33 scaleInertia(const PxMat33& inertia, const PxQuat& scaleRotation, const PxVec3& scale) |
255 | { |
256 | PX_ASSERT(inertia.column0.isFinite() && inertia.column1.isFinite() && inertia.column2.isFinite()); |
257 | PX_ASSERT(scaleRotation.isUnit()); |
258 | PX_ASSERT(scale.isFinite()); |
259 | |
260 | PxMat33 localInertiaT = rotateInertia(inertia, q: scaleRotation); // rotate inertia into scaling frame |
261 | PxVec3 diagonal(localInertiaT[0][0], localInertiaT[1][1], localInertiaT[2][2]); |
262 | |
263 | PxVec3 xyz2 = PxVec3(diagonal.dot(v: PxVec3(0.5f))) - diagonal; // original x^2, y^2, z^2 |
264 | PxVec3 scaledxyz2 = xyz2.multiply(a: scale).multiply(a: scale); |
265 | |
266 | PxReal xx = scaledxyz2.y + scaledxyz2.z, |
267 | yy = scaledxyz2.z + scaledxyz2.x, |
268 | zz = scaledxyz2.x + scaledxyz2.y; |
269 | |
270 | PxReal xy = localInertiaT[0][1] * scale.x * scale.y, |
271 | xz = localInertiaT[0][2] * scale.x * scale.z, |
272 | yz = localInertiaT[1][2] * scale.y * scale.z; |
273 | |
274 | PxMat33 scaledInertia( PxVec3(xx, xy, xz), |
275 | PxVec3(xy, yy, yz), |
276 | PxVec3(xz, yz, zz)); |
277 | |
278 | PxMat33 scaledIT = rotateInertia(inertia: scaledInertia * (scale.x * scale.y * scale.z), q: scaleRotation.getConjugate()); |
279 | PX_ASSERT(scaledIT.column0.isFinite() && scaledIT.column1.isFinite() && scaledIT.column2.isFinite()); |
280 | return scaledIT; |
281 | } |
282 | |
283 | /** |
284 | \brief Sum up individual mass properties. |
285 | |
286 | \param[in] props Array of mass properties to sum up. |
287 | \param[in] transforms Reference transforms for each mass properties entry. |
288 | \param[in] count The number of mass properties to sum up. |
289 | \return The summed up mass properties. |
290 | */ |
291 | static PxMassProperties sum(const PxMassProperties* props, const PxTransform* transforms, const PxU32 count) |
292 | { |
293 | PxReal combinedMass = 0.0f; |
294 | PxVec3 combinedCoM(0.0f); |
295 | PxMat33 combinedInertiaT = PxMat33(PxZero); |
296 | |
297 | for(PxU32 i = 0; i < count; i++) |
298 | { |
299 | PX_ASSERT(props[i].inertiaTensor.column0.isFinite() && props[i].inertiaTensor.column1.isFinite() && props[i].inertiaTensor.column2.isFinite()); |
300 | PX_ASSERT(props[i].centerOfMass.isFinite()); |
301 | PX_ASSERT(PxIsFinite(props[i].mass)); |
302 | |
303 | combinedMass += props[i].mass; |
304 | const PxVec3 comTm = transforms[i].transform(input: props[i].centerOfMass); |
305 | combinedCoM += comTm * props[i].mass; |
306 | } |
307 | |
308 | if(combinedMass > 0.f) |
309 | combinedCoM /= combinedMass; |
310 | |
311 | for(PxU32 i = 0; i < count; i++) |
312 | { |
313 | const PxVec3 comTm = transforms[i].transform(input: props[i].centerOfMass); |
314 | combinedInertiaT += translateInertia(inertia: rotateInertia(inertia: props[i].inertiaTensor, q: transforms[i].q), mass: props[i].mass, t: combinedCoM - comTm); |
315 | } |
316 | |
317 | PX_ASSERT(combinedInertiaT.column0.isFinite() && combinedInertiaT.column1.isFinite() && combinedInertiaT.column2.isFinite()); |
318 | PX_ASSERT(combinedCoM.isFinite()); |
319 | PX_ASSERT(PxIsFinite(combinedMass)); |
320 | |
321 | return PxMassProperties(combinedMass, combinedInertiaT, combinedCoM); |
322 | } |
323 | |
324 | |
325 | PxMat33 inertiaTensor; //!< The inertia tensor of the object. |
326 | PxVec3 centerOfMass; //!< The center of mass of the object. |
327 | PxReal mass; //!< The mass of the object. |
328 | }; |
329 | |
330 | #if !PX_DOXYGEN |
331 | } // namespace physx |
332 | #endif |
333 | |
334 | /** @} */ |
335 | #endif |
336 | |