def calc_heading(start_e, start_n, end_e, end_n):
'''Calculate the equivalent heading in degrees'''
# https://stackoverflow.com/a/5060108
dist_e = end_e - start_e
dist_n = end_n - start_n
angle = degrees(arctan2(dist_n, dist_e))
return ((90. - angle) % 360.)
def update(self):
"""move ship"""
if self.moving_right:
self.centerx += self.settings.ship_speed
if self.moving_left:
self.centerx -= self.settings.ship_speed
if self.moving_up:
self.centery -= self.settings.ship_speed
if self.moving_down:
self.centery += self.settings.ship_speed
#off screen check
if self.centerx >= self.settings.screen_size[0]:
self.centerx -= self.settings.ship_speed
if self.centerx <= 0:
self.centerx += self.settings.ship_speed
if self.centery >= self.settings.screen_size[1]:
self.centery -= self.settings.ship_speed
if self.centery <= 0:
self.centery += self.settings.ship_speed
#rotation
x1 = self.centerx
x2 = float(self.mouse_pos[0])
y1 = self.settings.screen_size[1] - self.centery
y2 = self.settings.screen_size[1] - float(self.mouse_pos[1])
angle = (arctan2((y2 - y1), (x2 - x1)) * 180.0 / 3.1416) - 90
self.image = pygame.transform.rotozoom(self.original_image, angle, 1)
self.rect = self.image.get_rect()
self.rect.centerx = self.centerx
self.rect.centery = self.centery
def enu2aer(e, n, u, deg=True):
# type: (float, float, float, bool) -> tuple
"""
ENU to Azimuth, Elevation, Range
Parameters
----------
e : float
ENU East coordinate (meters)
n : float
ENU North coordinate (meters)
u : float
ENU Up coordinate (meters)
deg : bool, optional
degrees input/output (False: radians in/out)
Results
-------
azimuth : float
azimuth to rarget
elevation : float
elevation to target
srange : float
slant range [meters]
"""
# 1 millimeter precision for singularity
if abs(e) < 1e-3:
e = 0.
if abs(n) < 1e-3:
n = 0.
if abs(u) < 1e-3:
u = 0.
r = hypot(e, n)
slantRange = hypot(r, u)
elev = arctan2(u, r)
az = arctan2(e, n) % tau
if deg:
az = degrees(az)
elev = degrees(elev)
return az, elev, slantRange
def enu2aer(e, n, u, deg=True):
"""
ENU to Azimuth, Elevation, Range
Parameters
----------
e : float
ENU East coordinate (meters)
n : float
ENU North coordinate (meters)
u : float
ENU Up coordinate (meters)
deg : bool, optional
degrees input/output (False: radians in/out)
Results
-------
azimuth : float
azimuth to rarget
elevation : float
elevation to target
srange : float
slant range [meters]
"""
# 1 millimeter precision for singularity
if abs(e) < 1e-3:
e = 0.
if abs(n) < 1e-3:
n = 0.
if abs(u) < 1e-3:
u = 0.
r = hypot(e, n)
slantRange = hypot(r, u)
elev = arctan2(u, r)
az = arctan2(e, n) % tau
if deg:
az = degrees(az)
elev = degrees(elev)
return az, elev, slantRange
def compute_euler_angles(matrix):
"computes the euler angles for a 3x3 rotation matrix"
if abs(matrix[2, 0]) == 1:
phi = 0 #in this case, phi value can be arbitrary
if matrix[2, 0] == -1:
theta = pi / 2
psi = arctan2(matrix[0, 1], matrix[0, 2])
return theta, psi, phi
else:
theta = -1 * pi / 2
psi = arctan2((-1 * matrix[0, 1]), (-1 * matrix[0, 2]))
return theta, psi, phi
else:
theta_1 = -1 * arcsin(matrix[2, 0])
theta_2 = pi - theta_1
psi_1, psi_2 = compute_psi(matrix, theta_1, theta_2)
phi_1, phi_2 = compute_phi(matrix, theta_1, theta_2)
return theta_1, psi_1, phi_1
def get_next_waypoint_index(self, x, y, theta):
index = self.get_closest_waypoint_index(x, y)
point = self.data[index]
heading = math.arctan2(point[1] - y, point[0] - x)
angle = abs(theta - heading)
angle = min(2 * math.pi - angle, angle)
if angle > (math.pi / 2.0):
index += 1
if index == len(self.data):
index = 0
return index
def do_steer(self, x_hat):
if self.waypoints:
cwp = self.waypoints[0]
x, y, theta = x_hat
cpos = Point(x, y)
dist = distance(cpos, cwp)
dx = cwp.x - cpos.x
dy = cwp.y - cpos.y
t_angle = math.arctan2(dy, dx)
dangle = clipangle(t_angle - theta)
u = 0.1 * np.matrix([3.0, 4.0 * dangle]).transpose()
if dist < 0.7:
print('At waypoint!', self.waypoints.pop(0))
else:
u = np.matrix([[0.0], [0.0]])
self.cmd_vel.publish(Twist())
return u
def do_steer(self, x_hat):
if self.waypoints:
cwp = self.waypoints[0]
x, y, theta = x_hat
cpos = Point(x, y)
dist = distance(cpos, cwp)
dx = cwp.x - cpos.x
dy = cwp.y - cpos.y
t_angle = math.arctan2(dy, dx)
dangle = clipangle(t_angle - theta)
u = 0.1*np.matrix([3.0, 4.0 * dangle]).transpose()
if dist < 0.7:
print('At waypoint!', self.waypoints.pop(0))
else:
u = np.matrix([[0.0], [0.0]])
self.cmd_vel.publish(Twist())
return u
def __init__(self, C, P, theta):
"""
Parameters
----------
C : 2D tuple
Center coordinate.
P : 2D tuple
Starting point coordinate
theta: float
Angle in radians of the arc
"""
self.C = np.array(C)
self.P = np.array(P)
self.theta = theta
self.ray = self.P-self.C
self.r = np.linalg.norm(self.P-self.C)
self.arc_length = self.r * theta
self.angle = np.arctan2(self.ray[1],self.ray[0]) #first y then x
def __init__(self, C, P, theta):
"""
Parameters
----------
C : 2D tuple
Center coordinate.
P : 2D tuple
Starting point coordinate
theta: float
Angle in radians of the arc
"""
self.C = np.array(C)
self.P = np.array(P)
self.theta = theta
self.ray = self.P - self.C
self.r = np.linalg.norm(self.P - self.C)
self.arc_length = self.r * theta
self.angle = np.arctan2(self.ray[1], self.ray[0]) #first y then x
def getEllipses(src, nsigs=[1.], **kwargs):
from matplotlib.patches import Ellipse
xc = src.getX()
yc = src.getY()
x2 = src.getIxx()
y2 = src.getIyy()
xy = src.getIxy()
# SExtractor manual v2.5, pg 29.
a2 = (x2 + y2)/2. + math.sqrt(((x2 - y2)/2.)**2 + xy**2)
b2 = (x2 + y2)/2. - math.sqrt(((x2 - y2)/2.)**2 + xy**2)
theta = math.rad2deg(math.arctan2(2.*xy, (x2 - y2)) / 2.)
a = math.sqrt(a2)
b = math.sqrt(b2)
ells = []
for nsig in nsigs:
ells.append(Ellipse([xc,yc], 2.*a*nsig, 2.*b*nsig, angle=theta, **kwargs))
return ells
def getEllipses(src, nsigs=[1.], **kwargs):
from matplotlib.patches import Ellipse
xc = src.getX()
yc = src.getY()
x2 = src.getIxx()
y2 = src.getIyy()
xy = src.getIxy()
# SExtractor manual v2.5, pg 29.
a2 = (x2 + y2)/2. + math.sqrt(((x2 - y2)/2.)**2 + xy**2)
b2 = (x2 + y2)/2. - math.sqrt(((x2 - y2)/2.)**2 + xy**2)
theta = math.rad2deg(math.arctan2(2.*xy, (x2 - y2)) / 2.)
a = math.sqrt(a2)
b = math.sqrt(b2)
ells = []
for nsig in nsigs:
ells.append(Ellipse([xc,yc], 2.*a*nsig, 2.*b*nsig, angle=theta, **kwargs))
return ells
def steer(pose):
# check for proximity to the target
dx = target.x - pose.x
dy = target.y - pose.y
# if the car is inside a 1.5 meter radius of the target
# then we have "captured" the target
if math.pow(dx, 2) + math.pow(dy, 2) < 2.25:
capturedTarget()
current_heading = pose.theta
desired_heading = math.arctan2( dy, dx )
# p component
pTurn = ( desired_heading - current_heading ) * p
turn(pTurn)
return
def steer(pose):
# check for proximity to the target
dx = target.x - pose.x
dy = target.y - pose.y
# if the car is inside a 1.5 meter radius of the target
# then we have "captured" the target
if math.pow(dx, 2) + math.pow(dy, 2) < 2.25:
capturedTarget()
current_heading = pose.theta
desired_heading = math.arctan2(dy, dx)
# p component
pTurn = (desired_heading - current_heading) * p
turn(pTurn)
return
def get_distance(self, point):
"""
single point distance function
"""
x, y, z = point
px = math.sqrt(x**2 + y**2)
py = math.arctan2(y, x) * self.polar
pz = z
d = 0
if self.TPMStype == 0: # 'Gyroid':
d = math.sin(px) * math.cos(py) + math.sin(py) * math.cos(
pz) + math.sin(pz) * math.cos(px)
elif self.TPMStype == 1: # 'SchwartzP':
d = math.cos(px) + math.cos(py) + math.cos(pz)
elif self.TPMStype == 2: # 'Diamond':
d = (math.sin(px) * math.sin(py) * math.sin(pz) +
math.sin(px) * math.cos(py) * math.cos(pz) +
math.cos(px) * math.sin(py) * math.cos(pz) +
math.cos(px) * math.cos(py) * math.sin(pz))
elif self.TPMStype == 3: # 'Neovius':
d = (3 * math.cos(px) + math.cos(py) + math.cos(pz) +
4 * math.cos(px) * math.cos(py) * math.cos(pz))
elif self.TPMStype == 4: # 'Lidinoid':
d = (0.5 * (math.sin(2 * px) * math.cos(py) * math.sin(pz) +
math.sin(2 * py) * math.cos(py) * math.sin(px) +
math.sin(2 * pz) * math.cos(px) * math.sin(pz)) - 0.5 *
(math.cos(2 * px) * math.cos(2 * py) + math.cos(2 * py) *
math.cos(2 * pz) + math.cos(2 * pz) * math.cos(2 * px)) +
0.15)
elif self.TPMStype == 5: # 'FischerKoch':
d = (math.cos(2 * px) * math.sin(py) * math.cos(pz) +
math.cos(2 * py) * math.sin(pz) * math.cos(px) +
math.cos(2 * pz) * math.sin(px) * math.cos(py))
return d
def OSGB36toWGS84(E, N):
#E, N are the British national grid coordinates - eastings and northings
a, b = 6377563.396, 6356256.909 #The Airy 180 semi-major and semi-minor axes used for OSGB36 (m)
F0 = 0.9996012717 #scale factor on the central meridian
lat0 = 49 * pi / 180 #Latitude of true origin (radians)
lon0 = -2 * pi / 180 #Longtitude of true origin and central meridian (radians)
N0, E0 = -100000, 400000 #Northing & easting of true origin (m)
e2 = 1 - (b * b) / (a * a) #eccentricity squared
n = (a - b) / (a + b)
lat, M = lat0, 0
while N - N0 - M >= 0.00001:
lat = (N - N0 - M) / (a * F0) + lat
M1 = (1 + n + (5. / 4) * n**2 + (5. / 4) * n**3) * (lat - lat0)
M2 = (3 * n + 3 * n**2 +
(21. / 8) * n**3) * sin(lat - lat0) * cos(lat + lat0)
M3 = ((15. / 8) * n**2 +
(15. / 8) * n**3) * sin(2 * (lat - lat0)) * cos(2 * (lat + lat0))
M4 = (35. / 24) * n**3 * sin(3 * (lat - lat0)) * cos(3 * (lat + lat0))
M = b * F0 * (M1 - M2 + M3 - M4)
nu = a * F0 / sqrt(1 - e2 * sin(lat)**2)
rho = a * F0 * (1 - e2) * (1 - e2 * sin(lat)**2)**(-1.5)
eta2 = nu / rho - 1
secLat = 1. / cos(lat)
VII = tan(lat) / (2 * rho * nu)
VIII = tan(lat) / (24 * rho * nu**3) * (5 + 3 * tan(lat)**2 + eta2 -
9 * tan(lat)**2 * eta2)
IX = tan(lat) / (720 * rho * nu**5) * (61 + 90 * tan(lat)**2 +
45 * tan(lat)**4)
X = secLat / nu
XI = secLat / (6 * nu**3) * (nu / rho + 2 * tan(lat)**2)
XII = secLat / (120 * nu**5) * (5 + 28 * tan(lat)**2 + 24 * tan(lat)**4)
XIIA = secLat / (5040 * nu**7) * (61 + 662 * tan(lat)**2 +
1320 * tan(lat)**4 + 720 * tan(lat)**6)
dE = E - E0
lat_1 = lat - VII * dE**2 + VIII * dE**4 - IX * dE**6
lon_1 = lon0 + X * dE - XI * dE**3 + XII * dE**5 - XIIA * dE**7
H = 0
x_1 = (nu / F0 + H) * cos(lat_1) * cos(lon_1)
y_1 = (nu / F0 + H) * cos(lat_1) * sin(lon_1)
z_1 = ((1 - e2) * nu / F0 + H) * sin(lat_1)
s = -20.4894 * 10**-6
tx, ty, tz = 446.448, -125.157, +542.060
rxs, rys, rzs = 0.1502, 0.2470, 0.8421
rx, ry, rz = rxs * pi / (180 * 3600.), rys * pi / (
180 * 3600.), rzs * pi / (180 * 3600.)
x_2 = tx + (1 + s) * x_1 + (-rz) * y_1 + (ry) * z_1
y_2 = ty + (rz) * x_1 + (1 + s) * y_1 + (-rx) * z_1
z_2 = tz + (-ry) * x_1 + (rx) * y_1 + (1 + s) * z_1
a_2, b_2 = 6378137.000, 6356752.3141
e2_2 = 1 - (b_2 * b_2) / (a_2 * a_2)
p = sqrt(x_2**2 + y_2**2)
lat = arctan2(z_2, (p * (1 - e2_2)))
latold = 2 * pi
while abs(lat - latold) > 10**-16:
lat, latold = latold, lat
nu_2 = a_2 / sqrt(1 - e2_2 * sin(latold)**2)
lat = arctan2(z_2 + e2_2 * nu_2 * sin(latold), p)
lon = arctan2(y_2, x_2)
H = p / cos(lat) - nu_2
lat = lat * 180 / pi
lon = lon * 180 / pi
return lat, lon
def WGS84toOSGB36(lat, lon):
#First convert to radians
#These are on the wrong ellipsoid currently: GRS80. (Denoted by _1)
lat_1 = lat*pi/180
lon_1 = lon*pi/180
#Want to convert to the Airy 1830 ellipsoid, which has the following:
a_1, b_1 =6378137.000, 6356752.3141 #The GSR80 semi-major and semi-minor axes used for WGS84(m)
e2_1 = 1- (b_1*b_1)/(a_1*a_1) #The eccentricity of the GRS80 ellipsoid
nu_1 = a_1/sqrt(1-e2_1*sin(lat_1)**2)
#First convert to cartesian from spherical polar coordinates
H = 0 #Third spherical coord.
x_1 = (nu_1 + H)*cos(lat_1)*cos(lon_1)
y_1 = (nu_1+ H)*cos(lat_1)*sin(lon_1)
z_1 = ((1-e2_1)*nu_1 +H)*sin(lat_1)
#Perform Helmut transform (to go between GRS80 (_1) and Airy 1830 (_2))
s = 20.4894*10**-6 #The scale factor -1
tx, ty, tz = -446.448, 125.157, -542.060 #The translations along x,y,z axes respectively
rxs,rys,rzs = -0.1502, -0.2470, -0.8421 #The rotations along x,y,z respectively, in seconds
rx, ry, rz = rxs*pi/(180*3600.), rys*pi/(180*3600.), rzs*pi/(180*3600.) #In radians
x_2 = tx + (1+s)*x_1 + (-rz)*y_1 + (ry)*z_1
y_2 = ty + (rz)*x_1 + (1+s)*y_1 + (-rx)*z_1
z_2 = tz + (-ry)*x_1 + (rx)*y_1 + (1+s)*z_1
#Back to spherical polar coordinates from cartesian
#Need some of the characteristics of the new ellipsoid
a, b = 6377563.396, 6356256.909 #The GSR80 semi-major and semi-minor axes used for WGS84(m)
e2 = 1- (b*b)/(a*a) #The eccentricity of the Airy 1830 ellipsoid
p = sqrt(x_2**2 + y_2**2)
#Lat is obtained by an iterative proceedure:
lat = arctan2(z_2,(p*(1-e2))) #Initial value
latold = 2*pi
while abs(lat - latold)>10**-16:
lat, latold = latold, lat
nu = a/sqrt(1-e2*sin(latold)**2)
lat = arctan2(z_2+e2*nu*sin(latold), p)
#Lon and height are then pretty easy
lon = arctan2(y_2,x_2)
H = p/cos(lat) - nu
#E, N are the British national grid coordinates - eastings and northings
F0 = 0.9996012717 #scale factor on the central meridian
lat0 = 49*pi/180 #Latitude of true origin (radians)
lon0 = -2*pi/180 #Longtitude of true origin and central meridian (radians)
N0, E0 = -100000, 400000 #Northing & easting of true origin (m)
n = (a-b)/(a+b)
#meridional radius of curvature
rho = a*F0*(1-e2)*(1-e2*sin(lat)**2)**(-1.5)
eta2 = nu*F0/rho-1
M1 = (1 + n + (5/4)*n**2 + (5/4)*n**3) * (lat-lat0)
M2 = (3*n + 3*n**2 + (21/8)*n**3) * sin(lat-lat0) * cos(lat+lat0)
M3 = ((15/8)*n**2 + (15/8)*n**3) * sin(2*(lat-lat0)) * cos(2*(lat+lat0))
M4 = (35/24)*n**3 * sin(3*(lat-lat0)) * cos(3*(lat+lat0))
#meridional arc
M = b * F0 * (M1 - M2 + M3 - M4)
I = M + N0
II = nu*F0*sin(lat)*cos(lat)/2
III = nu*F0*sin(lat)*cos(lat)**3*(5- tan(lat)**2 + 9*eta2)/24
IIIA = nu*F0*sin(lat)*cos(lat)**5*(61- 58*tan(lat)**2 + tan(lat)**4)/720
IV = nu*F0*cos(lat)
V = nu*F0*cos(lat)**3*(nu/rho - tan(lat)**2)/6
VI = nu*F0*cos(lat)**5*(5 - 18* tan(lat)**2 + tan(lat)**4 + 14*eta2 - 58*eta2*tan(lat)**2)/120
N = I + II*(lon-lon0)**2 + III*(lon- lon0)**4 + IIIA*(lon-lon0)**6
E = E0 + IV*(lon-lon0) + V*(lon- lon0)**3 + VI*(lon- lon0)**5
#Job's a good'n.
return E,N
def ecef2geodetic(x, y, z, ell=None, deg=True):
"""
convert ECEF (meters) to geodetic coordinates
Parameters
----------
x : float
target x ECEF coordinate (meters)
y : float
target y ECEF coordinate (meters)
z : float
target z ECEF coordinate (meters)
ell : Ellipsoid, optional
reference ellipsoid
deg : bool, optional
degrees input/output (False: radians in/out)
Returns
-------
lat : float
target geodetic latitude
lon : float
target geodetic longitude
h : float
target altitude above geodetic ellipsoid (meters)
based on:
You, Rey-Jer. (2000). Transformation of Cartesian to Geodetic Coordinates without Iterations.
Journal of Surveying Engineering. doi: 10.1061/(ASCE)0733-9453
"""
if ell is None:
ell = Ellipsoid()
r = sqrt(x**2 + y**2 + z**2)
E = sqrt(ell.semimajor_axis**2 - ell.semiminor_axis**2)
# eqn. 4a
u = sqrt(0.5 * (r**2 - E**2) +
0.5 * sqrt((r**2 - E**2)**2 + 4 * E**2 * z**2))
Q = hypot(x, y)
huE = hypot(u, E)
# eqn. 4b
Beta = arctan(huE / u * z / hypot(x, y))
# eqn. 13
eps = ((ell.semiminor_axis * u - ell.semimajor_axis * huE + E**2) *
sin(Beta)) / (ell.semimajor_axis * huE * 1 / cos(Beta) -
E**2 * cos(Beta))
Beta += eps
# %% final output
lat = arctan(ell.semimajor_axis / ell.semiminor_axis * tan(Beta))
lon = arctan2(y, x)
# eqn. 7
alt = hypot(z - ell.semiminor_axis * sin(Beta),
Q - ell.semimajor_axis * cos(Beta))
# inside ellipsoid?
inside = x**2 / ell.semimajor_axis**2 + y**2 / ell.semimajor_axis**2 + z**2 / ell.semiminor_axis**2 < 1
if inside:
alt = -alt
if deg:
lat = degrees(lat)
lon = degrees(lon)
return lat, lon, alt
def OSGB36toWGS84(E,N):
#E, N are the British national grid coordinates - eastings and northings
a, b = 6377563.396, 6356256.909 #The Airy 180 semi-major and semi-minor axes used for OSGB36 (m)
F0 = 0.9996012717 #scale factor on the central meridian
lat0 = 49*pi/180 #Latitude of true origin (radians)
lon0 = -2*pi/180 #Longtitude of true origin and central meridian (radians)
N0, E0 = -100000, 400000 #Northing & easting of true origin (m)
e2 = 1 - (b*b)/(a*a) #eccentricity squared
n = (a-b)/(a+b)
lat,M = lat0, 0
while N-N0-M >= 0.00001:
lat = (N-N0-M)/(a*F0) + lat;
M1 = (1 + n + (5./4)*n**2 + (5./4)*n**3) * (lat-lat0)
M2 = (3*n + 3*n**2 + (21./8)*n**3) * sin(lat-lat0) * cos(lat+lat0)
M3 = ((15./8)*n**2 + (15./8)*n**3) * sin(2*(lat-lat0)) * cos(2*(lat+lat0))
M4 = (35./24)*n**3 * sin(3*(lat-lat0)) * cos(3*(lat+lat0))
M = b * F0 * (M1 - M2 + M3 - M4)
nu = a*F0/sqrt(1-e2*sin(lat)**2)
rho = a*F0*(1-e2)*(1-e2*sin(lat)**2)**(-1.5)
eta2 = nu/rho-1
secLat = 1./cos(lat)
VII = tan(lat)/(2*rho*nu)
VIII = tan(lat)/(24*rho*nu**3)*(5+3*tan(lat)**2+eta2-9*tan(lat)**2*eta2)
IX = tan(lat)/(720*rho*nu**5)*(61+90*tan(lat)**2+45*tan(lat)**4)
X = secLat/nu
XI = secLat/(6*nu**3)*(nu/rho+2*tan(lat)**2)
XII = secLat/(120*nu**5)*(5+28*tan(lat)**2+24*tan(lat)**4)
XIIA = secLat/(5040*nu**7)*(61+662*tan(lat)**2+1320*tan(lat)**4+720*tan(lat)**6)
dE = E-E0
lat_1 = lat - VII*dE**2 + VIII*dE**4 - IX*dE**6
lon_1 = lon0 + X*dE - XI*dE**3 + XII*dE**5 - XIIA*dE**7
H = 0
x_1 = (nu/F0 + H)*cos(lat_1)*cos(lon_1)
y_1 = (nu/F0+ H)*cos(lat_1)*sin(lon_1)
z_1 = ((1-e2)*nu/F0 +H)*sin(lat_1)
s = -20.4894*10**-6
tx, ty, tz = 446.448, -125.157, + 542.060
rxs,rys,rzs = 0.1502, 0.2470, 0.8421
rx, ry, rz = rxs*pi/(180*3600.), rys*pi/(180*3600.), rzs*pi/(180*3600.)
x_2 = tx + (1+s)*x_1 + (-rz)*y_1 + (ry)*z_1
y_2 = ty + (rz)*x_1 + (1+s)*y_1 + (-rx)*z_1
z_2 = tz + (-ry)*x_1 + (rx)*y_1 + (1+s)*z_1
a_2, b_2 =6378137.000, 6356752.3141
e2_2 = 1- (b_2*b_2)/(a_2*a_2)
p = sqrt(x_2**2 + y_2**2)
lat = arctan2(z_2,(p*(1-e2_2)))
latold = 2*pi
while abs(lat - latold)>10**-16:
lat, latold = latold, lat
nu_2 = a_2/sqrt(1-e2_2*sin(latold)**2)
lat = arctan2(z_2+e2_2*nu_2*sin(latold), p)
lon = arctan2(y_2,x_2)
H = p/cos(lat) - nu_2
lat = lat*180/pi
lon = lon*180/pi
return lat, lon
def ecef2geodetic(x, y, z, ell=None, deg=True):
"""
convert ECEF (meters) to geodetic coordinates
Parameters
----------
x : float
target x ECEF coordinate (meters)
y : float
target y ECEF coordinate (meters)
z : float
target z ECEF coordinate (meters)
ell : Ellipsoid, optional
reference ellipsoid
deg : bool, optional
degrees input/output (False: radians in/out)
Returns
-------
lat : float
target geodetic latitude
lon : float
target geodetic longitude
h : float
target altitude above geodetic ellipsoid (meters)
based on:
You, Rey-Jer. (2000). Transformation of Cartesian to Geodetic Coordinates without Iterations.
Journal of Surveying Engineering. doi: 10.1061/(ASCE)0733-9453
"""
if ell is None:
ell = Ellipsoid()
r = sqrt(x**2 + y**2 + z**2)
E = sqrt(ell.semimajor_axis**2 - ell.semiminor_axis**2)
# eqn. 4a
u = sqrt(0.5 * (r**2 - E**2) + 0.5 * sqrt((r**2 - E**2)**2 + 4 * E**2 * z**2))
Q = hypot(x, y)
huE = hypot(u, E)
# eqn. 4b
Beta = arctan(huE / u * z / hypot(x, y))
# eqn. 13
eps = ((ell.semiminor_axis * u - ell.semimajor_axis * huE + E**2) * sin(Beta)) / (ell.semimajor_axis * huE * 1 / cos(Beta) - E**2 * cos(Beta))
Beta += eps
# %% final output
lat = arctan(ell.semimajor_axis / ell.semiminor_axis * tan(Beta))
lon = arctan2(y, x)
# eqn. 7
alt = hypot(z - ell.semiminor_axis * sin(Beta),
Q - ell.semimajor_axis * cos(Beta))
# inside ellipsoid?
inside = x**2 / ell.semimajor_axis**2 + y**2 / ell.semimajor_axis**2 + z**2 / ell.semiminor_axis**2 < 1
if inside:
alt = -alt
if deg:
lat = degrees(lat)
lon = degrees(lon)
return lat, lon, alt
def avg_lyapunov():
"""Return the average lyapunov exponent for each table type, over a number of trajectories, for
both angular and radial separation. For section 3.3."""
radius = lambda ba, bb: sqrt((ba.pos[0] - bb.pos[0])**2 +
(ba.pos[1] - bb.pos[1])**2)
angle = lambda b1, b2: abs(
arctan2(b1.pos[1], b1.pos[0]) - arctan2(b2.pos[1], b2.pos[0]))
x_range = [0.1 * i for i in range(6)
] # Range of x-coordinates to start at (y = 0 always).
v_range = [[1, 5], [2, 4], [3, 3], [4, 2], [5, 1]] # Range of velocities.
rect_lyap_rad, ell_lyap_rad, stad_lyap_rad = [], [], [
] # Lists of computed radial Lyapunov exponents for tables.
rect_lyap_ang, ell_lyap_ang, stad_lyap_ang = [], [], [
] # Lists of computed angular Lyapunov exponents for tables.
rect, ell, stad = Rectangle(2, 1), Ellipse(2, 1), Stadium(2, 1) # Tables.
for x in x_range:
for v in v_range:
# Offset y position by 0.1 between balls to act as slight difference in initial conditions.
rect_lyap_rad.append(
lyapunov(radius,
rect,
Ball([x, 0], v),
Ball([x, 0.1], v),
n=2000,
show=False))
rect_lyap_ang.append(
lyapunov(angle,
rect,
Ball([x, 0], v),
Ball([x, 0.1], v),
n=2000,
show=False))
ell_lyap_rad.append(
lyapunov(radius,
ell,
Ball([x, 0], v),
Ball([x, 0.1], v),
n=2000,
show=False))
ell_lyap_ang.append(
lyapunov(angle,
ell,
Ball([x, 0], v),
Ball([x, 0.1], v),
n=2000,
show=False))
stad_lyap_rad.append(
lyapunov(radius,
stad,
Ball([x, 0], v),
Ball([x, 0.1], v),
n=2000,
show=False))
stad_lyap_ang.append(
lyapunov(angle,
stad,
Ball([x, 0], v),
Ball([x, 0.1], v),
n=2000,
show=False))
print(
f"Rectangle radial separation Lyapunov exponent: {mean(rect_lyap_rad)} +/- {std(rect_lyap_rad)}"
)
print(
f"Ellipse radial separation Lyapunov exponent: {mean(ell_lyap_rad)} +/- {std(ell_lyap_rad)}"
)
print(
f"Stadium radial separation Lyapunov exponent: {mean(stad_lyap_rad)} +/- {std(stad_lyap_rad)}"
)
print(
f"Rectangle angular separation Lyapunov exponent: {mean(rect_lyap_ang)} +/- {std(rect_lyap_ang)}"
)
print(
f"Ellipse angular separation Lyapunov exponent: {mean(ell_lyap_ang)} +/- {std(ell_lyap_ang)}"
)
print(
f"Stadium angular separation Lyapunov exponent: {mean(stad_lyap_ang)} +/- {std(stad_lyap_ang)}"
)
def wgs2osgb(lat, lon = None): # derived from WGS84toOSGB36() by Hannah Fry
'''
convert WGS84 latitude, longitude coordinates to OSGB36 numeric coordinates
arguments are either pair of floats, or a double list/tuple of floats
return double tuple of east, west integers
does not check validity of argument values
'''
# check for single list or tuple, or 2 arguments
if lon is None: # assume lat is list or tuple
lon = lat[1] # order matters!
lat = lat[0]
# convert to radians
# these are on the wrong ellipsoid currently: GRS80. (Denoted by _G)
lat_G = lat*pi/180
lon_G = lon*pi/180
nu_G = A_G/sqrt(1-E2_G*sin(lat_G)**2)
# convert to cartesian from spherical polar coordinates
x_G = (nu_G + H)*cos(lat_G)*cos(lon_G)
y_G = (nu_G + H)*cos(lat_G)*sin(lon_G)
z_G = ((1 - E2_G)*nu_G + H)*sin(lat_G)
# perform Helmut transform (to go from GRS80 (_G) to Airy 1830 (_A))
x_A = TX_GA + (1+S)*x_G + (-RZ_GA)*y_G + (RY_GA)*z_G
y_A = TY_GA + (RZ_GA)*x_G+ (1 + S)*y_G + (-RX_GA)*z_G
z_A = TZ_GA + (-RY_GA)*x_G + (RX_GA)*y_G +(1 + S)*z_G
# back to spherical polar coordinates from cartesian
# need some of the characteristics of the new ellipsoid
p_A = sqrt(x_A**2 + y_A**2)
# latitude is obtained by iteration
lat = arctan2(z_A,(p_A*(1 - E2_A))) # initial value
latold = 2*pi
while abs(lat - latold)>10**-16:
lat, latold = latold, lat
nu_A = A_A/sqrt(1 - E2_A*sin(latold)**2)
lat = arctan2(z_A + E2_A*nu_A*sin(latold), p_A)
# longitude and height are then pretty easy
lon = arctan2(y_A,x_A)
h = p_A/cos(lat) - nu_A
# east, north are the UK National Grid coordinates - eastings and northings
# meridional radius of curvature
rho = A_A*F0*(1 - E2_A)*(1 - E2_A*sin(lat)**2)**(-1.5)
eta2 = nu_A*F0/rho-1
m1 = (1 + N_A + (5/4)*N_A**2 + (5/4)*N_A**3) * (lat-LAT0)
m2 = (3*N_A + 3*N_A**2 + (21/8)*N_A**3) * sin(lat - LAT0) * cos(lat + LAT0)
m3 = ((15/8)*N_A**2 + (15/8)*N_A**3) * sin(2*(lat - LAT0)) * cos(2*(lat + LAT0))
m4 = (35/24)*N_A**3 * sin(3*(lat - LAT0)) * cos(3*(lat + LAT0))
# meridional arc
m = B_A * F0 * (m1 - m2 + m3 - m4)
i = m + N0
ii = nu_A*F0*sin(lat)*cos(lat)/2
iii = nu_A*F0*sin(lat)*cos(lat)**3*(5- tan(lat)**2 + 9*eta2)/24
iiia = nu_A*F0*sin(lat)*cos(lat)**5*(61- 58*tan(lat)**2 + tan(lat)**4)/720
iv = nu_A*F0*cos(lat)
v = nu_A*F0*cos(lat)**3*(nu_A/rho - tan(lat)**2)/6
vi = nu_A*F0*cos(lat)**5*(5 - 18* tan(lat)**2 + tan(lat)**4 + 14*eta2 - 58*eta2*tan(lat)**2)/120
north = i + ii*(lon - LON0)**2 + iii*(lon - LON0)**4 + iiia*(lon - LON0)**6
east = E0 + iv*(lon - LON0) + v*(lon - LON0)**3 + vi*(lon - LON0)**5
# round down to nearest metre and return as integer value double tuple
return (int(east), int(north))
def osgb2wgs(east, north = None): # derived from OSGB36toWGS84() by Hannah Fry
'''
convert OSGB36 numeric coordinates to WGS lat, lon coordinates
arguments are either a pair of integers, or a NGR
return double tuple of lat, lon floats to precision of 5dp
'''
# check for single tuple or double argument
if north is None: # assume NGR
(east, north) = ngr2osgb(east)
east, north = int(east), int(north)
# Initialise the iterative variables
lat, m = LAT0, 0
while north - N0 - m >= 0.00001: #Accurate to 0.01mm
lat = (north - N0 - m)/(A_A*F0) + lat;
m1 = (1 + N_A + (5./4)*N_A**2 + (5./4)*N_A**3) * (lat - LAT0)
m2 = (3*N_A + 3*N_A**2 + (21./8)*N_A**3) * sin(lat - LAT0) * cos(lat + LAT0)
m3 = ((15./8)*N_A**2 + (15./8)*N_A**3) * sin(2*(lat - LAT0)) * cos(2*(lat + LAT0))
m4 = (35./24)*N_A**3 * sin(3*(lat - LAT0)) * cos(3*(lat + LAT0))
#meridional arc
m = B_A * F0 * (m1 - m2 + m3 - m4)
# transverse radius of curvature
nu_A = A_A*F0/sqrt(1 - E2_A*sin(lat)**2)
# meridional radius of curvature
rho = A_A*F0*(1 - E2_A)*(1 - E2_A*sin(lat)**2)**(-1.5)
eta2 = nu_A/rho-1
secLat = 1./cos(lat)
vii = tan(lat)/(2*rho*nu_A)
viii = tan(lat)/(24*rho*nu_A**3)*(5+3*tan(lat)**2+eta2 - 9*tan(lat)**2*eta2)
ix = tan(lat)/(720*rho*nu_A**5)*(61 + 90*tan(lat)**2 + 45*tan(lat)**4)
x = secLat/nu_A
xi = secLat/(6*nu_A**3)*(nu_A/rho+2*tan(lat)**2)
xii = secLat/(120*nu_A**5)*(5+28*tan(lat)**2 + 24*tan(lat)**4)
xiia = secLat/(5040*nu_A**7)*(61 + 662*tan(lat)**2 + 1320*tan(lat)**4 + 720*tan(lat)**6)
dE = east - E0
# these are on the wrong ellipsoid currently: Airy1830 (denoted by _A)
lat_A = lat - vii*dE**2 + viii*dE**4 - ix*dE**6
lon_A = LON0 + x*dE - xi*dE**3 + xii*dE**5 - xiia*dE**7
# convert to the GRS80 ellipsoid (denoted by _G).
# first convert to cartesian from spherical polar coordinates
H = 0 # third spherical coord.
x_A = (nu_A/F0 + H)*cos(lat_A)*cos(lon_A)
y_A = (nu_A/F0+ H)*cos(lat_A)*sin(lon_A)
z_A = ((1 - E2_A)*nu_A/F0 + H)*sin(lat_A)
# Perform Helmut transform (to go from Airy 1830 to GRS80)
x_G = TX_AG + (1 + S)*x_A + (-RZ_AG)*y_A + (RY_AG)*z_A
y_G = TY_AG + (RZ_AG)*x_A + (1 + S)*y_A + (-RX_AG)*z_A
z_G = TZ_AG + (-RY_AG)*x_A + (RX_AG)*y_A + (1 + S)*z_A
# back to spherical polar coordinates from cartesian
# need a characteristic of the new ellipsoid
p_G = sqrt(x_G**2 + y_G**2)
# lat is obtained by an iterative procedure:
lat = arctan2(z_G,(p_G*(1 - E2_G))) #Initial value
latold = 2*pi
while abs(lat - latold)>10**-16:
lat, latold = latold, lat
nu_G = A_G/sqrt(1 - E2_G*sin(latold)**2)
lat = arctan2(z_G + E2_G*nu_G*sin(latold), p_G)
#Lon and height are then pretty easy
lon = arctan2(y_G, x_G)
H = p_G/cos(lat) - nu_G
#Convert to degrees & returns floats double tuple
lat = lat*180/pi
lon = lon*180/pi
return (round(lat, 5), round(lon, 5))
def WGS84toOSGB36(lat, lon):
'''Hannah's code to convert WGS84 Lat and Lon to OS Eastings and Northings'''
#First convert to radians
#These are on the wrong ellipsoid currently: GRS80. (Denoted by _1)
lat_1 = lat * pi / 180
lon_1 = lon * pi / 180
#Want to convert to the Airy 1830 ellipsoid, which has the following:
a_1, b_1 = 6378137.000, 6356752.3141 #The GSR80 semi-major and semi-minor axes used for WGS84(m)
e2_1 = 1 - (b_1 * b_1) / (a_1 * a_1
) #The eccentricity of the GRS80 ellipsoid
nu_1 = a_1 / sqrt(1 - e2_1 * sin(lat_1)**2)
#First convert to cartesian from spherical polar coordinates
H = 0 #Third spherical coord.
x_1 = (nu_1 + H) * cos(lat_1) * cos(lon_1)
y_1 = (nu_1 + H) * cos(lat_1) * sin(lon_1)
z_1 = ((1 - e2_1) * nu_1 + H) * sin(lat_1)
#Perform Helmut transform (to go between GRS80 (_1) and Airy 1830 (_2))
s = 20.4894 * 10**-6 #The scale factor -1
tx, ty, tz = -446.448, 125.157, -542.060 #The translations along x,y,z axes respectively
rxs, rys, rzs = -0.1502, -0.2470, -0.8421 #The rotations along x,y,z respectively, in seconds
rx, ry, rz = rxs * pi / (180 * 3600.), rys * pi / (
180 * 3600.), rzs * pi / (180 * 3600.) #In radians
x_2 = tx + (1 + s) * x_1 + (-rz) * y_1 + (ry) * z_1
y_2 = ty + (rz) * x_1 + (1 + s) * y_1 + (-rx) * z_1
z_2 = tz + (-ry) * x_1 + (rx) * y_1 + (1 + s) * z_1
#Back to spherical polar coordinates from cartesian
#Need some of the characteristics of the new ellipsoid
a, b = 6377563.396, 6356256.909 #The GSR80 semi-major and semi-minor axes used for WGS84(m)
e2 = 1 - (b * b) / (a * a) #The eccentricity of the Airy 1830 ellipsoid
p = sqrt(x_2**2 + y_2**2)
#Lat is obtained by an iterative proceedure:
lat = arctan2(z_2, (p * (1 - e2))) #Initial value
latold = 2 * pi
while abs(lat - latold) > 10**-16:
lat, latold = latold, lat
nu = a / sqrt(1 - e2 * sin(latold)**2)
lat = arctan2(z_2 + e2 * nu * sin(latold), p)
#Lon and height are then pretty easy
lon = arctan2(y_2, x_2)
H = p / cos(lat) - nu
#E, N are the British national grid coordinates - eastings and northings
F0 = 0.9996012717 #scale factor on the central meridian
lat0 = 49 * pi / 180 #Latitude of true origin (radians)
lon0 = -2 * pi / 180 #Longtitude of true origin and central meridian (radians)
N0, E0 = -100000, 400000 #Northing & easting of true origin (m)
n = (a - b) / (a + b)
#meridional radius of curvature
rho = a * F0 * (1 - e2) * (1 - e2 * sin(lat)**2)**(-1.5)
eta2 = nu * F0 / rho - 1
M1 = (1 + n + (5 / 4) * n**2 + (5 / 4) * n**3) * (lat - lat0)
M2 = (3 * n + 3 * n**2 +
(21 / 8) * n**3) * sin(lat - lat0) * cos(lat + lat0)
M3 = ((15 / 8) * n**2 +
(15 / 8) * n**3) * sin(2 * (lat - lat0)) * cos(2 * (lat + lat0))
M4 = (35 / 24) * n**3 * sin(3 * (lat - lat0)) * cos(3 * (lat + lat0))
#meridional arc
M = b * F0 * (M1 - M2 + M3 - M4)
I = M + N0
II = nu * F0 * sin(lat) * cos(lat) / 2
III = nu * F0 * sin(lat) * cos(lat)**3 * (5 - tan(lat)**2 + 9 * eta2) / 24
IIIA = nu * F0 * sin(lat) * cos(lat)**5 * (61 - 58 * tan(lat)**2 +
tan(lat)**4) / 720
IV = nu * F0 * cos(lat)
V = nu * F0 * cos(lat)**3 * (nu / rho - tan(lat)**2) / 6
VI = nu * F0 * cos(lat)**5 * (5 - 18 * tan(lat)**2 + tan(lat)**4 +
14 * eta2 - 58 * eta2 * tan(lat)**2) / 120
N = I + II * (lon - lon0)**2 + III * (lon - lon0)**4 + IIIA * (lon -
lon0)**6
E = E0 + IV * (lon - lon0) + V * (lon - lon0)**3 + VI * (lon - lon0)**5
#Job's a good'n.
return E, N
def GetTheta(self):
return math.degrees(math.arctan2(self.GetDeltaY(), self.GetDeltaX()))
def from_cartesian(cls, x, y):
magnitude = math.sqrt(math.hypot(x, y))
direction = math.arctan2(y, x)
return cls(magnitude, direction)
def OSGB36toWGS84(E, N):
# E, N are the British national grid coordinates - eastings and northings
a = 6377563.396
b = 6356256.909
# The Airy 180 semi-major and semi-minor axes used for OSGB36 (m)
F0 = 0.9996012717
lat0 = 49 * pi / 180
lon0 = -2 * pi / 180
N0, E0 = -100000, 400000
e2 = 1 - (b * b) / (a * a)
n = (a - b) / (a + b)
# Initialise the iterative variables
lat, M = lat0, 0
while N - N0 - M >= 0.00001:
lat = (N - N0 - M) / (a * F0) + lat
M1 = (1 + n + (5. / 4) * n**2 + (5. / 4) * n**3) * (lat - lat0)
M2 = (3 * n + 3 * n**2 +
(21. / 8) * n**3) * sin(lat - lat0) * cos(lat + lat0)
M3 = ((15. / 8) * n**2 +
(15. / 8) * n**3) * sin(2 * (lat - lat0)) * cos(2 * (lat + lat0))
M4 = (35. / 24) * n**3 * sin(3 * (lat - lat0)) * cos(3 * (lat + lat0))
# meridional arc
M = b * F0 * (M1 - M2 + M3 - M4)
# transverse radius of curvature
nu = a * F0 / sqrt(1 - e2 * sin(lat)**2)
# meridional radius of curvature
rho = a * F0 * (1 - e2) * (1 - e2 * sin(lat)**2)**(-1.5)
eta2 = nu / rho - 1
secLat = 1. / cos(lat)
VII = tan(lat) / (2 * rho * nu)
VIII = tan(lat) / (24 * rho * nu**3) * (5 + 3 * tan(lat)**2 + eta2 -
9 * tan(lat)**2 * eta2)
IX = tan(lat) / (720 * rho * nu**5) * (61 + 90 * tan(lat)**2 +
45 * tan(lat)**4)
X = secLat / nu
XI = secLat / (6 * nu**3) * (nu / rho + 2 * tan(lat)**2)
XII = secLat / (120 * nu**5) * (5 + 28 * tan(lat)**2 + 24 * tan(lat)**4)
XIIA = secLat / (5040 * nu**7) * (61 + 662 * tan(lat)**2 +
1320 * tan(lat)**4 + 720 * tan(lat)**6)
dE = E - E0
# These are on the wrong ellipsoid currently: Airy1830. (Denoted by _1)
lat_1 = lat - VII * dE**2 + VIII * dE**4 - IX * dE**6
lon_1 = lon0 + X * dE - XI * dE**3 + XII * dE**5 - XIIA * dE**7
# Want to convert to the GRS80 ellipsoid.
# First convert to cartesian from spherical polar coordinates
H = 0
x_1 = (nu / F0 + H) * cos(lat_1) * cos(lon_1)
y_1 = (nu / F0 + H) * cos(lat_1) * sin(lon_1)
z_1 = ((1 - e2) * nu / F0 + H) * sin(lat_1)
# Perform Helmut transform (to go between Airy 1830 (_1) and GRS80 (_2))
s = -20.4894 * 10**-6
tx, ty, tz = 446.448, -125.157, +542.060
rxs, rys, rzs = 0.1502, 0.2470, 0.8421
rx, ry, rz = rxs * pi / (180 * 3600.), rys * pi / (
180 * 3600.), rzs * pi / (180 * 3600.)
x_2 = tx + (1 + s) * x_1 + (-rz) * y_1 + (ry) * z_1
y_2 = ty + (rz) * x_1 + (1 + s) * y_1 + (-rx) * z_1
z_2 = tz + (-ry) * x_1 + (rx) * y_1 + (1 + s) * z_1
# Back to spherical polar coordinates from cartesian
# Need some of the characteristics of the new ellipsoid
a_2, b_2 = 6378137.000, 6356752.3141
e2_2 = 1 - (b_2 * b_2) / (a_2 * a_2)
p = sqrt(x_2**2 + y_2**2)
# Lat is obtained by an iterative proceedure:
lat = arctan2(z_2, (p * (1 - e2_2)))
latold = 2 * pi
while abs(lat - latold) > 10**-16:
lat, latold = latold, lat
nu_2 = a_2 / sqrt(1 - e2_2 * sin(latold)**2)
lat = arctan2(z_2 + e2_2 * nu_2 * sin(latold), p)
# Lon and height are then pretty easy
lon = arctan2(y_2, x_2)
H = p / cos(lat) - nu_2
# Convert to degrees
lat = lat * 180 / pi
lon = lon * 180 / pi
# Job's a good'n.
return lat, lon
def OSGB36toWGS84_(E,N):
'''
From http://hannahfry.co.uk/2012/02/01/converting-british-national-grid-to-latitude-and-longitude-ii/
'''
#E, N are the British national grid coordinates - eastings and northings
a, b = 6377563.396, 6356256.909 #The Airy 180 semi-major and semi-minor axes used for OSGB36 (m)
F0 = 0.9996012717 #scale factor on the central meridian
lat0 = 49*pi/180 #Latitude of true origin (radians)
lon0 = -2*pi/180 #Longtitude of true origin and central meridian (radians)
N0, E0 = -100000, 400000 #Northing & easting of true origin (m)
e2 = 1 - (b*b)/(a*a) #eccentricity squared
n = (a-b)/(a+b)
#Initialise the iterative variables
lat,M = lat0, 0
while N-N0-M >= 0.00001: #Accurate to 0.01mm
lat = (N-N0-M)/(a*F0) + lat;
M1 = (1 + n + (5./4)*n**2 + (5./4)*n**3) * (lat-lat0)
M2 = (3*n + 3*n**2 + (21./8)*n**3) * sin(lat-lat0) * cos(lat+lat0)
M3 = ((15./8)*n**2 + (15./8)*n**3) * sin(2*(lat-lat0)) * cos(2*(lat+lat0))
M4 = (35./24)*n**3 * sin(3*(lat-lat0)) * cos(3*(lat+lat0))
#meridional arc
M = b * F0 * (M1 - M2 + M3 - M4)
#transverse radius of curvature
nu = a*F0/sqrt(1-e2*sin(lat)**2)
#meridional radius of curvature
rho = a*F0*(1-e2)*(1-e2*sin(lat)**2)**(-1.5)
eta2 = nu/rho-1
secLat = 1./cos(lat)
VII = tan(lat)/(2*rho*nu)
VIII = tan(lat)/(24*rho*nu**3)*(5+3*tan(lat)**2+eta2-9*tan(lat)**2*eta2)
IX = tan(lat)/(720*rho*nu**5)*(61+90*tan(lat)**2+45*tan(lat)**4)
X = secLat/nu
XI = secLat/(6*nu**3)*(nu/rho+2*tan(lat)**2)
XII = secLat/(120*nu**5)*(5+28*tan(lat)**2+24*tan(lat)**4)
XIIA = secLat/(5040*nu**7)*(61+662*tan(lat)**2+1320*tan(lat)**4+720*tan(lat)**6)
dE = E-E0
#These are on the wrong ellipsoid currently: Airy1830. (Denoted by _1)
lat_1 = lat - VII*dE**2 + VIII*dE**4 - IX*dE**6
lon_1 = lon0 + X*dE - XI*dE**3 + XII*dE**5 - XIIA*dE**7
#Want to convert to the GRS80 ellipsoid.
#First convert to cartesian from spherical polar coordinates
H = 0 #Third spherical coord.
x_1 = (nu/F0 + H)*cos(lat_1)*cos(lon_1)
y_1 = (nu/F0+ H)*cos(lat_1)*sin(lon_1)
z_1 = ((1-e2)*nu/F0 +H)*sin(lat_1)
#Perform Helmut transform (to go between Airy 1830 (_1) and GRS80 (_2))
s = -20.4894*10**-6 #The scale factor -1
tx, ty, tz = 446.448, -125.157, + 542.060 #The translations along x,y,z axes respectively
rxs,rys,rzs = 0.1502, 0.2470, 0.8421 #The rotations along x,y,z respectively, in seconds
rx, ry, rz = rxs*pi/(180*3600.), rys*pi/(180*3600.), rzs*pi/(180*3600.) #In radians
x_2 = tx + (1+s)*x_1 + (-rz)*y_1 + (ry)*z_1
y_2 = ty + (rz)*x_1 + (1+s)*y_1 + (-rx)*z_1
z_2 = tz + (-ry)*x_1 + (rx)*y_1 + (1+s)*z_1
#Back to spherical polar coordinates from cartesian
#Need some of the characteristics of the new ellipsoid
a_2, b_2 =6378137.000, 6356752.3141 #The GSR80 semi-major and semi-minor axes used for WGS84(m)
e2_2 = 1- (b_2*b_2)/(a_2*a_2) #The eccentricity of the GRS80 ellipsoid
p = sqrt(x_2**2 + y_2**2)
#Lat is obtained by an iterative proceedure:
lat = arctan2(z_2,(p*(1-e2_2))) #Initial value
latold = 2*pi
while abs(lat - latold)>10**-16:
lat, latold = latold, lat
nu_2 = a_2/sqrt(1-e2_2*sin(latold)**2)
lat = arctan2(z_2+e2_2*nu_2*sin(latold), p)
#Lon and height are then pretty easy
lon = arctan2(y_2,x_2)
H = p/cos(lat) - nu_2
#Uncomment this line if you want to print the results
#print [(lat-lat_1)*180/pi, (lon - lon_1)*180/pi]
#Convert to degrees
lat = lat*180/pi
lon = lon*180/pi
#Job's a good'n.
return lat, lon
def OSIRISH36toWGS84(E,N):
#E, N are the British national grid coordinates - eastings and northings
a, b = 6377,340.189 #The Airy 180 semi-major and semi-minor axes used for OSGB36 (m)
F0 = 1.000035 #scale factor on the central meridian
lat0 = 53*pi/180 #Latitude of true origin (radians)
lon0 = -8*pi/180 #Longtitude of true origin and central meridian (radians)
N0, E0 = 250000 , 200000 #Northing & easting of true origin (m)
e2 = 1 - (b*b)/(a*a) #eccentricity squared
n = (a-b)/(a+b)
#Initialise the iterative variables
lat,M = lat0, 0
while N-N0-M >= 0.00001: #Accurate to 0.01mm
lat = (N-N0-M)/(a*F0) + lat;
M1 = (1 + n + (5./4)*n**2 + (5./4)*n**3) * (lat-lat0)
M2 = (3*n + 3*n**2 + (21./8)*n**3) * sin(lat-lat0) * cos(lat+lat0)
M3 = ((15./8)*n**2 + (15./8)*n**3) * sin(2*(lat-lat0)) * cos(2*(lat+lat0))
M4 = (35./24)*n**3 * sin(3*(lat-lat0)) * cos(3*(lat+lat0))
#meridional arc
M = b * F0 * (M1 - M2 + M3 - M4)
#transverse radius of curvature
nu = a*F0/sqrt(1-e2*sin(lat)**2)
#meridional radius of curvature
rho = a*F0*(1-e2)*(1-e2*sin(lat)**2)**(-1.5)
eta2 = nu/rho-1
secLat = 1./cos(lat)
VII = tan(lat)/(2*rho*nu)
VIII = tan(lat)/(24*rho*nu**3)*(5+3*tan(lat)**2+eta2-9*tan(lat)**2*eta2)
IX = tan(lat)/(720*rho*nu**5)*(61+90*tan(lat)**2+45*tan(lat)**4)
X = secLat/nu
XI = secLat/(6*nu**3)*(nu/rho+2*tan(lat)**2)
XII = secLat/(120*nu**5)*(5+28*tan(lat)**2+24*tan(lat)**4)
XIIA = secLat/(5040*nu**7)*(61+662*tan(lat)**2+1320*tan(lat)**4+720*tan(lat)**6)
dE = E-E0
#These are on the wrong ellipsoid currently: Airy1830. (Denoted by _1)
lat_1 = lat - VII*dE**2 + VIII*dE**4 - IX*dE**6
lon_1 = lon0 + X*dE - XI*dE**3 + XII*dE**5 - XIIA*dE**7
#Want to convert to the GRS80 ellipsoid.
#First convert to cartesian from spherical polar coordinates
H = 0 #Third spherical coord.
x_1 = (nu/F0 + H)*cos(lat_1)*cos(lon_1)
y_1 = (nu/F0+ H)*cos(lat_1)*sin(lon_1)
z_1 = ((1-e2)*nu/F0 +H)*sin(lat_1)
#Perform Helmut transform (to go between Airy 1830 (_1) and GRS80 (_2))
s = -20.4894*10**-6 #The scale factor -1
tx, ty, tz = 446.448, -125.157, + 542.060 #The translations along x,y,z axes respectively
rxs,rys,rzs = 0.1502, 0.2470, 0.8421 #The rotations along x,y,z respectively, in seconds
rx, ry, rz = rxs*pi/(180*3600.), rys*pi/(180*3600.), rzs*pi/(180*3600.) #In radians
x_2 = tx + (1+s)*x_1 + (-rz)*y_1 + (ry)*z_1
y_2 = ty + (rz)*x_1 + (1+s)*y_1 + (-rx)*z_1
z_2 = tz + (-ry)*x_1 + (rx)*y_1 + (1+s)*z_1
#Back to spherical polar coordinates from cartesian
#Need some of the characteristics of the new ellipsoid
a_2, b_2 =6378137.000, 6356752.3141 #The GSR80 semi-major and semi-minor axes used for WGS84(m)
e2_2 = 1- (b_2*b_2)/(a_2*a_2) #The eccentricity of the GRS80 ellipsoid
p = sqrt(x_2**2 + y_2**2)
#Lat is obtained by an iterative proceedure:
lat = arctan2(z_2,(p*(1-e2_2))) #Initial value
latold = 2*pi
while abs(lat - latold)>10**-16:
lat, latold = latold, lat
nu_2 = a_2/sqrt(1-e2_2*sin(latold)**2)
lat = arctan2(z_2+e2_2*nu_2*sin(latold), p)
#Lon and height are then pretty easy
lon = arctan2(y_2,x_2)
H = p/cos(lat) - nu_2
#Uncomment this line if you want to print the results
#print [(lat-lat_1)*180/pi, (lon - lon_1)*180/pi]
#Convert to degrees
lat = lat*180/pi
lon = lon*180/pi
#Job's a good'n.
return lat, lon
def edge_probability(self, u, v):
""" Probability of edge in the similarity graph based on geo-damped Lin similarity.
Edge probability consists of two independent contributions:
- probability induced by graphical distance between agents
- probability induced by similarity between agents
Parameters
----------
u, v : int
Indices of vertices
"""
# Compute contribution of geo-attributes to the edge probability.
# Get vectors with (lon,lat)-pairs of all locations for 2 nodes.
geo_attrs_u = self.geo_attrs[u]
geo_attrs_v = self.geo_attrs[v]
# Compute minimum geo-distance between locations of a and b
# NOTE: For the moment, it selects the closest distance for
# matching types of locations (e.g., between 2 households,
# but not between household of `a` and workplace of `b`)
min_dist = numpy.PINF
for i in xrange(0, len(geo_attrs_u), 2):
dlon = geo_attrs_u[i] - geo_attrs_v[i]
# TODO: precompute sines and cosines before computing individual edge probabilities
lat1, lat2 = geo_attrs_u[i + 1], geo_attrs_v[i + 1]
cos_lat1, cos_lat2, cos_dlon = numpy.cos(lat1), numpy.cos(
lat2), numpy.cos(dlon)
sin_lat1, sin_lat2, sin_dlon = numpy.sin(lat1), numpy.sin(
lat2), numpy.sin(dlon)
y = sqrt((cos_lat2 * sin_dlon)**2 +
(cos_lat1 * sin_lat2 - sin_lat1 * cos_lat2 * cos_dlon)**2)
x = sin_lat1 * sin_lat2 + cos_lat1 * cos_lat2 * cos_dlon
# TODO: clarify about negative distances
min_dist = min(min_dist, arctan2(y, x))
if self.damping > 0.:
# Scale distance by half-similarity scale to make it adimensional and damp by a factor 2.
prob_geo = (1. + self.geo_scaling * min_dist)**(-self.damping)
else:
# Compute geo-similarity with exponential damping.
prob_geo = 2**(-self.geo_scaling * min_dist)
# logging.debug( "minimum distance rho({0},{1})={2}; geo-induced probability Pr({0},{1})={3}".\
# format(a, b, min_dist, prob_geo) )
# If probability induced by the geo-attributes is smaller than a certain lower bound threshold,
# the Lin similarity contribution is disregarded.
if prob_geo <= self.similarity_threshold:
return 0.
# Compute contribution of non-geographic attributes to the edge probability.
# This contribution is based on Lin similarity metric.
# Lin similarity handles categorical ('c') and ordinal ('o') attributes
# (while geographic ('g') attributes are subject of geo-damping).
# In order to reduce calculation time, code below evaluates
# the similarity between node `a` and node `b` on the sampled attributes.
# First, find the frequency of agents sharing all attributes shared by the two analysed nodes.
# The idea is that the lower the number of agents sharing the same attribute,
# the more information this shared attribute contains about the two agents.
# Since attributes are dependent, one must estimate the probability (get frequency)
# of observing all different attributes at the same time. If they were independent,
# one might handle attributes independently by computing their contributions separately
# and summing up.
attrs_u, attrs_v = self.nongeo_attrs[u], self.nongeo_attrs[v]
# Select nodes as similar if categorical attributes are the same to `a` and `b`
# If vertices share categorical attribute, the number of agents sharing this attribute is considered.
# TODO: ensure that we do not have to cut of by categorical_attrs
similar_nodes = True # start with all sampled items as similar
for sample, attr_u, attr_v in islice(
izip(self.sampled_nongeo_attrs, attrs_u, attrs_v),
self.num_categorical):
if attr_u == attr_v:
# filter out indices of samples with the same attribute
similar_nodes &= sample == attr_u
# logging.debug( "similar categorical attributes in the sample: {0} out of {1}".\
# format(numpy.sum(similar_nodes) if not isinstance(similar_nodes, bool) else "all",
# self.sample_size) )
# Select nodes as similar if ordinal attributes are between values for `a` and `b`.
# If vertices do not share an attribute and the attribute is ordinal,
# the number of agents sharing attributes between the two values is considered.
for sample, attr_u, attr_v in islice(
izip(self.sampled_nongeo_attrs, attrs_u, attrs_v),
self.num_categorical, self.num_categorical + self.num_ordinal):
attr_min, attr_max = min(attr_u, attr_v), max(attr_u, attr_v)
# filter out indices of samples with the attribute in the range of values between `a` and `b`
similar_nodes &= (attr_min <= sample) & (sample <= attr_max)
num_similar = numpy.sum(similar_nodes) if not isinstance(
similar_nodes, bool) else self.sample_size
# logging.debug( "similar attributes in the sample after ordinal attributes filtering: {0} out of {1}".\
# format(num_similar, self.sample_size) )
# If the superposition is zero on the sample, then there is no agents with the same characteristics.
# The probability of finding something with the same feature of both `a` and `b` is really small.
if num_similar == 0:
return prob_geo # TODO: consult why not zero
# Second, find the frequency of agents sharing all attributes with each analysed node separately.
# TODO: clarify whether we need to take geo-filtering into account as in original script
# NOTE: `numpy.sum` performs better than `sum` on numpy-arrays
num_equal_u = numpy.sum(
self.sampled_vertex_attrs == self.vertex_attrs[u])
num_equal_v = numpy.sum(
self.sampled_vertex_attrs == self.vertex_attrs[v])
# logging.debug( "similar attributes (in the sample) to the 1st vertex: {0}, to the 2nd vertex: {1}".\
# format(num_equal_u, num_equal_v) )
# Compute Lin similarity (use inverses of frequencies estimated above)
num_sample = float(self.sample_size)
num_total = len(self)
prob_lin = log2(num_sample / num_similar)
if num_equal_u == 0: # there is no agents as `a`
# We make assumption that `a` is the only one with this characteristic in the whole dataset.
if num_equal_v == 0: # the same thing for `b`
prob_lin /= log2(num_total)
else: # `a` is unique, but `b` has similar vertices (agents) in the dataset (population)
prob_lin /= 0.5 * log2(num_sample * num_total / num_equal_v)
elif num_equal_v == 0: # `b` is unique, but `a` has similar vertices (agents) in the dataset (population)
prob_lin /= (log2(num_sample * num_total / num_equal_u))
else: # both `a` and `b` have similar vertices (agents) in the dataset (population)
prob_lin /= log2(num_sample * num_sample /
(num_equal_u * num_equal_v))
return prob_geo * prob_lin
