Python quat函数代码示例

	def _get_local_quat(self):
		rot = self.get_fixed_attribute("r", default=vec3(0,0,0))
		qx = quat().fromAngleAxis(rot[0], (1, 0, 0))
		qy = quat().fromAngleAxis(rot[1], (0, 1, 0))
		qz = quat().fromAngleAxis(rot[2], (0, 0, 1))
		q = qz*qy*qx
		return q

    def __mul__(self, other):
        """Multiplication.

        >>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
        >>> print q*2.0
        (1.9378, 0.4320, 0.2160, 0.1080)
        >>> print 2.0*q
        (1.9378, 0.4320, 0.2160, 0.1080)
        >>> print q*q
        (0.8775, 0.4186, 0.2093, 0.1046)
        """
        T = type(other)
        # quat*scalar
        if T==types.FloatType or T==types.IntType or T==types.LongType:
            return quat(self.w*other, self.x*other, self.y*other, self.z*other)
        # quat*quat
        if isinstance(other, quat):
            w1,x1,y1,z1 = self.w,self.x,self.y,self.z
            w2,x2,y2,z2 = other.w,other.x,other.y,other.z
            return quat(w1*w2-x1*x2-y1*y2-z1*z2,
                        w1*x2+x1*w2+y1*z2-z1*y2,
                        w1*y2+y1*w2-x1*z2+z1*x2,
                        w1*z2+z1*w2+x1*y2-y1*x2)
        # unsupported
        else:
            # Try to delegate the operation to the other operand
            if getattr(other,"__rmul__",None)!=None:
                return other.__rmul__(self)
            else:
                raise TypeError("unsupported operand type for *")

   def __init__(self, beta=0, axis=Up, pos=Zero, beta_f=0, axis_f=Up, fwd=Fwd, left=Left, up=Up):
      """Sets angle, axis vector, and position vector;
      also sets up a coordinate axis.

      Since rotations require a normalized axis, we
      take the norm of the axis we want to rotate by.

      Note that the angle is given in bradians.

      Note that the position is set *without* altering
      the coordinate system.  To match the
      coordinate system with the current rotations,
      call match_coordinates()."""
      self.beta = beta
      self.axis = axis.norm()
      self.pos = pos

      self.beta_f = beta_f
      self.axis_f = axis_f

      # We'll set up the object's coordinate system now:
      self.Up = up
      self.Fwd = fwd
      self.Left = left

      self.coordination = quat(1)  # this is the identity rotation.

      # These are the dual quaternions used for the wireframe's position.
      # First, we rotate around the center; then translate; finally rotate again.
      self.init_rotation = quat(); self.init_rotation.rotation(self.beta, self.axis)
      self.translation = quat(); self.translation.translation(self.pos)
      self.final_rotation = quat(); self.final_rotation.rotation(self.beta_f, self.axis_f)

      self.orientation = self.final_rotation*self.translation*self.init_rotation
      self.world = self.orientation.matrix()

def marg_quats(*args, **kwargs):
   '''
   computes MARG sensor rotation quaternions
   '''
   marg_quat = kwargs.get('marg', quat(1))
   quats = {}
   for name in ['gyro', 'acc', 'mag']:
      sensor_quat = kwargs.get(name, quat(1))
      assert isinstance(sensor_quat, quat)
      quats[name] = marg_quat * sensor_quat
   return quats

 def getcoordsys(self):
    inv = self.coordination.inverse()
    
    up = quat()
    up.pureVector(self.Up)
    up = self.coordination*up*inv
    
    left = quat()
    left.pureVector(self.Left)
    left = self.coordination*left*inv
    
    fwd = quat()
    fwd.pureVector(self.Fwd)
    fwd = self.coordination*fwd*inv
    return (fwd.extractPureVector(), left.extractPureVector(), up.extractPureVector())

 def fromMat(self, m):
     try:
         return self._fromMat(m)
     except:
         pass
     bestqlist = []
     bestcnt = 0
     for exponent in range(2, 7):
         qlist = []
         dist = 10**-exponent
         for axis in [(-1,0,0),(+1,0,0),(0,-1,0),(0,1,0),(0,0,-1),(0,0,1)]:
             rot = m * _mat4.rotation(dist, axis)
             try:
                 qlist += [self._fromMat(rot)]
             except:
                 pass
         if len(qlist) >= bestcnt:
             bestcnt = len(qlist)
             bestqlist += qlist
         else:
             break
     qlist = bestqlist
     r = quat(0,0,0,0)
     for q in qlist:
         r += q
     r = r.normalize()
     #print("Got a matrix-2-quaternion lerp of", r, "using", len(qlist), "checks and dist", dist)
     self.w, self.x, self.y, self.z = r[:]
     return self

 def __pos__(self):
     """
     >>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
     >>> print +q
     (0.9689, 0.2160, 0.1080, 0.0540)
     """
     return quat(+self.w, +self.x, +self.y, +self.z)

