def earth_distance(lat_lng1, lat_lng2):
"""
Compute the distance (in km) along earth between two latitude and longitude pairs
Parameters
----------
lat_lng1: tuple
the first latitude and longitude pair
lat_lng2: tuple
the second latitude and longitude pair
Returns
-------
float
the distance along earth in km
"""
lat1, lng1 = [l*pi/180 for l in lat_lng1]
lat2, lng2 = [l*pi/180 for l in lat_lng2]
dlat, dlng = lat1-lat2, lng1-lng2
ds = 2 * asin(sqrt(sin(dlat/2.0) ** 2 + cos(lat1) * cos(lat2) * sin(dlng/2.0) ** 2))
return 6371.01 * ds # spherical earth...
python类asin()的实例源码
def eci_to_latlon(pos, phi_0=0):
(x, y, z) = pos
rg = (x*x + y*y + z*z)**0.5
z = z/rg
if abs(z) > 1.0:
z = int(z)
lat = degrees(asin(z))
lon = degrees(atan2(y, x)-phi_0)
if lon > 180:
lon -= 360
elif lon < -180:
lon += 360
assert -90 <= lat <= 90
assert -180 <= lon <= 180
return lat, lon
def distance(self, loc):
"""
Calculate the great circle distance between two points
on the earth (specified in decimal degrees)
"""
assert type(loc) == type(self)
# convert decimal degrees to radians
lon1, lat1, lon2, lat2 = map(radians, [
self.lon,
self.lat,
loc.lon,
loc.lat,
])
# haversine formula
dlon = lon2 - lon1
dlat = lat2 - lat1
a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2
c = 2 * asin(sqrt(a))
r = 6371000 # Radius of earth in meters.
return c * r
def distance(pointA, pointB):
"""
Calculate the great circle distance between two points
on the earth (specified in decimal degrees)
http://stackoverflow.com/questions/15736995/how-can-i-quickly-estimate-the-distance-between-two-latitude-longitude-points
"""
# convert decimal degrees to radians
lon1, lat1, lon2, lat2 = map(math.radians, [pointA[1], pointA[0], pointB[1], pointB[0]])
# haversine formula
dlon = lon2 - lon1
dlat = lat2 - lat1
a = math.sin(dlat/2)**2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon/2)**2
c = 2 * math.asin(math.sqrt(a))
r = 3956 # Radius of earth in miles. Use 6371 for kilometers
return c * r
def winding_angle(self, path, point):
wa = 0
for i in range(len(path)-1):
p = np.array([path[i].x, path[i].y])
pn = np.array([path[i+1].x, path[i+1].y])
vp = p - point
vpn = pn - point
vp_norm = sqrt(vp[0]**2 + vp[1]**2)
vpn_norm = sqrt(vpn[0]**2 + vpn[1]**2)
assert (vp_norm > 0)
assert (vpn_norm > 0)
z = np.cross(vp, vpn)/(vp_norm * vpn_norm)
z = min(max(z, -1.0), 1.0)
wa += asin(z)
return wa
def compute_all_ic_rels_with_threshold(threshold):
"""
Computes all intercity relations for documents classified with higher probability
than the given threshold.
:param threshold: The minimum probability
:result: A list of dictionaries with city_a, city_b, category and score fields.
"""
query = '''
UNWIND [{0}] AS category
MATCH (a:City)-[:{1}]->(i:Index)<-[:{1}]-(b:City)
WHERE ID(a) < ID(b) AND a.name <> b.name AND i[category] >= {2}
MATCH (a)-[r:{3}]->(b)
WITH a.name AS city_a, a.population AS pop_a, b.name AS city_b,
b.population AS pop_b, r.total AS total, COUNT(i) AS count, category,
2 * 6371 * asin(sqrt(haversin(radians(a.latitude - b.latitude)) +
cos(radians(a.latitude)) * cos(radians(b.latitude)) *
haversin(radians(a.longitude - b.longitude)))) AS dist
RETURN city_a, pop_a, city_b, pop_b, dist, total, category, SUM(count) AS score
'''.format(','.join('"{}"'.format(c) for c in CAT_NO_OTHER), OCCURS_IN, threshold, RELATES_TO)
return perform_query(query, [], access_mode='read')
def gps_newpos(lat, lon, bearing, distance):
"""Extrapolate latitude/longitude given a heading and distance
thanks to http://www.movable-type.co.uk/scripts/latlong.html .
"""
from math import sin, asin, cos, atan2, radians, degrees
lat1 = radians(lat)
lon1 = radians(lon)
brng = radians(bearing)
dr = distance / radius_of_earth
lat2 = asin(sin(lat1) * cos(dr) +
cos(lat1) * sin(dr) * cos(brng))
lon2 = lon1 + atan2(sin(brng) * sin(dr) * cos(lat1),
cos(dr) - sin(lat1) * sin(lat2))
return (degrees(lat2), degrees(lon2))
def haversine(self, lat1, lon1, lat2, lon2):
"""
Calculate the great circle distance between two points
on the earth (specified in decimal degrees)
"""
# convert decimal degrees to radians
lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
# haversine formula
dlon = lon2 - lon1
dlat = lat2 - lat1
a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2
c = 2 * asin(sqrt(a))
# 6367 km is the radius of the Earth
km = 6367 * c
return km
# might extend capability to restrict towns to network boundary
def haversine(self, lat1, lon1, lat2, lon2):
"""
Calculate the great circle distance between two points
on the earth (specified in decimal degrees)
"""
# convert decimal degrees to radians
lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
# haversine formula
dlon = lon2 - lon1
dlat = lat2 - lat1
a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2
c = 2 * asin(sqrt(a))
# 6367 km is the radius of the Earth
km = 6367 * c
return km
def haversine(self, lat1, lon1, lat2, lon2):
"""
Calculate the great circle distance between two points
on the earth (specified in decimal degrees)
"""
# convert decimal degrees to radians
lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
# haversine formula
dlon = lon2 - lon1
dlat = lat2 - lat1
a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2
c = 2 * asin(sqrt(a))
# 6367 km is the radius of the Earth
km = 6367 * c
return km
def spherical_distance(a, b):
lat1 = a[1]
lon1 = a[0]
lat2 = b[1]
lon2 = b[0]
R = 6371000.0
rlon1 = lon1 * math.pi / 180.0
rlon2 = lon2 * math.pi / 180.0
rlat1 = lat1 * math.pi / 180.0
rlat2 = lat2 * math.pi / 180.0
dlon = (rlon1 - rlon2) / 2.0
dlat = (rlat1 - rlat2) / 2.0
lat12 = (rlat1 + rlat2) / 2.0
sindlat = math.sin(dlat)
sindlon = math.sin(dlon)
cosdlon = math.cos(dlon)
coslat12 = math.cos(lat12)
f = sindlat * sindlat * cosdlon * cosdlon + sindlon * sindlon * coslat12 * coslat12
f = math.sqrt(f)
f = math.asin(f) * 2.0 # the angle between the points
f *= R
return f
def coords_down_bearing(lat, lon, bearing, distance, body):
'''
Takes a latitude, longitude and bearing in degrees, and a
distance in meters over a given body. Returns a tuple
(latitude, longitude) of the point you've calculated.
'''
bearing = math.radians(bearing)
R = body.equatorial_radius
lat = math.radians(lat)
lon = math.radians(lon)
lat2 = math.asin( math.sin(lat)*math.cos(distance/R) +
math.cos(lat)*math.sin(distance/R)*math.cos(bearing))
lon2 = lon + math.atan2(math.sin(bearing)*math.sin(distance/R
)*math.cos(lat),math.cos(distance/R)-math.sin(lat
)*math.sin(lat2))
lat2 = math.degrees(lat2)
lon2 = math.degrees(lon2)
return (lat2, lon2)
def rpy(self):
acc = self.acceleration()
yaw = self.yaw()
norm = np.linalg.norm(acc)
# print(acc)
if norm < 1e-6:
return (0.0, 0.0, yaw)
else:
thrust = acc + np.array([0, 0, 9.81])
z_body = thrust / np.linalg.norm(thrust)
x_world = np.array([math.cos(yaw), math.sin(yaw), 0])
y_body = np.cross(z_body, x_world)
x_body = np.cross(y_body, z_body)
pitch = math.asin(-x_body[2])
roll = math.atan2(y_body[2], z_body[2])
return (roll, pitch, yaw)
# "private" methods
def haversine(lon1, lat1, lon2, lat2):
"""
Calculate the great circle distance between two points
on the earth (specified in decimal degrees)
Taken from: http://stackoverflow.com/questions/4913349/haversine-formula-in-python-bearing-and-distance-between-two-gps-points
"""
# convert decimal degrees to radians
lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
# haversine formula
dlon = lon2 - lon1
dlat = lat2 - lat1
a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2
c = 2 * asin(sqrt(a))
km = 6367 * c
return km
def haversine(lon1, lat1, lon2, lat2):
"""
Calculate the great circle distance between two points
on the earth (specified in decimal degrees)
Taken from: http://stackoverflow.com/questions/4913349/haversine-formula-in-python-bearing-and-distance-between-two-gps-points
"""
# convert decimal degrees to radians
lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
# haversine formula
dlon = lon2 - lon1
dlat = lat2 - lat1
a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2
c = 2 * asin(sqrt(a))
km = 6367 * c
return km
def haversine(lat1, lon1, lat2, lon2):
"""
Calculate the great circle distance between two points
on the earth (specified in decimal degrees)
"""
# convert decimal degrees to radians
lat1, lon1, lat2, lon2 = map(radians, [lat1, lon1, lat2, lon2])
# haversine formula
dlon = lon2 - lon1
dlat = lat2 - lat1
a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2
c = 2 * asin(sqrt(a))
r = 3956 # Radius of earth in miles
return c * r
def point_from_distance_and_angle(point: CoordinatePoint, distance_in_m: float,
angle_in_degrees: int) -> CoordinatePoint:
"""Calculates a point with the given angle and distance meters away from the given point"""
lat1 = math.radians(point.latitude)
lon1 = math.radians(point.longitude)
distance = distance_in_m / 1000 / 6371
angle = math.radians(angle_in_degrees)
lat2 = math.asin(math.sin(lat1) * math.cos(distance) +
math.cos(lat1) * math.sin(distance) * math.cos(angle))
lon2 = lon1 + math.asin((math.sin(distance) /
math.cos(lat2)) * math.sin(angle))
return CoordinatePoint(latitude=math.degrees(lat2), longitude=math.degrees(lon2))
def get_euler(self):
t = self.x * self.y + self.z * self.w
if t > 0.4999:
heading = 2 * math.atan2(self.x, self.w)
attitude = math.pi / 2
bank = 0
elif t < -0.4999:
heading = -2 * math.atan2(self.x, self.w)
attitude = -math.pi / 2
bank = 0
else:
sqx = self.x ** 2
sqy = self.y ** 2
sqz = self.z ** 2
heading = math.atan2(2 * self.y * self.w - 2 * self.x * self.z,
1 - 2 * sqy - 2 * sqz)
attitude = math.asin(2 * t)
bank = math.atan2(2 * self.x * self.w - 2 * self.y * self.z,
1 - 2 * sqx - 2 * sqz)
return heading, attitude, bank
def get_cycle(self, lat, lng, rad):
# unit of radius: meter
cycle = []
d = (rad / 1000.0) / 6378.8
lat1 = (math.pi / 180.0) * lat
lng1 = (math.pi / 180.0) * lng
r = [x * 10 for x in range(36)]
for a in r:
tc = (math.pi / 180.0) * a
y = math.asin(
math.sin(lat1) * math.cos(d) + math.cos(lat1) * math.sin(d) * math.cos(tc))
dlng = math.atan2(math.sin(
tc) * math.sin(d) * math.cos(lat1), math.cos(d) - math.sin(lat1) * math.sin(y))
x = ((lng1 - dlng + math.pi) % (2.0 * math.pi)) - math.pi
cycle.append(
(float(y * (180.0 / math.pi)), float(x * (180.0 / math.pi))))
return cycle
def quat2euler(q):
x, y, z, w = q.ravel()
ysqr = y * y
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + ysqr)
X = atan2(t0, t1)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
Y = asin(t2)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (ysqr + z * z)
Z = atan2(t3, t4)
return np.array([[X], [Y], [Z]])
def getTiltHeading(self):
magValue=self.getMag()
accelValue=self.getRealAccel()
X=self.X
Y=self.Y
Z=self.Z
pitch = math.asin(-accelValue[X])
print(accelValue[Y],pitch,math.cos(pitch),accelValue[Y]/math.cos(pitch),math.asin(accelValue[Y]/math.cos(pitch)))
roll = math.asin(accelValue[Y]/math.cos(pitch))
xh = magValue[X] * math.cos(pitch) + magValue[Z] * math.sin(pitch)
yh = magValue[X] * math.sin(roll) * math.sin(pitch) + magValue[Y] * math.cos(roll) - magValue[Z] * math.sin(roll) * math.cos(pitch)
zh = -magValue[X] * (roll) * math.sin(pitch) + magValue[Y] * math.sin(roll) + magValue[Z] * math.cos(roll) * math.cos(pitch)
heading = 180 * math.atan2(yh, xh)/math.pi
if (yh >= 0):
return heading
else:
return (360 + heading)
def __ComputeCurved(vpercent, w, vec, via, pts, segs):
"""Compute the curves part points"""
radius = via[1]/2.0
# Compute the bezier middle points
req_angle = asin(vpercent/100.0)
oppside = tan(req_angle)*(radius-(w/sin(req_angle)))
length = sqrt(radius*radius + oppside*oppside)
d = req_angle - acos(radius/length)
vecBC = [vec[0]*cos(d)+vec[1]*sin(d), -vec[0]*sin(d)+vec[1]*cos(d)]
pointBC = via[0] + wxPoint(int(vecBC[0] * length), int(vecBC[1] * length))
d = -d
vecAE = [vec[0]*cos(d)+vec[1]*sin(d), -vec[0]*sin(d)+vec[1]*cos(d)]
pointAE = via[0] + wxPoint(int(vecAE[0] * length), int(vecAE[1] * length))
curve1 = __Bezier(pts[1], pointBC, pts[2], n=segs)
curve2 = __Bezier(pts[4], pointAE, pts[0], n=segs)
return curve1 + [pts[3]] + curve2
def haversine(pt_a, pt_b):
"""
`pt_a` and `pt_b` are tuples with two float numbers each, i.e.
representing lng/lat pairs:
(99,100) (-42, 85)
The geo distance between the two points is returned in kilometers
"""
from math import radians, cos, sin, asin, sqrt
lng_a = radians(float(pt_a[0]))
lat_a = radians(float(pt_a[1]))
lng_b = radians(float(pt_b[0]))
lat_b = radians(float(pt_b[1]))
# haversine formula
dlng = lng_b - lng_a
dlat = lat_b - lat_a
whatevs = sin(dlat /2 ) ** 2 + cos(lat_a) * cos(lat_b) * sin(dlng / 2) ** 2
c = 2 * asin(sqrt(whatevs))
r = 6371 # Radius of earth in kilometers.
# return the final calculation
return c * r
def cvt_inert2att_Q_to_angles(Q):
"""Creates a angle tuple from Q, assuming an inertial to attitude Q"""
V1_body = Vector(1.,0.,0.)
V1_eci_pt = Q.inv_cnvrt(V1_body)
coord1 = math.atan2(V1_eci_pt.y,V1_eci_pt.x)
if coord1 < 0. : coord1 += PI2
coord2 = math.asin(unit_limit(V1_eci_pt.z))
V3_body = Vector(0.,0.,1.)
V3_eci_pt = Q.inv_cnvrt(V3_body)
NP_eci = Vector(0.,0.,1.)
V_left = cross(NP_eci,V1_eci_pt)
V_left = V_left / V_left.length()
NP_in_plane = cross(V1_eci_pt,V_left)
x = dot(V3_eci_pt,NP_in_plane)
y = dot(V3_eci_pt,V_left)
pa = math.atan2(y,x)
if pa < 0. : pa += PI2
return (coord1,coord2,pa)
def cvt_v2v3_using_body2inertial_Q_to_c1c2pa_tuple(Q,v2,v3):
"""Given Q and v2,v3 gives pos on sky and V3 PA """
Vp_body = Vector(0.,0.,0.)
Vp_body.set_xyz_from_angs(v2,v3)
Vp_eci_pt = Q.cnvrt(Vp_body)
coord1 = atan2(Vp_eci_pt.y,Vp_eci_pt.x)
if coord1 < 0. : coord1 += PI2
coord2 = asin(unit_limit(Vp_eci_pt.z))
V3_body = Vector(0.,0.,1.)
V3_eci_pt = Q.cnvrt(V3_body)
NP_eci = Vector(0.,0.,1.)
V_left = cross(NP_eci,Vp_eci_pt)
if V_left.length()>0.:
V_left = V_left/V_left.length()
NP_in_plane = cross(Vp_eci_pt,V_left)
x = dot(V3_eci_pt,NP_in_plane)
y = dot(V3_eci_pt,V_left)
pa = atan2(y,x)
if pa < 0. : pa += PI2
return (coord1,coord2,pa)
def cvt_c1c2_using_body2inertial_Q_to_v2v3pa_tuple(Q,coord1,coord2):
"""Given Q and a position, returns v2,v3,V3PA tuple """
Vp_eci = Vector(1.,0.,0.)
Vp_eci.set_xyz_from_angs(coord1,coord2)
Vp_body_pt = Q.inv_cnvrt(Vp_eci)
v2 = atan2(Vp_body_pt.y,Vp_body_pt.x)
v3 = asin(unit_limit(Vp_body_pt.z))
V3_body = Vector(0.,0.,1.)
V3_eci_pt = self.cnvrt(V3_body)
NP_eci = Vector(0.,0.,1.)
V_left = cross(NP_eci,Vp_eci)
if V_left.length()>0.:
V_left = V_left / V_left.length()
NP_in_plane = cross(Vp_eci,V_left)
x = dot(V3_eci_pt,NP_in_plane)
y = dot(V3_eci_pt,V_left)
pa = atan2(y,x)
if pa < 0. : pa += PI2
return (v2,v3,pa)
############################################################
def add_kapla_tower(scn, width, height, length, radius, level_count: int, x=0, y=0, z=0):
"""Create a Kapla tower, return a list of created nodes"""
tower = []
level_y = y + height / 2
for i in range(level_count // 2):
def fill_ring(r, ring_y, size, r_adjust, y_off):
step = asin((size * 1.01) / 2 / (r - r_adjust)) * 2
cube_count = (2 * pi) // step
error = 2 * pi - step * cube_count
step += error / cube_count # distribute error
a = 0
while a < (2 * pi - error):
world = gs.Matrix4.TransformationMatrix(gs.Vector3(cos(a) * r + x, ring_y, sin(a) * r + z), gs.Vector3(0, -a + y_off, 0))
tower.append(plus.AddPhysicCube(scn, world, width, height, length, 2)[0])
a += step
fill_ring(radius - length / 2, level_y, width, length / 2, pi / 2)
level_y += height
fill_ring(radius - length + width / 2, level_y, length, width / 2, 0)
fill_ring(radius - width / 2, level_y, length, width / 2, 0)
level_y += height
return tower
def add_kapla_tower(scn, width, height, length, radius, level_count: int, x=0, y=0, z=0):
"""Create a Kapla tower, return a list of created nodes"""
level_y = y + height / 2
for i in range(level_count // 2):
def fill_ring(r, ring_y, size, r_adjust, y_off):
step = asin((size * 1.01) / 2 / (r - r_adjust)) * 2
cube_count = (2 * pi) // step
error = 2 * pi - step * cube_count
step += error / cube_count # distribute error
a = 0
while a < (2 * pi - error):
world = gs.Matrix4.TransformationMatrix((cos(a) * r + x, ring_y, sin(a) * r + z), (0, -a + y_off, 0))
plus.AddPhysicCube(scn, world, width, height, length, 2)
a += step
fill_ring(radius - length / 2, level_y, width, length / 2, pi / 2)
level_y += height
fill_ring(radius - length + width / 2, level_y, length, width / 2, 0)
fill_ring(radius - width / 2, level_y, length, width / 2, 0)
level_y += height
def intersectChordCircle(C, P, chord_length):
'''
Accepts center of circle, a point on the circle, and chord length.
Returns a list of two points on the circle at chord_length distance away from original point
'''
C = dPnt(C)
P = dPnt(P)
d = chord_length
r = distance(C, P)
# point on circle given chordlength & starting point=2*asin(d/2r)
d_div_2r = d / (2.0 * r)
angle = 2 * asin(d_div_2r)
P = []
P.append(polar(C, r, angle))
P.append(polar(C, r, - angle))
return P
#def tangentOnCircleFromPoint(C, r, P):
def getcycle(self,rpoint):
cycle = []
lat = rpoint[0]
lng = rpoint[1]
rad = rpoint[2] #unit: meter
d = (rad/1000.0)/6378.8;
lat1 = (math.pi/180.0)* lat
lng1 = (math.pi/180.0)* lng
r = [x*30 for x in range(12)]
for a in r:
tc = (math.pi/180.0)*a;
y = math.asin(math.sin(lat1)*math.cos(d)+math.cos(lat1)*math.sin(d)*math.cos(tc))
dlng = math.atan2(math.sin(tc)*math.sin(d)*math.cos(lat1),math.cos(d)-math.sin(lat1)*math.sin(y))
x = ((lng1-dlng+math.pi) % (2.0*math.pi)) - math.pi
cycle.append( ( float(y*(180.0/math.pi)),float(x*(180.0/math.pi)) ) )
return cycle
