def zoomToAll(self):
if self.m_nImgs < 1:
return
posA=N.array(self.m_imgPosArr)
sizA=N.array(self.m_imgSizeArr)
a=N.array([N.minimum.reduce(posA),
N.maximum.reduce(posA+sizA),
])
from .all import U
MC = N.array([0.5, 0.5]) # mosaic viewer's center (0.5, 0.5)
a -= MC
hypot = N.array((N.hypot(a[0][0], a[0][1]),
N.hypot(a[1][0], a[1][1])))
theta = N.array((N.arctan2(a[0][1], a[0][0]),
N.arctan2(a[1][1], a[1][0]))) # radians
phi = theta + U.deg2rad(self.m_rot)
mimXY = N.array((hypot[0]*N.cos(phi[0]), hypot[0]*N.sin(phi[0])))
maxXY = N.array((hypot[1]*N.cos(phi[1]), hypot[1]*N.sin(phi[1])))
a = N.array((mimXY, maxXY))
a.sort(0)
if self.m_aspectRatio == -1:
a = N.array(([a[0][0],-a[1][1]],[a[1][0],-a[0][1]]))
self.zoomToRect(x0=a[0][0], y0=a[0][1],
x1=a[-1][0],y1=a[-1][1])
def align_magnetism(m, vectors):
""" Rotates a matrix, to align its components with the direction
of the magnetism """
if not len(m) == 2 * len(vectors): # stop if they don't have
# compatible dimensions
raise
# pauli matrices
from scipy.sparse import csc_matrix, bmat
sx = csc_matrix([[0.0, 1.0], [1.0, 0.0]])
sy = csc_matrix([[0.0, -1j], [1j, 0.0]])
sz = csc_matrix([[1.0, 0.0], [0.0, -1.0]])
n = len(m) / 2 # number of sites
R = [[None for i in range(n)] for j in range(n)] # rotation matrix
from scipy.linalg import expm # exponenciate matrix
for (i, v) in zip(range(n), vectors): # loop over sites
vv = np.sqrt(v.dot(v)) # norm of v
if vv > 0.000001: # if nonzero scale
u = v / vv
else: # if zero put to zero
u = np.array([0.0, 0.0, 0.0])
# rot = u[0]*sx + u[1]*sy + u[2]*sz
uxy = np.sqrt(u[0] ** 2 + u[1] ** 2) # component in xy plane
phi = np.arctan2(u[1], u[0])
theta = np.arctan2(uxy, u[2])
r1 = phi * sz / 2.0 # rotate along z
r2 = theta * sy / 2.0 # rotate along y
# a factor 2 is taken out due to 1/2 of S
rot = expm(1j * r2) * expm(1j * r1)
R[i][i] = rot # save term
R = bmat(R) # convert to full sparse matrix
mout = R * csc_matrix(m) * R.H # rotate matrix
return mout.todense() # return dense matrix
def test_euler(self): """Test axis-angle and euler representation conversions.""" q1 = Quaternion([0,0,0,0], np.float64) #q1.from_euler(0, 0, 0) #print(q1) #self.assertEqual(q1, Quaternion([1,0,0,0])) for i in range(100): (phi, theta, psi) = self.rand_euler() q1.from_euler(phi, theta, psi) q_phi = np.arctan2(q1[0]*q1[2] + q1[1]*q1[3],-(q1[1]*q1[2]-q1[0]*q1[3])) q_theta = np.arccos(-q1[0]**2 -q1[1]**2 + q1[2]**2 + q1[3]**2) q_psi = np.arctan2(q1[0]*q1[2] - q1[1]*q1[3], q1[1]*q1[2] + q1[0]*q1[3]) # Convert between different conventions if q_phi < 0: q_phi += 2 * np.pi #if q_theta < 0: #q_theta += 2 * np.pi if q_psi < 0: q_psi += 2 * np.pi r1 = m.matrix([phi, theta, psi]) r1q = m.matrix([q_phi, q_theta, q_psi]) #print(r1- r1q) self.assertTrue(np.linalg.norm(r1 - r1q) < 1e-10)
def reconstruct_common(cls, t, x, y, z=None, initial=None):
"""Reconstruct angles from 3 detections
This function converts the coordinates to be suitable for the
algorithm.
:param t: arrival times in detector 0, 1 and 2 in ns.
:param x,y: positions of detector 0, 1 and 2 in m.
:param z: height of detectors 0, 1 and 2 is ignored.
:param initial: dictionary containing values from previous
reconstructions is ignored.
"""
if len(t) > 3 or len(x) > 3 or len(y) > 3:
warning_only_three()
dt = make_relative(t)
dx = make_relative(x)
dy = make_relative(y)
r1 = vector_length(dx[1], dy[1])
r2 = vector_length(dx[2], dy[2])
phi1 = arctan2(dy[1], dx[1])
phi2 = arctan2(dy[2], dx[2])
return cls.reconstruct(dt[1], dt[2], r1, r2, phi1, phi2)
def nominal_q(sx, sy, az_in, el_in, az_out, el_out, dz):
nx, ny = sx + dz * tan(az_in), sy + dz * tan(el_in)
nd = sqrt( (nx-sx)**2 + (ny-sy)**2 + dz**2 )
#plt.subplot(131); plot(nx/5,ny/5,'G: direct flight beam center')
px, py = sx + dz * tan(az_out), sy + dz * tan(el_out)
pd = sqrt( (px-sx)**2 + (py-sy)**2 + dz**2 )
#plt.subplot(122); plot(px/5,py/5,'G: direct flight scattered beam')
if 0:
# Correction to move px,py into the q normal plane. This is
# insignificant for small angle scattering.
nx_hat, ny_hat, nz_hat = (nx-sx)/nd, (ny-sy)/nd, dz/nd
px_hat, py_hat, pz_hat = (px-sx)/pd, (py-sy)/pd, dz/pd
d = nd / (px_hat*nx_hat + py_hat*ny_hat + pz_hat*nz_hat)
px, py = sx + px_hat*d, sy + py_hat*d
#plt.subplot(122); plot((px)/5,(py)/5,'G: scattered beam on q normal plane')
# Note: px,py is the location of the scattered neutron relative to the
# beam center without gravity in detector coordinates, not the qx,qy vector
# in inverse coordinates. This allows us to compute the scattered angle at
# the sample, returning theta and phi.
qd = sqrt((px-nx)**2 + (py-ny)**2)
theta, phi = arctan2(qd, nd)/2, arctan2(py-ny, px-nx)
return theta, phi
def cart2sph(z, y, x):
"""Convert from cartesian coordinates (x,y,z) to spherical (elevation,
azimuth, radius). Output is in degrees.
usage:
array3xN[el,az,rad] = cart2sph(array3xN[x,y,z])
OR
elevation, azimuth, radius = cart2sph(x,y,z)
If working in DKL space, z = Luminance, y = S and x = LM"""
width = len(z)
elevation = numpy.empty([width,width])
radius = numpy.empty([width,width])
azimuth = numpy.empty([width,width])
radius = numpy.sqrt(x**2 + y**2 + z**2)
azimuth = numpy.arctan2(y, x)
#Calculating the elevation from x,y up
elevation = numpy.arctan2(z, numpy.sqrt(x**2+y**2))
#convert azimuth and elevation angles into degrees
azimuth *=(180.0/numpy.pi)
elevation *=(180.0/numpy.pi)
sphere = numpy.array([elevation, azimuth, radius])
sphere = numpy.rollaxis(sphere, 0, 3)
return sphere
def get_UDP(self, Npackets, LO_freq, skip_packets=2, channels = None):
#Npackets = np.int(time_interval * self.accum_freq)
I_buffer = np.empty((Npackets + skip_packets, len(channels)))
Q_buffer = np.empty((Npackets + skip_packets, len(channels)))
self.fpga.write_int('pps_start', 1)
count = 0
while count < Npackets + skip_packets:
packet = self.s.recv(8192)
data = np.fromstring(packet,dtype = '<i').astype('float')
data /= 2.0**17
data /= (self.accum_len/512.)
ts = (np.fromstring(packet[-4:],dtype = '<i').astype('float')/ self.fpga_samp_freq)*1.0e3 # ts in ms
odd_chan = channels[1::2]
even_chan = channels[0::2]
I_odd = data[1024 + ((odd_chan - 1) / 2)]
Q_odd = data[1536 + ((odd_chan - 1) /2)]
I_even = data[0 + (even_chan/2)]
Q_even = data[512 + (even_chan/2)]
even_phase = np.arctan2(Q_even,I_even)
odd_phase = np.arctan2(Q_odd,I_odd)
if len(channels) % 2 > 0:
I = np.hstack(zip(I_even[:len(I_odd)], I_odd))
Q = np.hstack(zip(Q_even[:len(Q_odd)], Q_odd))
I = np.hstack((I, I_even[-1]))
Q = np.hstack((Q, Q_even[-1]))
I_buffer[count] = I
Q_buffer[count] = Q
else:
I = np.hstack(zip(I_even, I_odd))
Q = np.hstack(zip(Q_even, Q_odd))
I_buffer[count] = I
Q_buffer[count] = Q
count += 1
return I_buffer[skip_packets:],Q_buffer[skip_packets:]
def rotzyx_angles(H):
"""Returns the angles such that `H[0:3, 0:3] = R_z(a_z) R_y(a_y) R_x(a_x)`
:param H: homogeneous matrix
:type H: 4x4 ndarray
:rtype: 3-tuple
**Example:**
>>> angles = array((3.14/3, 3.14/6, 1))
>>> (rotzyx_angles(rotzyx(*angles)) == angles).all()
True
"""
assert ishomogeneousmatrix(H)
if abs(H[0,0])<tol and abs(H[1,0])<tol:
# singularity
az = 0
ay = arctan2(-H[2,0], H[0,0])
ax = arctan2(-H[1,2], H[1,1])
else:
az= arctan2(H[1,0],H[0,0])
sz = sin(az)
cz = cos(az)
ay = arctan2(-H[2,0], cz*H[0,0] + sz*H[1,0])
ax = arctan2(sz*H[0,2] - cz*H[1,2], cz*H[1,1] - sz*H[0,1])
return (az, ay, ax)
def main():
book = xlrd.open_workbook('sense_hat.xlsx')
sheet = book.sheet_by_index(0)
g_deg_x =0
g_deg_y =0
g_deg_z =0
a_deg_x =0
a_deg_y =0
a_deg_z =0
g_deg_x_l = []
a_deg_x_l = []
num = []
for row in range(1, sheet.nrows-1):
data = []
for col in range(0, sheet.ncols-2):
data.append(sheet.cell(row,col).value)
g_deg_x += np.degrees(data[0])*0.1
g_deg_y += np.degrees(data[1])*0.1
g_deg_z += np.degrees(data[2])*0.1
a_deg_x = np.degrees(np.arctan2(data[3], np.sqrt(data[4]**2 + data[5]**2) ))
a_deg_y = np.degrees(np.arctan2(data[4], np.sqrt(data[3]**2 + data[5]**2) ))
a_deg_z = np.degrees(np.arctan2(data[5], np.sqrt(data[3]**2 + data[4]**2) ))
g_deg_x_l.append(g_deg_x)
a_deg_x_l.append(a_deg_x)
num.append(row-1)
plt.plot(num,g_deg_x_l, 'r-', label='Gyro')
plt.plot(num,a_deg_x_l, 'b-', label='Accel')
plt.legend()
plt.show()
def getgeodesicpts(m):
"""
computes the lat/lon values of the points on the surface of the sphere
corresponding to a twenty-sided (icosahedral) geodesic.
@param m: the number of points on the edge of a single geodesic triangle.
There are 10*(m-1)**2+2 total geodesic points, including the poles.
@return: C{B{lats, lons}} - rank 1 numpy float32 arrays containing
the latitudes and longitudes of the geodesic points (in degrees). These
points are nearly evenly distributed on the surface of the sphere.
"""
x,y,z = _spherepack.ihgeod(m)
# convert cartesian coords to lat/lon.
rad2dg = 180./math.pi
r1 = x*x+y*y
r = numpy.sqrt(r1+z*z)
r1 = numpy.sqrt(r1)
xtmp = numpy.where(numpy.logical_or(x,y),x,numpy.ones(x.shape,numpy.float32))
ztmp = numpy.where(numpy.logical_or(r1,z),z,numpy.ones(z.shape,numpy.float32))
lons = rad2dg*numpy.arctan2(y,xtmp)+180.
lats = rad2dg*numpy.arctan2(r1,ztmp)-90.
lat = numpy.zeros(10*(m-1)**2+2,numpy.float32)
lon = numpy.zeros(10*(m-1)**2+2,numpy.float32)
# first two points are poles.
lat[0] = 90; lat[1] = -90.
lon[0] = 0.; lon[1] = 0.
lat[2:] = lats[0:2*(m-1),0:m-1,:].flatten()
lon[2:] = lons[0:2*(m-1),0:m-1,:].flatten()
return lat,lon
def lombscargle(ages, signal, ofac=4, hifac=1):
r"""Calculates Lomb-Scargle Periodogram.
Enter `signal` at times `ages` to compute the periodogram, with
oversampling factor `ofac` and up to frequencies `hifac` * Nyquist.
Return frequencies considered `freq`, the associated spectral `power`,
and estimated significance of the power values `prob`.
Note: the significance returned is the false alarm probability of the null
hypothesis, i.e. that the data is composed of independent Gaussian random
variables. Low probability values indicate a high degree of significance
in the associated periodic signal."""
N, T = len(signal), ages.ptp()
# Mean and variance.
mu, s2 = signal.mean(), signal.var()
# Calculate sampling frequencies.
start = 1.0 / (T * ofac)
stop = hifac * N / (2.0 * T)
dt = 1.0 / (T * ofac) # Interval for the frequencies. Can be tweaked.
freq = np.arange(start, stop + dt, dt)
# Angular frequencies and constant offsets.
w = 2.0 * np.pi * freq
dot = np.dot(w[:, None], ages[None, :])
A = np.sum(np.sin(2.0 * dot), axis=1)
B = np.sum(np.cos(2.0 * dot), axis=1)
tau = np.arctan2(A, B) / (2.0 * w)
# Spectral power.
cterm = np.cos(dot - (w * tau)[:, None])
sterm = np.sin(dot - (w * tau)[:, None])
ry = (np.sum(np.dot(cterm, np.diag(signal - mu)), axis=1) ** 2.0 /
np.sum(cterm ** 2, axis=1))
iy = (np.sum(np.dot(sterm, np.diag(signal - mu)), axis=1) ** 2.0 /
np.sum(sterm ** 2, axis=1))
# TODO: Phase (untested!)
phLS = np.arctan2(ry, iy)
power = (np.sum(np.dot(cterm, np.diag(signal - mu)), axis=1) ** 2.0 /
np.sum(cterm ** 2, axis=1) +
np.sum(np.dot(sterm, np.diag(signal - mu)), axis=1) ** 2.0 /
np.sum(sterm ** 2, axis=1))
power /= (2.0 * s2)
# Estimate of the number of independent frequencies.
M = 2.0 * len(freq) / ofac
# Statistical significant of power.
prob = M * np.exp(-power)
inds = prob > 0.01
prob[inds] = 1.0 - (1.0 - np.exp(-power[inds])) ** M
return freq, power, prob, phLS
def fourier(self):
"""
Generate a profile of fourier coefficients, amplitudes and phases
"""
if pynbody.config['verbose'] : print 'Profile: fourier()'
f = {'c': np.zeros((7, self.nbins),dtype=complex),
'amp': np.zeros((7, self.nbins)),
'phi': np.zeros((7, self.nbins))}
for i in range(self.nbins):
if self._profiles['n'][i] > 100:
phi = np.arctan2(self.sim['y'][self.binind[i]], self.sim['x'][self.binind[i]])
mass = self.sim['mass'][self.binind[i]]
hist, binphi = np.histogram(phi,weights=mass,bins=100)
binphi = .5*(binphi[1:]+binphi[:-1])
for m in range(7) :
f['c'][m,i] = np.sum(hist*np.exp(-1j*m*binphi))
f['c'][:,self['mass']>0] /= self['mass'][self['mass']>0]
f['amp'] = np.sqrt(np.imag(f['c'])**2 + np.real(f['c'])**2)
f['phi'] = np.arctan2(np.imag(f['c']), np.real(f['c']))
return f
def twistUpdate(self):
if self.goal is None:
w = 0
v = 0
else:
errX = self.goal.position.x-self.pose.position.x
errY = self.goal.position.y-self.pose.position.y
if errX*errX<0.01 and errY*errY<0.01:
self.goal = None
w = 0
v = 0
errYaw = 0
else:
euler = tf.transformations.euler_from_quaternion([self.pose.orientation.x,self.pose.orientation.y,self.pose.orientation.z,self.pose.orientation.w])
goalYaw = numpy.arctan2(errY,errX)
errYaw = goalYaw-euler[2]
errYaw = numpy.arctan2(numpy.sin(errYaw),numpy.cos(errYaw))
w = -self.Kp*errYaw
if abs(w)>self.w_max:
w = self.w_max*(abs(w)/w)
if abs(errYaw)<0.75:
v = self.v_max / ( abs(w) + 1 )**0.5
else:
v = 0
rospy.logdebug("%s pX:%f pY:%f eX:%f eY:%f eYaw:%f -> w:%f v:%f"
, self.nodeName,self.pose.position.x,self.pose.position.y,errX,errY,errYaw,w,v
)
self.twist.linear.x = v
self.twist.angular.z = w
def lambet(self):
"""Ecliptic longitude and latitude."""
from astropy.coordinates import Angle
lam = np.arctan2(self._rot.T[1], self._rot.T[0])
bet = np.arctan2(self._rot.T[2],
np.sqrt(self._rot.T[0]**2 + self._rot.T[1]**2))
return Angle(np.degrees(lam) * u.deg), Angle(np.degrees(bet) * u.deg)
def get_params(A):
"""This is a copy of spm's spm_imatrix where
we already know the rotations and translations matrix,
shears and zooms (as outputs from fsl FLIRT/avscale)
Let A = the 4x4 rotation and translation matrix
R = [ c5*c6, c5*s6, s5]
[-s4*s5*c6-c4*s6, -s4*s5*s6+c4*c6, s4*c5]
[-c4*s5*c6+s4*s6, -c4*s5*s6-s4*c6, c4*c5]
"""
import numpy as np
def rang(b):
a = min(max(b, -1), 1)
return a
Ry = np.arcsin(A[0,2])
#Rx = np.arcsin(A[1,2]/np.cos(Ry))
#Rz = np.arccos(A[0,1]/np.sin(Ry))
if (abs(Ry)-np.pi/2)**2 < 1e-9:
Rx = 0
Rz = np.arctan2(-rang(A[1,0]), rang(-A[2,0]/A[0,2]))
else:
c = np.cos(Ry)
Rx = np.arctan2(rang(A[1,2]/c), rang(A[2,2]/c))
Rz = np.arctan2(rang(A[0,1]/c), rang(A[0,0]/c))
rotations = [Rx, Ry, Rz]
translations = [A[0,3], A[1,3], A[2,3]]
return rotations, translations
def rz_2_lq(str,stz):
'''
convert radial/Z component to max P/ max S component
find maximum P wave amplitude to determine incidence angle for every
trace
'''
stl = stz.copy()
stq = str.copy()
for idx,tr in enumerate(str):
R = seispy.data.phase_window(tr,['P'],(-10,10)).data.min()
Z = seispy.data.phase_window(stz[idx],['P'],(-10,10)).data.min()
if R < 0 and Z < 0:
R*=-1
Z*=-1
deg = np.arctan2(R,Z)
elif R < 0 and Z > 0:
deg = np.arctan2(R,Z)
else:
deg = -1*np.arctan2(R,Z)
stq[idx].data = np.cos(deg)*tr.data-np.sin(deg)*stz[idx].data
stl[idx].data = np.sin(deg)*tr.data+np.cos(deg)*stz[idx].data
stq[idx].stats.channel = 'BHS'
stl[idx].stats.channel = 'BHP'
return stl, stq
def _lambet(self):
"""Lower overhead ecliptic longitude and latitude. [deg]"""
from astropy.coordinates import Angle
lam = np.arctan2(self._rot.T[1], self._rot.T[0])
bet = np.arctan2(self._rot.T[2],
np.sqrt(self._rot.T[0]**2 + self._rot.T[1]**2))
return np.degrees(lam), np.degrees(bet)
def __iter__(self):
MAX_X,MAX_Y = self.dimensions
MIN_V, MAX_V = self.velocity
wt_min = 0.
if self.init_stationary:
x, y, x_waypoint, y_waypoint, velocity, wt = \
init_random_waypoint(self.nr_nodes, MAX_X, MAX_Y, MIN_V, MAX_V, wt_min,
(self.wt_max if self.wt_max is not None else 0.))
else:
NODES = np.arange(self.nr_nodes)
print NODES
x = U(0, MAX_X, NODES)
y = U(0, MAX_Y, NODES)
x_waypoint = U(0, MAX_X, NODES)
y_waypoint = U(0, MAX_Y, NODES)
wt = np.zeros(self.nr_nodes)
velocity = U(MIN_V, MAX_V, NODES)
theta = np.arctan2(y_waypoint - y, x_waypoint - x)
costheta = np.cos(theta)
sintheta = np.sin(theta)
while True:
# update node position
x += velocity * costheta
y += velocity * sintheta
# calculate distance to waypoint
d = np.sqrt(np.square(y_waypoint-y) + np.square(x_waypoint-x))
# update info for arrived nodes
arrived = np.where(np.logical_and(d<=velocity, wt<=0.))[0]
# step back for nodes that surpassed waypoint
x[arrived] = x_waypoint[arrived]
y[arrived] = y_waypoint[arrived]
if self.wt_max:
velocity[arrived] = 0.
wt[arrived] = U(0, self.wt_max, arrived)
# update info for paused nodes
wt[np.where(velocity==0.)[0]] -= 1.
# update info for moving nodes
arrived = np.where(np.logical_and(velocity==0., wt<0.))[0]
if arrived.size > 0:
x_waypoint[arrived] = U(0, MAX_X, arrived)
y_waypoint[arrived] = U(0, MAX_Y, arrived)
velocity[arrived] = U(MIN_V, MAX_V, arrived)
theta[arrived] = np.arctan2(y_waypoint[arrived] - y[arrived], x_waypoint[arrived] - x[arrived])
costheta[arrived] = np.cos(theta[arrived])
sintheta[arrived] = np.sin(theta[arrived])
self.velocity = velocity
self.wt = wt
yield np.dstack((x,y))[0]
def inverse_kepler_2d(xv, m, t):
"""Compute the Keplerian parameters for the osculating orbit.
No partial derivatives are computed (even though it would be much easier)
because you can use the partials for kepler_2d and invert the matrix.
The value of t0 computed is the value within one half-period of t.
"""
mu = G*m
#a_guess = np.hypot(xv[0], xv[1])
h = (xv[0]*xv[3]-xv[1]*xv[2])
r = np.hypot(xv[0], xv[1])
eps2, eps1 = np.array([xv[3], -xv[2]])*h/mu - xv[:2]/r
e = np.hypot(eps1, eps2)
p = h**2/mu
a = p/(1-e**2)
pb = 2*np.pi*(a**3/mu)**(0.5)
om = np.arctan2(eps1, eps2)
true_anomaly = np.arctan2(xv[1], xv[0])-om
eccentric_anomaly = np.arctan2(np.sqrt(1-e**2)*np.sin(true_anomaly),
e+np.cos(true_anomaly))
mean_anomaly = eccentric_anomaly - e*np.sin(eccentric_anomaly)
true_anomaly_0 = -om
eccentric_anomaly_0 = np.arctan2(np.sqrt(1-e**2)*np.sin(true_anomaly_0),
e+np.cos(true_anomaly_0))
mean_anomaly_0 = eccentric_anomaly_0 - e*np.sin(eccentric_anomaly_0)
return Kepler2DParameters(a=a, pb=pb, eps1=eps1, eps2=eps2,
t0=t-(mean_anomaly-mean_anomaly_0)*pb/(2*np.pi))
def get_polar_line(self, line, odom):
"""
Transforms a line from [x1 y1 x2 y2] from the world frame to the
vehicle frame using odomotrey [x y ang].
Returns [range theta]
"""
# Line points
x1 = line[0]
y1 = line[1]
x2 = line[2]
y2 = line[3]
# Compute line (a, b, c) and range
line = np.array([y1 - y2, x2 - x1, x1 * y2 - x2 * y1])
pt = np.array([odom[0], odom[1], 1])
dist = np.dot(pt, line) / np.linalg.norm(line[:2])
# Compute angle
if dist > 0:
ang = np.arctan2(line[1], line[0])
else:
ang = np.arctan2(-line[1], -line[0])
# Return in the vehicle frame
return np.array([np.abs(dist), angle_wrap(ang - odom[2])])
def __call__(self, ra, dec, inverse=False):
"""Convert RA/Dec into map coordinates, or the reverse.
Args:
ra: float or array of floats
dec: float or array of floats
inverse: if True, convert from map coordinates to RA/Dec
Returns:
x,y with the same format as ra/dec
"""
if not inverse:
ra_ = self._wrapRA(ra)
# Snyder 1987, eq 16-1 to 16-4
theta = self.n * ra_
rho = self._rho(dec)
return rho*np.sin(theta * self.deg2rad), self.rho_0 - rho*np.cos(theta * self.deg2rad)
else:
# ra/dec actually x/y
# Snyder 1987, eq 14-10 to 14-11
rho = np.sqrt(ra**2 + (self.rho_0 - dec)**2) * np.sign(self.n)
if self.n >= 0:
theta = np.arctan2(ra, self.rho_0 - dec) / self.deg2rad
else:
theta = np.arctan2(-ra, -(self.rho_0 - dec)) / self.deg2rad
return self.ra_0 - theta/self.n, (self.G - rho)/ self.deg2rad
def _cart_to_sph(x, y, z):
"""Aux function"""
hypotxy = np.hypot(x, y)
r = np.hypot(hypotxy, z)
elev = np.arctan2(z, hypotxy)
az = np.arctan2(y, x)
return az, elev, r
def car2sph(xyz):
ptsnew = numpy.zeros(xyz.shape)
xy = xyz[:,0]**2 + xyz[:,1]**2
ptsnew[:,0] = numpy.sqrt(xy + xyz[:,2]**2) # r
ptsnew[:,1] = numpy.arctan2(numpy.sqrt(xy), xyz[:,2]) # tetha
ptsnew[:,2] = numpy.arctan2(xyz[:,1], xyz[:,0]) # phi
return ptsnew
def R(r, t):
xaccel = np.cos(np.arctan2(r[3], r[1])) * (C / (r[1]**2 + r[3]**2)**.5);
xvel = r[0];
yaccel = np.sin(np.arctan2(r[3], r[1])) * C / (r[1]**2 + r[3]**2)**.5;
yvel = r[2];
return np.array([xaccel, xvel, yaccel, yvel])
def pintadubing(lsec,color = 'b'):
'''
This function draws a complete dubing trajectory, departing for a list
of primary and secundary waypoint such as those generated by secnwayp()
waypoints and trayectory are draw in blue
circles are draw in dashed black
'''
currutaca = lsec[0][1]
for i in lsec:
pintacirculo(i[0]) #Circles involved in Dubbing trajectory
for j in i:
pl.plot(j[0],j[1],'o'+color) #waypoint
pl.plot([currutaca[0],i[1][0]],[currutaca[1],i[1][1]],color)
print i
#draw the arc covered is radius > 0...
if i[0][2]>0:
print i, 'paso'
currutaca = i[2]
ang = [np.arctan2(i[1][1]-i[0][1],i[1][0]-i[0][0])\
,np.arctan2(i[2][1]-i[0][1],i[2][0]-i[0][0])]
print ang
ang[1] = ang[1] - i[0][3]*((ang[1]>ang[0])*(i[0][3]>0.)+\
(ang[1]<ang[0])*(i[0][3]<0.))*2*np.pi
print ang, '\n'
pintacirculo(i[0],ang,color,1)
if (lsec[0][0][2] > 0.) and (lsec[-1][0][2]>0.):
pl.plot([lsec[-1][2][0],lsec[0][1][0]],\
[lsec[-1][2][1],lsec[0][1][1]],color)
def cv_coord(a,b,c,fr=None,to=None,degr=False):
if degr:
degrad = deg2rad
raddeg = rad2deg
else:
degrad = lambda x: x
raddeg = lambda x: x
if fr=='sph':
x=c*cos(degrad(a))*cos(degrad(b))
y=c*sin(degrad(a))*cos(degrad(b))
z=c*sin(degrad(b))
elif fr=='rect':
x=a
y=b
z=c
elif fr is None:
raise Exception('You must specify the input coordinate system')
else:
raise Exception('Unknown input coordinate system')
if to=='rect':
return (x,y,z)
elif to=='sph':
ra = raddeg(arctan2(y,x))
dec = raddeg(arctan2(z,sqrt(x**2+y**2)))
rad = sqrt(x**2+y**2+z**2)
return (ra,dec,rad)
elif to is None:
raise Exception('You must specify the output coordinate system')
else:
raise Exception('Unknown output coordinate system')
def inv_kin(p, ga, l=[100,100,100]):
w = np.array([0]*4,dtype=float) #horizontal coordinate
z = np.array([0]*4,dtype=float) #vertical coordinate
gripper_angle=ga
w[3] = np.sqrt ( np.square ( p[2] ) + np.square ( p[0] ))
z[3] = p[1]
w[2] = w[3] - l[2] * np.cos(gripper_angle)
z[2] = z[3] - l[2] * np.sin(gripper_angle)
l12 = np.sqrt ( np.square ( w[2] ) + np.square ( z[2] ))
a12 = np.arctan2(z[2], w[2])
a = [0]*4
a[0] = np.arctan2(p[0],p[2])
a[1] = np.arccos ( ( np.square( l[0] ) + np.square ( l12 ) - np.square ( l[1])) / (2 * l[0] * l12 )) + a12
w[1] = l[0] * np.cos(a[1])
z[1] = l[0] * np.sin(a[1])
a[2] = np.arctan2 ( ( z[2] - z[1] ) , ( w[2] - w[1] ) ) - a[1]
a[3] = gripper_angle - a[1] - a[2]
a[1]=a[1]-np.pi/2
return a
def test_arctan2_invalid(self):
with pytest.raises(u.UnitsError) as exc:
np.arctan2(np.array([1, 2, 3]) * u.N, 1. * u.s)
assert "compatible dimensions" in exc.value.args[0]
with pytest.raises(u.UnitsError) as exc:
np.arctan2(np.array([1, 2, 3]) * u.N, 1.)
assert "dimensionless quantities when other arg" in exc.value.args[0]
def new_model(self):
base_sphere=uniform_unit_sphere(self.targetN,base_grid=self.base_grid)
x_uni,y_uni,z=base_sphere.make_xyz()
self.actualN=len(x_uni)
rad=numpy.sqrt(x_uni**2 + y_uni**2)
phi=numpy.arctan2(y_uni,x_uni)
n_vec=2000
phi_new_vec=numpy.linspace(-numpy.pi, numpy.pi, n_vec)
phi_old_vec=phi_new_vec + self.rho_peturb*(numpy.sin(2.*phi_new_vec)/2.)
phi_new=numpy.interp(phi,phi_old_vec,phi_new_vec)
x=rad*numpy.cos(phi_new)
y=rad*numpy.sin(phi_new)
rad=numpy.sqrt(x**2 + y**2)
phi=numpy.arctan2(y,x)
vel=self.omega*rad
vx=-vel*numpy.sin(phi)
vy= vel*numpy.cos(phi)
vz=0.
mass=numpy.ones_like(x)/self.actualN
Ep=3./5
self.internalE=Ep*self.ethep_ratio
internal_energy=numpy.ones_like(x)*self.internalE
return (mass,x,y,z,vx,vy,vz,internal_energy)
def find_best_new(self):
x, y, th = np.unravel_index(self.posecells.argmax(), self.posecells.shape)
mx = self.posecells[x,y,th]
# get the sums for each axis
x_sums = np.zeros(PC_DIM_XY)
y_sums = np.zeros(PC_DIM_XY)
z_sums = np.zeros(PC_DIM_TH)
for i in range(x - PC_CELLS_TO_AVG, x + PC_CELLS_TO_AVG + 1):
for j in range(y - PC_CELLS_TO_AVG, y + PC_CELLS_TO_AVG + 1):
for k in range(th - PC_CELLS_TO_AVG, th + PC_CELLS_TO_AVG + 1):
# Use modulo for wrapping
im = i % PC_DIM_XY
jm = j % PC_DIM_XY
km = k % PC_DIM_TH
x_sums[im] += self.posecells[im ,jm, km]
y_sums[jm] += self.posecells[im ,jm, km]
z_sums[km] += self.posecells[im ,jm, km]
# now find the (x, y, th) using population vector decoding to handle the wrap around
sum_x1 = 0
sum_x2 = 0
sum_y1 = 0
sum_y2 = 0
for i in range(PC_DIM_XY):
sum_x1 += PC_XY_SUM_SIN_LOOKUP[i] * x_sums[i]
sum_x2 += PC_XY_SUM_COS_LOOKUP[i] * x_sums[i]
sum_y1 += PC_XY_SUM_SIN_LOOKUP[i] * y_sums[i]
sum_y2 += PC_XY_SUM_COS_LOOKUP[i] * y_sums[i]
x = np.arctan2(sum_x1, sum_x2) * PC_DIM_XY / (2.0 * np.pi) - 1.0
while x < 0:
x += PC_DIM_XY
while x > PC_DIM_XY:
x -= PC_DIM_XY
y = np.arctan2(sum_y1, sum_y2) * PC_DIM_XY / (2.0 * np.pi) - 1.0
while y < 0:
y += PC_DIM_XY
while x > PC_DIM_XY:
y -= PC_DIM_XY
sum_x1 = 0
sum_x2 = 0
for i in range(PC_DIM_TH):
sum_x1 += PC_TH_SUM_SIN_LOOKUP[i] * z_sums[i]
sum_x2 += PC_TH_SUM_COS_LOOKUP[i] * z_sums[i]
th = np.arctan2(sum_x1, sum_x2) * PC_DIM_TH / (2.0 * np.pi) - 1.0
while th < 0:
th += PC_DIM_TH
while x > PC_DIM_TH:
th -= PC_DIM_TH
self.best_x = x
self.best_y = y
self.best_th = th
def cartToPol(x, y):
x = np.array(x)
y = np.array(y)
return np.arctan2(y, x), np.sqrt(x**2 + y**2)
def fetch(db, stations, phasors=False, parm="Gain:0:0", direction=None,
asPolar=True):
"""
Fetch the value of a complex, station bound, parameter from a LOFAR
parameter database. The parameter values for all station given in "stations"
should be defined on the same grid.
db: A lofar.parmdb.parmdb instance.
stations: List of stations for which to retrieve the associated value
(will be added as an infix / suffix to the parameter base name).
phasors: (default False) If set to true, use "Ampl", "Phase" infix
instead of "Real", "Imag".
parm: (default "Gain:0:0") Base name of parameter to fetch.
direction: (default None) Source name added to the parameter name as a
suffix.
asPolar: (default True) Return value as (amplitude, phase) if set to
True, (real, imaginary) otherwise. Conversion is performed as
needed, depending on the value of 'phasors'.
"""
suffix = ""
if direction:
suffix = ":%s" % direction
infix = ("Real", "Imag")
if phasors:
infix = ("Ampl", "Phase")
el0 = []
el1 = []
for station in stations:
fqname = "%s:%s:%s%s" % (parm, infix[0], station, suffix)
el0.append(__fetch_value(db, fqname))
fqname = "%s:%s:%s%s" % (parm, infix[1], station, suffix)
el1.append(__fetch_value(db, fqname))
el0 = numpy.array(el0)
el1 = numpy.array(el1)
if phasors and not asPolar:
re = numpy.zeros(el0.shape)
im = numpy.zeros(el1.shape)
for i in range(0, len(stations)):
re[i] = el0[i] * numpy.cos(el1[i])
im[i] = el0[i] * numpy.sin(el1[i])
return (re, im)
if not phasors and asPolar:
ampl = numpy.zeros(el0.shape)
phase = numpy.zeros(el1.shape)
for i in range(0, len(stations)):
ampl[i] = numpy.sqrt(numpy.power(el0[i], 2) + numpy.power(el1[i], 2))
phase[i] = numpy.arctan2(el1[i], el0[i])
return (ampl, phase)
return (el0, el1)
# define parameters related to calculation
maxl = 10
_,beam_sig,del_bl,num_bl = sys.argv
beam_sig=float(beam_sig); del_bl=float(del_bl);num_bl=int(num_bl)
fqs = n.arange(50,91,2)*0.001
savekey = 'grid_del_bl_{0:.2f}_num_bl_{1}_beam_sig_{2:.2f}'.format(del_bl,num_bl,beam_sig)
#global tx,ty,tz,dOmega,theta,phi,amp
im = a.img.Img(size=200, res=.5) #make an image of the sky to get sky coords
tx,ty,tz = im.get_top(center=(200,200)) #get coords of the zenith?
dOmega = get_dOmega(tx,ty)
valid = n.logical_not(tx.mask)
tx,ty,tz,dOmega = tx.flatten(),ty.flatten(),tz.flatten(),dOmega.flatten()
theta = n.arctan2(ty,tx) # using math convention of theta=[0,2pi], phi=[0,pi]
phi = n.arccos(n.sqrt(1-tx*tx-ty*ty))
amp = uf.gaussian(beam_sig,n.zeros_like(theta),phi)
baselines = agg.make_pos_array(del_bl,num_bl)
num0,num1 = len(baselines),(maxl+1)*(maxl+1)
print "num baselines = {0}\nnum lms = {1}".format(num0,num1)
lms = n.zeros([num1,2])
ii=0
for ll in range(maxl+1):
for mm in range(-ll,ll+1):
lms[ii] = n.array([ll,mm])
ii+=1
matrix = n.zeros([num0,num1,len(fqs)],dtype=n.complex)
assignment_matrix = n.arange(num0*num1).reshape((num0,num1))
def get_altitude_azimuth(utc_time, lon, lat):
"""Returns sun altitude and azimuth from utc_time, lon, and lat.
"""
lon = np.deg2rad(lon)
lat = np.deg2rad(lat)
ra_, dec = sun_ra_dec(utc_time)
h__ = local_hour_angle(utc_time, lon, ra_)
return (np.arcsin(np.sin(lat) * np.sin(dec) + np.cos(lat) * np.cos(dec) * np.cos(h__)),np.arctan2(-np.sin(h__), (np.cos(lat) * np.tan(dec) - np.sin(lat) * np.cos(h__))))
def log(self):
norm = abs(self.normalized())
imgs = norm.imaginaries
lens = np.sqrt(np.sum(imgs**2, axis=-1))
lens = np.arctan2(lens, norm.reals) / (lens + 1e-10)
return imgs * lens[...,np.newaxis]
def cart2sph(x, y, z):
hxy = np.hypot(x, y)
r = np.hypot(hxy, z)
elevation = np.arctan2(z, hxy)*180/math.pi
azimuth = np.arctan2(y, x)*180/math.pi
return r[2], azimuth[2], elevation[2]
def ephemeris(time, latitude, longitude, pressure=101325, temperature=12):
"""
Python-native solar position calculator.
The accuracy of this code is not guaranteed.
Consider using the built-in spa_c code or the PyEphem library.
Parameters
----------
time : pandas.DatetimeIndex
latitude : float
longitude : float
pressure : float or Series, default 101325
Ambient pressure (Pascals)
temperature : float or Series, default 12
Ambient temperature (C)
Returns
-------
DataFrame with the following columns:
* apparent_elevation : apparent sun elevation accounting for
atmospheric refraction.
* elevation : actual elevation (not accounting for refraction)
of the sun in decimal degrees, 0 = on horizon.
The complement of the zenith angle.
* azimuth : Azimuth of the sun in decimal degrees East of North.
This is the complement of the apparent zenith angle.
* apparent_zenith : apparent sun zenith accounting for atmospheric
refraction.
* zenith : Solar zenith angle
* solar_time : Solar time in decimal hours (solar noon is 12.00).
References
-----------
.. [1] Grover Hughes' class and related class materials on Engineering
Astronomy at Sandia National Laboratories, 1985.
See also
--------
pyephem, spa_c, spa_python
"""
# Added by Rob Andrews (@Calama-Consulting), Calama Consulting, 2014
# Edited by Will Holmgren (@wholmgren), University of Arizona, 2014
# Most comments in this function are from PVLIB_MATLAB or from
# pvlib-python's attempt to understand and fix problems with the
# algorithm. The comments are *not* based on the reference material.
# This helps a little bit:
# http://www.cv.nrao.edu/~rfisher/Ephemerides/times.html
# the inversion of longitude is due to the fact that this code was
# originally written for the convention that positive longitude were for
# locations west of the prime meridian. However, the correct convention (as
# of 2009) is to use negative longitudes for locations west of the prime
# meridian. Therefore, the user should input longitude values under the
# correct convention (e.g. Albuquerque is at -106 longitude), but it needs
# to be inverted for use in the code.
Latitude = latitude
Longitude = -1 * longitude
Abber = 20 / 3600.
LatR = np.radians(Latitude)
# the SPA algorithm needs time to be expressed in terms of
# decimal UTC hours of the day of the year.
# if localized, convert to UTC. otherwise, assume UTC.
try:
time_utc = time.tz_convert('UTC')
except TypeError:
time_utc = time
# strip out the day of the year and calculate the decimal hour
DayOfYear = time_utc.dayofyear
DecHours = (time_utc.hour + time_utc.minute/60. + time_utc.second/3600. +
time_utc.microsecond/3600.e6)
# np.array needed for pandas > 0.20
UnivDate = np.array(DayOfYear)
UnivHr = np.array(DecHours)
Yr = np.array(time_utc.year) - 1900
YrBegin = 365 * Yr + np.floor((Yr - 1) / 4.) - 0.5
Ezero = YrBegin + UnivDate
T = Ezero / 36525.
# Calculate Greenwich Mean Sidereal Time (GMST)
GMST0 = 6 / 24. + 38 / 1440. + (
45.836 + 8640184.542 * T + 0.0929 * T ** 2) / 86400.
GMST0 = 360 * (GMST0 - np.floor(GMST0))
GMSTi = np.mod(GMST0 + 360 * (1.0027379093 * UnivHr / 24.), 360)
# Local apparent sidereal time
LocAST = np.mod((360 + GMSTi - Longitude), 360)
EpochDate = Ezero + UnivHr / 24.
T1 = EpochDate / 36525.
ObliquityR = np.radians(
23.452294 - 0.0130125 * T1 - 1.64e-06 * T1 ** 2 + 5.03e-07 * T1 ** 3)
MlPerigee = 281.22083 + 4.70684e-05 * EpochDate + 0.000453 * T1 ** 2 + (
3e-06 * T1 ** 3)
MeanAnom = np.mod((358.47583 + 0.985600267 * EpochDate - 0.00015 *
T1 ** 2 - 3e-06 * T1 ** 3), 360)
Eccen = 0.01675104 - 4.18e-05 * T1 - 1.26e-07 * T1 ** 2
EccenAnom = MeanAnom
E = 0
while np.max(abs(EccenAnom - E)) > 0.0001:
E = EccenAnom
EccenAnom = MeanAnom + np.degrees(Eccen)*np.sin(np.radians(E))
TrueAnom = (
2 * np.mod(np.degrees(np.arctan2(((1 + Eccen) / (1 - Eccen)) ** 0.5 *
np.tan(np.radians(EccenAnom) / 2.), 1)), 360))
EcLon = np.mod(MlPerigee + TrueAnom, 360) - Abber
EcLonR = np.radians(EcLon)
DecR = np.arcsin(np.sin(ObliquityR)*np.sin(EcLonR))
RtAscen = np.degrees(np.arctan2(np.cos(ObliquityR)*np.sin(EcLonR),
np.cos(EcLonR)))
HrAngle = LocAST - RtAscen
HrAngleR = np.radians(HrAngle)
HrAngle = HrAngle - (360 * ((abs(HrAngle) > 180)))
SunAz = np.degrees(np.arctan2(-np.sin(HrAngleR),
np.cos(LatR)*np.tan(DecR) -
np.sin(LatR)*np.cos(HrAngleR)))
SunAz[SunAz < 0] += 360
SunEl = np.degrees(np.arcsin(
np.cos(LatR) * np.cos(DecR) * np.cos(HrAngleR) +
np.sin(LatR) * np.sin(DecR)))
SolarTime = (180 + HrAngle) / 15.
# Calculate refraction correction
Elevation = SunEl
TanEl = pd.Series(np.tan(np.radians(Elevation)), index=time_utc)
Refract = pd.Series(0, index=time_utc)
Refract[(Elevation > 5) & (Elevation <= 85)] = (
58.1/TanEl - 0.07/(TanEl**3) + 8.6e-05/(TanEl**5))
Refract[(Elevation > -0.575) & (Elevation <= 5)] = (
Elevation *
(-518.2 + Elevation*(103.4 + Elevation*(-12.79 + Elevation*0.711))) +
1735)
Refract[(Elevation > -1) & (Elevation <= -0.575)] = -20.774 / TanEl
Refract *= (283/(273. + temperature)) * (pressure/101325.) / 3600.
ApparentSunEl = SunEl + Refract
# make output DataFrame
DFOut = pd.DataFrame(index=time_utc)
DFOut['apparent_elevation'] = ApparentSunEl
DFOut['elevation'] = SunEl
DFOut['azimuth'] = SunAz
DFOut['apparent_zenith'] = 90 - ApparentSunEl
DFOut['zenith'] = 90 - SunEl
DFOut['solar_time'] = SolarTime
DFOut.index = time
return DFOut
def angle(self):
angle = np.arctan2(self.end.y - self.start.y,
self.end.x - self.start.x)
return angle if angle >= 0 else angle + 2 * np.pi
# this is for calculating wind speed
radius_earth = 6373000.0 # radius earth
wind_speed = []
for counter, i in enumerate(df.latitude):
try:
lat1 = np.radians(df.latitude[counter])
lat2 = np.radians(df.latitude[counter + 1])
lon1 = np.radians(df.longitude[counter])
lon2 = np.radians(df.longitude[counter + 1])
dlat = lat2 - lat1
dlon = lon2 - lon1
a = np.sin(dlat / 2)**2 + np.cos(lat1) * np.cos(lat2) * np.sin(
dlon / 2)**2
c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1 - a))
distance = radius_earth * c
time_difference = df.timedelay[counter]
except:
True
if distance > 10:
distance = 0
wind_speed.append(distance / time_difference)
# for plotting temperature, pressure and wind speed
fig, ax = plt.subplots(4, figsize=(16, 9))
ax[0].plot(timelist, df.temperature, color='red')
ax[0].set_xlabel('time (s)')
ax[0].set_ylabel('temperature (℃)')
def angle(x):
x=x.flatten()
return arctan2(x[1],x[0])
def main(argv):
# The Initial condition for the star orbiting a black hole
# The black holes' collective mass is given by the 'mass',
# Newton's constant by 'G', the star's initial position by
# (x0, y0, z0), the star's initial velocity by (vx, vy, vz)
# and q is the mass ratio between the black holes.
# Initializing default parameters
# Default mass, G, and q values
kwargs = {'mass': 1.0, 'G': 1.0, 'q': 1.0}
x0 = 4.0
y0 = 0.0
z0 = 0.0
vx0 = 0.0
vy0 = 0.5
vz0 = 0.0
# dt is the timestep. The error will be proportional to dt**4
dt = 1.0e-2
# start time
t = 0.0
# max time
tmax = 100
# Black Hole 1's initial position
BH1x = 1.0
BH1y = 0.0
BH1z = 0.0
# Black Hole 2's initial position
BH2x = -1.0
BH2y = 0.0
BH2z = 0.0
# Processing command line arguments
# This will possibly change some of the default values
i = 0
record_comment = False
use_RK_45 = False
if len(argv) == 0:
print('# Running with default settings')
# for better options menu https://docs.python.org/3/library/argparse.html#sub-commands
while i < len(argv): # while there are unprocessed arguments
if argv[i] == '--star':
i += 1
while i < len(argv): # while there are unprocessed star
#print('Star arguments')
if argv[i] == '--x0' or argv[i] == '-x':
i += 1
x0 = float(argv[i])
#print('X position changed')
elif argv[i] == '--y0' or argv[i] == '-y':
i += 1
y0 = float(argv[i])
#print('Y position changed')
elif argv[i] == '--z0' or argv[i] == '-z':
i += 1
z0 = float(argv[i])
#print('Z position changed')
elif argv[i] == '--vx0' or argv[i] == '-vx':
i += 1
vx0 = float(argv[i])
#print('Velocity x vector changed')
elif argv[i] == '--vy0' or argv[i] == '-vy':
i += 1
vy0 = float(argv[i])
#print('Velocity y vector changed')
elif argv[i] == '--vz0' or argv[i] == '-vz':
i += 1
vz0 = float(argv[i])
#print('Velocity z vector changed')
else:
# If the *current* argument is not for a star, counter the *next* increment
i -= 1
break
# move to the next argument
i += 1
elif argv[i] == '--tstep' or argv[i] == '-ts':
i += 1
dt = float(argv[i])
#print('Time step changed')
elif argv[i] == '--tmax' or argv[i] == '-tm':
i += 1
tmax = float(argv[i])
#print('Maximum run time changed')
elif argv[i] == '--mratio' or argv[i] == '-q':
i += 1
kwargs['q'] = float(argv[i])
#print('Mass ratio changed')
elif argv[i] == '--sep' or argv[i] == '-s':
i += 1
BH1x = float(argv[i]) / 2
BH2x = -1.0 * BH1x
# Print('Seperation distance of black holes changes)
elif argv[i] == '--help' or argv[i] == '-h':
print_help()
exit(0)
elif argv[i] == '--default' or argv[i] == '-d':
print_default()
exit(0)
elif argv[i] == '-r' or argv[i] == '--record':
record_comment = True
elif argv[i] == '-45' or argv[i] == '--rk45':
use_RK_45 = True
else:
print('\n "', argv[i], '" is not an option!!')
print_help()
exit(1)
i += 1
# Calculate initial star position
initial_position = np.array((x0, y0, z0), dtype=np.float64)
# Calculate initial star velocity
initial_velocity = np.array((vx0, vy0, vz0), dtype=np.float64)
# Calculate initial star angle(phi)
# TODO: need to check if this is the right initialization
initial_phi = np.array(np.arctan2(y0, x0), dtype=np.float64)
# Concatanate star parameters
Y = np.concatenate((initial_position, initial_velocity))
Y = np.append(Y, initial_phi)
# Puts blach hole 1 position parameters into an array
initial_bh1_pos = np.array((BH1x, BH1y, BH1z), dtype=np.float64)
BH1 = initial_bh1_pos
# Puts black hole 2 position parameters into an array
initial_bh2_pos = np.array((BH2x, BH2y, BH2z), dtype=np.float64)
BH2 = initial_bh2_pos
kwargs['bh1'] = BH1
kwargs['bh2'] = BH2
omega, BH_dist = calc_omega(kwargs['mass'], kwargs['G'], BH1, BH2)
kwargs['omega'] = omega
kwargs['BH_dist'] = BH_dist
if record_comment:
print('# Star Position: x:', x0, ' y:', y0, ' z:', z0)
print('# Star Velocity Components: vx0: ', vx0, ' vy0:', vy0, ' vz0:',
vz0)
print('# Time Step:', dt, '\tRun Time max:', tmax)
print('# Black hole separation:', abs(BH1x) * 2)
print('')
#phi_list = np.zeros(shape=2, dtype=np.float64)
#phi_list[1] = np.linalg.norm(np.cross(Y[0:3], Y[3:])) /\
#(np.linalg.norm(Y[0:3]) ** 2)
star_x_min_max = [Y[0], Y[0]]
star_y_min_max = [Y[1], Y[1]]
star_z_min_max = [Y[2], Y[2]]
while t < tmax:
pos_r = np.linalg.norm(Y[0:3])
# phi_list[0] = phi_list[1]
# #phi_list[1] = np.linalg.norm(np.cross(Y[0:3], Y[3:])) / (pos_r ** 2)
# phi_list[1] = np.arctan2(Y[1], Y[0])
# phi_list = np.unwrap(phi_list)
BH1 = kwargs['bh1']
BH2 = kwargs['bh2']
print(t, Y[0], Y[1], Y[2], BH1[0], BH1[1], BH1[2], BH2[0], BH2[1],
BH2[2], pos_r, Y[6])
# The Runge-Kutta routine returns the new value of Y, t, and a
# possibly updated value of dt
if use_RK_45:
# fine-tunes the dt
t, Y, dt = RK45_Step(t, Y, dt, Keppler_Binary_RHS, **kwargs)
else:
# does not change the dt
t, Y, dt = RK4_Step(t, Y, dt, Keppler_Binary_RHS, **kwargs)
update_min_max(star_x_min_max, Y, 0)
update_min_max(star_y_min_max, Y, 1)
update_min_max(star_z_min_max, Y, 2)
print("# Xmin\tXmax\tYmin\tYmax\tZmin\tZmax")
print("#", star_x_min_max[0], " ", star_x_min_max[1], " ",
star_y_min_max[0], " ", star_y_min_max[1], " ", star_z_min_max[0],
" ", star_z_min_max[1])
def plot_histogram(values,
fig=None,
ax=None,
weighted=False,
divided_by_bin_size=False,
hist_scale=1.0,
bins='auto',
log_x=False,
log_y=False,
vmin=None,
vmax=None,
plot_type='steps',
horizontal=False,
fit_limits=None,
extra_artists=[],
color='k',
lw=1.0,
ls='-',
alpha=1.0,
x_lims=None,
y_lims=None,
label=None,
legend_loc=None,
xlabel=None,
ylabel=None,
xlabel_color='k',
ylabel_color='k',
minorticks_on=False,
output_path=None,
fig_kwargs={},
render_now=True):
if fig is None or ax is None:
fig, ax = create_2d_subplots(**fig_kwargs)
hist, bin_edges, bin_centers = compute_histogram(
values,
weights=None,
bins=bins,
vmin=vmin,
vmax=vmax,
decide_bins_in_log_space=(log_y if horizontal else log_x))
hist = np.asfarray(hist)
bin_sizes = bin_edges[1:] - bin_edges[:-1]
if divided_by_bin_size:
hist /= bin_sizes
if weighted:
hist *= bin_centers*bin_sizes
if hist_scale != 1.0:
hist *= hist_scale
if plot_type == 'steps':
if horizontal:
ax.step(hist, bin_edges[:-1], c=color, ls=ls, lw=lw, label=label)
else:
ax.step(bin_edges[:-1], hist, c=color, ls=ls, lw=lw, label=label)
elif plot_type == 'bar':
if horizontal:
ax.barh(bin_edges[:-1],
hist,
align='edge',
height=bin_sizes,
log=log_x,
color=color,
alpha=alpha,
linewidth=lw,
label=label)
else:
ax.bar(bin_edges[:-1],
hist,
align='edge',
width=bin_sizes,
log=log_y,
color=color,
alpha=alpha,
linewidth=lw,
label=label)
elif plot_type == 'fillstep':
if horizontal:
ax.fill_betweenx(bin_edges[:-1],
hist,
step='pre',
color=color,
alpha=alpha)
else:
ax.fill_between(bin_edges[:-1],
hist,
step='pre',
color=color,
alpha=alpha)
if horizontal:
ax.step(hist, bin_edges[:-1], c=color, ls=ls, lw=lw, label=label)
else:
ax.step(bin_edges[:-1], hist, c=color, ls=ls, lw=lw, label=label)
elif plot_type == 'fill':
if horizontal:
ax.fill_betweenx(bin_centers,
hist,
color=color,
alpha=alpha,
label=label)
else:
ax.fill_between(bin_centers,
hist,
color=color,
alpha=alpha,
label=label)
if horizontal:
ax.plot(hist,
bin_centers,
c=color,
ls=ls,
lw=lw,
alpha=alpha,
label=label)
else:
ax.plot(bin_centers,
hist,
c=color,
ls=ls,
lw=lw,
alpha=alpha,
label=label)
elif plot_type == 'points':
if horizontal:
ax.plot(hist,
bin_centers,
c=color,
ls=ls,
lw=lw,
alpha=alpha,
marker='o')
else:
ax.plot(bin_centers,
hist,
c=color,
ls=ls,
lw=lw,
alpha=alpha,
marker='o')
else:
raise ValueError(f'Invalid plot type {plot_type}')
if extra_artists is not None:
for artist in extra_artists:
ax.add_artist(artist)
if log_x:
ax.set_xscale('log')
if log_y:
ax.set_yscale('log')
if fit_limits is not None:
start_idx = np.argmin(np.abs(bin_centers - fit_limits[0]))
end_idx = np.argmin(np.abs(bin_centers - fit_limits[1]))
coefs = np.polyfit(
np.log10(bin_centers[start_idx:end_idx])
if log_x else bin_centers[start_idx:end_idx],
np.log10(hist[start_idx:end_idx])
if log_y else hist[start_idx:end_idx], 1)
print(f'Slope of fitted line: {coefs[0]}')
fit_values = np.poly1d(coefs)(
np.log10(bin_centers) if log_x else bin_centers)
if log_y:
fit_values = 10**fit_values
ax.plot(bin_centers, fit_values, 'k--', alpha=0.3, lw=1.0)
shift = 3
xylabel = ((bin_centers[shift] + bin_centers[shift + 1])/2,
(fit_values[shift] + fit_values[shift + 1])/2)
p1 = ax.transData.transform_point(
(bin_centers[shift], fit_values[shift]))
p2 = ax.transData.transform_point(
(bin_centers[shift + 1], fit_values[shift + 1]))
dy = (p2[1] - p1[1])
dx = (p2[0] - p1[0])
rotn = np.degrees(np.arctan2(dy, dx))
ax.annotate(f'{coefs[0]:.1f}',
xy=xylabel,
ha='center',
va='center',
rotation=rotn,
backgroundcolor='w',
alpha=0.5,
fontsize='x-small')
if minorticks_on:
ax.minorticks_on()
set_2d_plot_extent(ax, x_lims, y_lims)
set_2d_axis_labels(ax,
xlabel,
ylabel,
xcolor=xlabel_color,
ycolor=ylabel_color)
ax.tick_params(axis='x', labelcolor=xlabel_color)
ax.tick_params(axis='y', labelcolor=ylabel_color)
if legend_loc:
ax.legend(loc=legend_loc)
if render_now:
render(fig, output_path=output_path)
return fig, ax
def get_angle_at_time(planet, t):
pos_vec = planet_positions[:, planet, t]
print pos_vec
return np.arctan2(pos_vec[1], pos_vec[0])
def cartesion_to_spherical(x, y, z):
r3 = np.sqrt(x**2 + y**2 + z**2)
phi = np.arctan2(y, x)
theta = np.arccos(z/r3)
return r3, theta, phi
def apparent_rightascension(t='now'):
"""Returns the apparent right ascension of the Sun."""
y = np.cos(apparent_obliquity_of_ecliptic(t)) * np.sin(apparent_longitude(t))
x = np.cos(apparent_longitude(t))
app_ra = np.arctan2(y, x)
return Longitude(app_ra.to(u.hourangle))
def msg_callback(car_state, target_info):
global mu_0
global mu_t_1
global sigma_t_1
global N
global it
global max_tg1
global min_tg1
global max_tg2
global min_tg2
# Parameter initialization
vehicle_pose_ = [car_state.PosX, car_state.PosY]
vehicle_pose__ = [-1*car_state.PosX, car_state.PosY]
vehicle_poses_.append(vehicle_pose__)
heading_ = car_state.heading
target1_ = [target_info.targetPosX1, target_info.targetPosY1]
target2_ = [target_info.targetPosX2, target_info.targetPosY2]
velocity_ = car_state.velocity
dt = car_state.yaw_rate_dt
yaw_rate = car_state.yaw_rate
vehicle_pose, heading, target1, target2 = changing_avm_coordinate(vehicle_pose_, heading_, target1_, target2_)
avm_vehicle_pose, avm_heading, t1, t2 = changing_avm_coordinate([target_info.PosX, target_info.PosY], target_info.heading, [0,0], [0,0])
avm_vehicle_poses.append(avm_vehicle_pose)
if len(targets_origin) is 0 :
result = [(target1[0] + target2[0]) / 2, (target1[1] + target2[1]) / 2]
targets_origin.append(result)
tg1 = [target1[0] - result[0], target1[1] - result[1]]
tg2 = [target2[0] - result[0], target2[1] - result[1]]
target_origin_poses.append(tg1)
target_origin_poses.append(tg2)
vehicle_poses.append(vehicle_pose)
# EKF SLAM
print('\n\n********************\n[%d] iteration\n********************' % it)
if it == 0:
# initialization
mu_t_1, sigma_t_1 = EKF_SLAM.initialization(N, avm_vehicle_pose, avm_heading, FVC(target1), FVC(target2))
mu_0 = mu_t_1
# initialization/
it = it + 1
# update
dt = car_state.yaw_rate_dt # 0.02 # car_state.sampling_time # Not yet
v_t_1 = car_state.velocity / 3.6
w_t_1 = car_state.yaw_rate + 1e-10 # 1 + 0.05 * np.random.randn() + 0 # car_state.yaw_rate # Not yet
w_t_1 = np.deg2rad(w_t_1)
map_id1 = 1 # fix
map_id2 = 2 # fix
# eg coord.
x_t_1 = vehicle_pose[0]
y_t_1 = vehicle_pose[1]
theta_t_1 = heading
map_x1 = FVC(target1)[0]
map_y1 = FVC(target1)[1]
map_x2 = FVC(target2)[0]
map_y2 = FVC(target2)[1]
# eg coord./
u_t = np.array([v_t_1, w_t_1])
z_t = np.array([[np.sqrt(pow(map_x1-x_t_1, 2)+pow(map_y1-y_t_1, 2)),
np.sqrt(pow(map_x2-x_t_1, 2)+pow(map_y2-y_t_1, 2))],
[np.arctan2(map_y1-y_t_1, map_x1-x_t_1) - theta_t_1,
np.arctan2(map_y2-y_t_1, map_x2-x_t_1) - theta_t_1],
[map_id1, map_id2]])
# check/
c_t = [map_id1, map_id2] # fix
y_t = np.array([[x_t_1], [y_t_1], [theta_t_1], [map_x1], [map_y1], [map_x2], [map_y2]])
# update/
mu_t, sigma_t = EKF_SLAM.EKF_SLAM(mu_t_1, sigma_t_1, u_t, z_t, c_t, dt, N)
# test
print('\nreal state >>')
print(y_t)
print('\nmu_t >>')
print(mu_t)
print('\nsigma_t >>')
print(sigma_t)
# test/
# update
mu_t_1 = mu_t
sigma_t_1 = sigma_t
# update/
# EKF SLAM/
ekf_pose = mu_t[0:3]
ekf_target1 = mu_t[3:5]
ekf_target2 = mu_t[5:7]
slam_vehicle_poses.append(ekf_pose)
# EKF SLAM/
# < For visualization >
# raw
t_state = vs.draw_t(vehicle_pose, heading, FVC(target1), FVC(target2), blue, skyblue)
#vs.draw_path(t_state, vehicle_poses_, yello)
vs.draw_path(t_state, vehicle_poses, green)
vs.draw_point(t_state, target_origin_poses[0], target_origin_poses[1], white, 3, 0)
# raw + EKF-SLAM
vs.draw_path(t_state, slam_vehicle_poses, yello)
vs.draw_point(t_state, ekf_target1, ekf_target2, magenta, 2, -1)
vs.draw_vehicle(t_state, ekf_pose[0:2], ekf_pose[2], red, 2)
vs.draw_vehicle(t_state, avm_vehicle_pose, avm_heading, orange, 2)
vs.draw_path(t_state, avm_vehicle_poses, blue)
# Meaning of color
t_state_meaning = [['< Vehicle >', white], ['< Path >', white], ['< Target points >', white]]
t_state_color_meaning = [['Blue : Invehicle odometry', blue], ['Red : EKF-SLAM odometry', red], ['Orange : AVM odometry', orange],
['Green : Invehicle path', green], ['Yello : EKF-SLAM path', yello], ['Blue : AVM path', blue],
['White : First frame points', white], ['Skyblue : Raw points', skyblue], ['Magenta : EKF-SLAM points', magenta]]
t_state = cv2.flip(t_state, 1)
t_state = vs.color_meaning_print(t_state, t_state_meaning, t_state_color_meaning)
# Only EKF-SLAM result
slam_coord_system = vs.draw_t(ekf_pose[0:2], ekf_pose[2], ekf_target1, ekf_target2, red, magenta)
vs.draw_path(slam_coord_system, slam_vehicle_poses, yello)
vs.draw_point(slam_coord_system, target_origin_poses[0], target_origin_poses[1], white, 3, 0)
# AVM raw
vs.draw_vehicle(slam_coord_system, avm_vehicle_pose, avm_heading, orange, 2)
vs.draw_path(slam_coord_system, avm_vehicle_poses, blue)
# Meaning of color
ekf_state_meaning = [['< Vehicle >', white], ['< Path >', white], ['< Target points >', white]]
ekf_state_color_meaning = [['Red : EKF-SLAM odometry', red], ['Orange : AVM odometry', orange],
['Yello : EKF-SLAM path', yello], ['Blue : AVM path', blue],
['Magenta : EKF-SLAM points', magenta], ['Skyblue : AVM points', skyblue]]
slam_coord_system = cv2.flip(slam_coord_system, 1)
slam_coord_system = vs.color_meaning_print(slam_coord_system, ekf_state_meaning, ekf_state_color_meaning)
slam_pub.publish(bridge.cv2_to_imgmsg(t_state, "bgr8"))
ekf_slam_pub.publish(bridge.cv2_to_imgmsg(slam_coord_system, "bgr8"))
def voronoi_finite_polygons_2d(self, vor):
"""
Reconstruct infinite voronoi regions in a 2D diagram to finite
regions.
based on https://stackoverflow.com/a/20678647.
Parameters
----------
vor : Voronoi
Input diagram
Returns
-------
regions : list of tuples
Indices of vertices in each revised Voronoi regions.
vertices : list of tuples
Coordinates for revised Voronoi vertices. Same as coordinates
of input vertices, with 'points at infinity' appended to the
end.
"""
new_regions = []
orig_vertices = vor.vertices.tolist()
new_vertices = list(orig_vertices)
# list of tuple of indices in orig_vertices, each describing a ridge
# made by a pair of returned vertices
ridge_vertices = vor.ridge_vertices
# list of tuples, each two indices in points. They describe a ridge
# perpendicular to their line
ridge_points = vor.ridge_points
assert len(ridge_vertices) == len(ridge_points)
# list of tuple, each a point passed by the user
points = vor.points
# for each point in points, it's the index in regions that contains it
point_region = vor.point_region
assert len(points) == len(point_region)
# list of lists, each is indices in orig_vertices making the polygon
regions = vor.regions
assert len(points) == len([r for r in regions if r])
center = vor.points.mean(axis=0)
w, h = self.screen_size
radius = 100 * max(w, h)
# Construct a map containing all ridges for a given point
# for every point it lists all counter points and vertices between them
all_ridges = defaultdict(list)
for (p1, p2), (v1, v2) in zip(ridge_points, ridge_vertices):
# points p1, p2 will have the same vertices between them
all_ridges[p1].append((p1, p2, v1, v2))
all_ridges[p2].append((p1, p2, v1, v2))
# compute the polygons for each region
for p1, region in enumerate(point_region):
# The region polygon. First get all vertices for the region within
# the screen
region_vertices = []
for v in regions[region]:
# skip infinite vertices
if v < 0:
continue
# skip vertices outside the screen
# x, y = orig_vertices[v]
# if not (0 <= x <= w - 1 and 0 <= y <= h - 1):
# continue
region_vertices.append(
[(None, None), None, v, orig_vertices[v]])
# all the ridges that make the region
for p1_, p2_, v1, v2 in all_ridges[p1]:
# if the second point of the ridge is infinite,
# one and only one of the vertex indices will be -1
if v2 < 0:
v1, v2 = v2, v1
if v1 >= 0:
assert v2 >= 0
# x1, y1 = orig_vertices[v1]
# x2, y2 = orig_vertices[v2]
#
# # assume that if a vertex is outside the screen,
# # the other vertex must be on the other side of the p1-p2
# # perpendicular
# if not (0 <= x1 <= w - 1 and 0 <= y1 <= h - 1):
# region_vertices.append(
# [[x2, y2], [x1 - x2, y1 - y2], None, [x1, y1]])
# if not (0 <= x2 <= w - 1 and 0 <= y2 <= h - 1):
# region_vertices.append(
# [[x1, y1], [x2 - x1, y2 - y1], None, [x2, y2]])
continue
# assume that if there's an infinite point, the pair will be
# within the screen boundary
assert v2 >= 0
# Compute the missing endpoint of an infinite ridge
t = vor.points[p2_] - vor.points[p1_] # tangent
t /= np.linalg.norm(t)
# normal, facing to the left of the line between p1 - p2
n = np.array([-t[1], t[0]])
# find midpoint between the two points
midpoint = vor.points[[p1_, p2_]].mean(axis=0)
# find whether the normal points in the same direction as the
# line made by the center of the voronoi points with midpoint.
# If it faces the opposite direction, reorient to face from
# the center to midpoint direction (i.e. away from the center)
# It will be the same for p1, and p2 when we encounter each
# this ensures that we get the same far point for each
direction = np.sign(np.dot(midpoint - center, n)) * n
# add a point very far from the last point
far_point = orig_vertices[v2] + direction * radius
region_vertices.append(
[orig_vertices[v2], direction, None, far_point])
# for (x1, y1), direction, v, (x2, y2) in temp_vertices:
# if v is not None:
# assert x2 >= 0 and y2 >= 0
# finish
vs = np.asarray([v[3] for v in region_vertices])
c = vs.mean(axis=0)
angles = np.arctan2(vs[:, 1] - c[1], vs[:, 0] - c[0])
region_vertices = [region_vertices[i] for i in np.argsort(angles)]
region_verts_i = []
for i, item in enumerate(region_vertices):
if item[1] is None:
region_verts_i.append(item[2])
else:
region_verts_i.append(len(new_vertices))
new_vertices.append(item[3])
new_regions.append(region_verts_i)
# region is sorted according to points order
new_vertices = np.asarray(new_vertices)
return new_regions, new_vertices
def update_controls(self):
######################################################
# RETRIEVE SIMULATOR FEEDBACK
######################################################
x = self._current_x
y = self._current_y
yaw = self._current_yaw
v = self._current_speed
self.update_desired_speed()
v_desired = self._desired_speed
t = self._current_timestamp
waypoints = self._waypoints
throttle_output = 0
steer_output = 0
brake_output = 0
######################################################
######################################################
# MODULE 7: DECLARE USAGE VARIABLES HERE
######################################################
######################################################
"""
Use 'self.vars.create_var(<variable name>, <default value>)'
to create a persistent variable (not destroyed at each iteration).
This means that the value can be stored for use in the next
iteration of the control loop.
Example: Creation of 'v_previous', default value to be 0
self.vars.create_var('v_previous', 0.0)
Example: Setting 'v_previous' to be 1.0
self.vars.v_previous = 1.0
Example: Accessing the value from 'v_previous' to be used
throttle_output = 0.5 * self.vars.v_previous
"""
self.vars.create_var('v_previous', 0.0)
# Skip the first frame to store previous values properly
if self._start_control_loop:
"""
Controller iteration code block.
Controller Feedback Variables:
x : Current X position (meters)
y : Current Y position (meters)
yaw : Current yaw pose (radians)
v : Current forward speed (meters per second)
t : Current time (seconds)
v_desired : Current desired speed (meters per second)
(Computed as the speed to track at the
closest waypoint to the vehicle.)
waypoints : Current waypoints to track
(Includes speed to track at each x,y
location.)
Format: [[x0, y0, v0],
[x1, y1, v1],
...
[xn, yn, vn]]
Example:
waypoints[2][1]:
Returns the 3rd waypoint's y position
waypoints[5]:
Returns [x5, y5, v5] (6th waypoint)
Controller Output Variables:
throttle_output : Throttle output (0 to 1)
steer_output : Steer output (-1.22 rad to 1.22 rad)
brake_output : Brake output (0 to 1)
"""
######################################################
######################################################
# MODULE 7: IMPLEMENTATION OF LONGITUDINAL CONTROLLER
######################################################
######################################################
"""
Implement a longitudinal controller here.
"""
# PID Parameters
Ts = 0.033 # Sample time
kp = 0.5 # Proportional Gain (10)
ki = 0.42 # Integral Gain (5)
kd = 0.12 # Derivative Gain (5)
# Constants calculation
q0 = kp + ((Ts * ki) / 2) + (kd / Ts)
q1 = ((Ts * ki) / 2) - kp - ((2 * kd) / Ts)
q2 = kd / Ts
# Errors calculation
self.err2 = self.err1
self.err1 = self.err
self.err = v_desired - v
# Output calculation
self.u1 = self.u
self.u = self.u1 + (q0 * self.err) + (q1 * self.err1) + (q2 *
self.err2)
# Change these outputs with the longitudinal controller. Note that
# brake_output is optional and is not required to pass the
# assignment, as the car will naturally slow down over time.
if (self.u > 0):
throttle_output = self.u
brake_output = 0
else:
throttle_output = 0
brake_output = -self.u
######################################################
######################################################
# MODULE 7: IMPLEMENTATION OF LATERAL CONTROLLER
######################################################
######################################################
"""
Implement a lateral controller here.
"""
# Angle conversion to work within: (0, -pi) or (0, -180)
yaw = -np.pi - yaw
# Front axle coordinates calculation
xf = x + (1.5 * np.cos(yaw))
yf = y - (1.5 * np.sin(yaw))
# Distance between the front axle and the closest waypoint
ex0 = self.rx - xf
ey0 = self.ry - yf
# Distance between the front axle and the current waypoint
ex1 = waypoints[0][0] - xf
ey1 = waypoints[0][1] - yf
# Error to the closes waypoint
de0 = np.sqrt((ex0**2) + (ey0**2))
# Error to the curret waypoint
de1 = np.sqrt((ex1**2) + (ey1**2))
# Conditions to find the cross track error
if de0 < de1:
# Keep the closest distance and its keypoints
self.cterr = de0
else:
# Keep the current distance and its keypoints
self.cterr = de1
self.rx = waypoints[0][0]
self.ry = waypoints[0][1]
# Distance calculation between two consecutive waypoints
pathangx = waypoints[1][0] - waypoints[0][
0] # (Siguiente - Actual)
pathangy = waypoints[0][1] - waypoints[1][
1] # (Siguiente - Actual)
# Angle calculation between the two keypoints: (0, -180)
pathang = np.arctan2(pathangy, pathangx) - np.pi
headerror = (pathang - yaw) * -1
pathd = pathang * (180 / np.pi)
# Conditions to determine the correct sign of the cross track error
if pathd > -170:
if (ex0 < 0) or (ex1 < 0):
self.cterr = -self.cterr
else:
self.cterr = self.cterr
elif pathd < -190:
if (ex0 < 0) or (ex1 < 0):
self.cterr = self.cterr
else:
self.cterr = -self.cterr
elif (pathd >= -190) and (pathd <= -170):
if (ey0 < 0) or (ey1 < 0):
self.cterr = -self.cterr
else:
self.cterr = self.cterr
# Cross track error coefficient
ksteer = 2 * self.cterr
corsteer = np.arctan2(ksteer, v)
if self.cterr < 0:
errorcor = -np.absolute(corsteer)
else:
errorcor = np.absolute(corsteer)
# Steering angle calculation
steer = headerror + errorcor
# Maximum and minimum steering angle
if steer > 1.22:
steer = 1.22
elif steer < -1.22:
steer = -1.22
steer_output = steer
######################################################
# SET CONTROLS OUTPUT
######################################################
self.set_throttle(throttle_output) # in percent (0 to 1)
self.set_steer(steer_output) # in rad (-1.22 to 1.22)
self.set_brake(brake_output) # in percent (0 to 1)
######################################################
######################################################
# MODULE 7: STORE OLD VALUES HERE (ADD MORE IF NECESSARY)
######################################################
######################################################
"""
Store old values
"""
self.vars.v_previous = v # Store forward speed to be used in next step
bw = img.mean(axis=2)
Hx = np.array([
[-1, 0, 1],
[-2, 0, 2],
[-1, 0, 1]],
dtype=np.float32)
Hy = Hx.T
Gx = convolve2d(bw, Hx)
plt.imshow(Gx, cmap='gray')
plt.show()
Gy = convolve2d(bw, Hy)
plt.imshow(Gy, cmap='gray')
plt.show()
G = np.sqrt(Gx*Gx + Gy*Gy)
theta = np.arctan2(Gx, Gy)
plt.imshow(G, cmap='gray')
plt.show()
plt.imshow(theta, cmap='gray')
plt.show()
# https://en.wikipedia.org/wiki/Edge_detection
# https://en.wikipedia.org/wiki/Sobel_operator
def getAngleABC(A, B, C):
ba = np.arctan2(A[1] - B[1], A[0] - B[0])
bc = np.arctan2(C[1] - B[1], C[0] - B[0])
return ba - bc
def cart2pol(x, y):
rho = np.sqrt(x**2 + y**2)
phi = np.arctan2(y, x)
return(rho, phi)
def get_profile(file,pxsize,PA_disk,inc,d,size,padir,widir,dr,**kwargs):
############################################################
#
# Extract a radial cut of the brightness along diffent
# position angles.
#
# file: the fits file of the observation
# pxsize: pixel scale (arcsec/px)
# PA_aperture: position angle of the aperture measured east-north (deg)
# inc: disk's inclination (deg)
# d: distance to the source (pc)
# size: semi-major axis of the disk (AU)
# padir: position angle of the desired direction (deg)
# widir: width of the cone along the desired direction (deg)
# dr: width of the annulus (AU)
#
# The ouput is a matrix whose the rows and columns represent
# the position angle and the radial distance of the flux
# measurement.
#
############################################################
############################################################
# Load ALMA data
if kwargs['type']=='obs':
hdulist=fits.open(file)
data_obs=hdulist[0].data[0][0]
"""
xc=hdulist[0].header['CRPIX1']
yc=hdulist[0].header['CRPIX2']
"""
xc=data_obs.shape[1]*0.5
yc=data_obs.shape[0]*0.5
elif kwargs['type']=='mod':
hdulist=fits.open(file)
data_obs=hdulist[0].data
xc=data_obs.shape[0]*0.5
yc=data_obs.shape[1]*0.5
############################################################
# Derived properties
angle_annulus=((PA_disk-90.0)*units.deg).to(units.rad).value
if padir<270.0:
padir=padir+90.0
else:
padir=padir-270.0
e=np.sin((inc*units.deg).to(units.rad).value)
d_au=(d*units.pc).to(units.au).value
xc_array=[]
yc_array=[]
############################################################
# Creating elliptical aperture
linear_lim=2*(size) # AU
angular_lim=linear_lim/d_au # rad
angular_lim=(angular_lim*units.rad).to(units.arcsec).value # arcsec
pixel_lim=int(round(angular_lim/pxsize))
dr=topx(dr,pxsize,d) # width of each annular aperture
a_in_array=[]
for i in np.arange(yc+dr,yc+0.5*pixel_lim,dr):
a_in_array.append(i-xc)
a_out_array=[i+dr for i in a_in_array]
b_out_array=[i*(1-e**2)**0.5 for i in a_out_array]
a_in_array=np.array(a_in_array)
a_out_array=np.array(a_out_array)
apertures=[EllipticalAnnulus((yc,xc),a_in=ain,a_out=aout,b_out=bout,theta=angle_annulus)
for (ain,aout,bout) in zip(a_in_array,a_out_array,b_out_array)]
print("Number of annular apertures: %d"%len(apertures))
"""
# Do a check?
plt.imshow(data_obs)
apertures[-1].plot(color='red',lw=1)
plt.show()
"""
############################################################
# Determine Nbins
Nbins=round(360.0/widir)
############################################################
# Define class "Bin"
class Bin:
def __init__(self,ID,theta_min,theta_max,plist):
self.ID=ID
self.theta_min=theta_min
self.theta_max=theta_max
self.plist=plist
def showFlux(self):
i=0
for pixel in self.plist:
print(i,aperture_data[pixel[0],pixel[1]])
i+=1
def getArea(self,aperture):
value=aperture.area/Nbins
return value
def getFlux(self):
flux=0.0
for pixel in self.plist:
flux+=aperture_data[pixel[0],pixel[1]]
return flux
def getTheta(self):
value=(self.theta_max-self.theta_min)*0.5+self.theta_min
return value
def getError_beam(self,aperture):
beam_x=0.074 # arcsec
beam_y=0.057 # arcsec
flux_array=[]
for pixel in self.plist:
flux_array.append(aperture_data[pixel[0],pixel[1]])
area=aperture.area/Nbins
flux_array=np.array(flux_array)#/area
sigma=np.std(flux_array)
beam_area=np.pi*(beam_x)*(beam_y)/(4*np.log(2))
Nbeam=((aperture.area*pxsize**2)/Nbins)/beam_area
return sigma/(Nbeam)**0.5
def getError_pixel(self,aperture):
flux_array=[]
for pixel in self.plist:
flux_array.append(aperture_data[pixel[0],pixel[1]])
area=aperture.area/Nbins
flux_array=np.array(flux_array)/area
sigma=np.std(flux_array)
Npixel=len(self.plist)
return sigma/(Npixel)**0.5
M=[]
E_beam=[]
E_pixel=[]
a_in_array=[i*pxsize*d for i in a_in_array]
a_out_array=[i*pxsize*d for i in a_out_array]
a_mid=np.array([(j-i)*0.5+i for (j,i) in zip(a_out_array,a_in_array)])
meanpxn=[]
for ii in range(0,len(apertures)):
############################################################
# Creating bin
sbin=Bin(ii,padir-0.5*widir,padir+0.5*widir,[])
############################################################
# Creating aperture mask
mask=apertures[ii].to_mask(method="center")
"""
# Do a check?
plt.imshow(mask)
plt.colorbar()
plt.show()
"""
############################################################
# Extracting pixels located inside the aperture
aperture_data=mask.multiply(data_obs)
"""
# Do a check?
plt.imshow(aperture_data)
plt.colorbar()
plt.show()
"""
############################################################
# Creating array of pixel's index within the aperture
# relative to the star
pixel_list=[]
ycc=int(aperture_data.shape[0]*0.5)
xcc=int(aperture_data.shape[1]*0.5)
for i in range(0,aperture_data.shape[1]): # Over columns
for j in range(0,aperture_data.shape[0]): # Over rows
if aperture_data[j,i]!=0.0:
pixel_list.append((j-ycc,i-xcc))
############################################################
# Filling in sbin data
for point in pixel_list:
phi=np.arctan2(point[0],point[1])
if phi<0.0:
phi=2*np.pi+phi
phi=phi*180.0/np.pi
if sbin.theta_min<=phi<sbin.theta_max:
pixel=(int(point[0]+ycc),int(point[1]+xcc))
sbin.plist.append(pixel)
############################################################
# Writing result
#value.showFlux()
M.append(sbin.getFlux()/sbin.getArea(apertures[ii]))
E_beam.append(sbin.getError_beam(apertures[ii]))
E_pixel.append(sbin.getError_pixel(apertures[ii]))
meanpxn.append(len(sbin.plist))
#print("I have %.1f pixels"%(len(sbin.plist)))
#print(np.mean(meanpxn))
M=np.array(M)
E_beam=np.array(E_beam)
E_pixel=np.array(E_pixel)
print()
print("Max value (Jy/beam/bin_area): %.1e"%(max(M)))
print("Max error (per beam): %.1e"%(np.nanmax(E_beam)))
print("Max error (per pixel): %.1e"%(np.nanmax(E_pixel)))
"""
############################################################
# Plotting
fig=plt.figure(figsize=(5,12))
ax=plt.axes()
ax.errorbar(a_mid,M,yerr=E_beam,marker=".",fmt="-",color="red",capsize=2,elinewidth=0.5)
# ax.tick_params(labelleft=False,left=False)
# ax.set_ylabel(r"%.1f"%((midtheta[i]*units.rad).to(units.deg).value))
ax.set_xlabel(r"$r$(AU)")
#ax.set_ylim(0,0.0175)
plt.show()
"""
return a_mid,M,E_beam,E_pixel
def reflect(self, r, nslits, wave=None, order=1):
"""
Propagate an input ray for a given wavelength and order.
wave is in angstroms
ruling is in mm^-1
If more than one wave provided, wavelength samples are ordered
along the first axis.
Taken from xidl/DEEP2/spec2d/pro/model/qmodel.pro.
Args:
r (numpy.ndarray):
Rays to propagate.
nslits (:obj:`int`):
Number of slits
wave (`numpy.ndarray`_):
The wavelengths in angstroms for the propagated coordinates.
order (:obj:`int`):
The grating order.
Returns:
Rays reflected off the grating
"""
if wave is None and self.central_wave is None:
msgs.error('Must define a wavelength for the calculation.')
if wave is None:
msgs.info('Using central wavelength for calculation.')
_wave = numpy.array([self.central_wave
]) if wave is None else numpy.atleast_1d(wave)
if _wave.ndim > 1:
raise NotImplementedError(
'Input wavelength must be one number or a vector.')
nwave = _wave.size
_wave_arr = numpy.tile(_wave, (nslits, 1))
_r = numpy.atleast_2d(r)
if _r.ndim > 2:
raise NotImplementedError(
'Rays must be 1D for a single ray, or 2D for multiple.')
# Transform into the grating conjugate surface
_r = OpticalModel.conjugate_surface_transform(_r,
self.transform,
forward=True)
# Get the grating input angles
alpha = -numpy.arctan2(-_r[:, 1], -_r[:, 2])
gamma = numpy.arctan2(
_r[:, 0],
numpy.sqrt(numpy.square(_r[:, 1]) + numpy.square(_r[:, 2])))
# Use the grating equation to get the output angle (minus sign
# in front of sin(alpha) is for reflection)
beta = (numpy.arcsin((order * 1e-7 * self.ruling * _wave_arr.ravel() /
numpy.cos(gamma)) - numpy.sin(alpha))).reshape(
_wave_arr.shape).T
# Revert to ray vectors
wavesign = 1 - 2 * (_wave_arr.T < 0)
_r = numpy.array([
numpy.sin(gamma),
numpy.sin(-beta * wavesign).T.flatten() * numpy.cos(gamma),
numpy.cos(-beta * wavesign).T.flatten() * numpy.cos(gamma)
]).T
#if nwave == 1:
# # Flatten if only one wave provided
# _r = _r[0]
# Return vectors transformed out of the grating conjugate surface
return OpticalModel.conjugate_surface_transform(_r, self.transform)
def compute_iso2(self,
points,
lst_points,
A,
B,
no_go_zones,
safe_zones,
nb_secteurs=200):
A = np.array((A[0], A[1])).reshape(1, -1)
B = np.array((B[0], B[1])).reshape(1, -1)
points = self.sort_dist(points, A, B, coeff_min=1)
# on determine tous les angles entre les pts et A
angles = np.arctan2(points[:, 1] - A[0, 1], points[:, 0] -
A[0, 0]).reshape(-1, 1) % (2 * np.pi)
#angles = np.arctan2(points[:, 1] - B[0, 1], points[:, 0] - B[0, 0]).reshape(-1, 1)%(2*np.pi)
points = np.hstack((points, angles))
points2 = np.zeros((1, 3))
angleAB = np.arctan2(B[0, 1] - A[0, 1], B[0, 0] - A[0, 0])
angles2 = (angles + np.pi - angleAB) % (2 * np.pi)
angle_min, angle_m, angle_max = min(angles2), np.mean(angles), max(
angles2)
# print(angle_min, angle_m, angle_max)
angle_min, angle_max = (angle_min - np.pi + angleAB), (angle_max -
np.pi + angleAB)
# if not angle_min < angle_m < angle_max:
# angle_max = angle_max - 2*np.pi
# print(angle_min, angle_m, angle_max)
secteurs = np.linspace(angle_min, angle_max, nb_secteurs)
secteurs = secteurs % (2 * np.pi)
for i in range(nb_secteurs - 1):
pts = points[secteurs[i] < points[:, 3], :]
pts = pts[pts[:, 3] < secteurs[i + 1]]
# plt.scatter(points[:, 0], points[:, 1])
# plt.scatter(pts[:, 0], pts[:, 1])
# plt.scatter(A[0], A[1])
#pts = self.sort_dist(pts[:, :-1], A, d_min=d_mean*0.5)
if len(pts) != 0:
#if cdist(C, pts[-1, :2].reshape(1, -1)) < r :
ok = True
cur_pt = (pts[-1, 0], pts[-1, 1])
j = int(pts[-1, 2])
lst_pt = (lst_points[j, 0], lst_points[j, 1])
if deep_DEBUG:
plt.plot([cur_pt[0], lst_pt[0]], [cur_pt[1], lst_pt[1]])
if LineString((cur_pt, lst_pt)).crosses(no_go_zones) or (
(not LineString((cur_pt, lst_pt)).within(safe_zones))
and not safe_zones.is_empty):
ok = False
if ok:
pts = pts[-1, :]
if points2.shape[0] == 1:
points2 = pts
else:
points2 = np.vstack((points2, pts))
if deep_DEBUG:
plt.scatter(points[:, 0], points[:, 1])
plt.scatter(points2[:, 0], points2[:, 1])
plt.scatter(A[0, 0], A[0, 1])
plt.scatter(B[0, 0], B[0, 1])
plt.plot(*safe_zones.exterior.xy)
r = 500
# angles = [angles.min(), angles.max()]
for a in secteurs:
plt.plot((A[0, 0], A[0, 0] + r * np.cos(a)),
(A[0, 1], A[0, 1] + r * np.sin(a)))
plt.show()
points = points2
return points
def raytrace(model, nimgs=None, ipeps=None, speps=None, initial_guess=None, verbose=False, viz=False):
"""Find the positions of images by raytracing back to the source.
ipeps - Radius on the image plane to consider two image as one.
speps - Radius on the source plane to determine if a pixel maps near to the source
"""
global fig
obj,ps,src_index = model
# if len(model) == 2:
# [obj,ps], src_index = model
# else:
# obj_index, src_index = model[1:]
# obj,ps = model[0]['obj,data'][obj_index]
ploc = obj.basis.ploc
src = ps['src'][src_index]
zcap = obj.sources[src_index].zcap
srcdiff = None
#zcap=1
if ipeps is None:
#ipeps = 2 * obj.basis.top_level_cell_size
ipeps = 0.01 * obj.basis.mapextent
if speps is None:
speps = obj.basis.top_level_cell_size #/ sqrt(2)
#speps = 0.01 * obj.basis.mapextent
#---------------------------------------------------------------------------
# (1) Make an initial guess where the images are.
#
# srcdiff is a matrix giving the distance between the src and the point on
# the source plane where each pixel maps back to. Taking the n pixels with
# the smallest of those differences gives a good estimate for the image
# positions.
#---------------------------------------------------------------------------
if not initial_guess:
srcdiff = obj.basis.srcdiff(ps, src_index)
initial_guess = []
asort = argsort(srcdiff)
for j in asort:
if srcdiff[j] > speps: break
initial_guess.append(ploc[j])
# if not initial_guess:
# initial_guess = []
# for i in range(20):
# r = obj.basis.mapextent * random()
# t = 2 * np.pi * random()
# sx = r * np.cos(t)
# sy = r * np.sin(t)
# initial_guess.append(complex(sx,sy))
#-------------------------------------------------------------------------------
# for ii in initial_guess:
# if abs(ploc[j] - ii) <= eps: break
# else:
# if len(initial_guess) >= nimgs: break
# raw_input()
# #figure(f.number)
# figure()
# if not any(abs(ploc[j] - ploc[asort[:i]]) <= eps):
# srcdiff[j] = 10
# initial_guess.append(ploc[j])
#-------------------------------------------------------------------------------
#---------------------------------------------------------------------------
# (2) Minimize the lens equation beginning from each of the initial guesses.
# Only those images that truly satisfy the equation are accepted.
#---------------------------------------------------------------------------
initial_guess.append(src)
if verbose: print 'Initial guesses', initial_guess
def lenseq(theta0):
theta = complex(*theta0)
r = src - theta + obj.basis.deflect(theta, ps) / zcap
return r.real, r.imag
def lenseq_prime(theta0):
theta = complex(*theta0)
dist = theta - obj.basis.ploc
dxdx = -1 + dot(ps['kappa'], (poten_dxdx(dist,obj.basis.cell_size))) / zcap
dydy = -1 + dot(ps['kappa'], (poten_dydy(dist,obj.basis.cell_size))) / zcap
dxdy = -1 + dot(ps['kappa'], (poten_dxdy(dist,obj.basis.cell_size))) / zcap
dydx = -1 + dot(ps['kappa'], (poten_dydx(dist,obj.basis.cell_size))) / zcap
return [ [dxdx, dydx], [dxdy, dydy] ]
xs = []
#if obj.shear: s1,s2 = ps['shear']
for img in initial_guess:
#x,_,ier,mesg = fsolve(lenseq, [img.real,img.imag], fprime=lenseq_prime, full_output=True) #, xtol=1e-12)
x,_,ier,mesg = fsolve(lenseq, [img.real,img.imag], full_output=True, xtol=1e-12)
#x = fmin(lenseq, [img.real,img.imag], full_output=False, disp=False, xtol=1e-10, ftol=1e-10)
if not ier:
print mesg
continue
r = complex(*x)
# if an initial guess was poor then the minimum will not be near zero.
# Only accept solutions that are very close to zero.
leq = abs(complex(*lenseq(x)))
if leq < 2e-10:
#print leq
xs.append([img, r, leq])
else:
#print 'Image at %s rejected. %e' % (r, leq)
pass
#---------------------------------------------------------------------------
# (3) Sort by how well we were able to minimize each function.
#---------------------------------------------------------------------------
xs.sort(lambda x,y: -1 if x[2] < y[2] else 1 if x[2] > y[2] else 0)
#---------------------------------------------------------------------------
# (4) Only accept a solution if it is distinct from previous solutions.
#---------------------------------------------------------------------------
imgs0 = []
for img,r0,t0 in xs:
for r,t in imgs0:
if abs(r0-r) < ipeps: break
else:
tau = abs(r0-src)**2 / 2
tau *= zcap
tau -= dot(ps['kappa'], poten(r0 - obj.basis.ploc, obj.basis.cell_size, obj.basis.maprad))
tau -= np.sum( [ ps[e.name] * e.poten(r0).T for e in obj.extra_potentials ] )
#print tau
#print '!!', poten(i - obj.basis.ploc, obj.basis.cell_size)[0]
#print '!!', dot(ps['kappa'], poten(complex(1,0) - obj.basis.ploc, obj.basis.cell_size))
imgs0.append([r0,tau])
if viz:
if fig == None:
fig = pl.figure()
if srcdiff is None:
srcdiff = obj.basis.srcdiff(ps, src_index)
#print obj.sources
reorder = empty_like(srcdiff)
reorder.put(obj.basis.pmap, srcdiff)
sd = zeros((2*obj.basis.pixrad+1)**2)
sd[obj.basis.insideL] = reorder
#sd[:len(srcdiff)] = srcdiff #reorder
sd = sd.reshape((2*obj.basis.pixrad+1,2*obj.basis.pixrad+1))
R = obj.basis.mapextent
kw = {'extent': [-R,R,-R,R],
'interpolation': 'nearest',
'aspect': 'equal',
'origin': 'upper',
#'cmap': cm.terrain,
'fignum': False,
#'vmin': -1,
#'vmax': 1
}
pl.cla()
pl.matshow(sd, **kw)
#pl.colorbar()
# print '??'
# for i in initial_guess: print i
# print '??'
#arrival_plot({'obj,data':[[obj,ps]]}, src_index=src_index, clevels=150)
xs = [r.real for r in initial_guess]
ys = [r.imag for r in initial_guess]
pl.scatter(xs,ys)
# print '**'
# for i in obj.sources[src_index].images: print i
# print '**'
# xs = [r.pos.real for r in obj.sources[src_index].images]
# ys = [r.pos.imag for r in obj.sources[src_index].images]
# pl.scatter(xs,ys, color='r', s=40)
xs = [r.real for r,tau in imgs0]
ys = [r.imag for r,tau in imgs0]
pl.scatter(xs,ys, color='g')
pl.draw()
print '*'*80
print 'PRESS ANY KEY TO CONTINUE'
print '*'*80
raw_input()
#---------------------------------------------------------------------------
# (5) Determine magnification information
#---------------------------------------------------------------------------
from scipy.linalg import det
imgs = []
for img in imgs0:
#print 'tau', tau
r, tau = img
theta = arctan2(r.imag, r.real) * 180/pi
A = zcap*identity(2) - obj.basis.magnification(r, theta, ps)
detA = det(A)
trA = A.trace()
mu = 1. / detA # magnification factor
parity = ['sad', 'sad', 'max', 'min'][(detA > 0)*2 + (trA > 0)]
print mu
imgs.append(img + [ [mu, A], parity ])
#Mavg = average(map(lambda x: (x[3] != 'max') * x[2][3], imgs))
#imgs = filter(lambda x: abs(x[2][3]) > Mavg*0.8, imgs)
#print imgs
#---------------------------------------------------------------------------
# (6) Sort by arrival time
#---------------------------------------------------------------------------
imgs.sort(lambda x,y: -1 if x[1] < y[1] else 1 if x[1] > y[1] else 0)
# if fig == None:
# fig = figure()
# f = pl.gcf()
## figure() #fig.number)
## reorder = empty_like(srcdiff)
## reorder.put(obj.basis.pmap, srcdiff)
## sd = zeros((2*obj.basis.pixrad+1)**2)
## sd[obj.basis.insideL] = reorder
## #sd[:len(srcdiff)] = srcdiff #reorder
## sd = sd.reshape((2*obj.basis.pixrad+1,2*obj.basis.pixrad+1))
## R = obj.basis.mapextent
## kw = {'extent': [-R,R,-R,R],
## 'interpolation': 'nearest',
## 'aspect': 'equal',
## 'origin': 'upper',
## 'cmap': cm.terrain,
## 'fignum': False,
## 'vmin': 0,
## 'vmax': R}
## matshow(sd, **kw) #, fignum=fig.number)
## kw.pop('cmap')
## over(contour, sd, 100, extend='both', colors='w', alpha=1.0, **kw)
# #figure(f.number)
# figure()
#print imgs
return imgs
x0 = psfc_min_ds.x.values y0 = psfc_min_ds.y.values xrel = ds.x.values - x0 + 1e-13 # hack to avoid zero-division for now yrel = ds.y.values - y0 # meshgrid X, Y = np.meshgrid(xrel, yrel) # calc r and theta r = np.sqrt(X**2 + Y**2) # since we are using gridpt-wise min instead of interpolating, # we need to ignore that grid point for the theta calc is_center = (X == 0) & (Y == 0) # assert np.count_nonzero(is_center) == 1 theta = np.zeros(ds.psfc.shape) theta[~is_center] = np.arctan2(Y[~is_center], X[~is_center]) # ^ arctan2 chooses correct quadrants for you # Calculate tangential wind speed (positive cyclonic) and radial windspeed (positive inward) # The below does work but uses more steps! # uh = np.sqrt(ds.u**2 + ds.v**2) # horizontal wind magnitude # theta_wind = np.arctan2(ds.v.values, ds.u.values) # wind vector angle (direction) # urad = -uh*np.cos(theta_wind - theta) # radial wind # vtan = uh*np.sin(theta_wind - theta) # tangential wind # from Kelly, fewer steps way # note that cos(theta) is equivalent to x/r and sin(theta) to y/r, # so we could've gotten away with not calculating theta at all vtan = np.cos(theta) * ds.v - np.sin(theta) * ds.u urad = -(np.cos(theta) * ds.u + np.sin(theta) * ds.v) # note also that if we already had u_h, we only really need to calculate one,
def _computeDirection(self, dx, dy):
return np.arctan2(dy, dx)
def select_input(self, x_rand, x_near):
return np.arctan2(x_rand[1] - x_near[1], x_rand[0] - x_near[0])
def cart2pol(x, y):
theta = np.arctan2(y, x)
rho = np.hypot(x, y)
return (theta, rho)
def convert(slope):
angle = np.rad2deg(np.arctan2(slope, 1))
answer = 90 - angle
return round(answer)