Python math.atan2() Examples

def heading_between_points(x1, y1, x2, y2):
    """Returns the angle between 2 points in degrees.

    :param x1: x coordinate of point 1
    :param y1: y coordinate of point 1
    :param x2: x coordinate of point 2
    :param y2: y coordinate of point 2
    :return: angle in degrees
    """
    def angle_trunc(a):
        while a < 0.0:
            a += math.pi * 2
        return a
    deltax = x2 - x1
    deltay = y2 - y1
    return math.degrees(angle_trunc(math.atan2(deltay, deltax))) 

def draw_marker(self, name, mp, tp, weight, c):
    if name in self.markers:
      m_shape, ref, orient, units = self.markers[name]

      c.save()
      c.translate(*mp)
      if orient == 'auto':
        angle = math.atan2(tp[1]-mp[1], tp[0]-mp[0])
        c.rotate(angle)
      elif isinstance(orient, int):
        angle = math.radians(orient)
        c.rotate(angle)

      if units == 'stroke':
        c.scale(weight, weight)
        
      c.translate(-ref[0], -ref[1])
      
      self.draw_shape(m_shape)
      c.restore() 

def rotm2eul(m):
    """ m (3x3, rotation matrix) --> rotation m = Rz*Ry*Rx
    """
    c = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])

    singular = c < 1e-6
    if not singular :
        x = math.atan2(R[2,1] , R[2,2])
        y = math.atan2(-R[2,0], c)
        z = math.atan2(R[1,0], R[0,0])
    else :
        x = math.atan2(-R[1,2], R[1,1])
        y = math.atan2(-R[2,0], c)
        z = 0

    return numpy.array([x, y, z])


#EOF 

def _get_observation(self):
    world_translation_minitaur, world_rotation_minitaur = (
        self._pybullet_client.getBasePositionAndOrientation(
            self.minitaur.quadruped))
    world_translation_ball, world_rotation_ball = (
        self._pybullet_client.getBasePositionAndOrientation(self._ball_id))
    minitaur_translation_world, minitaur_rotation_world = (
        self._pybullet_client.invertTransform(world_translation_minitaur,
                                              world_rotation_minitaur))
    minitaur_translation_ball, _ = (
        self._pybullet_client.multiplyTransforms(minitaur_translation_world,
                                                 minitaur_rotation_world,
                                                 world_translation_ball,
                                                 world_rotation_ball))
    distance = math.sqrt(minitaur_translation_ball[0]**2 +
                         minitaur_translation_ball[1]**2)
    angle = math.atan2(minitaur_translation_ball[0],
                       minitaur_translation_ball[1])
    self._observation = [angle - math.pi / 2, distance]
    return self._observation 

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 

def _ecg_simulate_derivsecgsyn(t, x, rr, ti, sfint, ai, bi):

    ta = math.atan2(x[1], x[0])
    r0 = 1
    a0 = 1.0 - np.sqrt(x[0] ** 2 + x[1] ** 2) / r0

    ip = np.floor(t * sfint).astype(int)
    w0 = 2 * np.pi / rr[min(ip, len(rr) - 1)]
    # w0 = 2*np.pi/rr[ip[ip <= np.max(rr)]]

    fresp = 0.25
    zbase = 0.005 * np.sin(2 * np.pi * fresp * t)

    dx1dt = a0 * x[0] - w0 * x[1]
    dx2dt = a0 * x[1] + w0 * x[0]

    # matlab rem and numpy rem are different
    # dti = np.remainder(ta - ti, 2*np.pi)
    dti = (ta - ti) - np.round((ta - ti) / 2 / np.pi) * 2 * np.pi
    dx3dt = -np.sum(ai * dti * np.exp(-0.5 * (dti / bi) ** 2)) - 1 * (x[2] - zbase)

    dxdt = np.array([dx1dt, dx2dt, dx3dt])
    return dxdt 

def map(self, p, inv=False):
        # cartesian to lat/lon
        # if inv is true lat/lon to cartesian
        R = self.R
        if not inv:
            ed = R * (vec(p - self.c).norm())
            ed = point(ed.dx, ed.dy, ed.dz)
            lon = math.atan2(ed.y, ed.x)
            lat1 = math.acos(abs(ed.z) / R)
            if ed.z > 0:
                lat = math.pi / 2 - lat1
            else:
                lat = -(math.pi / 2 - lat1)
            return point(lon, lat) * 180 / math.pi
        if inv:
            p = p * math.pi / 180
            z = R * math.sin(p.y)
            y = R * math.cos(p.y) * math.sin(p.x)
            x = R * math.cos(p.y) * math.cos(p.x)
            return point(x, y, z) 

