Python numpy.arctan2() Examples

def doa(self, receiver, source):
        ''' Computes the direction of arrival wrt a source and receiver '''

        s_ind = self.key2ind(source)
        r_ind = self.key2ind(receiver)

        # vector from receiver to source
        v = self.X[:,s_ind] - self.X[:,r_ind]

        azimuth = np.arctan2(v[1], v[0])
        elevation = np.arctan2(v[2], la.norm(v[:2]))

        azimuth = azimuth + 2*np.pi if azimuth < 0. else azimuth
        elevation = elevation + 2*np.pi if elevation < 0. else elevation

        return np.array([azimuth, elevation]) 

def vector_angle(u, v, direction=None):
    '''
    vector_angle(u, v) yields the angle between the two vectors u and v. The optional argument 
    direction is by default None, which specifies that the smallest possible angle between the
    vectors be reported; if the vectors u and v are 2D vectors and direction parameters True and
    False specify the clockwise or counter-clockwise directions, respectively; if the vectors are
    3D vectors, then direction may be a 3D point that is not in the plane containing u, v, and the
    origin, and it specifies around which direction (u x v or v x u) the the counter-clockwise angle
    from u to v should be reported (the cross product vector that has a positive dot product with
    the direction argument is used as the rotation axis).
    '''
    if direction is None:
        return np.arccos(vector_angle_cos(u, v))
    elif direction is True:
        return np.arctan2(v[1], v[0]) - np.arctan2(u[1], u[0])
    elif direction is False:
        return np.arctan2(u[1], u[0]) - np.arctan2(v[1], v[0])
    else:
        axis1 = normalize(u)
        axis2 = normalize(np.cross(u, v))
        if np.dot(axis2, direction) < 0:
            axis2 = -axis2
        return np.arctan2(np.dot(axis2, v), np.dot(axis1, v)) 

def mtx_freq2visi(M, p_mic_x, p_mic_y):
    """
    build the matrix that maps the Fourier series to the visibility
    :param M: the Fourier series expansion is limited from -M to M
    :param p_mic_x: a vector that constains microphones x coordinates
    :param p_mic_y: a vector that constains microphones y coordinates
    :return:
    """
    num_mic = p_mic_x.size
    ms = np.reshape(np.arange(-M, M + 1, step=1), (1, -1), order='F')
    G = np.zeros((num_mic * (num_mic - 1), 2 * M + 1), dtype=complex, order='C')
    count_G = 0
    for q in range(num_mic):
        p_x_outer = p_mic_x[q]
        p_y_outer = p_mic_y[q]
        for qp in range(num_mic):
            if not q == qp:
                p_x_qqp = p_x_outer - p_mic_x[qp]
                p_y_qqp = p_y_outer - p_mic_y[qp]
                norm_p_qqp = np.sqrt(p_x_qqp ** 2 + p_y_qqp ** 2)
                phi_qqp = np.arctan2(p_y_qqp, p_x_qqp)
                G[count_G, :] = (-1j) ** ms * sp.special.jv(ms, norm_p_qqp) * \
                                np.exp(1j * ms * phi_qqp)
                count_G += 1
    return G 

def __init__(self, line):
        data = line.split(' ')
        data[1:] = [float(x) for x in data[1:]]
        self.classname = data[0]
        self.xmin = data[1] 
        self.ymin = data[2]
        self.xmax = data[1]+data[3]
        self.ymax = data[2]+data[4]
        self.box2d = np.array([self.xmin,self.ymin,self.xmax,self.ymax])
        self.centroid = np.array([data[5],data[6],data[7]])
        self.unused_dimension = np.array([data[8],data[9],data[10]])
        self.w = data[8]
        self.l = data[9]
        self.h = data[10]
        self.orientation = np.zeros((3,))
        self.orientation[0] = data[11]
        self.orientation[1] = data[12]
        self.heading_angle = -1 * np.arctan2(self.orientation[1], self.orientation[0]) 

