def setup(self, parentNode, density=2000, param=None, generatedDir = None ):
# Computation the offset according to the attribute self.transform
offset = [0,0,0,0,0,0,1]
offset[:3] = self.transform[:3]
offset[3:] = Quaternion.from_euler(self.transform[3:6])
scale3d = self.transform[6:]
if self.type not in BoneType:
self.type = "short" # --> to set 1 frame on bone which not have a type
# Creation of the shearless affine body
self.body = RigidScale.API.ShearlessAffineBody(parentNode, self.name)
# Depending on the frame set in the constructor, let decide how the body will be initialized
if (self.frame == None) or (len(self.frame) < FramePerBoneType[self.type]):
self.body.setFromMesh(self.filepath, density, offset, scale3d, self.voxelSize, FramePerBoneType[self.type], generatedDir=generatedDir)
for p in self.body.frame: self.frame.append(p.offset())
else:
scale3dList = []
for i in range(len(self.frame)):
scale3dList.append(scale3d)
self.body.setManually(self.filepath, self.frame, self.voxelSize, density=1000, generatedDir=generatedDir)
# Add of the behavior model, the collision model and the visual model
localGeneratedDir=None if generatedDir is None else generatedDir+self.name
self.behavior = self.body.addBehavior(self.elasticity, IntegrationPointPerBoneType[self.type], generatedDir=localGeneratedDir )
self.collision = self.body.addCollisionMesh(self.filepath, scale3d, offset, generatedDir=localGeneratedDir)
self.visual = self.collision.addVisualModel()
return self.body
def setup(self, parentNode, density=2000, param=None, generatedDir=None):
# Computation the offset according to the attribute self.transform
offset = [0, 0, 0, 0, 0, 0, 1]
offset[:3] = self.transform[:3]
offset[3:] = Quaternion.from_euler(self.transform[3:6])
scale3d = self.transform[6:]
if self.type not in BoneType:
self.type = "short" # --> to set 1 frame on bone which not have a type
# Creation of the shearless affine body
self.body = RigidScale.API.ShearlessAffineBody(parentNode, self.name)
# Depending on the frame set in the constructor, let decide how the body will be initialized
if (self.frame
== None) or (len(self.frame) < FramePerBoneType[self.type]):
self.body.setFromMesh(self.filepath, density, offset, scale3d,
self.voxelSize, FramePerBoneType[self.type])
else:
scale3dList = []
for i in range(len(self.frame)):
scale3dList.append(scale3d)
self.body.setManually(self.filepath,
self.frame,
self.voxelSize,
density=1000,
generatedDir=generatedDir)
# Add of the behavior model, the collision model and the visual model
localGeneratedDir = None if generatedDir is None else generatedDir + self.name
self.behavior = self.body.addBehavior(
self.elasticity,
IntegrationPointPerBoneType[self.type],
generatedDir=localGeneratedDir)
self.collision = self.body.addCollisionMesh(
self.filepath, scale3d, offset, generatedDir=localGeneratedDir)
self.visual = self.collision.addVisualModel()
return self.body
def onBeginAnimationStep(self, dt):
# current mu from current plane angle
currentMu = math.tan( self.currentAngle )
if self.counter < 100 : # does not rotate the plane for 100 time steps
self.counter += 1
return 0
# is it a mu we want to test?
if numpy.allclose( self.muToTest, currentMu, 1e-3, 1e-3 ) :
# at the end of 100 time steps, check if the box was sticking or sliding
self.counter = 0
self.muToTest += 0.1
# look at the box velocity along its x-axis
localbox = Quaternion.rotate(Quaternion.conj( Frame.Frame( shared.plane.position[0] ).rotation ),
shared.box.velocity[0][:3])
vel = localbox[0]
#print 'plane/ground angle:', self.currentAngle
#print 'velocity:',vel
#print shared.box.position[0], shared.box.velocity[0][:3]
#print vel, currentMu, shared.mu
testVel = (vel > 1e-1)
if testVel:
testMu = (currentMu>=shared.mu-1e-2)
else:
testMu = (currentMu>=shared.mu)
EXPECT_FALSE( testVel ^ testMu, str(vel)+' '+str(currentMu)+'mu='+str(shared.mu) ) # xor
#print testVel, testMu
#sys.stdout.flush()
# all finished
if currentMu >= shared.mu + .1:
self.sendSuccess()
# update plane orientation
self.currentAngle += 0.001
q = Quaternion.from_euler( [0,0,-self.currentAngle] )
p = shared.plane.position
p[0] = [0,0,0,q[3],q[2],q[1],q[0]]
shared.plane.position = p
return 0
def onBeginAnimationStep(self, dt):
# current mu from current plane angle
currentMu = math.tan(self.currentAngle)
if self.counter < 100: # does not rotate the plane for 100 time steps
self.counter += 1
return 0
# is it a mu we want to test?
if numpy.allclose(self.muToTest, currentMu, 1e-3, 1e-3):
# at the end of 100 time steps, check if the box was sticking or sliding
self.counter = 0
self.muToTest += 0.1
# look at the box velocity along its x-axis
localbox = Quaternion.rotate(
Quaternion.conj(
Frame.Frame(shared.plane.position[0]).rotation),
shared.box.velocity[0][:3])
vel = localbox[0]
#print 'plane/ground angle:', self.currentAngle
#print 'velocity:',vel
#print shared.box.position[0], shared.box.velocity[0][:3]
# print vel, currentMu, shared.mu
testVel = (vel > 1e-1)
if testVel:
testMu = (currentMu >= shared.mu - 1e-2)
else:
testMu = (currentMu >= shared.mu)
EXPECT_FALSE(testVel ^ testMu,
str(vel) + ' ' + str(currentMu) + 'mu=' +
str(shared.mu)) # xor
#print testVel, testMu
#sys.stdout.flush()
# all finished
if currentMu >= shared.mu + .1:
self.sendSuccess()
# update plane orientation
self.currentAngle += 0.001
q = Quaternion.from_euler([0, 0, -self.currentAngle])
p = shared.plane.position
p[0] = [0, 0, 0, q[3], q[2], q[1], q[0]]
shared.plane.position = p
return 0
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
