def addOffset(self, name, offset=[0,0,0,0,0,0,1], index=-1):
## adding a relative offset to the rigid body (e.g. used as a joint location)
# @warning the translation due to the center of mass offset is automatically removed. If necessary a function without this mecanism could be added
#return ShearlessAffineBody.Offset(self.rigidNode, self.scaleNode, name, (self.bodyOffset*Frame.Frame(offset)).offset(), index) # this line does not cover the case where the shapeFunction is within the affineNode
if index > -1:
return ShearlessAffineBody.Offset(self.rigidNode, self.scaleNode, name, (self.bodyOffset*Frame.frame(offset)).offset(), index)
else:
# computation of absolute position of the offset
offset_abs = self.bodyOffset*Frame.Frame(offset)
# computation of the index of the closest point to the offset
ind = 0
min_dist = sys.float_info.max
for i, p in enumerate(self.frame):
dist = numpy.linalg.norm(offset_abs.translation - numpy.array(p.translation), 2)
if(dist < min_dist):
min_dist = dist
ind = i
# add of the offset according to this position
offset_computed = (self.frame[ind]*offset_abs).offset()
return ShearlessAffineBody.Offset(self.rigidNode, self.scaleNode, name, offset_computed, ind)
def addAbsoluteMappedPoint(self, name, position=[0,0,0]):
## adding a position given in absolute coordinates to the rigid body
frame = Frame.Frame(); frame.translation = position
return ShearlessAffineBody.MappedPoint(self.affineNode, name, offset)
def addMappedPoint(self, name, relativePosition=[0,0,0]):
## adding a relative position to the rigid body
# @warning the translation due to the center of mass offset is automatically removed. If necessary a function without this mecanism could be added
frame = Frame.Frame(); frame.translation = relativePosition
return ShearlessAffineBody.MappedPoint(self.affineNode, name, offset)
def addVisualModel(self, filepath, scale3d=[1,1,1], offset=[0,0,0,0,0,0,1], name_suffix='', generatedDir=None):
## adding a visual model to the rigid body with a relative offset
self.visual = ShearlessAffineBody.VisualModel(self.affineNode, filepath, scale3d, (self.bodyOffset*Frame.Frame(offset)).offset(), name_suffix, generatedDir=generatedDir)
return self.visual
def addCollisionMesh(self, filepath, scale3d=[1,1,1], offset=[0,0,0,0,0,0,1], name_suffix='', generatedDir=None):
## adding a collision mesh to the rigid body with a relative offset
## body offset is added to the offset
# (only a Triangle collision model is created, more models can be added manually)
self.collision = ShearlessAffineBody.CollisionMesh(self.affineNode, filepath, scale3d, (self.bodyOffset*Frame.Frame(offset)).offset(), name_suffix, generatedDir=generatedDir)
return self.collision
def setManually(self, filepath=None, offset=[[0,0,0,0,0,0,1]], voxelSize=0.01, density=1000, mass=1, inertia=[1,1,1], inertia_forces=False, generatedDir=None):
if len(offset) == 0:
Sofa.msg_error("RigidScale.API","ShearlessAffineBody should have at least 1 ShearLessAffine")
return
self.framecom = Frame.Frame()
self.bodyOffset = Frame.Frame([0,0,0,0,0,0,1])
path_affine_rigid = '@' + Tools.node_path_rel(self.affineNode, self.rigidNode)
path_affine_scale = '@' + Tools.node_path_rel(self.affineNode, self.scaleNode)
if len(offset) == 1:
self.frame = [Frame.Frame(offset[0])]
str_position = ""
for p in offset:
str_position = str_position + concat(p) + " "
### scene creation
# rigid dof
self.rigidDofs = self.rigidNode.createObject('MechanicalObject', template='Rigid3'+template_suffix, name='dofs', position=str_position, rest_position=str_position)
# scale dofs
self.scaleDofs = self.scaleNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='dofs', position=concat([1,1,1]*len(offset)))
# The positiveNode is now commented for the moment since it seems not working
"""positiveNode = self.scaleNode.createChild('positive')
positiveNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='positivescaleDOFs')
positiveNode.createObject('DifferenceFromTargetMapping', template='Vec3'+template_suffix+',Vec3'+template_suffix, applyRestPosition=1, targets=concat(target_scale))
positiveNode.createObject('UniformCompliance', isCompliance=1, compliance=0)
positiveNode.createObject('UnilateralConstraint')
positiveNode.createObject('Stabilization', name='Stabilization')"""
# affine dofs
self.affineDofs = self.affineNode.createObject('MechanicalObject', template='Affine', name='parent', showObject=0)
self.affineNode.createObject('RigidScaleToAffineMultiMapping', template='Rigid,Vec3,Affine', input1=path_affine_rigid, input2=path_affine_scale, output='@.', autoInit='1', printLog='0')
if filepath:
self.image = SofaImage.API.Image(self.rigidNode, name="image_" + self.name, imageType="ImageUC")
self.shapeFunction = Flexible.API.ShapeFunction(self.affineNode)
if generatedDir is None:
self.image.addMeshLoader(filepath, value=1, insideValue=1) # TODO support multiple meshes closingValue=1,
self.image.addMeshToImage(voxelSize)
self.shapeFunction.addVoronoi(self.image, position='@dofs.rest_position')
# mass
self.affineMassNode = self.affineNode.createChild('mass')
self.affineMassNode.createObject('TransferFunction', name='density', template='ImageUC,ImageD', inputImage='@' + Tools.node_path_rel(self.affineMassNode, self.image.node) + '/image.image', param='0 0 1 '+str(density))
self.affineMassNode.createObject('MechanicalObject', template='Vec3'+template_suffix)
self.affineMassNode.createObject('LinearMapping', template='Affine,Vec3'+template_suffix)
self.affineMassNode.createObject('MassFromDensity', name='MassFromDensity', template='Affine,ImageD', image='@density.outputImage', transform='@' + Tools.node_path_rel(self.affineMassNode, self.image.node) + '/image.transform', lumping='0')
self.mass = self.affineNode.createObject('AffineMass', massMatrix='@mass/MassFromDensity.massMatrix')
else:
self.image.addContainer(filename=self.node.name + "_rasterization.raw", directory=generatedDir)
self.shapeFunction.shapeFunction = serialization.importImageShapeFunction(self.affineNode, generatedDir+self.node.name+"_SF_indices.raw", generatedDir+self.node.name+"_SF_weights.raw", 'dofs')
self.mass = serialization.importAffineMass(self.affineNode, generatedDir+self.node.name+"_affinemass.json")
# computation of the object mass center
massInfo = SofaPython.mass.RigidMassInfo()
massInfo.setFromMesh(filepath, density, [1,1,1])
# get the object mass center
self.framecom.rotation = massInfo.inertia_rotation
self.framecom.translation = massInfo.com
else:
if (mass and inertia) and inertia != [0,0,0]:
Sofa.msg_info("RigidScale", "A RigidMass and a UniformMass are created for respectively the rigid and the scale since there is no mesh which can be used to compute the model mass.")
self.mass = self.rigidNode.createObject('RigidMass', name='mass', mass=mass, inertia=concat(inertia[:3]), inertia_forces=inertia_forces)
self.scaleNode.createObject('UniformMass', name='mass', mass=mass)
self.frame = []
for o in offset:
self.frame.append(Frame.Frame(o))