Python math.atan2() Examples

The following are code examples for showing how to use math.atan2(). They are extracted from open source Python projects. You can vote up the examples you like or vote down the exmaples you don't like. You can also save this page to your account.

Example 1
Project: micros_mars_task_alloc   Author: liminglong   File: robot_follower.py    (license) View Source Project 7 votes vote down vote up
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() 
Example 2
Project: pygame_cards   Author: vannov   File: card_sprite.py    (license) View Source Project 6 votes vote down vote up
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 
Example 3
Project: pypilot   Author: pypilot   File: calibration_fit.py    (license) View Source Project 6 votes vote down vote up
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 
Example 4
Project: satellite-tracker   Author: lofaldli   File: coords.py    (MIT License) View Source Project 6 votes vote down vote up
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 
Example 5
Project: sc-controller   Author: kozec   File: tools.py    (license) View Source Project 6 votes vote down vote up
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 
Example 6
Project: sc-controller   Author: kozec   File: actions.py    (license) View Source Project 6 votes vote down vote up
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 
Example 7
Project: sc-controller   Author: kozec   File: modifiers.py    (license) View Source Project 6 votes vote down vote up
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) 
Example 8
Project: CodeDay-Pong-AI   Author: ianjury   File: pong.py    (license) View Source Project 6 votes vote down vote up
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 
Example 9
Project: PokeAlarm   Author: PokeAlarm   File: Utils.py    (license) View Source Project 6 votes vote down vote up
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 
Example 10
Project: robodk_postprocessors   Author: ros-industrial   File: robodk.py    (license) View Source Project 6 votes vote down vote up
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] 
Example 11
Project: robodk_postprocessors   Author: ros-industrial   File: robodk.py    (license) View Source Project 6 votes vote down vote up
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] 
Example 12
Project: osm2gtfs   Author: grote   File: osm_stops.py    (license) View Source Project 6 votes vote down vote up
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 
Example 13
Project: osm2gtfs   Author: grote   File: stops_creator_accra.py    (license) View Source Project 6 votes vote down vote up
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 
Example 14
Project: bpy_lambda   Author: bcongdon   File: panel.py    (license) View Source Project 6 votes vote down vote up
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) 
Example 15
Project: bpy_lambda   Author: bcongdon   File: panel.py    (license) View Source Project 6 votes vote down vote up
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) 
Example 16
Project: bpy_lambda   Author: bcongdon   File: archipack_2d.py    (license) View Source Project 6 votes vote down vote up
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 
Example 17
Project: bpy_lambda   Author: bcongdon   File: io_export_md3.py    (license) View Source Project 6 votes vote down vote up
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 
Example 18
Project: house-of-enlightenment   Author: house-of-enlightenment   File: spireBasic.py    (license) View Source Project 6 votes vote down vote up
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) 
Example 19
Project: house-of-enlightenment   Author: house-of-enlightenment   File: wheel.py    (license) View Source Project 6 votes vote down vote up
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) 
Example 20
Project: CON-SAI   Author: SSL-Roots   File: paint_widget.py    (license) View Source Project 6 votes vote down vote up
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) 
Example 21
Project: diff_drive   Author: merose   File: goal_controller.py    (license) View Source Project 6 votes vote down vote up
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 
Example 22
Project: fright-before-christmas-clone   Author: bobhob314   File: test.py    (license) View Source Project 6 votes vote down vote up
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 
Example 23
Project: fright-before-christmas-clone   Author: bobhob314   File: test.py    (license) View Source Project 6 votes vote down vote up
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 
Example 24
Project: Houston   Author: squaresLab   File: util.py    (license) View Source Project 6 votes vote down vote up
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)) 
Example 25
Project: drone   Author: arunsoman   File: complementary_filter.py    (license) View Source Project 6 votes vote down vote up
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 
Example 26
Project: drone   Author: arunsoman   File: hmc5883l.py    (license) View Source Project 6 votes vote down vote up
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 
Example 27
Project: BlenderRobotDesigner   Author: HBPNeurorobotics   File: helpers.py    (license) View Source Project 6 votes vote down vote up
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 
Example 28
Project: QEsg   Author: jorgealmerio   File: QEsg_20Sancad.py    (license) View Source Project 6 votes vote down vote up
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 
Example 29
Project: joysix   Author: niberger   File: quaternion.py    (GNU General Public License v3.0) View Source Project 5 votes vote down vote up
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())) 
Example 30
Project: joysix   Author: niberger   File: quaternion.py    (GNU General Public License v3.0) View Source Project 5 votes vote down vote up
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())) 
Example 31
Project: joysix   Author: niberger   File: quaternion.py    (GNU General Public License v3.0) View Source Project 5 votes vote down vote up
def log(q):
	im = q.im()
	imn = np.linalg.norm(im)
	n = math.atan2(imn, q.w)
	if(abs(n) < 1e-6):
		return 2*im
	else:
		return 2*(n/imn)*im 
