rotationsx.py 文件源码

def cvt_inert2att_Q_to_angles(Q):
   """Creates a angle tuple from Q, assuming an inertial to attitude Q"""
   V1_body = Vector(1.,0.,0.)
   V1_eci_pt = Q.inv_cnvrt(V1_body)
   coord1  = math.atan2(V1_eci_pt.y,V1_eci_pt.x)
   if coord1 < 0. : coord1 += PI2
   coord2 = math.asin(unit_limit(V1_eci_pt.z))

   V3_body = Vector(0.,0.,1.)
   V3_eci_pt = Q.inv_cnvrt(V3_body)
   NP_eci = Vector(0.,0.,1.)
   V_left = cross(NP_eci,V1_eci_pt)
   V_left = V_left / V_left.length()
   NP_in_plane = cross(V1_eci_pt,V_left)
   x = dot(V3_eci_pt,NP_in_plane)
   y = dot(V3_eci_pt,V_left)
   pa  = math.atan2(y,x)
   if pa < 0. : pa += PI2
   return (coord1,coord2,pa)