def get_euler(self):
        t = self.x * self.y + self.z * self.w
        if t > 0.4999:
            heading = 2 * math.atan2(self.x, self.w)
            attitude = math.pi / 2
            bank = 0
        elif t < -0.4999:
            heading = -2 * math.atan2(self.x, self.w)
            attitude = -math.pi / 2
            bank = 0
        else:
            sqx = self.x ** 2
            sqy = self.y ** 2
            sqz = self.z ** 2
            heading = math.atan2(2 * self.y * self.w - 2 * self.x * self.z,
                                 1 - 2 * sqy - 2 * sqz)
            attitude = math.asin(2 * t)
            bank = math.atan2(2 * self.x * self.w - 2 * self.y * self.z,
                              1 - 2 * sqx - 2 * sqz)
        return heading, attitude, bank 

def addArrow(self, painterPath, startX, startY, endX, endY):
        """Add arrows to the edges
           http://kapo-cpp.blogspot.com/2008/10/drawing-arrows-with-cairo.html
        """
        arrowLength = 12.0
        arrowDegrees = 0.15      # Radian

        angle = math.atan2(endY - startY, endX - startX) + math.pi
        arrowX1 = endX + arrowLength * math.cos(angle - arrowDegrees)
        arrowY1 = endY + arrowLength * math.sin(angle - arrowDegrees)
        arrowX2 = endX + arrowLength * math.cos(angle + arrowDegrees)
        arrowY2 = endY + arrowLength * math.sin(angle + arrowDegrees)

        painterPath.moveTo(endX, endY)
        painterPath.lineTo(arrowX1, arrowY1)
        painterPath.moveTo(endX, endY)
        painterPath.lineTo(arrowX2, arrowY2) 

def addArrow(self, painterPath, startX, startY, endX, endY):
        """Add arrows to the edges
           http://kapo-cpp.blogspot.com/2008/10/drawing-arrows-with-cairo.html
        """
        arrowLength = 12.0
        arrowDegrees = 0.15      # Radian

        angle = math.atan2(endY - startY, endX - startX) + math.pi
        arrowX1 = endX + arrowLength * math.cos(angle - arrowDegrees)
        arrowY1 = endY + arrowLength * math.sin(angle - arrowDegrees)
        arrowX2 = endX + arrowLength * math.cos(angle + arrowDegrees)
        arrowY2 = endY + arrowLength * math.sin(angle + arrowDegrees)

        painterPath.moveTo(endX, endY)
        painterPath.lineTo(arrowX1, arrowY1)
        painterPath.moveTo(endX, endY)
        painterPath.lineTo(arrowX2, arrowY2) 

def cairo_draw_arrow(head, tail, fill, c):
  width = c.get_line_width()
  c.save()
  dy = head[1] - tail[1]
  dx = head[0] - tail[0]
  angle = math.atan2(dy,dx)
  c.translate(head[0],head[1])
  c.rotate(angle)
  c.scale(width, width)

  # Now positioned to draw arrow at 0,0 with point facing right
  apath = [(-4,0), (-4.5,2), (0,0)]

  mirror = [(x,-y) for x, y in reversed(apath[1:-1])] # Mirror central points
  apath.extend(mirror)

  c.move_to(*apath[0])
  for p in apath[1:]:
    c.line_to(*p)
  c.close_path()

  c.set_source_rgba(*fill)
  c.fill()

  c.restore() 

def apply(self, fish, state):
        if state['closecount'] == 0:
            return

        center = state['center']
        distance_to_center = dist(
            center[0], center[1],
            fish.position[0], fish.position[1]
            )

        if distance_to_center > self.parameters['threshold']:
            angle_to_center = math.atan2(
                fish.position[1] - center[1],
                fish.position[0] - center[0]
                )
            fish.turnrate += (angle_to_center - fish.direction) / self.parameters['weight']
            fish.speed += distance_to_center / self.parameters['speedfactor'] 

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 

def orient_tickmark(self, t, trans=None):
    """Return the position, normalized local x vector, normalized
    local y vector, and angle of a tick at position t.

    Normally only used internally.
    """
    if isinstance(trans, basestring): trans = totrans(trans)
    if trans == None:
      f = self.f
    else:
      f = lambda t: trans(*self.f(t))

    eps = _epsilon * abs(self.high - self.low)

    X, Y = f(t)
    Xprime, Yprime = f(t + eps)
    xhatx, xhaty = (Xprime - X)/eps, (Yprime - Y)/eps

    norm = math.sqrt(xhatx**2 + xhaty**2)
    if norm != 0: xhatx, xhaty = xhatx/norm, xhaty/norm
    else: xhatx, xhaty = 1., 0.

    angle = math.atan2(xhaty, xhatx) + math.pi/2.
    yhatx, yhaty = math.cos(angle), math.sin(angle)

    return (X, Y), (xhatx, xhaty), (yhatx, yhaty), angle 