def stanleyControl(state, cx, cy, cyaw, last_target_idx):
    """
    :param state: (State object)
    :param cx: ([float])
    :param cy: ([float])
    :param cyaw: ([float])
    :param last_target_idx: (int)
    :return: (float, int, float)
    """
    # Cross track error
    current_target_idx, error_front_axle = calcTargetIndex(state, cx, cy)

    if last_target_idx >= current_target_idx:
        current_target_idx = last_target_idx

    # theta_e corrects the heading error
    theta_e = normalizeAngle(cyaw[current_target_idx] - state.yaw)
    # theta_d corrects the cross track error
    theta_d = np.arctan2(K_STANLEY_CONTROL * error_front_axle, state.v)
    # Steering control
    delta = theta_e + theta_d

    return delta, current_target_idx, error_front_axle 

def calcTargetIndex(state, cx, cy):
    """
    :param state: (State object)
    :param cx: [float]
    :param cy: [float]
    :return: (int, float)
    """
    # Calc front axle position
    fx = state.x + CAR_LENGTH * np.cos(state.yaw)
    fy = state.y + CAR_LENGTH * np.sin(state.yaw)

    # Search nearest point index
    dx = [fx - icx for icx in cx]
    dy = [fy - icy for icy in cy]
    d = [np.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)]
    error_front_axle = min(d)
    target_idx = d.index(error_front_axle)

    target_yaw = normalizeAngle(np.arctan2(fy - cy[target_idx], fx - cx[target_idx]) - state.yaw)
    if target_yaw > 0.0:
        error_front_axle = - error_front_axle

    return target_idx, error_front_axle 

def vehicle_flat_reverse(zflag, params={}):
    # Get the parameter values
    b = params.get('wheelbase', 3.)

    # Create a vector to store the state and inputs
    x = np.zeros(3)
    u = np.zeros(2)

    # Given the flat variables, solve for the state
    x[0] = zflag[0][0]  # x position
    x[1] = zflag[1][0]  # y position
    x[2] = np.arctan2(zflag[1][1], zflag[0][1])  # tan(theta) = ydot/xdot

    # And next solve for the inputs
    u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2])
    thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2])
    u[1] = np.arctan2(thdot_v, u[0]**2 / b)

    return x, u


# Function to compute the RHS of the system dynamics 

def vehicle_flat_reverse(zflag, params={}):
    # Get the parameter values
    b = params.get('wheelbase', 3.)

    # Create a vector to store the state and inputs
    x = np.zeros(3)
    u = np.zeros(2)

    # Given the flat variables, solve for the state
    x[0] = zflag[0][0]  # x position
    x[1] = zflag[1][0]  # y position
    x[2] = np.arctan2(zflag[1][1], zflag[0][1])  # tan(theta) = ydot/xdot

    # And next solve for the inputs
    u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2])
    thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2])
    u[1] = np.arctan2(thdot_v, u[0]**2 / b)

    return x, u


# Function to compute the RHS of the system dynamics 

def raw_angles(self, measured):
        """
        Determine the raw angles from the count data.  This corresponds to the angle of U^N,
        i.e., it is N times the phase of U.
        """

        angles = OrderedDict()

        # The ordering here is chosen to maintain compatibility.
        for N, (Cp_Ns, Cm_Ns, Cp_Nc, Cm_Nc) in measured.items():
            # See the description of RobustPhaseEstimationDesign.
            # We estimate P^+_{Ns} and P^-_{Nc} from the similarly named counts.
            # The MLE for these probabilities is:
            Pp_Ns = Cp_Ns / (Cp_Ns + Cm_Ns)
            Pp_Nc = Cp_Nc / (Cp_Nc + Cm_Nc)

            angles[N] = numpy.arctan2(2 * Pp_Ns - 1, 2 * Pp_Nc - 1) % (2 * numpy.pi)

        return angles 

