python类atan2()的实例源码

def __init__(self, sprites, dest_pos, speed=None):
        """ Initializes an object of SpriteMove class.
        :param sprites: list of card sprites to be moved
        :param dest_pos: tuple with coordinates (x,y) of destination position
        :param speed: integer number, on how many pixels card(s) should move per frame.
                    If not specified (None), "move_speed" value from the config json is used.
        """
        self.sprites = sprites
        self.dest_pos = dest_pos
        for sprite in self.sprites:
            sprite.start_pos = sprite.pos
            sprite.angle = math.atan2(dest_pos[1] - sprite.start_pos[1],
                                      dest_pos[0] - sprite.start_pos[0])
            sprite.distance = SpriteMove.calc_distance(dest_pos, sprite.start_pos)
            if speed is None:
                sprite.speed = CardSprite.card_json["move_speed"]
            else:
                sprite.speed = speed
            sprite.completed = False

def ComputeCoverage(sigma_points, bias):
    def ang(p):
        v = rotvecquat(vector.sub(p.compass, bias), vec2vec2quat(p.down, [0, 0, 1]))
        return math.atan2(v[1], v[0])

    angles = sorted(map(ang, sigma_points))
    #print 'angles', angles

    max_diff = 0
    for i in range(len(angles)):
        diff = -angles[i]
        j = i+1
        if j == len(angles):
            diff += 2*math.pi
            j = 0
        diff += angles[j]
        max_diff = max(max_diff, diff)
    return max_diff

def eci_to_latlon(pos, phi_0=0):
    (x, y, z) = pos
    rg = (x*x + y*y + z*z)**0.5
    z = z/rg
    if abs(z) > 1.0:
        z = int(z)

    lat = degrees(asin(z))
    lon = degrees(atan2(y, x)-phi_0)
    if lon > 180:
        lon -= 360
    elif lon < -180:
        lon += 360
    assert -90 <= lat <= 90
    assert -180 <= lon <= 180
    return lat, lon

def quat2euler(q0, q1, q2, q3):
    """
    Converts quaterion to (pitch, yaw, roll).
    Values are in -PI to PI range.
    """
    qq0, qq1, qq2, qq3 = q0**2, q1**2, q2**2, q3**2
    xa = qq0 - qq1 - qq2 + qq3
    xb = 2 * (q0 * q1 + q2 * q3)
    xn = 2 * (q0 * q2 - q1 * q3)
    yn = 2 * (q1 * q2 + q0 * q3)
    zn = qq3 + qq2 - qq0 - qq1

    pitch = atan2(xb , xa)
    yaw   = atan2(xn , sqrt(1 - xn**2))
    roll  = atan2(yn , zn)
    return pitch, yaw, roll

def compute_side(self, x, y):
        """ Computes which sides of dpad are supposed to be active """
        ## dpad(up, down, left, right)
        ## dpad8(up, down, left, right, upleft, upright, downleft, downright)
        side = self.SIDE_NONE
        if x*x + y*y > self.MIN_DISTANCE_P2:
            # Compute angle from center of pad to finger position
            angle = (atan2(x, y) * 180.0 / PI) + 180
            # Translate it to index
            index = 0
            for a1, a2, i in self.ranges:
                if angle >= a1 and angle < a2:
                    index = i
                    break
            side = self.SIDES[index]
        return side

def mode_LINEAR(self, x, y, range):
        """
        Input value is scaled, so entire output range is covered by
        reduced input range of deadzone.
        """
        if y == 0:
            # Small optimalization for 1D input, for example trigger
            return copysign(
                clamp(
                    0,
                    ((x - self.lower) / (self.upper - self.lower)) * range,
                    range),
                x
            ), 0
        distance = clamp(self.lower, sqrt(x*x + y*y), self.upper)
        distance = (distance - self.lower) / (self.upper - self.lower) * range

        angle = atan2(x, y)
        return distance * sin(angle), distance * cos(angle)

