def __init__(self, name='', tx=0, ty=0, tz=0, sx=0, sy=0, sz=0, s=0):
'''New transform.
@keyword name: Optional, unique name (string).
@keyword tx: X translation (meter).
@keyword ty: Y translation (meter).
@keyword tz: Z translation (meter).
@keyword s: Scale ppm (float).
@keyword sx: X rotation (degree seconds).
@keyword sy: Y rotation (degree seconds).
@keyword sz: Z rotation (degree seconds).
@raise NameError: If transform name already exists.
'''
if tx:
self.tx = float(tx)
if ty:
self.ty = float(ty)
if tz:
self.tz = float(tz)
if sx: # secs to rads
self.rx = radians(sx / 3600.0)
self.sx = sx
if sy:
self.ry = radians(sy / 3600.0)
self.sy = sy
if sz:
self.rz = radians(sz / 3600.0)
self.sz = sz
if s:
self.s = float(s)
self.s1 = s * 1.e-6 + 1 # normalize ppm to (s + 1)
self._register(Transforms, name)
def _toZBll(lat, lon):
'''(INTERNAL) Return zone, Band and central lat- and longitude.
@param lat: Latitude (degrees).
@param lon: Longitude (degrees).
@return: 4-Tuple (zone, Band, lat, lon).
'''
# return zone, Band and central
# lat- and longitude (in radians)
lat = wrap90(lat)
if -80 > lat or lat > 84:
raise ValueError('%s outside UTM: %s' % ('lat', lat))
B = _Bands[int(lat + 80) >> 3]
lon = wrap180(lon)
z = int((lon + 180) / 6) + 1 # longitudinal zone
if B == 'X':
x = {32: 9, 34: 21, 36: 33}.get(z, None)
if x: # Svalbard
if lon >= x:
z += 1
else:
z -= 1
elif B == 'V' and z == 31 and lon >= 3:
z += 1 # southern Norway
b = radians(lon - _cmlon(z)) # lon off central meridian
a = radians(lat) # lat off equator
return z, B, a, b
def __init__(self,
latlon0,
par1,
par2=None,
E0=0,
N0=0,
k0=1,
opt3=0,
name='',
auth=''):
'''New Lambert conformal conic projection.
@param latlon0: Origin including an ellipsoidal datum (LatLon).
@param par1: First standard parallel (degrees90).
@keyword par2: Second standard parallel (degrees90).
@keyword E0: False easting in meter (scalar).
@keyword N0: False northing in meter (scalar).
@keyword k0: Scale factor (scalar).
@keyword opt3: Optional meridian (degrees180).
@keyword name: Name of the conic (string).
@keyword auth: Authentication authority (string).
@return: A Lambert projection (L{Conic}).
@example:
>>> from pygeodesy import Conic, Datums, ellipsoidalNvector
>>> ll0 = ellipsoidalNvector.LatLon(23, -96, datum=Datums.NAD27)
>>> Snyder = Conic(ll0, 33, 45, E0=0, N0=0, name='Snyder')
'''
if latlon0 is not None:
if not isinstance(latlon0, _LL):
raise TypeError('%s not %s: %r' %
('latlon0', 'ellipsoidal', latlon0))
self._lat0, self._lon0 = latlon0.to2ab()
self._par1 = radians(par1)
if par2 is None:
self._par2 = self._par1
else:
self._par2 = radians(par2)
self._opt3 = radians(opt3)
if k0 != 1:
self._k0 = float(k0)
if N0:
self._N0 = float(N0)
if E0:
self._E0 = float(E0)
self.toDatum(latlon0.datum)._dup2(self)
self._register(Conics, name)
elif name:
self._name = name
if auth:
self._auth = auth
def _rhumb3(self, other):
'''(INTERNAL) Rhumb_ helper function.
@param other: The other point (spherical LatLon).
'''
self.others(other)
a1 = radians(self.lat)
a2 = radians(other.lat)
# if |db| > 180 take shorter rhumb
# line across the anti-meridian
db = radiansPI(other.lon - self.lon)
ds = log(tanPI_2_2(a2) / tanPI_2_2(a1))
return (a2 - a1), db, ds
def crossingParallels(self, other, lat, wrap=False):
'''Return the pair of meridians at which a great circle defined
by this and an other point crosses the given latitude.
@param other: The other point defining great circle (L{LatLon}).
@param lat: Latitude at the crossing (degrees).
@keyword wrap: Wrap and unroll longitudes (bool).
@return: 2-Tuple (lon1, lon2) in (degrees180) or None if the
great circle doesn't reach the given I{lat}.
'''
self.others(other)
a1, b1 = self.to2ab()
a2, b2 = other.to2ab()
a = radians(lat)
db, b2 = unrollPI(b1, b2, wrap=wrap)
ca, ca1, ca2, cdb = map1(cos, a, a1, a2, db)
sa, sa1, sa2, sdb = map1(sin, a, a1, a2, db)
x = sa1 * ca2 * ca * sdb
y = sa1 * ca2 * ca * cdb - ca1 * sa2 * ca
z = ca1 * ca2 * sa * sdb
h = hypot(x, y)
if h < EPS or abs(z) > h:
return None # great circle doesn't reach latitude
m = atan2(-y, x) + b1 # longitude at max latitude
d = acos(z / h) # delta longitude to intersections
return degrees180(m - d), degrees180(m + d)
def rhumbDistanceTo(self, other, radius=R_M):
'''Returns distance from this to an other point along
a rhumb (loxodrome) line.
@param other: The other point (spherical LatLon).
@keyword radius: Mean radius of earth (scalar, default meter).
@return: Distance (in the same units as radius).
@raise TypeError: The other point is not spherical.
@example:
>>> p = LatLon(51.127, 1.338)
>>> q = LatLon(50.964, 1.853)
>>> d = p.rhumbDistanceTo(q) # 403100
'''
# see http://williams.best.vwh.net/avform.htm#Rhumb
da, db, ds = self._rhumb3(other)
# on Mercator projection, longitude distances shrink
# by latitude; the 'stretch factor' q becomes ill-
# conditioned along E-W line (0/0); use an empirical
# tolerance to avoid it
if abs(da) < EPS:
q = cos(radians(self.lat))
else:
q = da / ds
return float(radius) * hypot(da, q * db)
def _r3(a, f):
'''(INTERNAL) Reduced cos, sin, tan.
'''
t = (1 - f) * tan(radians(a))
c = 1 / hypot(1, t)
s = t * c
return c, s, t
def _trackDistanceTo3(self, start, end, radius, wrap):
'''(INTERNAL) Helper for along-/crossTrackDistanceTo.
'''
self.others(start, name='start')
self.others(end, name='end')
r = float(radius)
if r < EPS:
raise ValueError('%s invalid: %r' % ('radius', radius))
r = start.distanceTo(self, r, wrap=wrap) / r
b = radians(start.initialBearingTo(self, wrap=wrap))
e = radians(start.initialBearingTo(end, wrap=wrap))
x = asin(sin(r) * sin(b - e))
return r, x, e - b
def greatCircle(self, bearing):
'''Compute the vector normal to great circle obtained by heading
on the given initial bearing from this point.
Direction of vector is such that initial bearing vector
b = c × n, where n is an n-vector representing this point.
@param bearing: Bearing from this point (compass degrees).
@return: Vector representing great circle (L{Vector3d}).
@example:
>>> p = LatLon(53.3206, -1.7297)
>>> g = p.greatCircle(96.0)
>>> g.toStr() # (-0.794, 0.129, 0.594)
'''
a, b = self.to2ab()
t = radians(bearing)
ca, cb, ct = map1(cos, a, b, t)
sa, sb, st = map1(sin, a, b, t)
return Vector3d(sb * ct - cb * sa * st, -cb * ct - sb * sa * st,
ca * st) # XXX .unit()?
def destination(self, distance, bearing, radius=R_M, height=None):
'''Locate the destination from this point after having
travelled the given distance on the given initial bearing.
@param distance: Distance travelled (same units as radius).
@param bearing: Bearing from this point (compass degrees).
@keyword radius: Optional, mean earth radius (meter).
@keyword height: Optional height at destination (meter).
@return: Destination point (L{LatLon}).
@example:
>>> p1 = LatLon(51.4778, -0.0015)
>>> p2 = p1.destination(7794, 300.7)
>>> p2.toStr() # '51.5135°N, 000.0983°W'
@JSname: I{destinationPoint}.
'''
a, b = self.to2ab()
r = float(distance) / float(radius) # angular distance in radians
t = radians(bearing)
a, b = _destination2(a, b, r, t)
h = self.height if height is None else height
return self.classof(a, b, height=h)
def _cosine_single_sample(self, target, output):
"""
Calculates cosine loss for a single sample.
Arguments:
target: Target value at which loss is evaluated of shape (1, 3)
output: Output value, shape (1, 6) from predicted from network
prior to normalization of each sin/cos pair.
Returns:
loss: The loss of a single sample.
"""
radian_target = radians(target)
radian_target_cos = torch.cos(radian_target)
radian_target_sin = torch.sin(radian_target)
target_biternion = []
for i in range(3):
target_biternion.append(radian_target_cos[i])
target_biternion.append(radian_target_sin[i])
target = torch.tensor(target_biternion)
if output.is_cuda:
device = output.get_device()
target = target.to(device)
angles = output_to_angles(output)
return 3 - torch.dot(angles, target)
def _inverse(self, other, azis):
'''(INTERNAL) Inverse Vincenty method.
@raise TypeError: The other point is not L{LatLon}.
@raise ValueError: If this and the other point's L{Datum}
ellipsoids are not compatible.
@raise VincentyError: Vincenty fails to converge for the current
L{LatLon.epsilon} and L{LatLon.iterations}
limit or this and the other point coincide.
'''
E = self.ellipsoids(other)
c1, s1, _ = _r3(self.lat, E.f)
c2, s2, _ = _r3(other.lat, E.f)
c1c2, s1s2 = c1 * c2, s1 * s2
c1s2, s1c2 = c1 * s2, s1 * c2
ll = dl = radians(other.lon - self.lon)
for _ in range(self._iterations):
cll, sll, ll_ = cos(ll), sin(ll), ll
ss = hypot(c2 * sll, c1s2 - s1c2 * cll)
if ss < EPS:
raise VincentyError('%r coincident with %r' % (self, other))
cs = s1s2 + c1c2 * cll
s = atan2(ss, cs)
sa = c1c2 * sll / ss
c2a = 1 - (sa * sa)
if abs(c2a) < EPS:
c2a = 0 # equatorial line
ll = dl + E.f * sa * s
else:
c2sm = cs - 2 * s1s2 / c2a
ll = dl + _dl(E.f, c2a, sa, s, cs, ss, c2sm)
if abs(ll - ll_) < self._epsilon:
break
else:
raise VincentyError('no convergence %r to %r' % (self, other))
if c2a: # e22 == (a / b) ** 2 - 1
A, B = _p2(c2a, E.e22)
s = A * (s - _ds(B, cs, ss, c2sm))
b = E.b
# if self.height or other.height:
# b += self._alter(other)
d = b * s
if azis: # forward and reverse azimuth
cll, sll = cos(ll), sin(ll)
f = degrees360(atan2(c2 * sll, c1s2 - s1c2 * cll))
r = degrees360(atan2(c1 * sll, -s1c2 + c1s2 * cll))
d = d, f, r
return d
def _direct(self, distance, bearing, llr, height=None):
'''(INTERNAL) Direct Vincenty method.
@raise VincentyError: Vincenty fails to converge for the current
L{LatLon.epsilon} and L{LatLon.iterations}
limit.
'''
E = self.ellipsoid()
c1, s1, t1 = _r3(self.lat, E.f)
i = radians(bearing) # initial bearing (forward azimuth)
ci, si = cos(i), sin(i)
s12 = atan2(t1, ci) * 2
sa = c1 * si
c2a = 1 - sa**2
if c2a < EPS:
c2a = 0
A, B = 1, 0
else: # e22 == (a / b)**2 - 1
A, B = _p2(c2a * E.e22)
s = d = distance / (E.b * A)
for _ in range(self._iterations):
cs, ss, c2sm = cos(s), sin(s), cos(s12 + s)
s_, s = s, d + _ds(B, cs, ss, c2sm)
if abs(s - s_) < self._epsilon:
break
else:
raise VincentyError('no convergence %r' % (self, ))
t = s1 * ss - c1 * cs * ci
# final bearing (reverse azimuth +/- 180)
r = degrees360(atan2(sa, -t))
if llr:
# destination latitude in [-270, 90)
a = degrees90(
atan2(s1 * cs + c1 * ss * ci, (1 - E.f) * hypot(sa, t)))
# destination longitude in [-180, 180)
b = degrees180(
atan2(ss * si, c1 * cs - s1 * ss * ci) -
_dl(E.f, c2a, sa, s, cs, ss, c2sm) + radians(self.lon))
h = self.height if height is None else height
r = self.classof(a, b, height=h, datum=self.datum), r
return r
def maxLat(self, bearing):
'''Return the maximum latitude reached when travelling
on a great circle on given bearing from this point
(based on 'Clairaut's formula').
The maximum latitude is independent of longitude,
it is the same for all points on a given latitude.
Negate the result for the minimum latitude (on
the Southern hemisphere).
@param bearing: Initial bearing (compass degrees).
@return: Maximum latitude (degrees90).
@JSname: I{maxLatitude}.
'''
m = acos(abs(sin(radians(bearing)) * cos(radians(self.lat))))
return degrees90(m)
def toNed(distance, bearing, elevation):
'''Create an NED vector from distance, bearing and elevation
(in local coordinate system).
@param distance: NED vector length in meter (scalar).
@param bearing: NED vector bearing in compass degrees (scalar).
@param elevation: NED vector elevation in degrees from local
coordinate frame horizontal (scalar).
@return: NED vector equivalent to distance, bearing and
elevation (L{Ned}).
@JSname: I{fromDistanceBearingElevation}.
'''
b, e = radians(bearing), radians(elevation)
d = float(distance)
dce = d * cos(e)
return Ned(cos(b) * dce, sin(b) * dce, -sin(e) * d)
def toWm(latlon, lon=None, radius=R_MA, Wm=Wm):
'''Convert a lat-/longitude point to a WM coordinate.
@param latlon: Latitude (degrees) or an (ellipsoidal or
spherical) geodetic I{LatLon} point.
@keyword lon: Optional longitude (degrees or None).
@keyword radius: Optional earth radius (meter).
@keyword Wm: Optional Wm class for the WM coordinate (L{Wm}).
@return: The WM coordinate (L{Wm}).
@raise ValueError: If I{lon} value is missing, if I{latlon}
is not scalar, if I{latlon} is beyond
the valid WM range and L{rangerrors} is
set to True or if I{radius} is invalid.
@example:
>>> p = LatLon(48.8582, 2.2945) # 448251.8 5411932.7
>>> w = toWm(p) # 448252 5411933
>>> p = LatLon(13.4125, 103.8667) # 377302.4 1483034.8
>>> w = toWm(p) # 377302 1483035
'''
r, e = radius, None
try:
lat, lon = latlon.lat, latlon.lon
if isinstance(latlon, _eLLb):
r = latlon.datum.ellipsoid.a
e = latlon.datum.ellipsoid.e
lat = clipDMS(lat, _LatLimit)
except AttributeError:
lat, lon = parseDMS2(latlon, lon, clipLat=_LatLimit)
s = sin(radians(lat))
y = atanh(s) # == log(tan((90 + lat) / 2)) == log(tanPI_2_2(radians(lat)))
if e:
y -= e * atanh(e * s)
return Wm(r * radians(lon), r * y, radius=r)
def rhumbDestination(self, distance, bearing, radius=R_M, height=None):
'''Return the destination point having travelled along
a rhumb (loxodrome) line from this point the given
distance on the given bearing.
@param distance: Distance travelled (same units as radius).
@param bearing: Bearing from this point (compass degrees).
@keyword radius: Optional, mean earth radius (meter).
@keyword height: Optional height, overriding the default
height (meter or same unit as radius).
@return: The destination point (spherical LatLon).
@example:
>>> p = LatLon(51.127, 1.338)
>>> q = p.rhumbDestination(40300, 116.7) # 50.9642°N, 001.8530°E
@JSname: I{rhumbDestinationPoint}
'''
a1, b1 = self.to2ab()
r = float(distance) / float(radius) # angular distance in radians
t = radians(bearing)
da = r * cos(t)
a2 = a1 + da
# normalize latitude if past pole
if a2 > PI_2:
a2 = PI - a2
elif a2 < -PI_2:
a2 = -PI - a2
dp = log(tanPI_2_2(a2) / tanPI_2_2(a1))
# E-W course becomes ill-conditioned with 0/0
if abs(dp) > EPS:
q = da / dp
else:
q = cos(a1)
if abs(q) > EPS:
b2 = b1 + r * sin(t) / q
else:
b2 = b1
h = self.height if height is None else height
return self.classof(degrees90(a2), degrees180(b2), height=h)
def toOsgr(latlon, lon=None, datum=Datums.WGS84):
'''Convert lat-/longitude to a OSGR coordinate.
@param latlon: Latitude in degrees (scalar) or an
ellipsoidal LatLon location.
@keyword lon: Longitude in degrees (scalar or None).
@keyword datum: Datum to use (L{Datum}).
@return: The OSGR coordinate (L{Osgr}).
@raise TypeError: If latlon is not ellipsoidal.
@raise ValueError: If lon is invalid, not None.
@example:
>>> p = LatLon(52.65798, 1.71605)
>>> r = toOsgr(p) # TG 51409 13177
>>> # for conversion of (historical) OSGB36 lat-/longitude:
>>> r = toOsgr(52.65757, 1.71791, datum=Datums.OSGB36)
'''
if isscalar(latlon) and isscalar(lon):
# XXX any ellipsoidal LatLon with .convertDatum
latlon = LatLonEllipsoidalBase(latlon, lon, datum=datum)
elif not hasattr(latlon, 'convertDatum'):
raise TypeError('%s not ellipsoidal: %r' % ('latlon', latlon))
elif lon is not None:
raise ValueError('%s not %s: %r' % ('lon', None, lon))
if latlon.datum != _OSGB36:
latlon = latlon.convertDatum(_OSGB36)
E = _OSGB36.ellipsoid
a, b = radians(latlon.lat), radians(latlon.lon)
ca, sa, ta = cos(a), sin(a), tan(a)
s = 1 - E.e2 * sa * sa
v = E.a * _F0 / sqrt(s)
r = s / E.e12 # = v / r = v / (v * E.e12 / s)
ca3 = ca * ca * ca
ca5 = ca * ca * ca3
ta2 = ta * ta
ta4 = ta2 * ta2
x2 = r - 1 # η
I4 = (E.b * _M(E.Mabcd, a) + _N0,
(v / 2) * sa * ca,
(v / 24) * sa * ca3 * (5 - ta2 + 9 * x2),
(v / 720) * sa * ca5 * (61 - 58 * ta2 + ta4))
V4 = (_E0,
v * ca,
(v / 6) * ca3 * (r - ta2),
(v / 120) * ca5 * (5 - 18 * ta2 + ta4 + 14 * x2 - 58 * ta2 * x2))
d = b - _B0
d2 = d * d
d3 = d2 * d
d5 = d2 * d3
n = fdot(I4, 1, d2, d3 * d, d5 * d)
e = fdot(V4, 1, d, d3, d5)
return Osgr(e, n)
def nearestOn2(point, points, radius=R_M, **options):
'''Locate the closest point on any segment between two consecutive
points of a path.
If the given point is within the extent of any segment, the
closest point is on the segment. Otherwise the closest point
is the nearest of the segment end points.
Distances are approximated by function L{equirectangular_},
subject to the supplied I{options}.
@param point: The reference point (L{LatLon}).
@param points: The points of the path (L{LatLon}[]).
@keyword radius: Optional, mean earth radius (meter).
@keyword options: Optional keyword arguments for function
L{equirectangular_}.
@return: 2-Tuple (closest, distance) of the closest point
(L{LatLon}) on the path and the distance to that
point. The distance is the L{equirectangular_}
distance between the given and the closest point
in meter, rather the units of I{radius}.
@raise LimitError: Lat- and/or longitudinal delta exceeds
I{limit}, see function L{equirectangular_}.
@raise TypeError: Some I{points} are not I{LatLon}.
@raise ValueError: Insufficient number of I{points}.
'''
def _d2yx(p2, p1, u):
# equirectangular_ returns a 4-tuple (distance in
# degrees squared, delta lat, delta lon, p2.lon
# unroll/wrap); the previous p2.lon unroll/wrap
# is also applied to the next edge's p1.lon
return equirectangular_(p1.lat, p1.lon + u, p2.lat, p2.lon, **options)
# point (x, y) on axis rotated by angle a ccw:
# x' = y * sin(a) + x * cos(a)
# y' = y * cos(a) - x * sin(a)
#
# distance (w) along and perpendicular (h) to
# a line thru point (dx, dy) and the origin:
# w = (y * dy + x * dx) / hypot(dx, dy)
# h = (y * dx - x * dy) / hypot(dx, dy)
#
# closest point on that line thru (dx, dy):
# xc = w * dx / hypot(dx, dy)
# yc = w * dy / hypot(dx, dy)
n, points = point.points(points, closed=False)
u = 0
c = p2 = points[0]
d, _, _, _ = _d2yx(p2, point, 0)
for i in range(1, n):
p1, p2 = p2, points[i]
# iff wrapped, unroll lon1 (actually previous
# lon2) like function unroll180/-PI would've
d21, y21, x21, u = _d2yx(p2, p1, u)
if d21 > EPS:
# distance point to p1
d2, y01, x01, _ = _d2yx(point, p1, 0)
if d2 > EPS:
w = y01 * y21 + x01 * x21
if w > 0:
if w < d21:
# closest is between p1 and p2, use
# original delta's, not y21 and x21
f = w / d21
p1 = point.classof(favg(p1.lat, p2.lat, f=f),
favg(p1.lon, p2.lon + u, f=f))
d2, _, _, _ = _d2yx(point, p1, 0)
else: # p2 is closer
d2, _, _, _ = _d2yx(point, p2, u)
p1 = p2 # in case d2 < d
if d2 < d: # p1 is closer
c, d = p1, d2
# distance in degrees squared to meter
d = radians(sqrt(d)) * float(radius)
return c, d
def toLatLon(self, LatLon):
'''Convert this UTM coordinate to an (ellipsoidal) geodetic point.
@param LatLon: The I{LatLon} class to use for the point
(I{LatLon}) or None.
@return: Point of this UTM coordinate (I{LatLon}) or 5-tuple
(lat, lon, datum, convergence, scale) if I{LatLon}
is None.
@raise TypeError: If I{LatLon} is not ellipsoidal.
@raise ValueError: Invalid meridional radius or H-value.
@example:
>>> u = Utm(31, 'N', 448251.795, 5411932.678)
>>> from pygeodesy import ellipsoidalVincenty as eV
>>> ll = u.toLatLon(eV.LatLon) # 48°51′29.52″N, 002°17′40.20″E
'''
if self._latlon:
return self._latlon5(LatLon)
E = self._datum.ellipsoid # XXX vs LatLon.datum.ellipsoid
x = self._easting - _FalseEasting # relative to central meridian
y = self._northing
if self._hemi == 'S': # relative to equator
y -= _FalseNorthing
# from Karney 2011 Eq 15-22, 36
A0 = _K0 * E.A
if A0 < EPS:
raise ValueError('%s invalid: %r' % ('meridional', E.A))
x /= A0 # η eta
y /= A0 # ξ ksi
B6 = _K6s(E.Beta6, x, y) # 6th-order Krüger series, 1-origin
y = -B6.ys(-y) # ξ'
x = -B6.xs(-x) # η'
shx = sinh(x)
cy, sy = cos(y), sin(y)
H = hypot(shx, cy)
if H < EPS:
raise ValueError('%s invalid: %r' % ('H', H))
T = t0 = sy / H # τʹ
q = 1.0 / E.e12
d = 1
sd = Fsum(T)
# toggles on +/-1.12e-16 eg. 31 N 400000 5000000
while abs(d) > EPS: # 1e-12
h = hypot1(T)
s = sinh(E.e * atanh(E.e * T / h))
t = T * hypot1(s) - s * h
d = (t0 - t) / hypot1(t) * (q + T**2) / h
sd.fadd(d)
T = sd.fsum() # τi
a = atan(T) # lat
b = atan2(shx, cy) + radians(_cmlon(self._zone))
ll = _eLLb(degrees90(a), degrees180(b), datum=self._datum)
# convergence: Karney 2011 Eq 26, 27
p = -B6.ps(-1)
q = B6.qs(0)
ll._convergence = degrees(atan(tan(y) * tanh(x)) + atan2(q, p))
# scale: Karney 2011 Eq 28
ll._scale = E.e2s(sin(a)) * hypot1(T) * H * (A0 / E.a / hypot(p, q))
self._latlon = ll
return self._latlon5(LatLon)
def _inverse(self, other, azis, wrap):
'''(INTERNAL) Inverse Vincenty method.
@raise TypeError: The other point is not L{LatLon}.
@raise ValueError: If this and the I{other} point's L{Datum}
ellipsoids are not compatible.
@raise VincentyError: Vincenty fails to converge for the current
L{LatLon.epsilon} and L{LatLon.iterations}
limit and/or if this and the I{other} point
are near-antipodal or coincide.
'''
E = self.ellipsoids(other)
c1, s1, _ = _r3(self.lat, E.f)
c2, s2, _ = _r3(other.lat, E.f)
c1c2, s1c2 = c1 * c2, s1 * c2
c1s2, s1s2 = c1 * s2, s1 * s2
dl, _ = unroll180(self.lon, other.lon, wrap=wrap)
ll = dl = radians(dl)
for _ in range(self._iterations):
cll, sll, ll_ = cos(ll), sin(ll), ll
ss = hypot(c2 * sll, c1s2 - s1c2 * cll)
if ss < EPS:
raise VincentyError('%r coincides with %r' % (self, other))
cs = s1s2 + c1c2 * cll
s = atan2(ss, cs)
sa = c1c2 * sll / ss
c2a = 1 - sa**2
if abs(c2a) < EPS:
c2a = 0 # equatorial line
ll = dl + E.f * sa * s
else:
c2sm = cs - 2 * s1s2 / c2a
ll = dl + _dl(E.f, c2a, sa, s, cs, ss, c2sm)
if abs(ll - ll_) < self._epsilon:
break
# <http://github.com/chrisveness/geodesy/blob/master/latlon-vincenty.js>
# omitted and applied only after failure to converge, see footnote under
# Inverse at <http://en.wikipedia.org/wiki/Vincenty's_formulae>
# elif abs(ll) > PI and self.isantipode(other, eps=self._epsilon):
# raise VincentyError('%r antipodal to %r' % (self, other))
else:
t = 'antipodal ' if self.isantipode(other,
eps=self._epsilon) else ''
raise VincentyError('no convergence, %r %sto %r' %
(self, t, other))
if c2a: # e22 == (a / b)**2 - 1
A, B = _p2(c2a * E.e22)
s = A * (s - _ds(B, cs, ss, c2sm))
b = E.b
# if self.height or other.height:
# b += self._havg(other)
d = b * s
if azis: # forward and reverse azimuth
cll, sll = cos(ll), sin(ll)
f = degrees360(atan2(c2 * sll, c1s2 - s1c2 * cll))
r = degrees360(atan2(c1 * sll, -s1c2 + c1s2 * cll))
d = d, f, r
return d
from datum import Datums
from ellipsoidalBase import LatLonEllipsoidalBase
from utils import degrees90, degrees180, false2f, fdot, \
halfs, isscalar, radians
from math import cos, sin, sqrt, tan
# all public contants, classes and functions
__all__ = ('Osgr', # classes
'parseOSGR', 'toOsgr') # functions
__version__ = '17.04.07'
_10um = 1e-5 #: (INTERNAL) 0.01 millimeter (meter)
_100km = 100000 #: (INTERNAL) 100 km (int meter)
_A0, _B0 = radians(49), radians(-2) #: (INTERNAL) NatGrid true origin, 49°N,2°W.
_E0, _N0 = 400e3, -100e3 #: (INTERNAL) East-/northing of true origin (meter)
_F0 = 0.9996012717 #: (INTERNAL) NatGrid scale of central meridian
_OSGB36 = Datums.OSGB36 #: (INTERNAL) Airy130 ellipsoid
def _M(Mabcd, a):
'''(INTERNAL) Compute meridional arc.
'''
a_ = a - _A0
_a = a + _A0
return _F0 * fdot(Mabcd, a_, -sin(a_) * cos(_a),
sin(a_ * 2) * cos(_a * 2),
-sin(a_ * 3) * cos(_a * 3))
#!/usr/bin/env python
import heapq as hq
from buildMap import Map
from node import Node
from utils import radians
from geometry_msgs.msg import Twist, Pose
from safegoto import SafeGoTo
import rospy
import copy
import math
ROBOT_SIZE = 0.2 # change this
G_MULTIPLIER = 0.2
MOVES = [
(0.2, radians(0)), # move ahead
(-0.2, radians(0)), # move backwards
(0, radians(90)), # turn left
(0, -radians(90))
] # turn right
TOLERANCE = 0.2
class PathPlanner:
def __init__(self, start, theta, goal):
print("building map....")
# map remains constant
self.map = Map().grid_map
self.start = start
self.theta = theta
self.goal = goal