Example 32
Project: joysix   Author: niberger   File: joystick.py    (GNU General Public License v3.0) View Source Project 5 votes vote down vote up
def getValuesFromPose(self, P):
		'''return the virtual values of the pots corresponding to the pose P'''
		vals = []
		grads = []
		for i, r, l, placement, attach_p in zip(range(3), self.rs, self.ls, self.placements, self.attach_ps):
			#first pot axis
			a = placement.rot * col([1, 0, 0])
			#second pot axis
			b = placement.rot * col([0, 1, 0])
			#string axis
			c = placement.rot * col([0, 0, 1])

			#attach point on the joystick
			p_joystick = P * attach_p
			v = p_joystick - placement.trans
			va = v - dot(v, a)*a
			vb = v - dot(v, b)*b
			#angles of the pots
			alpha = math.atan2(dot(vb, a), dot(vb, c))
			beta = math.atan2(dot(va, b), dot(va, c))
			vals.append(alpha)
			vals.append(beta)
			
			#calculation of the derivatives
			dv = np.bmat([-P.rot.mat() * quat.skew(attach_p), P.rot.mat()])
			dva = (np.eye(3) - a*a.T) * dv
			dvb = (np.eye(3) - b*b.T) * dv
			dalpha = (1/dot(vb,vb)) * (dot(vb,c) * a.T - dot(vb,a) * c.T) * dvb
			dbeta = (1/dot(va,va)) * (dot(va,c) * b.T - dot(va,b) * c.T) * dva
			grads.append(dalpha)
			grads.append(dbeta)
		return (col(vals), np.bmat([[grads]])) 
Example 33
Project: coa_tools   Author: ndee85   File: edit_armature.py    (GNU General Public License v3.0) View Source Project 5 votes vote down vote up
def drag_bone(self,context, event ,bone=None):
        ### math.atan2(0.5, 0.5)*180/math.pi
        if bone != None:
            
            bone.hide = False
            mouse_vec_norm = (self.cursor_location - self.mouse_click_vec).normalized()
            mouse_vec = (self.cursor_location - self.mouse_click_vec)
            angle = (math.atan2(mouse_vec_norm[0], mouse_vec_norm[2])*180/math.pi)
            cursor_local = self.armature.matrix_world.inverted() * self.cursor_location
            cursor_local[1] = 0
            if event.shift:
                if angle > -22.5 and angle < 22.5:
                    ### up
                    bone.tail =  Vector((bone.head[0],cursor_local[1],cursor_local[2]))
                elif angle > 22.5 and angle < 67.5:
                    ### up right
                    bone.tail = (bone.head +  Vector((mouse_vec[0],0,mouse_vec[0])))
                elif angle > 67.5 and angle < 112.5:
                    ### right
                    bone.tail = Vector((cursor_local[0],cursor_local[1],bone.head[2]))
                elif angle > 112.5 and angle < 157.5:
                    ### down right
                    bone.tail = (bone.head +  Vector((mouse_vec[0],0,-mouse_vec[0])))
                elif angle > 157.5 or angle < -157.5:   
                    ### down
                    bone.tail = Vector((bone.head[0],cursor_local[1],cursor_local[2]))
                elif angle > -157.5 and angle < -112.5:
                    ### down left
                        bone.tail = (bone.head +  Vector((mouse_vec[0],0,mouse_vec[0])))
                elif angle > -112.5 and angle < -67.5:
                    ### left
                    bone.tail = Vector((cursor_local[0],cursor_local[1],bone.head[2]))
                elif angle > -67.5 and angle < -22.5:       
                    ### left up
                    bone.tail = (bone.head +  Vector((mouse_vec[0],0,-mouse_vec[0])))
            else:
                bone.tail = cursor_local 