def getStatistics(circle_x, circle_y, bar1_x, bar1_y, bar2_x, bar2_y):
    out = [0, 0, 0]
    midX = GLOBAL_WIDTH / 2
    midY = GLOBAL_HEIGHT / 2
    dx = midX - circle_x
    dy = midY - circle_y
    rads = atan2(-dy, dx)
    rads %= 2*pi
    angle = degrees(rads)
    if  (bar1_x - circle_x)**2 != 0:
        p1Distance = sqrt((bar1_y - circle_y)**2 / (bar1_x - circle_x)**2)
    if (bar2_x - circle_x)**2 != 0:
        p2Distance = sqrt((bar2_y - circle_y)**2 / (bar2_x - circle_x)**2)
    out[0] = angle
    out[1] = p1Distance
    out[2] = p2Distance
    return out

#determines how to move padel based on neural net input

def get_earth_dist(pt_a, pt_b=None):
    if type(pt_a) is str or pt_b is None:
        return 'unkn'  # No location set
    log.debug("Calculating distance from {} to {}".format(pt_a, pt_b))
    lat_a = radians(pt_a[0])
    lng_a = radians(pt_a[1])
    lat_b = radians(pt_b[0])
    lng_b = radians(pt_b[1])
    lat_delta = lat_b - lat_a
    lng_delta = lng_b - lng_a
    a = sin(lat_delta / 2) ** 2 + cos(lat_a) * cos(lat_b) * sin(lng_delta / 2) ** 2
    c = 2 * atan2(sqrt(a), sqrt(1 - a))
    radius = 6373000  # radius of earth in meters
    if config['UNITS'] == 'imperial':
        radius = 6975175  # radius of earth in yards
    dist = c * radius
    return dist


# Return the time as a string in different formats

def pose_2_xyzrpw(H):
    """Calculates the equivalent position and euler angles ([x,y,z,r,p,w] array) of the given pose according to the following operation:
    Note: transl(x,y,z)*rotz(w*pi/180)*roty(p*pi/180)*rotx(r*pi/180)
    See also: xyzrpw_2_pose()

    :param H: pose
    :type H: :class:`.Mat`"""
    x = H[0,3]
    y = H[1,3]
    z = H[2,3]
    if (H[2,0] > (1.0 - 1e-6)):
        p = -pi/2
        r = 0
        w = math.atan2(-H[1,2],H[1,1])
    elif H[2,0] < -1.0 + 1e-6:
        p = pi/2
        r = 0
        w = math.atan2(H[1,2],H[1,1])
    else:
        p = math.atan2(-H[2,0],sqrt(H[0,0]*H[0,0]+H[1,0]*H[1,0]))
        w = math.atan2(H[1,0],H[0,0])
        r = math.atan2(H[2,1],H[2,2])    
    return [x, y, z, r*180/pi, p*180/pi, w*180/pi]

def Pose_2_KUKA(H):
    """Converts a pose (4x4 matrix) to a Kuka target

    :param H: pose
    :type H: :class:`.Mat`"""
    x = H[0,3]
    y = H[1,3]
    z = H[2,3]
    if (H[2,0]) > (1.0 - 1e-6):
        p = -pi/2
        r = 0
        w = atan2(-H[1,2],H[1,1])
    elif (H[2,0]) < (-1.0 + 1e-6):
        p = pi/2
        r = 0
        w = atan2(H[1,2],H[1,1])
    else:
        p = atan2(-H[2,0],sqrt(H[0,0]*H[0,0]+H[1,0]*H[1,0]))
        w = atan2(H[1,0],H[0,0])
        r = atan2(H[2,1],H[2,2])
    return [x, y, z, w*180/pi, p*180/pi, r*180/pi]

def get_center_of_nodes(nodes):
        """Helper function to get center coordinates of a group of nodes

        """
        x = 0
        y = 0
        z = 0

        for node in nodes:
            lat = radians(float(node.lat))
            lon = radians(float(node.lon))

            x += cos(lat) * cos(lon)
            y += cos(lat) * sin(lon)
            z += sin(lat)

        x = float(x / len(nodes))
        y = float(y / len(nodes))
        z = float(z / len(nodes))

        center_lat = degrees(atan2(z, sqrt(x * x + y * y)))
        center_lon = degrees(atan2(y, x))

        return center_lat, center_lon

