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
52namespace physx
53{
54#endif
55
56/**
57\brief Utility class to compute and manipulate mass and inertia tensor properties.
58
59In most cases #PxRigidBodyExt::updateMassAndInertia(), #PxRigidBodyExt::setMassAndUpdateInertia() should be enough to
60setup the mass properties of a rigid body. This utility class targets users that need to customize the mass properties
61computation.
62*/
63class PxMassProperties
64{
65public:
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

source code of qtquick3dphysics/src/3rdparty/PhysX/include/extensions/PxMassProperties.h