def Path(self, trans=None, local=False):
    """Apply the transformation "trans" and return a Path object in
    global coordinates.  If local=True, return a Path in local coordinates
    (which must be transformed again)."""
    angle = math.atan2(self.ay, self.ax) + math.pi/2.
    bx = self.b * math.cos(angle)
    by = self.b * math.sin(angle)

    self.f = lambda t: (self.x + self.ax*math.cos(t) + bx*math.sin(t), self.y + self.ay*math.cos(t) + by*math.sin(t))
    self.low = -math.pi
    self.high = math.pi
    self.loop = True
    return Curve.Path(self, trans, local)

###################################################################### 

def draw(self, screen, mouse_pos):
        angle = math.atan2(mouse_pos[1]-(self.rect.top+32), mouse_pos[0]-(self.rect.left+26))
        image_rotate = pygame.transform.rotate(self.image, 360-angle*57.29)
        bunny_pos = (self.rect.left-image_rotate.get_rect().width/2, self.rect.top-image_rotate.get_rect().height/2)
        self.rotated_position = bunny_pos
        screen.blit(image_rotate, bunny_pos) 

def computeArrow(line):
    point0 = line.p2()
    arrowSize = 25.
    angle = math.atan2(-line.dy(), line.dx())
    point1 = line.p2() - QPointF(math.sin(angle + math.radians(75.)) * arrowSize,
                                          math.cos(angle + math.radians(75.)) * arrowSize)
    point2 = line.p2() - QPointF(math.sin(angle + math.pi - math.radians(75.)) * arrowSize,
                                          math.cos(angle + math.pi - math.radians(75.)) * arrowSize)

    return QPolygonF([point0,point1,point2]) 

def intersect(self,line,onLine=False):

            
            theta  = -((math.pi/2)-atan2(
            (line.points[1] - line.points[0]).x,
            (line.points[1] - line.points[0]).y
            ))

            rotatedLine = Line(Point(0,0),line.points[1].rotate(theta,line.points[0]) )

            rotatedBezier = QuadraticBezierCurve( self.points[0].rotate(theta,line.points[0]),
                                                  self.points[1].rotate(theta,line.points[0]),
                                                  self.points[2].rotate(theta,line.points[0]) )

            
            
            roots = rotatedBezier.findRoots(*[p.y for p in rotatedBezier.points])
    
            if (onLine):
                for root in list(roots):
                    point = rotatedBezier.evaluate(root)
                    
                    if min(rotatedLine.points[0].x-sys.float_info.epsilon,rotatedLine.points[1].x-sys.float_info.epsilon) > point.x or max(rotatedLine.points[0].x+sys.float_info.epsilon,rotatedLine.points[1].x+sys.float_info.epsilon) < point.x:
                        roots.remove(root)
                        
                    elif min(rotatedLine.points[0].y-sys.float_info.epsilon,rotatedLine.points[1].y-sys.float_info.epsilon) > point.y or max(rotatedLine.points[0].y+sys.float_info.epsilon,rotatedLine.points[1].y+sys.float_info.epsilon) < point.y:
                        roots.remove(root)
                
                
            result = []
            for root in roots:
                result.append((root,self.evaluate(root),rotatedBezier.evaluate(root)))
                
            result.sort(key=lambda item:item[2].x)
            result.reverse()
            
            return result 

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 

def test_underflow_sign(self):
        # check that -1e-1000 gives -0.0, not 0.0
        self.assertEqual(math.atan2(-1e-1000, -1), math.atan2(-0.0, -1))
        self.assertEqual(math.atan2(float('-1e-1000'), -1),
                         math.atan2(-0.0, -1)) 

def apply(self, fish, state):
        closest_fish = state['closest_fish']
        if closest_fish is None:
            return

        distance_to_closest_fish = state['distance_to_closest_fish']
        if distance_to_closest_fish < self.parameters['threshold']:
            angle_to_closest_fish = math.atan2(
                fish.position[1] - closest_fish.position[1],
                fish.position[0] - closest_fish.position[0]
                )
            fish.turnrate -= (angle_to_closest_fish - fish.direction) / self.parameters['weight']
            fish.speed += self.parameters['speedfactor'] / distance_to_closest_fish 