def get_crow_fly_distance(from_tuple, to_tuple):
    """
    Uses the Haversine formmula to compute distance
    (https://en.wikipedia.org/wiki/Haversine_formula#The_haversine_formula)
    """
    lat1, lon1 = from_tuple
    lat2, lon2 = to_tuple

    lat1 = float(lat1)
    lat2 = float(lat2)
    lon1 = float(lon1)
    lon2 = float(lon2)

    radius = 6371  # km

    dlat = math.radians(lat2 - lat1)
    dlon = math.radians(lon2 - lon1)
    a = math.sin(dlat / 2) * math.sin(dlat / 2) + math.cos(math.radians(lat1)) * \
        math.cos(math.radians(lat2)) * math.sin(dlon / 2) * math.sin(dlon / 2)
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
    d = radius * c

    return d * 1000  # meters

def _intersect_circle(self, center, radius, x):
        """ upper intersection of line parallel to y axis and a circle
            where line is given by x origin
            circle by center, radius as float
            return float y of upper intersection point, float angle
        """
        dx = x - center.x
        d = (radius * radius) - (dx * dx)
        if d <= 0:
            if x > center.x:
                return center.y, 0
            else:
                return center.y, pi
        else:
            y = sqrt(d)
            return center.y + y, atan2(y, dx)

def _intersect_elipsis(self, center, radius, x):
        """ upper intersection of line parallel to y axis and an ellipsis
            where line is given by x origin
            circle by center, radius.x and radius.y semimajor and seminimor axis (half width and height) as float
            return float y of upper intersection point, float angle
        """
        dx = x - center.x
        d2 = dx * dx
        A = 1 / radius.y / radius.y
        C = d2 / radius.x / radius.x - 1
        d = - 4 * A * C
        if d <= 0:
            if x > center.x:
                return center.y, 0
            else:
                return center.y, pi
        else:
            y0 = sqrt(d) / 2 / A
            d = (radius.x * radius.x) - d2
            y = sqrt(d)
            return center.y + y0, atan2(y, dx)

def rotate(self, a):
        """
            Rotate center so we rotate ccw arround p0
        """
        ca = cos(a)
        sa = sin(a)
        rM = Matrix([
            [ca, -sa],
            [sa, ca]
            ])
        p0 = self.p0
        self.c = p0 + rM * (self.c - p0)
        dp = p0 - self.c
        self.a0 = atan2(dp.y, dp.x)
        return self

    # make offset for line / arc, arc / arc

def Encode(self, normal):
        x = normal[0]
        y = normal[1]
        z = normal[2]
        # normalize
        l = math.sqrt((x*x) + (y*y) + (z*z))
        if l == 0:
            return 0
        x = x/l
        y = y/l
        z = z/l

        if (x == 0.0) & (y == 0.0) :
            if z > 0.0:
                return 0
            else:
                return (128 << 8)

        lng = math.acos(z) * 255 / (2 * math.pi)
        lat = math.atan2(y, x) * 255 / (2 * math.pi)
        retval = ((int(lat) & 0xFF) << 8) | (int(lng) & 0xFF)
        return retval

def recordCoordinate(item, p):
    x, y, z = p

    # precalculate angle from wheel center (0, -20, 13.9)
    dy = y + 20
    dz = z - 14.75
    theta = atan2(dy, dz)
    if theta < 0:
        theta = 2 * pi + theta

    r = sqrt(dy * dy + dz * dz)
    zr = cos(theta)
    yr = sin(theta)

    item['x'] = x
    item['y'] = y
    item['z'] = z
    item['theta'] = theta
    item['r'] = r

    # for backwards compatibility, remove in future?
    item['coord'] = (x, y, z, theta, r, zr, yr)

def recordCoordinate(item, p):
    x, y, z = p

    # precalculate angle from wheel center (0, -20, 13.9)
    dy = y + 20
    dz = z - 14.75
    theta = atan2(dy, dz)
    if theta < 0:
        theta = 2 * pi + theta

    r = sqrt(dy * dy + dz * dz)
    zr = cos(theta)
    yr = sin(theta)

    item['x'] = x
    item['y'] = y
    item['z'] = z
    item['theta'] = theta
    item['r'] = r

    # for backwards compatibility, remove in future?
    item['coord'] = (x, y, z, theta, r, zr, yr)

