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