def build_link(self, node):
(id_bda, ta), (id_bdb, tb) = self.link_building_status[node]
bda = self.factory.multiboxes[id_bda]
bdb = self.factory.multiboxes[id_bdb]
pos = bda.body.getPosition()
quat = LQuaternionf(bda.body.getQuaternion())
mat = TransformState.makePosQuatScale(pos, quat, Vec3(1, 1, 1)).getMat()
mat = ta.getMat() * mat
print "ta", ta
print "absol", TransformState.makeMat(mat)
mat = LMatrix4f.translateMat(Vec3(0.5, 0, 0)) * mat
anchor = TransformState.makeMat(mat).getPos()
print "absol", TransformState.makeMat(mat)
axis = self.quat_dict[1][1]
if node.orientation == 1:
t = LMatrix4f.rotateMat(*self.quat_dict[4]) * mat
else:
t = LMatrix4f.rotateMat(*self.quat_dict[2]) * mat
row = t.getRow(0)
print "rotation", t.getRow(0), type(t.getRow(0))
#axis = t.getQuat().getAxis()
axis = Vec3(row.getX(), row.getY(), row.getZ())
print "axis",axis
print "anchor", anchor
mat = LMatrix4f.translateMat(Vec3(0.5, 0, 0)) * mat
mat = tb.getInverse().getMat() * mat
t = TransformState.makeMat(mat)
posb = t.getPos()
quatb = t.getQuat()
bdb.body.setPosition(posb)
bdb.body.setQuaternion(quatb)
cs = OdeHingeJoint(self.physics.world, self.physics.servogroup)
cs.attach(bda.body, bdb.body)
cs.setAxis(axis)
cs.setAnchor(anchor)
#add the motor
cs.setParamFMax(self.satu_cmd)
cs.setParamFudgeFactor(0.5)
cs.setParamCFM(11.1111)
cs.setParamStopCFM(11.1111)
cs.setParamStopERP(0.444444)
cs.setParamLoStop(- math.pi * 0.5)
cs.setParamHiStop(math.pi * 0.5)
pid = PID()
self.dof_motors[node] = (cs, pid)
print "add constraint"
def change_transform(self, transform, face, type='shape'):
""" change to transform to go on a face """
#print " change face to {}".format(face)
mat = transform.getMat()
mat = LMatrix4f.rotateMat(*self.quat_dict[face]) * mat
mat = LMatrix4f.translateMat(Vec3(1.0, 0, 0)) * mat
transform = transform.makeMat(mat)
return transform
def change_back_transform(self, transform, face, type='bloc'):
#print "back transform"
mat = transform.getMat()
mult = LMatrix4f.rotateMat(*self.quat_dict[face])
mult.invertInPlace()
mat = LMatrix4f.translateMat(Vec3(-1.0, 0, 0)) * mat
mat = mult * mat
transform = transform.makeMat(mat)
return transform