def drawBallVelocity(self, painter):
        ballPos = self.ballOdom.pose.pose.position
        ballVel = self.ballOdom.twist.twist.linear

        if math.hypot(ballVel.x, ballVel.y) < 1.0:
            return 

        angleOfSpeed = math.atan2(ballVel.y, ballVel.x)

        paintDist = 10.0

        velPosX = paintDist * math.cos(angleOfSpeed) + ballPos.x
        velPosY = paintDist * math.sin(angleOfSpeed) + ballPos.y

        ballPosPoint = self.convertToDrawWorld(ballPos.x, ballPos.y)
        velPosPoint = self.convertToDrawWorld(velPosX, velPosY)

        painter.setPen(QPen(QColor(102,0,255),2))
        painter.drawLine(ballPosPoint, velPosPoint)

def getVelocity(self, cur, goal, dT):
        desired = Pose()
        d = self.getGoalDistance(cur, goal)
        a = atan2(goal.y - cur.y, goal.x - cur.x) - cur.theta
        b = cur.theta + a - goal.theta

        if abs(d) < self.linearTolerance:
            desired.xVel = 0
            desired.thetaVel = -self.kB * b
        else:
            desired.xVel = self.kP * d
            desired.thetaVel = self.kA*a + self.kB*b

        # Adjust velocities if linear or angular rates or accel too high.
        # TBD

        return desired

def update_velocity(self):
        HALF_WIDTH = const.WINDOW_WIDTH//2 - self.width//2
        if (const.BOMB_X+self.width/2) - self.dest_x == 0:
            self.vel_x = 0
            self.vel_y = self.SPEED
        elif (const.BOMB_Y+self.height/2) - self.dest_y == 0:
            if self.dest_x <= const.BOMB_X:
                ''' note the equals '''
                self.vel_x = -self.SPEED
                self.vel_y = 0
            elif self.dest_x > const.BOMB_X:
                self.vel_x = self.SPEED
                self.vel_y = 0
        else:
            '''(x+width/2, y+height/2) is the centre of the projectile image'''
            dy = self.dest_y - (const.BOMB_Y+self.height/2)
            dx = self.dest_x - (const.BOMB_X+self.width/2)
            radians = math.atan2(dy, dx)
            self.vel_y = math.sin(radians)*self.SPEED
            self.vel_x = math.cos(radians)*self.SPEED
            degrees = radians * 180.0 / math.pi

def update_velocity(self):
        HALF_WIDTH = const.WINDOW_WIDTH//2 - self.width//2

        if (const.FROST_POTION_X+self.width/2) - self.dest_x == 0:
            self.vel_x = 0
            self.vel_y = self.SPEED
        elif (const.FROST_POTION_Y+self.height/2) - self.dest_y == 0:
            if self.dest_x <= const.BOMB_X:
                ''' note the equals '''
                self.vel_x = -self.SPEED
                self.vel_y = 0
            elif self.dest_x > const.BOMB_X:
                self.vel_x = self.SPEED
                self.vel_y = 0
        else:
            dy = self.dest_y - (const.FROST_POTION_Y+self.height/2)
            dx = self.dest_x - (const.FROST_POTION_X+self.width/2)
            radians = math.atan2(dy, dx)# + math.pi/2
            self.vel_y = math.sin(radians)*self.SPEED
            self.vel_x = math.cos(radians)*self.SPEED
            degrees = radians * 180.0 / math.pi

def gps_newpos(lat, lon, bearing, distance):
    """Extrapolate latitude/longitude given a heading and distance
    thanks to http://www.movable-type.co.uk/scripts/latlong.html .
    """
    from math import sin, asin, cos, atan2, radians, degrees

    lat1 = radians(lat)
    lon1 = radians(lon)
    brng = radians(bearing)
    dr = distance / radius_of_earth

    lat2 = asin(sin(lat1) * cos(dr) +
                cos(lat1) * sin(dr) * cos(brng))
    lon2 = lon1 + atan2(sin(brng) * sin(dr) * cos(lat1),
                        cos(dr) - sin(lat1) * sin(lat2))
    return (degrees(lat2), degrees(lon2))

