physics mass properties computation fix

PixarAnimationStudios · Mar 7, 2022 · ac291f8 · ac291f8
1 parent 5a2c5d6
commit ac291f8
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 4 deletions.
diff --git a/pxr/usd/usdPhysics/massProperties.h b/pxr/usd/usdPhysics/massProperties.h
@@ -40,7 +40,7 @@ GfQuatf UsdPhysicsIndexedRotation(uint32_t axis, float s, float c)
 {
     float v[3] = { 0, 0, 0 };
     v[axis] = s;
-    return GfQuatf(v[0], v[1], v[2], c);
+    return GfQuatf(c, v[0], v[1], v[2]);
 }
 
 uint32_t UsdPhysicsGetNextIndex3(uint32_t i)
@@ -59,7 +59,7 @@ GfVec3f UsdPhysicsDiagonalize(const GfMatrix3f& m, GfQuatf& massFrame)
     for (uint32_t i = 0; i < MAX_ITERS; i++)
     {
         GfMatrix3f axes(q);
-        d = axes.GetTranspose() * m * axes;
+        d = axes * m * axes.GetTranspose();
 
         float d0 = fabs(d[1][2]), d1 = fabs(d[0][2]), d2 = fabs(d[0][1]);
         uint32_t a = uint32_t(d0 > d1 && d0 > d2 ? 0 : d1 > d2 ? 1 : 2); // rotation axis index, from largest
@@ -157,7 +157,7 @@ class UsdPhysicsMassProperties
         s.SetColumn(1, GfVec3f(-t[2], 0, t[0]));
         s.SetColumn(2, GfVec3f(t[1], -t[0], 0));
 
-        GfMatrix3f translatedIT = s.GetTranspose() * s * mass + inertia;
+        GfMatrix3f translatedIT = s * s.GetTranspose() * mass + inertia;
         return translatedIT;
     }
 
@@ -169,7 +169,7 @@ class UsdPhysicsMassProperties
     USDPHYSICS_API static GfMatrix3f RotateInertia(const GfMatrix3f& inertia, const GfQuatf& q)
     {
         GfMatrix3f m(q);
-        GfMatrix3f rotatedIT = m * inertia * m.GetTranspose();
+        GfMatrix3f rotatedIT = m.GetTranspose() * inertia * m;
         return rotatedIT;
     }
 

diff --git a/pxr/usd/usdPhysics/testenv/testUsdPhysicsRigidBodyAPI.py b/pxr/usd/usdPhysics/testenv/testUsdPhysicsRigidBodyAPI.py
@@ -452,6 +452,49 @@ def test_mass_rigid_body_cube_inertia_precedence(self):
 
         self.compare_mass_information(rigidBodyAPI, 1000.0, expectedInertia=Gf.Vec3f(2.0))
 
+    # inertia test, test compound body
+    def test_mass_rigid_body_cube_rigid_body_compound(self):
+        self.setup_scene()
+
+        # top level xform - rigid body
+        self.xform = UsdGeom.Xform.Define(self.stage, "/xform")
+        rigidBodyAPI = UsdPhysics.RigidBodyAPI.Apply(self.xform.GetPrim())        
+
+        size = 1.0
+        scale = Gf.Vec3f(3.0, 2.0, 3.0)        
+
+        # Create test collider cube0
+        cube = UsdGeom.Cube.Define(self.stage, "/xform/cube0")
+        cube.CreateSizeAttr(size)
+        cube.AddTranslateOp().Set(Gf.Vec3f(100.0, 20.0, 10.0))
+        cube0RotateOp = cube.AddRotateXYZOp()
+        cube0RotateOp.Set(Gf.Vec3f(0.0,0.0,45.0))
+        cube.AddScaleOp().Set(scale)
+        UsdPhysics.CollisionAPI.Apply(cube.GetPrim())
+
+        # Create test collider cube1
+        cube = UsdGeom.Cube.Define(self.stage, "/xform/cube1")
+        cube.CreateSizeAttr(size)
+        cube.AddTranslateOp().Set(Gf.Vec3f(-100.0, 20.0, 10.0))
+        cube1RotateOp = cube.AddRotateXYZOp()
+        cube1RotateOp.Set(Gf.Vec3f(0.0,0.0,45.0))
+        cube.AddScaleOp().Set(scale)
+        UsdPhysics.CollisionAPI.Apply(cube.GetPrim())
+
+        self.rigidBodyWorldTransform = UsdGeom.Xformable(self.xform.GetPrim()).ComputeLocalToWorldTransform(Usd.TimeCode.Default())
+        self.rigidBodyPrim = self.xform.GetPrim()
+
+        mass, inertia_compare, centerOfMass, principalAxes = rigidBodyAPI.ComputeMassProperties(self.mass_information_fn)
+
+        cube0RotateOp.Set(Gf.Vec3f(0.0,90.0,45.0))
+        cube1RotateOp.Set(Gf.Vec3f(0.0,0.0,45.0))
+
+        mass, inertia, centerOfMass, principalAxes = rigidBodyAPI.ComputeMassProperties(self.mass_information_fn)
+
+        toleranceEpsilon = 1
+        self.assertTrue(Gf.IsClose(inertia, inertia_compare, toleranceEpsilon))
+
+
     # principal axes tests
     # principal axes test, applied principal axis to a body
     def test_mass_rigid_body_cube_rigid_body_principal_axes(self):