def getWorldInertia(self):
""" @return inertia with respect to world reference frame
"""
R = Quaternion.to_matrix(self.inertia_rotation)
# I in world axis
I = numpy.dot(R.transpose(), numpy.dot(numpy.diag(self.diagonal_inertia), R))
# I at world origin, using // axis theorem
# see http://www.colorado.edu/physics/phys3210/phys3210_sp14/lecnotes.2014-03-07.More_on_Inertia_Tensors.html
# or https://en.wikipedia.org/wiki/Moment_of_inertia
a=numpy.array(self.com).reshape(3,1)
return I + self.mass*(pow(numpy.linalg.norm(self.com),2)*numpy.eye(3) - a*a.transpose())
def getWorldInertia(self):
""" @return inertia with respect to world reference frame
"""
R = Quaternion.to_matrix(self.inertia_rotation)
# I in world axis
I = numpy.dot(R.transpose(), numpy.dot(numpy.diag(self.diagonal_inertia), R))
# I at world origin, using // axis theorem
# see http://www.colorado.edu/physics/phys3210/phys3210_sp14/lecnotes.2014-03-07.More_on_Inertia_Tensors.html
# or https://en.wikipedia.org/wiki/Moment_of_inertia
a=numpy.array(self.com).reshape(3,1)
return I + self.mass*(pow(numpy.linalg.norm(self.com),2)*numpy.eye(3) - a*a.transpose())
def run():
ok = True
info = SofaPython.mass.RigidMassInfo()
# testing axis-aligned known geometric shapes
for m in xrange(len(meshes)):
mesh = meshes[m]
mesh_path = path + meshes[m]
for s in xrange(len(scales)):
scale = scales[s]
if mesh=="cylinder.obj" and scale[0]!=scale[1]:
continue
for d in xrange(len(densities)):
density=densities[d]
info.setFromMesh( mesh_path, density, scale )
error = " ("+meshes[m]+", s="+Tools.cat(scale)+" d="+str(density)+")"
ok &= EXPECT_TRUE( almostEqualReal(info.mass, masses[m][s][d]), "mass"+error+" "+str(info.mass)+"!="+str(masses[m][s][d]) )
ok &= EXPECT_TRUE( almostEqualLists(info.com,[x*0.5 for x in scale]), "com"+error+" "+Tools.cat(info.com)+"!="+Tools.cat([x*0.5 for x in scale]) )
ok &= EXPECT_TRUE( almostEqualLists(info.diagonal_inertia,inertia[m][s][d]), "inertia"+error+" "+str(info.diagonal_inertia)+"!="+str(inertia[m][s][d]) )
# testing diagonal inertia extraction from a rotated cuboid
mesh = "cube.obj"
mesh_path = path + mesh
scale = scales[3]
density = 1
theory = sorted(inertia[0][3][0])
for r in rotations:
info.setFromMesh( mesh_path, density, scale, r )
local = sorted(info.diagonal_inertia)
ok &= EXPECT_TRUE( almostEqualLists(local,theory), "inertia "+str(local)+"!="+str(theory)+" (rotation="+str(r)+")" )
# testing extracted inertia rotation
mesh = "rotated_cuboid_12_35_-27.obj"
mesh_path = path + mesh
density = 1
info.setFromMesh( mesh_path, density )
# theoretical results
scale = [2,3,1]
mass = density * scale[0]*scale[1]*scale[2]
inertiat = numpy.empty(3)
inertiat[0] = 1.0/12.0 * mass * (scale[1]*scale[1]+scale[2]*scale[2]) # x
inertiat[1] = 1.0/12.0 * mass * (scale[0]*scale[0]+scale[2]*scale[2]) # y
inertiat[2] = 1.0/12.0 * mass * (scale[0]*scale[0]+scale[1]*scale[1]) # z
# used quaternion in mesh
q = Quaternion.normalized( Quaternion.from_euler( [12*math.pi/180.0, 35*math.pi/180.0, -27*math.pi/180.0] ) )
# corresponding rotation matrices (ie frame defined by columns)
mt = Quaternion.to_matrix( q )
m = Quaternion.to_matrix( info.inertia_rotation )
# matching inertia
idxt = numpy.argsort(inertiat)
idx = numpy.argsort(info.diagonal_inertia)
# checking if each axis/column are parallel (same or opposite for unitary vectors)
for i in xrange(3):
ok &= EXPECT_TRUE( almostEqualLists(mt[:,idxt[i]].tolist(),m[:,idx[i]].tolist(),1e-5) or almostEqualLists(mt[:,idxt[i]].tolist(),(-m[:,idx[i]]).tolist(),1e-5), "wrong inertia rotation" )
# print mt[:,idxt]
# print m [:,idx ]
return ok
def run():
ok = True
info = SofaPython.mass.RigidMassInfo()
# testing axis-aligned known geometric shapes
for m in xrange(len(meshes)):
mesh = meshes[m]
mesh_path = path + meshes[m]
for s in xrange(len(scales)):
scale = scales[s]
if mesh == "cylinder.obj" and scale[0] != scale[1]:
continue
for d in xrange(len(densities)):
density = densities[d]
info.setFromMesh(mesh_path, density, scale)
error = " (" + meshes[m] + ", s=" + Tools.cat(
scale) + " d=" + str(density) + ")"
ok &= EXPECT_TRUE(
almostEqualReal(info.mass, masses[m][s][d]), "mass" +
error + " " + str(info.mass) + "!=" + str(masses[m][s][d]))
ok &= EXPECT_TRUE(
almostEqualLists(info.com, [x * 0.5 for x in scale]),
"com" + error + " " + Tools.cat(info.com) + "!=" +
Tools.cat([x * 0.5 for x in scale]))
ok &= EXPECT_TRUE(
almostEqualLists(info.diagonal_inertia, inertia[m][s][d]),
"inertia" + error + " " + str(info.diagonal_inertia) +
"!=" + str(inertia[m][s][d]))
# testing diagonal inertia extraction from a rotated cuboid
mesh = "cube.obj"
mesh_path = path + mesh
scale = scales[3]
density = 1
theory = sorted(inertia[0][3][0])
for r in rotations:
info.setFromMesh(mesh_path, density, scale, r)
local = sorted(info.diagonal_inertia)
ok &= EXPECT_TRUE(
almostEqualLists(local, theory), "inertia " + str(local) + "!=" +
str(theory) + " (rotation=" + str(r) + ")")
# testing extracted inertia rotation
mesh = "rotated_cuboid_12_35_-27.obj"
mesh_path = path + mesh
density = 1
info.setFromMesh(mesh_path, density)
# theoretical results
scale = [2, 3, 1]
mass = density * scale[0] * scale[1] * scale[2]
inertiat = numpy.empty(3)
inertiat[0] = 1.0 / 12.0 * mass * (
scale[1] * scale[1] + scale[2] * scale[2]) # x
inertiat[1] = 1.0 / 12.0 * mass * (
scale[0] * scale[0] + scale[2] * scale[2]) # y
inertiat[2] = 1.0 / 12.0 * mass * (
scale[0] * scale[0] + scale[1] * scale[1]) # z
# used quaternion in mesh
q = Quaternion.normalized(
Quaternion.from_euler([
12 * math.pi / 180.0, 35 * math.pi / 180.0, -27 * math.pi / 180.0
]))
# corresponding rotation matrices (ie frame defined by columns)
mt = Quaternion.to_matrix(q)
m = Quaternion.to_matrix(info.inertia_rotation)
# matching inertia
idxt = numpy.argsort(inertiat)
idx = numpy.argsort(info.diagonal_inertia)
# checking if each axis/column are parallel (same or opposite for unitary vectors)
for i in xrange(3):
ok &= EXPECT_TRUE(
almostEqualLists(mt[:, idxt[i]].tolist(), m[:, idx[i]].tolist(),
1e-5)
or almostEqualLists(mt[:, idxt[i]].tolist(),
(-m[:, idx[i]]).tolist(), 1e-5),
"wrong inertia rotation")
# print mt[:,idxt]
# print m [:,idx ]
return ok