def GetNeighborCells(self, p, nr, dp = None):
        '''
        Returns all cells no more than a given distance in any direction
        from a specified cell
        p:      The cell of which to get the neighbors
        nr:     Neighbor radius
        dp:     Direction preference
        '''
        pi, pj, pk = p
        tqm = self.qm * self.qp
        nc = [(pi - i * tqm, pj - j * tqm, pk) for i in range(-nr, nr + 1) for j in range(-nr, nr + 1)]
        if dp is not None:                      #Sort points based on direction preference
            dpa = np.arctan2(dp[1], dp[0])      #Get angle of direction prefered
            #Prefer directions in the direction of dp; sort based on magnitude of angle from last direction
            nc = sorted(nc,  key = lambda t : np.abs(np.arctan2(t[1], t[0]) - dpa)) 
        return nc
        
    #Gets the current 3d position of the player 

def xyz_from_rotm(R):
  R = R.reshape(-1,3,3)
  xyz = np.empty((R.shape[0],3), dtype=R.dtype)
  for bidx in range(R.shape[0]):
    if R[bidx,0,2] < 1:
      if R[bidx,0,2] > -1:
        xyz[bidx,1] = np.arcsin(R[bidx,0,2])
        xyz[bidx,0] = np.arctan2(-R[bidx,1,2], R[bidx,2,2])
        xyz[bidx,2] = np.arctan2(-R[bidx,0,1], R[bidx,0,0])
      else:
        xyz[bidx,1] = -np.pi/2
        xyz[bidx,0] = -np.arctan2(R[bidx,1,0],R[bidx,1,1])
        xyz[bidx,2] = 0
    else:
      xyz[bidx,1] = np.pi/2
      xyz[bidx,0] = np.arctan2(R[bidx,1,0], R[bidx,1,1])
      xyz[bidx,2] = 0
  return xyz.squeeze() 

def zyx_from_rotm(R):
  R = R.reshape(-1,3,3)
  zyx = np.empty((R.shape[0],3), dtype=R.dtype)
  for bidx in range(R.shape[0]):
    if R[bidx,2,0] < 1:
      if R[bidx,2,0] > -1:
        zyx[bidx,1] = np.arcsin(-R[bidx,2,0])
        zyx[bidx,0] = np.arctan2(R[bidx,1,0], R[bidx,0,0])
        zyx[bidx,2] = np.arctan2(R[bidx,2,1], R[bidx,2,2])
      else:
        zyx[bidx,1] = np.pi / 2
        zyx[bidx,0] = -np.arctan2(-R[bidx,1,2], R[bidx,1,1])
        zyx[bidx,2] = 0
    else:
      zyx[bidx,1] = -np.pi / 2
      zyx[bidx,0] = np.arctan2(-R[bidx,1,2], R[bidx,1,1])
      zyx[bidx,2] = 0
  return zyx.squeeze() 

def axisangle_from_rotm(R):
  # logarithm of rotation matrix
  # R = R.reshape(-1,3,3)
  # tr = np.trace(R, axis1=1, axis2=2)
  # phi = np.arccos(np.clip((tr - 1) / 2, -1, 1))
  # scale = np.zeros_like(phi)
  # div = 2 * np.sin(phi)
  # np.divide(phi, div, out=scale, where=np.abs(div) > 1e-6)
  # A = (R - R.transpose(0,2,1)) * scale.reshape(-1,1,1)
  # aa = np.stack((A[:,2,1], A[:,0,2], A[:,1,0]), axis=1)
  # return aa.squeeze()
  R = R.reshape(-1,3,3)
  omega = np.empty((R.shape[0], 3), dtype=R.dtype)
  omega[:,0] = R[:,2,1] - R[:,1,2]
  omega[:,1] = R[:,0,2] - R[:,2,0]
  omega[:,2] = R[:,1,0] - R[:,0,1]
  r = np.linalg.norm(omega, axis=1).reshape(-1,1)
  t = np.trace(R, axis1=1, axis2=2).reshape(-1,1)
  omega = np.arctan2(r, t-1) * omega
  aa = np.zeros_like(omega)
  np.divide(omega, r, out=aa, where=r != 0)
  return aa.squeeze() 