Example 34
Project: tree-gen   Author: friggog   File: leaf.py    (GNU General Public License v3.0) View Source Project 5 votes vote down vote up
def calc_bend_trf(self, bend):
        """calculate the transformations required to 'bend' the leaf out/up from WP"""
        normal = self.direction.cross(self.right)
        theta_pos = atan2(self.position.y, self.position.x)
        theta_bend = theta_pos - atan2(normal.y, normal.x)
        bend_trf_1 = Quaternion(Vector([0, 0, 1]), theta_bend * bend)
        self.direction.rotate(bend_trf_1)
        self.right.rotate(bend_trf_1)
        normal = self.direction.cross(self.right)
        phi_bend = normal.declination()
        if phi_bend > pi / 2:
            phi_bend = phi_bend - pi
        bend_trf_2 = Quaternion(self.right, phi_bend * bend)
        return bend_trf_1, bend_trf_2 
Example 35
Project: Fluent-Python   Author: Ehco1996   File: 10-7.py    (license) View Source Project 5 votes vote down vote up
def angle(self, n):
        # ?? n???? ???????????
        r = math.sqrt(sum(x * x for x in self[n:]))
        a = math.atan2(r, self[n - 1])
        if (n == len(self)) and (self[-1] < 0):
            return math.pi * 2 - a
        else:
            return a 
Example 36
Project: pybot   Author: spillai   File: transformations.py    (license) View Source Project 5 votes vote down vote up
def rotation_from_matrix(matrix):
    """Return rotation angle and axis from rotation matrix.

    >>> angle = (random.random() - 0.5) * (2*math.pi)
    >>> direc = numpy.random.random(3) - 0.5
    >>> point = numpy.random.random(3) - 0.5
    >>> R0 = rotation_matrix(angle, direc, point)
    >>> angle, direc, point = rotation_from_matrix(R0)
    >>> R1 = rotation_matrix(angle, direc, point)
    >>> is_same_transform(R0, R1)
    True

    """
    R = numpy.array(matrix, dtype=numpy.float64, copy=False)
    R33 = R[:3, :3]
    # direction: unit eigenvector of R33 corresponding to eigenvalue of 1
    l, W = numpy.linalg.eig(R33.T)
    i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0]
    if not len(i):
        raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
    direction = numpy.real(W[:, i[-1]]).squeeze()
    # point: unit eigenvector of R33 corresponding to eigenvalue of 1
    l, Q = numpy.linalg.eig(R)
    i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0]
    if not len(i):
        raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
    point = numpy.real(Q[:, i[-1]]).squeeze()
    point /= point[3]
    # rotation angle depending on direction
    cosa = (numpy.trace(R33) - 1.0) / 2.0
    if abs(direction[2]) > 1e-8:
        sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2]
    elif abs(direction[1]) > 1e-8:
        sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1]
    else:
        sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0]
    angle = math.atan2(sina, cosa)
    return angle, direction, point 
