def Eq2Cart(ra, dec, ra0, dec0):
# conversion from RA/Dec to standard coords; Snyder (1987) ch 22
# from matlab version; not fully tested
rads = np.pi / 180.
dec = rads * dec
ra_norm = rads * (ra - ra0)
if dec0 == 90:
# north or south pole so use polar gnomic
x = np.cos(dec) * np.sin(ra_norm)
y = -cotdg(dec / rads) * np.cos(ra_norm)
elif dec0 == -90:
x = -np.cos(dec) * np.sin(ra_norm)
y = cotdg(dec / rads) * np.cos(ra_norm)
else:
# oblique gnomic
dec0 = rads * dec0
cos0 = np.cos(dec0)
sin0 = np.sin(dec0)
cosdec = np.cos(dec)
sindec = np.sin(dec)
cos_ra_norm = np.cos(ra_norm)
den = cos0 * cosdec * cos_ra_norm + sin0 * sindec
x = (cosdec * np.sin(ra_norm)) / den
y = (cos0 * sindec - sin0 * cosdec * cos_ra_norm) / den
return -x, y # reverse RA axis
def laplacian_matrix(X, Tri):
print(" Calculating Laplacian on atrial mesh")
RAD_TO_DEG = 180.0 / np.pi
# approximately normalize edge lengths, to help with units in area calculation
av_edge = X[Tri[:, 0:2]]
av_edge_length = np.linalg.norm(av_edge[:, 1, :] - av_edge[:, 0, :],
axis=1).mean()
X = (X - X.mean(axis=0)) / av_edge_length
mesh = trimesh.Trimesh(vertices=X, faces=Tri, process=False)
areas = mesh.area_faces
angles = mesh.face_angles # angles within each face, ordered same way as vertices are listed in face
# make the mass matrix
vertex_faces = mesh.vertex_faces
MA = np.ma.masked_array(
areas[vertex_faces],
vertex_faces < 0) # vertex_faces is padded with -1s
M = MA.sum(
axis=1
) / 3.0 # NOTE: this is the Barycentric area, which is a standard approx for the Voronoi cell
# fill out Laplacian by loop over faces
L = lil_matrix((mesh.vertices.shape[0], mesh.vertices.shape[0]))
for ff, face in enumerate(mesh.faces):
cot_ang = cotdg(RAD_TO_DEG * angles[ff, 2])
L[face[0], face[1]] += cot_ang
L[face[1], face[0]] += cot_ang
cot_ang = cotdg(RAD_TO_DEG * angles[ff, 0])
L[face[1], face[2]] += cot_ang
L[face[2], face[1]] += cot_ang
cot_ang = cotdg(RAD_TO_DEG * angles[ff, 1])
L[face[2], face[0]] += cot_ang
L[face[0], face[2]] += cot_ang
# set the diagonals as -sum(rows)
L.setdiag(-L.sum(axis=1), k=0)
# convert to csr
L = L.tocsr()
L = L.multiply(
-0.5
) # multiply by the half factor, and by -1 to form the negative laplacian
# do not multiply by inverse mass matrix, instead use this mass matrix in the eigensolver routine
#L = L.multiply((1.0/M)).tocsr() # need tocsr because otherwise it because a coo_matrix
M = diags(M)
return L, M
def convert_polar_to_slope_intercept(n_pixels_from_center, projection_angle,
center_pixel):
""" From Eric Denovellis """
velocity = -cotdg(-projection_angle)
start_position = (
n_pixels_from_center / np.sin(-np.deg2rad(projection_angle)) -
velocity * center_pixel[0] + center_pixel[1])
return start_position, velocity
def perspective(fovy,aspect,zNear,zFar):
f = cotdg(fovy/2)
M = np.zeros((4,4))
M[0,0] = f/aspect
M[1,1] = f
M[2,2] = (zFar+zNear)/(zNear-zFar)
M[2,3] = (2*zFar*zNear)/(zNear-zFar)
M[3,2] = -1
return M
def periodic(x, phase_length=.25):
sawtooth = -2 / np.pi * np.arctan(
cotdg(np.rad2deg((x * np.pi) / phase_length)))
transform = (1 + sawtooth) / 2.
return transform
def find_hit_position(angle, pos, track_map, dl=1.0):
"""
This function returns the point at which
a beam coming from point pos at angle given by angle variable
for the first time crosses a non zero point on the map
Timing at Marcin computer: dl = 5.0 -> about max 0.4 ms;
It is roughly valid: dl/n -> time*n
:param angle: angle (deg) in which the beam is going, e.g the body angle of the car
:param pos: point ((x,y) in pixels) where the beam starts, e.g. position of the car
:param track_map: a SPECIAL map which is non-zero in sand region and zero on track. Use map_lidar for it.
:param dl: determines the precision of the collision point determination
- the algorithm moves along the beam and checks every dl pixels (does not need to be integer)
if it left the track
:return the point on the track boundary which the beam first hits ((x,y) in pixels)
(e.g. which the car would hit if it would go on a straight line)
"""
(h, w) = track_map.shape
found = False
# x = meters2pixels(pos[0])
# y = meters2pixels(pos[1])
x = pos[0]
y = pos[1]
# Depending on direction we take y = ax+b or x = ay+b
if (45.0 < angle < 135.0) or (225.0 < angle < 315.0):
# x = ay+b
dy = dl * sindg(angle)
# dy = dl
a = cotdg(angle)
b = x - a * y
while 0 < x < w and 0 < y < h:
if track_map[int(y), int(x)] > 0:
found = True
break
else:
y = y + dy
x = a * y + b
else:
# dx = dl
dx = dl * cosdg(angle)
a = tandg(angle)
b = y - a * x
while 0 < x < w and 0 < y < h:
if track_map[int(y), int(x)] > 0:
found = True
break
else:
x = x + dx
y = a * x + b
if found:
hit_pos = (x, y)
else:
hit_pos = None
return hit_pos