def slerp(t, q0, q1, shortest=True):
    """Spherical linear interpolation between two quaternions.

    The return value is an interpolation between q0 and q1. For t=0.0
    the return value equals q0, for t=1.0 it equals q1.
    q0 and q1 must be unit quaternions.
    If shortest is True the interpolation is always done along the
    shortest path.
    """
    global _epsilon

    ca = q0.dot(q1)
    if shortest and ca<0:
        ca = -ca
        neg_q1 = True
    else:
        neg_q1 = False
    
    if ca>=1.0:
        o = 0.0
    elif ca<=-1.0:
        o = math.pi
    else:
        o = math.acos(ca)
    so = math.sin(o)

    if (abs(so)<=_epsilon):
        return quat(q0)

    a = math.sin(o*(1.0-t)) / so
    b = math.sin(o*t) / so
    if neg_q1:
        return q0*a - q1*b
    else:
        return q0*a + q1*b

    def log(self):
        """Return the natural logarithm of self."""
        global _epsilon
        
        b = math.sqrt(self.x*self.x + self.y*self.y + self.z*self.z)
        res = quat()
        if abs(b)<=_epsilon:
            res.x = 0.0
            res.y = 0.0
            res.z = 0.0
            if self.w<=_epsilon:
                raise ValueError("math domain error")
            res.w = math.log(self.w)
        else:
            t = math.atan2(b, self.w)
            f = t/b
            res.x = f*self.x
            res.y = f*self.y
            res.z = f*self.z
            ct = math.cos(t)
            if abs(ct)<=_epsilon:
                raise ValueError("math domain error")
            r = self.w/ct
            if r<=_epsilon:
                raise ValueError("math domain error")
            res.w = math.log(r)

        return res

    def __neg__(self):
        """Negation.

        >>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
        >>> print -q
        (-0.9689, -0.2160, -0.1080, -0.0540)
        """
        return quat(-self.w, -self.x, -self.y, -self.z)

 def conjugate(self):
     """Return conjugate.
     
     >>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
     >>> print q.conjugate()
     (0.9689, -0.2160, -0.1080, -0.0540)
     """
     return quat(self.w, -self.x, -self.y, -self.z)

   def translate_by_Left(self, velocity=1):
      """Tranlsates by the object's Left axis, multiplied by velocity."""

      # To avoid round-off errors, we'll keep the coordinate
      # axes constant, and just keep track of their orientation
      # (called self.coordination).

      # First, we need to calculate the Left vector.
      leftQuat = quat()
      leftQuat.pureVector(self.Left)
      left = self.coordination*leftQuat*self.coordination.inverse()

      # Second, we create the translation
      translate = quat()
      translate.translation(left.extractPureVector()*velocity)

      # Finally, we apply the translation!
      self.translation = self.translation*translate
      self.pos = self.translation.get_translation()

   def rotate_by_Up(self, beta=2):
      """Rotates by the object's up axis; also rotates
      the remaining two axes by the same amount."""

      # To avoid round-off errors, we'll keep the coordinate
      # axes constant, and just keep track of their orientation
      # (called self.coordination).
      upQuat = quat()
      upQuat.pureVector(self.Up)
      up = self.coordination*upQuat*self.coordination.inverse()

      rotation = quat()
      rotation.rotation(beta, up.extractPureVector())

      self.init_rotation = self.init_rotation*rotation
      self.beta, self.axis = self.init_rotation.get_angle_axis()

      # Now, we'll "rotate" the coordinate system, by applying
      # the same rotation to the coordinate system quaternion.
      self.coordination = rotation * self.coordination

 def get_final_local_transform(self):
         pm = self.xformparent.get_world_transform()
         wt = self.get_world_transform()
         if not self.phys_root:
                 wt = node.gettransformto(None, "inverse_initial_r")
         m = pm.inverse() * wt
         t, r, _ = m.decompose()
         q = quat(r).normalize()
         ps = pm.decompose()[2]
         pos = mat4.scaling(ps) * vec4(*t)
         return pos, q

    def __add__(self, other):
        """Addition.

        >>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
        >>> print q+q
        (1.9378, 0.4320, 0.2160, 0.1080)
        """
        if isinstance(other, quat):
            return quat(self.w+other.w, self.x+other.x,
                        self.y+other.y, self.z+other.z)
        else:
            raise TypeError("unsupported operand type for +")