def do_ac_analysis():
    circuit, n = simple_bjt_amp()
    simulator = circuit.simulator(temperature=25, nominal_temperature=25)
    analysis = simulator.ac(start_frequency=10, stop_frequency=1e6, number_of_points=100, variation='dec')
    gain = np.array(analysis[n.n5])
    figure = plt.figure(1, (20, 10))
    axe = plt.subplot(211)
    axe.grid(True)
    axe.set_xlabel("Frequency [Hz]")
    axe.set_ylabel("dB gain.")
    axe.semilogx(analysis.frequency, 20*np.log10(np.abs(gain)))

    axe = plt.subplot(212)
    axe.grid(True)
    axe.set_xlabel("Frequency [Hz]")
    axe.set_ylabel("Phase.")
    axe.semilogx(analysis.frequency, np.arctan2(gain.imag, gain.real))
    plt.show() 

def Hillshade(raster_file, azimuth, angle_altitude): 
    
    array = ReadRasterArrayBlocks(raster_file,raster_band=1)    
    
    x, y = np.gradient(array)
    slope = np.pi/2. - np.arctan(np.sqrt(x*x + y*y))
    aspect = np.arctan2(-x, y)
    azimuthrad = np.azimuth*np.pi / 180.
    altituderad = np.angle_altitude*np.pi / 180.
     
 
    shaded = np.sin(altituderad) * np.sin(slope)\
     + np.cos(altituderad) * np.cos(slope)\
     * np.cos(azimuthrad - aspect)
    return 255*(shaded + 1)/2
#============================================================================== 

def propagateRay(self, ray):
        """Refract, reflect, absorb, and/or scatter ray. This function may create and return new rays"""
        
        surface = self.surfaces[0]
        p1, ai = surface.intersectRay(ray)
        if p1 is not None:
            p1 = surface.mapToItem(ray, p1)
            rd = ray['dir']
            a1 = np.arctan2(rd[1], rd[0])
            ar = a1  + np.pi - 2*ai
            ray.setEnd(p1)
            dp = Point(np.cos(ar), np.sin(ar))
            ray = Ray(parent=ray, dir=dp)
        else:
            ray.setEnd(None)
        return [ray] 

def setFromQTransform(self, tr):
        p1 = Point(tr.map(0., 0.))
        p2 = Point(tr.map(1., 0.))
        p3 = Point(tr.map(0., 1.))
        
        dp2 = Point(p2-p1)
        dp3 = Point(p3-p1)
        
        ## detect flipped axes
        if dp2.angle(dp3) > 0:
            #da = 180
            da = 0
            sy = -1.0
        else:
            da = 0
            sy = 1.0
            
        self._state = {
            'pos': Point(p1),
            'scale': Point(dp2.length(), dp3.length() * sy),
            'angle': (np.arctan2(dp2[1], dp2[0]) * 180. / np.pi) + da
        }
        self.update() 

def generateShape(self):
        dt = self.deviceTransform()
        
        if dt is None:
            self._shape = self.path
            return None
        
        v = dt.map(QtCore.QPointF(1, 0)) - dt.map(QtCore.QPointF(0, 0))
        va = np.arctan2(v.y(), v.x())
        
        dti = fn.invertQTransform(dt)
        devPos = dt.map(QtCore.QPointF(0,0))
        tr = QtGui.QTransform()
        tr.translate(devPos.x(), devPos.y())
        tr.rotate(va * 180. / 3.1415926)
        
        return dti.map(tr.map(self.path)) 

def compute_pose_error(gt, pred):
    RE = 0
    snippet_length = gt.shape[0]
    scale_factor = np.sum(gt[:,:,-1] * pred[:,:,-1])/np.sum(pred[:,:,-1] ** 2)
    ATE = np.linalg.norm((gt[:,:,-1] - scale_factor * pred[:,:,-1]).reshape(-1))
    for gt_pose, pred_pose in zip(gt, pred):
        # Residual matrix to which we compute angle's sin and cos
        R = gt_pose[:,:3] @ np.linalg.inv(pred_pose[:,:3])
        s = np.linalg.norm([R[0,1]-R[1,0],
                            R[1,2]-R[2,1],
                            R[0,2]-R[2,0]])
        c = np.trace(R) - 1
        # Note: we actually compute double of cos and sin, but arctan2 is invariant to scale
        RE += np.arctan2(s,c)

    return ATE/snippet_length, RE/snippet_length 