Example 37
Project: otRebuilder   Author: Pal3love   File: pointPen.py    (MIT License) View Source Project 5 votes vote down vote up
def _flushContour(self):
		points = self._points
		nPoints = len(points)
		if not nPoints:
			return
		if points[0][1] == "move":
			# Open path.
			indices = range(1, nPoints - 1)
		elif nPoints > 1:
			# Closed path. To avoid having to mod the contour index, we
			# simply abuse Python's negative index feature, and start at -1
			indices = range(-1, nPoints - 1)
		else:
			# closed path containing 1 point (!), ignore.
			indices = []
		for i in indices:
			pt, segmentType, dummy, name, kwargs = points[i]
			if segmentType is None:
				continue
			prev = i - 1
			next = i + 1
			if points[prev][1] is not None and points[next][1] is not None:
				continue
			# At least one of our neighbors is an off-curve point
			pt = points[i][0]
			prevPt = points[prev][0]
			nextPt = points[next][0]
			if pt != prevPt and pt != nextPt:
				dx1, dy1 = pt[0] - prevPt[0], pt[1] - prevPt[1]
				dx2, dy2 = nextPt[0] - pt[0], nextPt[1] - pt[1]
				a1 = math.atan2(dx1, dy1)
				a2 = math.atan2(dx2, dy2)
				if abs(a1 - a2) < 0.05:
					points[i] = pt, segmentType, True, name, kwargs

		for pt, segmentType, smooth, name, kwargs in points:
			self._outPen.addPoint(pt, segmentType, smooth, name, **kwargs) 
Example 38
Project: sc8pr   Author: dmaccarthy   File: geom.py    (GNU General Public License v3.0) View Source Project 5 votes vote down vote up
def polar2d(vx, vy, deg=True):
    "2D Cartesian to Polar conversion"
    a = atan2(vy, vx)
    return hypot(vx, vy), (a / DEG if deg else a) 
Example 39
Project: Neural-Networks-for-Inverse-Kinematics   Author: paramrajpura   File: transformations.py    (license) View Source Project 5 votes vote down vote up
def rotation_from_matrix(matrix):
    """Return rotation angle and axis from rotation matrix.

    >>> angle = (random.random() - 0.5) * (2*math.pi)
    >>> direc = numpy.random.random(3) - 0.5
    >>> point = numpy.random.random(3) - 0.5
    >>> R0 = rotation_matrix(angle, direc, point)
    >>> angle, direc, point = rotation_from_matrix(R0)
    >>> R1 = rotation_matrix(angle, direc, point)
    >>> is_same_transform(R0, R1)
    True

    """
    R = numpy.array(matrix, dtype=numpy.float64, copy=False)
    R33 = R[:3, :3]
    # direction: unit eigenvector of R33 corresponding to eigenvalue of 1
    w, W = numpy.linalg.eig(R33.T)
    i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
    if not len(i):
        raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
    direction = numpy.real(W[:, i[-1]]).squeeze()
    # point: unit eigenvector of R33 corresponding to eigenvalue of 1
    w, Q = numpy.linalg.eig(R)
    i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
    if not len(i):
        raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
    point = numpy.real(Q[:, i[-1]]).squeeze()
    point /= point[3]
    # rotation angle depending on direction
    cosa = (numpy.trace(R33) - 1.0) / 2.0
    if abs(direction[2]) > 1e-8:
        sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2]
    elif abs(direction[1]) > 1e-8:
        sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1]
    else:
        sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0]
    angle = math.atan2(sina, cosa)
    return angle, direction, point 
Example 40
Project: SLP-Annotator   Author: PhonologicalCorpusTools   File: applyHandCode.py    (GNU General Public License v3.0) View Source Project 5 votes vote down vote up
def rotation_from_matrix(matrix):
    """Return rotation angle and axis from rotation matrix.

    >>> angle = (random.random() - 0.5) * (2*math.pi)
    >>> direc = numpy.random.random(3) - 0.5
    >>> point = numpy.random.random(3) - 0.5
    >>> R0 = rotation_matrix(angle, direc, point)
    >>> angle, direc, point = rotation_from_matrix(R0)
    >>> R1 = rotation_matrix(angle, direc, point)
    >>> is_same_transform(R0, R1)
    True

    """
    R = numpy.array(matrix, dtype=numpy.float64, copy=False)
    R33 = R[:3, :3]
    # direction: unit eigenvector of R33 corresponding to eigenvalue of 1
    w, W = numpy.linalg.eig(R33.T)
    i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
    if not len(i):
        raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
    direction = numpy.real(W[:, i[-1]]).squeeze()
    # point: unit eigenvector of R33 corresponding to eigenvalue of 1
    w, Q = numpy.linalg.eig(R)
    i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
    if not len(i):
        raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
    point = numpy.real(Q[:, i[-1]]).squeeze()
    point /= point[3]
    # rotation angle depending on direction
    cosa = (numpy.trace(R33) - 1.0) / 2.0
    if abs(direction[2]) > 1e-8:
        sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2]
    elif abs(direction[1]) > 1e-8:
        sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1]
    else:
        sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0]
    angle = math.atan2(sina, cosa)
    return angle, direction, point

