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