def align(self, marker, axis):
        '''
        Rotate the marker set around the given axis until it is aligned onto the given marker

        Parameters
        ----------
        marker : int or str
            the index or label of the marker onto which to align the set
        axis : int
            the axis around which the rotation happens
        '''

        index = self.key2ind(marker)
        axis = ['x','y','z'].index(axis) if isinstance(marker, (str, unicode)) else axis

        # swap the axis around which to rotate to last position
        Y = self.X
        if self.dim == 3:
            Y[axis,:], Y[2,:] = Y[2,:], Y[axis,:]

        # Rotate around z to align x-axis to second point
        theta = np.arctan2(Y[1,index],Y[0,index])
        c = np.cos(theta)
        s = np.sin(theta)
        H = np.array([[c, s],[-s, c]])
        Y[:2,:] = np.dot(H,Y[:2,:])

        if self.dim == 3:
            Y[axis,:], Y[2,:] = Y[2,:], Y[axis,:] 

def cart2polar(input_xyz):
    rho = np.sqrt(input_xyz[:,0]**2 + input_xyz[:,1]**2)
    phi = np.arctan2(input_xyz[:,1],input_xyz[:,0])
    return np.stack((rho,phi,input_xyz[:,2]),axis=1) 

def _get_relative_goal_loc(goal_loc, loc, theta):
  r = np.sqrt(np.sum(np.square(goal_loc - loc), axis=1))
  t = np.arctan2(goal_loc[:,1] - loc[:,1], goal_loc[:,0] - loc[:,0])
  t = t-theta[:,0] + np.pi/2
  return np.expand_dims(r,axis=1), np.expand_dims(t, axis=1) 

def calc_state(self):
		j = np.array([j.current_relative_position() for j in self.ordered_joints], dtype=np.float32).flatten()
		# even elements [0::2] position, scaled to -1..+1 between limits
		# odd elements  [1::2] angular speed, scaled to show -1..+1
		self.joint_speeds = j[1::2]
		self.joints_at_limit = np.count_nonzero(np.abs(j[0::2]) > 0.99)

		body_pose = self.robot_body.pose()
		parts_xyz = np.array([p.pose().xyz() for p in self.parts.values()]).flatten()
		self.body_xyz = (
		parts_xyz[0::3].mean(), parts_xyz[1::3].mean(), body_pose.xyz()[2])  # torso z is more informative than mean z
		self.body_rpy = body_pose.rpy()
		z = self.body_xyz[2]
		if self.initial_z == None:
			self.initial_z = z
		r, p, yaw = self.body_rpy
		self.walk_target_theta = np.arctan2(self.walk_target_y - self.body_xyz[1],
											self.walk_target_x - self.body_xyz[0])
		self.walk_target_dist = np.linalg.norm(
			[self.walk_target_y - self.body_xyz[1], self.walk_target_x - self.body_xyz[0]])
		angle_to_target = self.walk_target_theta - yaw

		rot_speed = np.array(
			[[np.cos(-yaw), -np.sin(-yaw), 0],
			 [np.sin(-yaw), np.cos(-yaw), 0],
			 [		0,			 0, 1]]
		)
		vx, vy, vz = np.dot(rot_speed, self.robot_body.speed())  # rotate speed back to body point of view

		more = np.array([ z-self.initial_z,
			np.sin(angle_to_target), np.cos(angle_to_target),
			0.3* vx , 0.3* vy , 0.3* vz ,  # 0.3 is just scaling typical speed into -1..+1, no physical sense here
			r, p], dtype=np.float32)
		return np.clip( np.concatenate([more] + [j] + [self.feet_contact]), -5, +5) 

