Python numpy.arctan2方法代码示例

# 需要导入模块: import numpy [as 别名]
# 或者: from numpy import arctan2 [as 别名]
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]) 

# 需要导入模块: import numpy [as 别名]
# 或者: from numpy import arctan2 [as 别名]
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 

# 需要导入模块: import numpy [as 别名]
# 或者: from numpy import arctan2 [as 别名]
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)) 

# 需要导入模块: import numpy [as 别名]
# 或者: from numpy import arctan2 [as 别名]
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]) 

# 需要导入模块: import numpy [as 别名]
# 或者: from numpy import arctan2 [as 别名]
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 

# 需要导入模块: import numpy [as 别名]
# 或者: from numpy import arctan2 [as 别名]
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 

# 需要导入模块: import numpy [as 别名]
# 或者: from numpy import arctan2 [as 别名]
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 

# 需要导入模块: import numpy [as 别名]
# 或者: from numpy import arctan2 [as 别名]
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 

# 需要导入模块: import numpy [as 别名]
# 或者: from numpy import arctan2 [as 别名]
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 

# 需要导入模块: import numpy [as 别名]
# 或者: from numpy import arctan2 [as 别名]
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() 

# 需要导入模块: import numpy [as 别名]
# 或者: from numpy import arctan2 [as 别名]
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() 

# 需要导入模块: import numpy [as 别名]
# 或者: from numpy import arctan2 [as 别名]
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() 

# 需要导入模块: import numpy [as 别名]
# 或者: from numpy import arctan2 [as 别名]
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() 

# 需要导入模块: import numpy [as 别名]
# 或者: from numpy import arctan2 [as 别名]
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
#============================================================================== 

# 需要导入模块: import numpy [as 别名]
# 或者: from numpy import arctan2 [as 别名]
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]