コード例 #1
0
ファイル: animation.py プロジェクト: buguen/minf
  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
コード例 #2
0
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