def angle(v1, v2):
    """ returns the angle (in radians) between two array-like vectors using the
        cross-product method, which is more accurate for small angles than the
        dot-product-acos method."""
    return np.arctan2(np.linalg.norm(np.cross(v1,v2)), np.dot(v1,v2)) 

def xyz2uv(xyz):
    '''
    xyz: ndarray in shape of [..., 3]
    '''
    x, y, z = np.split(xyz, 3, axis=-1)
    u = np.arctan2(x, z)
    c = np.sqrt(x**2 + z**2)
    v = np.arctan2(y, c)

    return np.concatenate([u, v], axis=-1) 

def gaussian(self, mean, covariance, label=None):
        """Draw 95% confidence ellipse of a 2-D Gaussian distribution.

        Parameters
        ----------
        mean : array_like
            The mean vector of the Gaussian distribution (ndim=1).
        covariance : array_like
            The 2x2 covariance matrix of the Gaussian distribution.
        label : Optional[str]
            A text label that is placed at the center of the ellipse.

        """
        # chi2inv(0.95, 2) = 5.9915
        vals, vecs = np.linalg.eigh(5.9915 * covariance)
        indices = vals.argsort()[::-1]
        vals, vecs = np.sqrt(vals[indices]), vecs[:, indices]

        center = int(mean[0] + .5), int(mean[1] + .5)
        axes = int(vals[0] + .5), int(vals[1] + .5)
        angle = int(180. * np.arctan2(vecs[1, 0], vecs[0, 0]) / np.pi)
        cv2.ellipse(
            self.image, center, axes, angle, 0, 360, self._color, 2)
        if label is not None:
            cv2.putText(self.image, label, center, cv2.FONT_HERSHEY_PLAIN,
                        2, self.text_color, 2) 

def fun(self, x, *args):
        self.nfev += 1

        r = sqrt(x[0] ** 2 + x[1] ** 2)
        theta = 1 / (2. * pi) * arctan2(x[1], x[0])

        return x[2] ** 2 + 100 * ((x[2] - 10 * theta) ** 2 + (r - 1) ** 2) 

def cart2polar(rvec):
    # The rows of rvec are the 3-component vectors
    # i.e. rvec is N x 3
    x,y,z = rvec.T
    r = lib.norm(rvec, axis=1)
    # theta is the polar angle, 0 < theta < pi
    # catch possible 0/0
    theta = np.arccos(z/(r+1e-200))
    # phi is the azimuthal angle, 0 < phi < 2pi (or -pi < phi < pi)
    phi = np.arctan2(y,x)
    return r, theta, phi 

def fuv2img(fuv, coorW=1024, floorW=1024, floorH=512):
    '''
    Project 1d signal in uv space to 2d floor plane image
    '''
    floor_plane_x, floor_plane_y = np.meshgrid(range(floorW), range(floorH))
    floor_plane_x, floor_plane_y = -(floor_plane_y - floorH / 2), floor_plane_x - floorW / 2
    floor_plane_coridx = (np.arctan2(floor_plane_y, floor_plane_x) / (2 * PI) + 0.5) * coorW - 0.5
    floor_plane = map_coordinates(fuv, floor_plane_coridx.reshape(1, -1), order=1, mode='wrap')
    floor_plane = floor_plane.reshape(floorH, floorW)
    return floor_plane 

def np_xy2coor(xy, z=50, coorW=1024, coorH=512, floorW=1024, floorH=512):
    '''
    xy: N x 2
    '''
    x = xy[:, 0] - floorW / 2 + 0.5
    y = xy[:, 1] - floorH / 2 + 0.5

    u = np.arctan2(x, -y)
    v = np.arctan(z / np.sqrt(x**2 + y**2))

    coorx = (u / (2 * PI) + 0.5) * coorW - 0.5
    coory = (-v / PI + 0.5) * coorH - 0.5

    return np.hstack([coorx[:, None], coory[:, None]])