# Function to translate handshape coding to degrees of rotation, adduction, flexion 
Example 41
Project: SLP-Annotator   Author: PhonologicalCorpusTools   File: position_hand.py    (GNU General Public License v3.0) View Source Project 5 votes vote down vote up
def rotation_from_matrix(matrix):
    """Return rotation angle and axis from rotation matrix.

    >>> angle = (random.random() - 0.5) * (2*math.pi)
    >>> direc = numpy.random.random(3) - 0.5
    >>> point = numpy.random.random(3) - 0.5
    >>> R0 = rotation_matrix(angle, direc, point)
    >>> angle, direc, point = rotation_from_matrix(R0)
    >>> R1 = rotation_matrix(angle, direc, point)
    >>> is_same_transform(R0, R1)
    True

    """
    R = numpy.array(matrix, dtype=numpy.float64, copy=False)
    R33 = R[:3, :3]
    # direction: unit eigenvector of R33 corresponding to eigenvalue of 1
    w, W = numpy.linalg.eig(R33.T)
    i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
    if not len(i):
        raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
    direction = numpy.real(W[:, i[-1]]).squeeze()
    # point: unit eigenvector of R33 corresponding to eigenvalue of 1
    w, Q = numpy.linalg.eig(R)
    i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
    if not len(i):
        raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
    point = numpy.real(Q[:, i[-1]]).squeeze()
    point /= point[3]
    # rotation angle depending on direction
    cosa = (numpy.trace(R33) - 1.0) / 2.0
    if abs(direction[2]) > 1e-8:
        sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2]
    elif abs(direction[1]) > 1e-8:
        sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1]
    else:
        sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0]
    angle = math.atan2(sina, cosa)
    return angle, direction, point

# Function to translate handshape coding to degrees of rotation, adduction, flexion 
Example 42
Project: code   Author: ActiveState   File: recipe-577713.py    (MIT License) View Source Project 5 votes vote down vote up
def midpoint(x1, y1, x2, y2):
#Input values as degrees

#Convert to radians
    lat1 = math.radians(x1)
    lon1 = math.radians(x2)
    lat2 = math.radians(y1)
    lon2 = math.radians(y2)


    bx = math.cos(lat2) * math.cos(lon2 - lon1)
    by = math.cos(lat2) * math.sin(lon2 - lon1)
    lat3 = math.atan2(math.sin(lat1) + math.sin(lat2), \
           math.sqrt((math.cos(lat1) + bx) * (math.cos(lat1) \
           + bx) + by**2))
    lon3 = lon1 + math.atan2(by, math.cos(lat1) + Bx)

    return [round(math.degrees(lat3), 2), round(math.degrees(lon3), 2)] 
Example 43
Project: code   Author: ActiveState   File: recipe-510397.py    (MIT License) View Source Project 5 votes vote down vote up
def _ang(vector):
    'Private module function.'
    return _math.atan2(vector.x, vector.y) % _PI_M_2 
Example 44
Project: s2sphere   Author: sidewalklabs   File: sphere.py    (license) View Source Project 5 votes vote down vote up
def angle(self, other):
        return math.atan2(self.cross_prod(other).norm(), self.dot_prod(other)) 
Example 45
Project: s2sphere   Author: sidewalklabs   File: sphere.py    (license) View Source Project 5 votes vote down vote up
def latitude(point):
        return Angle.from_radians(
            math.atan2(point[2],
                       math.sqrt(point[0] * point[0] + point[1] * point[1]))
        ) 
Example 46
Project: s2sphere   Author: sidewalklabs   File: sphere.py    (license) View Source Project 5 votes vote down vote up
def longitude(point):
        return Angle.from_radians(math.atan2(point[1], point[0])) 
