massProperties.h
1 //
2 // Copyright 2021 Pixar
3 //
4 // Licensed under the Apache License, Version 2.0 (the "Apache License")
5 // with the following modification; you may not use this file except in
6 // compliance with the Apache License and the following modification to it:
7 // Section 6. Trademarks. is deleted and replaced with:
8 //
9 // 6. Trademarks. This License does not grant permission to use the trade
10 // names, trademarks, service marks, or product names of the Licensor
11 // and its affiliates, except as required to comply with Section 4(c) of
12 // the License and to reproduce the content of the NOTICE file.
13 //
14 // You may obtain a copy of the Apache License at
15 //
16 // http://www.apache.org/licenses/LICENSE-2.0
17 //
18 // Unless required by applicable law or agreed to in writing, software
19 // distributed under the Apache License with the above modification is
20 // distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
21 // KIND, either express or implied. See the Apache License for the specific
22 // language governing permissions and limitations under the Apache License.
23 //
24 #ifndef PXR_USD_USD_MASS_PROPERTIES_H
25 #define PXR_USD_USD_MASS_PROPERTIES_H
26 
27 #include "pxr/pxr.h"
28 #include "pxr/usd/usdPhysics/api.h"
29 #include "pxr/usd/usd/common.h"
30 
31 #include "pxr/base/gf/vec3f.h"
32 #include "pxr/base/gf/quatf.h"
33 #include "pxr/base/gf/rotation.h"
34 #include "pxr/base/gf/matrix3f.h"
35 #include "pxr/base/gf/matrix4f.h"
36 
37 PXR_NAMESPACE_OPEN_SCOPE
38 
39 GfQuatf UsdPhysicsIndexedRotation(uint32_t axis, float s, float c)
40 {
41  float v[3] = { 0, 0, 0 };
42  v[axis] = s;
43  return GfQuatf(c, v[0], v[1], v[2]);
44 }
45 
46 uint32_t UsdPhysicsGetNextIndex3(uint32_t i)
47 {
48  return (i + 1 + (i >> 1)) & 3;
49 }
50 
51 
52 GfVec3f UsdPhysicsDiagonalize(const GfMatrix3f& m, GfQuatf& massFrame)
53 {
54  const uint32_t MAX_ITERS = 24;
55 
56  GfQuatf q = GfQuatf(1.0);
57 
58  GfMatrix3f d;
59  for (uint32_t i = 0; i < MAX_ITERS; i++)
60  {
61  GfMatrix3f axes(q);
62  d = axes * m * axes.GetTranspose();
63 
64  float d0 = fabs(d[1][2]), d1 = fabs(d[0][2]), d2 = fabs(d[0][1]);
65  uint32_t a = uint32_t(d0 > d1 && d0 > d2 ? 0 : d1 > d2 ? 1 : 2); // rotation axis index, from largest
66  // off-diagonal
67  // element
68 
69  uint32_t a1 = UsdPhysicsGetNextIndex3(a), a2 = UsdPhysicsGetNextIndex3(a1);
70  if (d[a1][a2] == 0.0f || fabs(d[a1][a1] - d[a2][a2]) > 2e6 * fabs(2.0 * d[a1][a2]))
71  break;
72 
73  float w = (d[a1][a1] - d[a2][a2]) / (2.0f * d[a1][a2]); // cot(2 * phi), where phi is the rotation angle
74  float absw = fabs(w);
75 
76  GfQuatf r;
77  if (absw > 1000)
78  r = UsdPhysicsIndexedRotation(a, 1 / (4 * w), 1.0f); // h will be very close to 1, so use small angle approx instead
79  else
80  {
81  float t = 1 / (absw + sqrt(w * w + 1)); // absolute value of tan phi
82  float h = 1 / sqrt(t * t + 1); // absolute value of cos phi
83 
84  r = UsdPhysicsIndexedRotation(a, sqrt((1 - h) / 2) * ((w >= 0.0f) ? 1.0f : -1.0f), sqrt((1 + h) / 2));
85  }
86 
87  q = (q * r).GetNormalized();
88  }
89 
90  massFrame = q;
91  return GfVec3f(d.GetColumn(0)[0], d.GetColumn(1)[1], d.GetColumn(2)[2]);
92 }
93 
94 // -------------------------------------------------------------------------- //
95 // MASSPROPERTIES //
96 // -------------------------------------------------------------------------- //
97 
104 {
105 public:
106 
108  USDPHYSICS_API UsdPhysicsMassProperties() : _inertiaTensor(0.0f), _centerOfMass(0.0f), _mass(1.0f)
109  {
110  _inertiaTensor[0][0] = 1.0;
111  _inertiaTensor[1][1] = 1.0;
112  _inertiaTensor[2][2] = 1.0;
113  }
114 
116  USDPHYSICS_API UsdPhysicsMassProperties(const float m, const GfMatrix3f& inertiaT, const GfVec3f& com)
117  : _inertiaTensor(inertiaT), _centerOfMass(com), _mass(m)
118  {
119  }
120 
124  USDPHYSICS_API UsdPhysicsMassProperties operator*(const float scale) const
125  {
126  return UsdPhysicsMassProperties(_mass * scale, _inertiaTensor * scale, _centerOfMass);
127  }
128 
131  USDPHYSICS_API void Translate(const GfVec3f& t)
132  {
133  _inertiaTensor = TranslateInertia(_inertiaTensor, _mass, t);
134  _centerOfMass += t;
135  }
136 
141  USDPHYSICS_API static GfVec3f GetMassSpaceInertia(const GfMatrix3f& inertia, GfQuatf& massFrame)
142  {
143 
144  GfVec3f diagT = UsdPhysicsDiagonalize(inertia, massFrame);
145  return diagT;
146  }
147 
153  USDPHYSICS_API static GfMatrix3f TranslateInertia(const GfMatrix3f& inertia, const float mass, const GfVec3f& t)
154  {
155  GfMatrix3f s;
156  s.SetColumn(0, GfVec3f(0, t[2], -t[1]));
157  s.SetColumn(1, GfVec3f(-t[2], 0, t[0]));
158  s.SetColumn(2, GfVec3f(t[1], -t[0], 0));
159 
160  GfMatrix3f translatedIT = s * s.GetTranspose() * mass + inertia;
161  return translatedIT;
162  }
163 
164 
169  USDPHYSICS_API static GfMatrix3f RotateInertia(const GfMatrix3f& inertia, const GfQuatf& q)
170  {
171  GfMatrix3f m(q);
172  GfMatrix3f rotatedIT = m.GetTranspose() * inertia * m;
173  return rotatedIT;
174  }
175 
181  USDPHYSICS_API static UsdPhysicsMassProperties Sum(const UsdPhysicsMassProperties* props, const GfMatrix4f* transforms, const uint32_t count)
182  {
183  float combinedMass = 0.0f;
184  GfVec3f combinedCoM(0.0f);
185  GfMatrix3f combinedInertiaT = GfMatrix3f(0.0f);
186 
187  for (uint32_t i = 0; i < count; i++)
188  {
189  combinedMass += props[i]._mass;
190  const GfVec3f comTm = transforms[i].Transform(props[i]._centerOfMass);
191  combinedCoM += comTm * props[i]._mass;
192  }
193 
194  if (combinedMass > 0.f)
195  combinedCoM /= combinedMass;
196 
197  for (uint32_t i = 0; i < count; i++)
198  {
199  const GfVec3f comTm = transforms[i].Transform(props[i]._centerOfMass);
200  combinedInertiaT += TranslateInertia(
201  RotateInertia(props[i]._inertiaTensor, GfQuatf(transforms[i].ExtractRotation().GetQuat())),
202  props[i]._mass, combinedCoM - comTm);
203  }
204 
205  return UsdPhysicsMassProperties(combinedMass, combinedInertiaT, combinedCoM);
206  }
207 
211  {
212  return _inertiaTensor;
213  }
214 
217  void SetInertiaTensor(const GfMatrix3f& inTensor)
218  {
219  _inertiaTensor = inTensor;
220  }
221 
224  const GfVec3f& GetCenterOfMass() const
225  {
226  return _centerOfMass;
227  }
228 
231  float GetMass() const
232  {
233  return _mass;
234  }
235 
238  void SetMass(float inMass)
239  {
240  _mass = inMass;
241  }
242 
243 private:
244  GfMatrix3f _inertiaTensor;
245  GfVec3f _centerOfMass;
246  float _mass;
247 };
248 
249 PXR_NAMESPACE_CLOSE_SCOPE
250 
251 #endif
void SetMass(float inMass)
Set mass.
const GfMatrix3f & GetInertiaTensor() const
Get inertia tensor.
Stores a 4x4 matrix of float elements.
Definition: matrix4f.h:88
Mass properties computation class.
Basic type for a vector of 3 float components.
Definition: vec3f.h:63
GF_API GfMatrix3f GetTranspose() const
Returns the transpose of the matrix.
GfVec3f GetColumn(int i) const
Gets a column of the matrix as a Vec3.
Definition: matrix3f.h:178
static USDPHYSICS_API GfMatrix3f RotateInertia(const GfMatrix3f &inertia, const GfQuatf &q)
Rotate an inertia tensor around the center of mass inertia The inertia tensor to rotate.
GfVec3d Transform(const GfVec3d &vec) const
Transforms the row vector vec by the matrix, returning the result.
Definition: matrix4f.h:648
Basic type: a quaternion, a complex number with a real coefficient and three imaginary coefficients,...
Definition: quatf.h:60
Stores a 3x3 matrix of float elements.
Definition: matrix3f.h:82
USDPHYSICS_API UsdPhysicsMassProperties operator *(const float scale) const
Scale mass properties.
void SetColumn(int i, const GfVec3f &v)
Sets a column of the matrix from a Vec3.
Definition: matrix3f.h:166
static USDPHYSICS_API GfVec3f GetMassSpaceInertia(const GfMatrix3f &inertia, GfQuatf &massFrame)
Get the entries of the diagonalized inertia tensor and the corresponding reference rotation.
float GetMass() const
Get mass.
USDPHYSICS_API UsdPhysicsMassProperties(const float m, const GfMatrix3f &inertiaT, const GfVec3f &com)
Construct from individual elements.
USDPHYSICS_API void Translate(const GfVec3f &t)
Translate the center of mass by a given vector and adjust the inertia tensor accordingly.
const GfVec3f & GetCenterOfMass() const
Get center of mass.
void SetInertiaTensor(const GfMatrix3f &inTensor)
Set inertia tensor inTensor New inertia tensor.
static USDPHYSICS_API UsdPhysicsMassProperties Sum(const UsdPhysicsMassProperties *props, const GfMatrix4f *transforms, const uint32_t count)
Sum up individual mass properties.
USDPHYSICS_API UsdPhysicsMassProperties()
Construct a MassProperties.
static USDPHYSICS_API GfMatrix3f TranslateInertia(const GfMatrix3f &inertia, const float mass, const GfVec3f &t)
Translate an inertia tensor using the parallel axis theorem inertia The inertia tensor to translate.