def checkSocket(self, task):
r_read, r_write, r_error = select.select([self.udpSocket],[],[],0)
while r_read:
msg = self.udpSocket.recv(1024)
#print msg
msg_list = msg.split(',')
node_id = int(msg_list[0])
[w, x, y, z] = [float(val) for val in msg_list[1:5]]
seqNum = int(msg_list[5])
#print seqNum
q = Quat()
q.setR(w)
q.setI(x)
q.setJ(y)
q.setK(z)
self.jointCur[mapping[node_id]] = q
# update root
root = jointNames[0]
self.joints[root].setQuat(self.modelQ[root]*(self.jointCal[root]*self.jointCur[root]))
#other joints
for name in jointNames[1:]:
parentName = parents[name]
_q = self.jointCal[parentName]*self.jointCur[parentName]
_q = _q.conjugate()
self.joints[name].setQuat((self.jointCal[name]*self.jointCur[name])*_q*self.modelQ[name])
r_read, r_write, r_error = select.select([self.udpSocket],[],[],0)
return Task.cont
def getRotationTo(src, dest, fallbackAxis=Vec3_ZERO):
# Quaternion q;
# Vector3 v0 = *this;
# Vector3 v1 = dest;
# v0.normalise();
# v1.normalise();
q = Quat()
v0 = Vec3(src)
v1 = Vec3(dest)
v0.normalize()
v1.normalize()
# Real d = v0.dotProduct(v1);
d = v0.dot(v1)
# if (d >= 1.0f)
# {
# return Quaternion::IDENTITY;
# }
if d >= 1.0:
return Quat(1, 0, 0, 0)
# if (d < (1e-6f - 1.0f))
if d < (1.0e-6 - 1.0):
# if (fallbackAxis != Vector3::ZERO)
# {
# // rotate 180 degrees about the fallback axis
# q.FromAngleAxis(Radian(Math::PI), fallbackAxis);
# }
if fallbackAxis != Vec3_ZERO:
q.setFromAxisAngleRad(pi, fallbackAxis)
# else
# {
# // Generate an axis
# Vector3 axis = Vector3::UNIT_X.crossProduct(*this);
# if (axis.isZeroLength()) // pick another if colinear
# axis = Vector3::UNIT_Y.crossProduct(*this);
# axis.normalise();
# q.FromAngleAxis(Radian(Math::PI), axis);
# }
else:
axis = Vec3(1, 0, 0).cross(src)
if axis.almostEqual(Vec3.zero()):
axis = Vec3(0, 1, 0).cross(src)
axis.normalize()
q.setFromAxisAngleRad(pi, axis)
# else
# {
# Real s = Math::Sqrt( (1+d)*2 );
# Real invs = 1 / s;
# Vector3 c = v0.crossProduct(v1);
# q.x = c.x * invs;
# q.y = c.y * invs;
# q.z = c.z * invs;
# q.w = s * 0.5f;
# q.normalise();
# }
else:
s = sqrt((1 + d) * 2)
invs = 1 / s
c = v0.cross(v1)
q.setI(c.x * invs)
q.setJ(c.y * invs)
q.setK(c.z * invs)
q.setR(s * .5)
q.normalize()
return q