def to_euler(self, use_deg=False):
        eulers = np.zeros((2, 3))

        if np.absolute(np.absolute(self.rotmat[2, 0]) - 1) < 1e-12:
            #GIMBAL LOCK!
            print('Gimbal')
            if np.absolute(self.rotmat[2, 0]) - 1 < 1e-12:
                eulers[:,0] = math.atan2(-self.rotmat[0,1], -self.rotmat[0,2])
                eulers[:,1] = -math.pi/2
            else:
                eulers[:,0] = math.atan2(self.rotmat[0,1], -elf.rotmat[0,2])
                eulers[:,1] = math.pi/2
            
            return eulers

        theta = - math.asin(self.rotmat[2,0])
        theta2 = math.pi - theta

        # psi1, psi2
        eulers[0,0] = math.atan2(self.rotmat[2,1]/math.cos(theta), self.rotmat[2,2]/math.cos(theta))
        eulers[1,0] = math.atan2(self.rotmat[2,1]/math.cos(theta2), self.rotmat[2,2]/math.cos(theta2))

        # theta1, theta2
        eulers[0,1] = theta
        eulers[1,1] = theta2

        # phi1, phi2
        eulers[0,2] = math.atan2(self.rotmat[1,0]/math.cos(theta), self.rotmat[0,0]/math.cos(theta))
        eulers[1,2] = math.atan2(self.rotmat[1,0]/math.cos(theta2), self.rotmat[0,0]/math.cos(theta2))

        if use_deg:
            eulers = rad2deg(eulers)

        return eulers 

def calc_angles(self):
        self.angle=np.zeros(self.finger_count,dtype=int)
        for i in range(self.finger_count):
            y = self.finger_pos[i][1]
            x = self.finger_pos[i][0]
            self.angle[i]=abs(math.atan2((self.hand_center[1]-y),(x-self.hand_center[0]))*180/math.pi) 

def posterior_hypers(N, sum_sin_x, sum_cos_x, a, b, k):
        assert N >= 0
        assert a > 0
        assert 0 <= b and b <= 2*pi
        assert k > 0
        p_cos = k * sum_cos_x + a * cos(b)
        p_sin = k * sum_sin_x + a * sin(b)
        an = (p_cos**2.0 + p_sin**2.0)**.5
        bn = - atan2(p_cos, p_sin) + pi/2
        return an, bn 

def getMouseValue(self, mouse, pivot):
        return math.atan2(mouse[0] - pivot[0], mouse[1] - pivot[1]) 

def start(self, editor):
        self.pivot = editor.getBonePivotTransformed(self.bone)
        self.original = euclid.Vector3(self.bone.rotation.x, self.bone.rotation.y, self.bone.rotation.z)
        for attr in self.attributes:
            self.values[attr] = getattr(self.original, attr) + math.atan2(self.mouse[0] - self.pivot[0], self.mouse[1] - self.pivot[1]) 

def polygonToRotRectangle(bbox):
    """
    :param bbox: The polygon stored in format [x1, y1, x2, y2, x3, y3, x4, y4]
    :return: Rotated Rectangle in format [cx, cy, w, h, theta]
    """
    bbox = np.array(bbox,dtype=np.float32)
    bbox = np.reshape(bbox,newshape=(2,4),order='F')
    angle = math.atan2(-(bbox[0,1]-bbox[0,0]),bbox[1,1]-bbox[1,0])

    center = [[0],[0]]

    for i in range(4):
        center[0] += bbox[0,i]
        center[1] += bbox[1,i]

    center = np.array(center,dtype=np.float32)/4.0

    R = np.array([[math.cos(angle), -math.sin(angle)], [math.sin(angle), math.cos(angle)]], dtype=np.float32)

    normalized = np.matmul(R.transpose(),bbox-center)

    xmin = np.min(normalized[0,:])
    xmax = np.max(normalized[0,:])
    ymin = np.min(normalized[1,:])
    ymax = np.max(normalized[1,:])

    w = xmax - xmin + 1
    h = ymax - ymin + 1

    return [float(center[0]),float(center[1]),w,h,angle] 

def haversine_distance(self, lat1, lon1, lat2, lon2):
        R = 6371.0088
        lat1 = math.radians(lat1)
        lon1 = math.radians(lon1)
        lat2 = math.radians(lat2)
        lon2 = math.radians(lon2)
        dlon = lon2 - lon1
        dlat = lat2 - lat1
        a = math.sin(dlat / 2) ** 2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon / 2) ** 2
        c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
        distance = R * c
        return distance * 1000 

def range_bearing_elevation(c,l):
    # rotate C onto X axis
    crx, cry, crz = latlngup_to_relxyz(c,c)
    # rotate L in the same way
    lrx, lry, lrz = latlngup_to_relxyz(c,l)

    # Now we have cartesian coordinates with C on
    # the X axis with ground plane YZ and north along +Z.

    dx, dy, dz = lrx-crx, lry-cry, lrz-crz
    rng = math.sqrt(dx*dx + dy*dy + dz*dz)
    bearing = (360 + 90 - rtod(math.atan2(dz,dy))) % 360
    elev = rtod(math.asin(dx / rng))

    return (rng, bearing, elev) 

def update(self, editor):
        for attr in self.values:
            angle = math.atan2(editor.mouse[0] - self.pivot[0], editor.mouse[1] - self.pivot[1])
            setattr(self.bone.rotation, attr, self.values[attr] - angle)