Example 47
Project: s2sphere   Author: sidewalklabs   File: sphere.py    (license) View Source Project 5 votes vote down vote up
def get_distance(self, other):
        assert self.is_valid()
        assert other.is_valid()

        from_lat = self.lat().radians
        to_lat = other.lat().radians
        from_lng = self.lng().radians
        to_lng = other.lng().radians
        dlat = math.sin(0.5 * (to_lat - from_lat))
        dlng = math.sin(0.5 * (to_lng - from_lng))
        x = dlat * dlat + dlng * dlng * math.cos(from_lat) * math.cos(to_lat)
        return Angle.from_radians(
            2 * math.atan2(math.sqrt(x), math.sqrt(max(0.0, 1.0 - x)))
        ) 
Example 48
Project: s2sphere   Author: sidewalklabs   File: sphere.py    (license) View Source Project 5 votes vote down vote up
def intersects_lat_edge(cls, a, b, lat, lng):
        assert is_unit_length(a)
        assert is_unit_length(b)

        z = robust_cross_prod(a, b).normalize()
        if z[2] < 0:
            z = -z

        y = robust_cross_prod(z, Point(0, 0, 1)).normalize()
        x = y.cross_prod(z)
        assert is_unit_length(x)
        assert x[2] >= 0

        sin_lat = math.sin(lat)
        if math.fabs(sin_lat) >= x[2]:
            return False

        assert x[2] > 0
        cos_theta = sin_lat / x[2]
        sin_theta = math.sqrt(1 - cos_theta * cos_theta)
        theta = math.atan2(sin_theta, cos_theta)

        ab_theta = SphereInterval.from_point_pair(
            math.atan2(a.dot_prod(y), a.dot_prod(x)),
            math.atan2(b.dot_prod(y), b.dot_prod(x)),
        )

        if ab_theta.contains(theta):
            isect = x * cos_theta + y * sin_theta
            if lng.contains(math.atan2(isect[1], isect[0])):
                return True
        if ab_theta.contains(-theta):
            isect = x * cos_theta - y * sin_theta
            if lng.contains(math.atan2(isect[1], isect[0])):
                return True
        return False 
Example 49
Project: autolab_core   Author: BerkeleyAutomation   File: transformations.py    (Apache License 2.0) View Source Project 5 votes vote down vote up
def rotation_from_matrix(matrix):
    """Return rotation angle and axis from rotation matrix.

    >>> angle = (random.random() - 0.5) * (2*math.pi)
    >>> direc = numpy.random.random(3) - 0.5
    >>> point = numpy.random.random(3) - 0.5
    >>> R0 = rotation_matrix(angle, direc, point)
    >>> angle, direc, point = rotation_from_matrix(R0)
    >>> R1 = rotation_matrix(angle, direc, point)
    >>> is_same_transform(R0, R1)
    True

    """
    R = numpy.array(matrix, dtype=numpy.float64, copy=False)
    R33 = R[:3, :3]
    # direction: unit eigenvector of R33 corresponding to eigenvalue of 1
    l, W = numpy.linalg.eig(R33.T)
    i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0]
    if not len(i):
        raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
    direction = numpy.real(W[:, i[-1]]).squeeze()
    # point: unit eigenvector of R33 corresponding to eigenvalue of 1
    l, Q = numpy.linalg.eig(R)
    i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0]
    if not len(i):
        raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
    point = numpy.real(Q[:, i[-1]]).squeeze()
    point /= point[3]
    # rotation angle depending on direction
    cosa = (numpy.trace(R33) - 1.0) / 2.0
    if abs(direction[2]) > 1e-8:
        sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2]
    elif abs(direction[1]) > 1e-8:
        sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1]
    else:
        sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0]
    angle = math.atan2(sina, cosa)
    return angle, direction, point 
Example 50
Project: SmartSocks   Author: waylybaye   File: speedtest.py    (MIT License) View Source Project 5 votes vote down vote up
def distance(origin, destination):
    """Determine distance between 2 sets of [lat,lon] in km"""

    lat1, lon1 = origin
    lat2, lon2 = destination
    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