def robot_listener(self):
        '''
        rospy.wait_for_service('spawn')
        spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
        spawner(4, 2, 0, 'turtle2')
        '''
        robot_vel_pub = rospy.Publisher('robot_0/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)

        rate = rospy.Rate(10.0)
        while not rospy.is_shutdown():
            try:
                (trans,rot) = self.listener.lookupTransform('/robot_3', '/robot_0', rospy.Time(0))
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue
            angular = 4 * math.atan2(trans[1], trans[0])
            linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
            cmd = geometry_msgs.msg.Twist()
            cmd.linear.x = linear
            cmd.angular.z = angular
            robot_vel_pub.publish(cmd)

            rate.sleep()

def ComplementaryFilter(gyrData, accData, pitch, roll):

    pitchAcc = 0.0
    rollAcc = 0.0

    # Integrate the gyroscope data -> int(angularSpeed) = angle
    pitch += (gyrData[0] / GYROSCOPE_SENSITIVITY) * \
        dt  # Angle around the X-axis
    roll -= (gyrData[1] / GYROSCOPE_SENSITIVITY) * \
        dt  # Angle around the Y-axis

    # Compensate for drift with accelerometer data if !bullshit
    # Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192
    forceMagnitudeApprox = abs(accData[0]) + abs(accData[1]) + abs(accData[2])
    if (forceMagnitudeApprox > 8192 and forceMagnitudeApprox < 32768):
            # Turning around the X axis results in a vector on the Y-axis
        pitchAcc = math.atan2(accData[1], accData[2]) * 180 / M_PI
        pitch = pitch * 0.98 + pitchAcc * 0.02

        # Turning around the Y axis results in a vector on the X-axis
        rollAcc = math.atan2(accData[0], accData[2]) * 180 / M_PI
        roll = roll * 0.98 + rollAcc * 0.02
    return pitch, roll

def heading(self):
        (x, y, z) = self.raw_data()
        headingRad = math.atan2(y, x)
        headingRad += self.__declination

        # Correct for reversed heading
        if headingRad < 0:
            headingRad += 2 * math.pi

        # Check for wrap and compensate
        elif headingRad > 2 * math.pi:
            headingRad -= 2 * math.pi

        # Convert to degrees from radians
        headingDeg = headingRad * 180 / math.pi
        return headingDeg

def _mat3_to_vec_roll(mat):
    """
    Function to convert a 3x3 rotation matrix to a rotation axis and a roll angle along this axis
    Python port of the C function defined in armature.c

    Thanks to blenderartists.org user vida_vida

    :param mat:
    :return:
    """
    from ..properties.globals import global_properties
    vec = mat.col[1] * global_properties.bone_length.get(bpy.context.scene)
    vecmat = _vec_roll_to_mat3(mat.col[1], 0)
    vecmatinv = vecmat.inverted()
    rollmat = vecmatinv * mat
    roll = math.atan2(rollmat[0][2], rollmat[2][2])
    return vec, roll

def ExtendToMont(self,Geom, dist=4):#Retorna geometria com linha estendida #x1,y1,x2,y2
        poli=Geom.asPolyline()
        pto1=poli[0]
        pto2=poli[1]
        x1=pto1.x()
        y1=pto1.y()
        x2=pto2.x()
        y2=pto2.y()
        Alfa=math.atan2(y2-y1,x2-x1)
        dx=dist*math.cos(Alfa)
        dy=dist*math.sin(Alfa)
        xp=x1-dx
        yp=y1-dy
        pto1_est=QgsPoint(xp,yp)
        newGeo=QgsGeometry.fromPolyline([pto1_est,pto2])
        return newGeo

def yaw(self):
        return math.atan2(2*(self.w*self.z() + self.x()*self.y()), 1 - 2*(self.y()*self.y() + self.z()*self.z()))

def roll(self):
        return math.atan2(2*(self.w*self.x() + self.z()*self.y()), 1 - 2*(self.y()*self.y() + self.x()*self.x()))