def destinationNed(self, delta):
'''Calculate the destination point using the supplied NED delta
from this point.
@arg delta: Delta from this to the other point in the local
tangent plane (LTP) of this point (L{Ned}).
@return: Destination point (L{Cartesian}).
@raise TypeError: If B{C{delta}} is not L{Ned}.
@example:
>>> a = LatLon(49.66618, 3.45063)
>>> delta = toNed(116807.681, 222.493, -0.5245) # [N:-86126, E:-78900, D:1069]
>>> b = a.destinationNed(delta) # 48.88667°N, 002.37472°E
@JSname: I{destinationPoint}.
'''
_xinstanceof(Ned, delta=delta)
n, e, d = self._rotation3()
# convert NED delta to standard coordinate frame of n-vector
dn = delta.ned
# rotate dn to get delta in cartesian (ECEF) coordinate
# reference frame using the rotation matrix column vectors
dc = Cartesian(fdot(dn, n.x, e.x, d.x),
fdot(dn, n.y, e.y, d.y),
fdot(dn, n.z, e.z, d.z))
# apply (cartesian) delta to this Cartesian to
# obtain destination point as cartesian
v = self.toCartesian().plus(dc) # the plus() gives a plain vector
return v.toLatLon(datum=self.datum, LatLon=self.classof) # Cartesian(v.x, v.y, v.z).toLatLon(...)
def rotate(self, axis, theta):
'''Rotate this vector around an axis by a specified angle.
See U{Rotation matrix from axis and angle
<https://WikiPedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle>}
and U{Quaternion-derived rotation matrix
<https://WikiPedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix>}.
@arg axis: The axis being rotated around (L{Vector3d}).
@arg theta: The angle of rotation (C{radians}).
@return: New, rotated vector (L{Vector3d}).
@JSname: I{rotateAround}.
'''
a = self.others(axis=axis).unit() # axis being rotated around
c = cos(theta)
b = a.times(1 - c)
s = a.times(sin(theta))
p = self.unit().xyz # point being rotated
# multiply p by a quaternion-derived rotation matrix
return self.classof(fdot(p, a.x * b.x + c, a.x * b.y - s.z, a.x * b.z + s.y),
fdot(p, a.y * b.x + s.z, a.y * b.y + c, a.y * b.z - s.x),
fdot(p, a.z * b.x - s.y, a.z * b.y + s.x, a.z * b.z + c))
def _M(Mabcd, a):
'''(INTERNAL) Compute meridional arc.
'''
a_ = a - _A0
_a = a + _A0
return fdot(Mabcd, a_, -sin(a_) * cos(_a),
sin(a_ * 2) * cos(_a * 2), -sin(a_ * 3) * cos(_a * 3))
def rotate(self, xyz, *xyz0):
'''Forward rotation M{M0' ⋅ ([x, y, z] - [x0, y0, z0])'}.
@param xyz: Local C{(x, y, z)} coordinates (C{3-tuple}).
@param xyz0: Optional, local C{(x0, y0, z0)} origin (C{3-tuple}).
@return: Rotated C{(x, y, z)} location (C{3-tuple}).
'''
if xyz0: # and len(xyz) == len(xyz0)
xyz = tuple(c - c0 for c, c0 in zip(xyz, xyz0))
# x' = M[0] * x + M[3] * y + M[6] * z
# y' = M[1] * x + M[4] * y + M[7] * z
# z' = M[2] * x + M[5] * y + M[8] * z
return (fdot(self[0::3], *xyz), fdot(self[1::3],
*xyz), fdot(self[2::3], *xyz))
def _rsT(T):
'''(INTERNAL) Helper for C{_RD}, C{_RF} and C{_RJ}.
'''
s = map2(sqrt, T[1:])
r = fdot(s[:3], s[1], s[2], s[0])
T = tuple((t + r) * 0.25 for t in T)
return r, s, T
def unrotate(self, xyz, *xyz0):
'''Inverse rotation M{[x0, y0, z0] + M0 ⋅ [x,y,z]'}.
@param xyz: Local C{(x, y, z)} coordinates (C{3-tuple}).
@param xyz0: Optional, local C{(x0, y0, z0)} origin (C{3-tuple}).
@return: Unrotated C{(x, y, z)} location (C{3-tuple}).
'''
# x' = x0 + M[0] * x + M[1] * y + M[2] * z
# y' = y0 + M[3] * x + M[4] * y + M[5] * z
# z' = z0 + M[6] * x + M[7] * y + M[8] * z
xyz = (fdot(self[0:3], *xyz), fdot(self[3:6],
*xyz), fdot(self[6:], *xyz))
if xyz0: # and len(xyz) == len(xyz0)
xyz = tuple(c0 + c for c0, c in zip(xyz0, xyz))
return xyz
def rotate(self, xyz, *xyz0):
'''Forward rotation M{M0' ⋅ ([x, y, z] - [x0, y0, z0])'}.
@arg xyz: Local C{(x, y, z)} coordinates (C{3-tuple}).
@arg xyz0: Optional, local C{(x0, y0, z0)} origin (C{3-tuple}).
@return: Rotated C{(x, y, z)} location (C{3-tuple}).
'''
if xyz0:
if len(xyz0) != len(xyz):
raise LenError(self.rotate, xyz0=len(xyz0), xyz=len(xyz))
xyz = tuple(c - c0 for c, c0 in zip(xyz, xyz0))
# x' = M[0] * x + M[3] * y + M[6] * z
# y' = M[1] * x + M[4] * y + M[7] * z
# z' = M[2] * x + M[5] * y + M[8] * z
return (fdot(xyz,
*self[0::3]), fdot(xyz,
*self[1::3]), fdot(xyz, *self[2::3]))
def dot(self, other):
'''Compute the dot (scalar) product of this and an other vector.
@param other: The other vector (L{Vector3d}).
@return: Dot product (C{float}).
@raise TypeError: Incompatible B{C{other}} C{type}.
'''
self.others(other)
return fdot(self.to3xyz(), *other.to3xyz())
def unrotate(self, xyz, *xyz0):
'''Inverse rotation M{[x0, y0, z0] + M0 ⋅ [x,y,z]'}.
@arg xyz: Local C{(x, y, z)} coordinates (C{3-tuple}).
@arg xyz0: Optional, local C{(x0, y0, z0)} origin (C{3-tuple}).
@return: Unrotated C{(x, y, z)} location (C{3-tuple}).
'''
if xyz0:
if len(xyz0) != len(xyz):
raise LenError(self.unrotate, xyz0=len(xyz0), xyz=len(xyz))
_xyz = (1.0, ) + xyz
# x' = x0 + M[0] * x + M[1] * y + M[2] * z
# y' = y0 + M[3] * x + M[4] * y + M[5] * z
# z' = z0 + M[6] * x + M[7] * y + M[8] * z
xyz_ = (fdot(_xyz, xyz0[0],
*self[0:3]), fdot(_xyz, xyz0[1], *self[3:6]),
fdot(_xyz, xyz0[2], *self[6:]))
else:
# x' = M[0] * x + M[1] * y + M[2] * z
# y' = M[3] * x + M[4] * y + M[5] * z
# z' = M[6] * x + M[7] * y + M[8] * z
xyz_ = (fdot(xyz,
*self[0:3]), fdot(xyz,
*self[3:6]), fdot(xyz, *self[6:]))
return xyz_
def transform(self, x, y, z, inverse=False):
'''Transform a (geocentric) Cartesian point, forward or inverse.
@arg x: X coordinate (C{meter}).
@arg y: Y coordinate (C{meter}).
@arg z: Z coordinate (C{meter}).
@kwarg inverse: Optional direction, forward or inverse (C{bool}).
@return: A L{Vector3Tuple}C{(x, y, z)}, transformed.
'''
if inverse:
_xyz = -1, -x, -y, -z
_s1 = self.s1 - 2 # == -(1 - s * 1e-6)) == -(1 - (s1 - 1))
else:
_xyz = 1, x, y, z
_s1 = self.s1
# x', y', z' = (.tx + x * .s1 - y * .rz + z * .ry,
# .ty + x * .rz + y * .s1 - z * .rx,
# .tz - x * .ry + y * .rx + z * .s1)
r = Vector3Tuple(fdot(_xyz, self.tx, _s1, -self.rz, self.ry),
fdot(_xyz, self.ty, self.rz, _s1, -self.rx),
fdot(_xyz, self.tz, -self.ry, self.rx, _s1))
return self._xnamed(r)
def dot(self, other):
'''Compute the dot (scalar) product of this and an other vector.
@arg other: The other vector (L{Vector3d}).
@return: Dot product (C{float}).
@raise TypeError: Incompatible B{C{other}} C{type}.
'''
if other is self:
d = self.length2
else:
self.others(other)
d = fdot(self.xyz, *other.xyz)
return d
def multiply(self, other):
'''Matrix multiply M{M0' ⋅ M} this matrix transposed with
an other matrix.
@param other: The other matrix (L{EcefMatrix}).
@return: The matrix product (L{EcefMatrix}).
@raise TypeError: If B{C{other}} is not L{EcefMatrix}.
'''
_TypeError(EcefMatrix, other=other)
# like LocalCartesian.MatrixMultiply, transposed(self) x other
# <https://GeographicLib.SourceForge.io/html/LocalCartesian_8cpp_source.html>
M = (fdot(self[r::3], *other[c::3]) for r in range(3) for c in range(3))
return EcefMatrix(*M)
def toOsgr(latlon, lon=None, datum=Datums.WGS84, Osgr=Osgr, name=''):
'''Convert a lat-/longitude point to an OSGR coordinate.
@param latlon: Latitude (C{degrees}) or an (ellipsoidal)
geodetic C{LatLon} point.
@keyword lon: Optional longitude in degrees (scalar or C{None}).
@keyword datum: Optional datum to convert (C{Datum}).
@keyword Osgr: Optional (sub-)class to return the OSGR
coordinate (L{Osgr}) or C{None}.
@keyword name: Optional B{C{Osgr}} name (C{str}).
@return: The OSGR coordinate (B{C{Osgr}}) or an
L{EasNor2Tuple}C{(easting, northing)} if B{C{Osgr}}
is C{None}.
@raise TypeError: Non-ellipsoidal B{C{latlon}} or B{C{datum}}
conversion failed.
@raise OSGRError: Invalid B{C{latlon}} or B{C{lon}}.
@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 not isinstance(latlon, _LLEB):
# XXX fix failing _LLEB.convertDatum()
latlon = _LLEB(*parseDMS2(latlon, lon), datum=datum)
elif lon is not None:
raise OSGRError('%s not %s: %r' % ('lon', None, lon))
elif not name: # use latlon.name
name = nameof(latlon)
E = _OSGB36.ellipsoid
ll = _ll2datum(latlon, _OSGB36, 'latlon')
a, b = map1(radians, ll.lat, ll.lon)
sa, ca = sincos2(a)
s = E.e2s2(sa) # v, r = E.roc2_(sa, _F0); r = v / r
v = E.a * _F0 / sqrt(s) # nu
r = s / E.e12 # nu / rho == v / (v * E.e12 / s)
x2 = r - 1 # η2
ta = tan(a)
ca3, ca5 = fpowers(ca, 5, 3) # PYCHOK false!
ta2, ta4 = fpowers(ta, 4, 2) # PYCHOK false!
vsa = v * sa
I4 = (E.b * _F0 * _M(E.Mabcd, a) + _N0,
(vsa / 2) * ca,
(vsa / 24) * ca3 * fsum_(5, -ta2, 9 * x2),
(vsa / 720) * ca5 * fsum_(61, ta4, -58 * ta2))
V4 = (_E0,
(v * ca),
(v / 6) * ca3 * (r - ta2),
(v / 120) * ca5 * fdot((-18, 1, 14, -58), ta2, 5 + ta4, x2, ta2 * x2))
d, d2, d3, d4, d5, d6 = fpowers(b - _B0, 6) # PYCHOK false!
n = fdot(I4, 1, d2, d4, d6)
e = fdot(V4, 1, d, d3, d5)
if Osgr is None:
r = EasNor2Tuple(e, n)
else:
r = Osgr(e, n)
if lon is None and isinstance(latlon, _LLEB):
r._latlon = latlon # XXX weakref(latlon)?
return _xnamed(r, name)
def toOsgr(latlon,
lon=None,
datum=Datums.WGS84,
Osgr=Osgr,
name=NN,
**Osgr_kwds):
'''Convert a lat-/longitude point to an OSGR coordinate.
@arg latlon: Latitude (C{degrees}) or an (ellipsoidal) geodetic
C{LatLon} point.
@kwarg lon: Optional longitude in degrees (scalar or C{None}).
@kwarg datum: Optional datum to convert B{C{lat, lon}} from
(L{Datum}, L{Ellipsoid}, L{Ellipsoid2} or
L{a_f2Tuple}).
@kwarg Osgr: Optional class to return the OSGR coordinate
(L{Osgr}) or C{None}.
@kwarg name: Optional B{C{Osgr}} name (C{str}).
@kwarg Osgr_kwds: Optional, additional B{C{Osgr}} keyword
arguments, ignored if B{C{Osgr=None}}.
@return: The OSGR coordinate (B{C{Osgr}}) or an
L{EasNor2Tuple}C{(easting, northing)} if B{C{Osgr}}
is C{None}.
@raise OSGRError: Invalid B{C{latlon}} or B{C{lon}}.
@raise TypeError: Non-ellipsoidal B{C{latlon}} or invalid
B{C{datum}} or conversion failed.
@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 not isinstance(latlon, _LLEB):
# XXX fix failing _LLEB.convertDatum()
latlon = _LLEB(*parseDMS2(latlon, lon), datum=datum)
elif lon is not None:
raise OSGRError(lon=lon, txt='not %s' % (None, ))
elif not name: # use latlon.name
name = nameof(latlon)
# if necessary, convert to OSGB36 first
ll = _ll2datum(latlon, _Datums_OSGB36, _latlon_)
try:
a, b = ll.philam
except AttributeError:
a, b = map1(radians, ll.lat, ll.lon)
sa, ca = sincos2(a)
E = _Datums_OSGB36.ellipsoid
s = E.e2s2(sa) # r, v = E.roc2_(sa, _F0); r = v / r
v = E.a * _F0 / sqrt(s) # nu
r = s / E.e12 # nu / rho == v / (v * E.e12 / s) == s / E.e12
x2 = r - 1 # η2
ta = tan(a)
ca3, ca5 = fpowers(ca, 5, 3) # PYCHOK false!
ta2, ta4 = fpowers(ta, 4, 2) # PYCHOK false!
vsa = v * sa
I4 = (E.b * _F0 * _M(E.Mabcd, a) + _N0, (vsa / 2) * ca,
(vsa / 24) * ca3 * fsum_(5, -ta2, 9 * x2),
(vsa / 720) * ca5 * fsum_(61, ta4, -58 * ta2))
V4 = (_E0, (v * ca), (v / 6) * ca3 * (r - ta2), (v / 120) * ca5 * fdot(
(-18, 1, 14, -58), ta2, 5 + ta4, x2, ta2 * x2))
d, d2, d3, d4, d5, d6 = fpowers(b - _B0, 6) # PYCHOK false!
n = fdot(I4, 1, d2, d4, d6)
e = fdot(V4, 1, d, d3, d5)
if Osgr is None:
r = _EasNor2Tuple(e, n)
else:
r = Osgr(e, n, datum=_Datums_OSGB36, **Osgr_kwds)
if lon is None and isinstance(latlon, _LLEB):
r._latlon = latlon # XXX weakref(latlon)?
return _xnamed(r, name)
def toLatLon(self, LatLon=None, datum=Datums.WGS84):
'''Convert this OSGR coordinate to an (ellipsoidal) geodetic
point.
While OS grid references are based on the OSGB36 datum, the
I{Ordnance Survey} have deprecated the use of OSGB36 for
lat-/longitude coordinates (in favour of WGS84). Hence, this
method returns WGS84 by default with OSGB36 as an option,
U{see<https://www.OrdnanceSurvey.co.UK/blog/2014/12/2>}.
I{Note formulation implemented here due to Thomas, Redfearn,
etc. is as published by OS, but is inferior to Krüger as
used by e.g. Karney 2011.}
@kwarg LatLon: Optional ellipsoidal class to return the
geodetic point (C{LatLon}) or C{None}.
@kwarg datum: Optional datum to convert to (L{Datum},
L{Ellipsoid}, L{Ellipsoid2}, L{Ellipsoid2}
or L{a_f2Tuple}).
@return: The geodetic point (B{C{LatLon}}) or a
L{LatLonDatum3Tuple}C{(lat, lon, datum)}
if B{C{LatLon}} is C{None}.
@raise OSGRError: No convergence.
@raise TypeError: If B{C{LatLon}} is not ellipsoidal or
B{C{datum}} is invalid or conversion failed.
@example:
>>> from pygeodesy import ellipsoidalVincenty as eV
>>> g = Osgr(651409.903, 313177.270)
>>> p = g.toLatLon(eV.LatLon) # 52°39′28.723″N, 001°42′57.787″E
>>> # to obtain (historical) OSGB36 lat-/longitude point
>>> p = g.toLatLon(eV.LatLon, datum=Datums.OSGB36) # 52°39′27.253″N, 001°43′04.518″E
'''
if self._latlon:
return self._latlon3(LatLon, datum)
E = self.datum.ellipsoid # _Datums_OSGB36.ellipsoid, Airy130
a_F0 = E.a * _F0
b_F0 = E.b * _F0
e, n = self.easting, self.northing
n_N0 = n - _N0
a, m = _A0, n_N0
sa = Fsum(a)
for self._iteration in range(1, _TRIPS):
a = sa.fsum_(m / a_F0)
m = n_N0 - b_F0 * _M(E.Mabcd, a) # meridional arc
if abs(m) < _10um:
break
else:
t = _dot_(_item_ps(self.classname, self.toStr(prec=-3)),
self.toLatLon.__name__)
raise OSGRError(_no_convergence_, txt=t)
sa, ca = sincos2(a)
s = E.e2s2(sa) # r, v = E.roc2_(sa, _F0)
v = a_F0 / sqrt(s) # nu
r = v * E.e12 / s # rho = a_F0 * E.e12 / pow(s, 1.5) == a_F0 * E.e12 / (s * sqrt(s))
vr = v / r # == s / E.e12
x2 = vr - 1 # η2
ta = tan(a)
v3, v5, v7 = fpowers(v, 7, 3) # PYCHOK false!
ta2, ta4, ta6 = fpowers(ta**2, 3) # PYCHOK false!
tar = ta / r
V4 = (a, tar / (2 * v), tar / (24 * v3) * fdot(
(1, 3, -9), 5 + x2, ta2, ta2 * x2), tar / (720 * v5) * fdot(
(61, 90, 45), 1, ta2, ta4))
csa = 1.0 / ca
X5 = (_B0, csa / v, csa / (6 * v3) * fsum_(vr, ta2, ta2),
csa / (120 * v5) * fdot(
(5, 28, 24), 1, ta2, ta4), csa / (5040 * v7) * fdot(
(61, 662, 1320, 720), 1, ta2, ta4, ta6))
d, d2, d3, d4, d5, d6, d7 = fpowers(e - _E0, 7) # PYCHOK false!
a = fdot(V4, 1, -d2, d4, -d6)
b = fdot(X5, 1, d, -d3, d5, -d7)
r = _LLEB(degrees90(a),
degrees180(b),
datum=self.datum,
name=self.name)
r._iteration = self._iteration # only ellipsoidal LatLon
self._latlon = r
return self._latlon3(LatLon, datum)
def _xV3(c1, u, x, y):
xy1 = x, y, _1_0 # transform to original space
return _V3(fdot(xy1, u.x, -u.y, c1.x),
fdot(xy1, u.y, u.x, c1.y), _0_0)
def _xdot(d, a1, b1, a, b, wrap):
# compute dot product d . (-b + b1, a - a1)
db, _ = unrollPI(b1, radians(b), wrap=wrap)
return fdot(d, db, radians(a) - a1)
def toLatLon(self, LatLon=None, datum=Datums.WGS84):
'''Convert this OSGR coordinate to an (ellipsoidal) geodetic
point.
I{Note formulation implemented here due to Thomas, Redfearn,
etc. is as published by OS, but is inferior to Krüger as
used by e.g. Karney 2011.}
@keyword LatLon: Optional ellipsoidal (sub-)class to return
the point (C{LatLon}) or C{None}.
@keyword datum: Optional datum to use (C{Datum}).
@return: The geodetic point (B{C{LatLon}}) or a
L{LatLonDatum3Tuple}C{(lat, lon, datum)}
if B{C{LatLon}} is C{None}.
@raise TypeError: If B{C{LatLon}} is not ellipsoidal or if
B{C{datum}} conversion failed.
@example:
>>> from pygeodesy import ellipsoidalVincenty as eV
>>> g = Osgr(651409.903, 313177.270)
>>> p = g.toLatLon(eV.LatLon) # 52°39′28.723″N, 001°42′57.787″E
>>> # to obtain (historical) OSGB36 lat-/longitude point
>>> p = g.toLatLon(eV.LatLon, datum=Datums.OSGB36) # 52°39′27.253″N, 001°43′04.518″E
'''
if self._latlon:
return self._latlon3(LatLon, datum)
E = _OSGB36.ellipsoid # Airy130
a_F0 = E.a * _F0
b_F0 = E.b * _F0
e, n = self._easting, self._northing
n_N0 = n - _N0
a, M = _A0, 0
sa = Fsum(a)
while True:
t = n_N0 - M
if t < _10um:
break
a = sa.fsum_(t / a_F0)
M = b_F0 * _M(E.Mabcd, a)
sa, ca = sincos2(a)
s = E.e2s2(sa) # v, r = E.roc2_(sa, _F0)
v = a_F0 / sqrt(s) # nu
r = v * E.e12 / s # rho
vr = v / r # == s / E.e12
x2 = vr - 1 # η2
ta = tan(a)
v3, v5, v7 = fpowers(v, 7, 3) # PYCHOK false!
ta2, ta4, ta6 = fpowers(ta**2, 3) # PYCHOK false!
tar = ta / r
V4 = (a,
tar / ( 2 * v),
tar / ( 24 * v3) * fdot((1, 3, -9), 5 + x2, ta2, ta2 * x2),
tar / (720 * v5) * fdot((61, 90, 45), 1, ta2, ta4))
csa = 1.0 / ca
X5 = (_B0,
csa / v,
csa / ( 6 * v3) * fsum_(vr, ta, ta),
csa / ( 120 * v5) * fdot((5, 28, 24), 1, ta2, ta4),
csa / (5040 * v7) * fdot((61, 662, 1320, 720), ta, ta2, ta4, ta6))
d, d2, d3, d4, d5, d6, d7 = fpowers(e - _E0, 7) # PYCHOK false!
a = fdot(V4, 1, -d2, d4, -d6)
b = fdot(X5, 1, d, -d3, d5, -d7)
self._latlon = _LLEB(degrees90(a), degrees180(b), datum=_OSGB36, name=self.name)
return self._latlon3(LatLon, datum)
def _xVector(c1, u, x, y):
xy1 = x, y, 1 # transform to original space
return _Vector(fdot(xy1, u.x, -u.y, c1.x),
fdot(xy1, u.y, u.x, c1.y), 0)