class _OnGreatCirclePath(object):
__doc__ = """ True if position B is on great circle and between endpoints of path A.
Parameters
----------
path: tuple of 2 n-vectors
2 n-vectors of positions defining path A, decomposed in E.
n_EB_E: 3 x m array
n-vector(s) of position B to measure the cross track distance to.
radius: real scalar
radius of sphere. (default 6371009.0)
rtol, atol: real scalars
defining relative and absolute tolerance
Returns
-------
on : bool array of length max(n, m)
True if position B is on great circle and between endpoints of path A.
Examples
--------
{0}
""".format(_examples.get_examples([10], OO=False))
class _DeltaE(object):
__doc__ = """
Return cartesian delta vector from positions A to B decomposed in E.
Parameters
----------
positionA, positionB: Nvector, GeoPoint or ECEFvector objects
position A and B, decomposed in E.
Returns
-------
p_AB_E: ECEFvector
Cartesian position vector(s) from A to B, decomposed in E.
Notes
-----
The calculation is excact, taking the ellipsity of the Earth into account.
It is also non-singular as both n-vector and p-vector are non-singular
(except for the center of the Earth).
Examples
--------
{0}
See also
--------
n_EA_E_and_p_AB_E2n_EB_E,
p_EB_E2n_EB_E,
n_EB_E2p_EB_E.
""".format(_examples.get_examples([1]))
class _CrossTrackDistance(object):
__doc__ = """ Return cross track distance between path A and position B.
Parameters
----------
path: tuple of 2 n-vectors
2 n-vectors of positions defining path A, decomposed in E.
n_EB_E: 3 x m array
n-vector(s) of position B to measure the cross track distance to.
method: string
defining distance calculated. Options are: 'greatcircle' or 'euclidean'
radius: real scalar
radius of sphere. (default 6371009.0)
Returns
-------
distance : array of length max(n, m)
cross track distance(s)
Examples
--------
{0}
""".format(_examples.get_examples([10], OO=False))
class FrameB(FrameN):
__doc__ = """
Body frame
Parameters
----------
position: ECEFvector, GeoPoint or Nvector object
position of the vehicle's reference point which also coincides with
the origin of the frame B.
yaw, pitch, roll: real scalars
defining the orientation of frame B in [deg] or [rad].
degrees : bool
if True yaw, pitch, roll are given in degrees otherwise in radians
Notes
-----
The frame is fixed to the vehicle where the x-axis points forward, the
y-axis to the right (starboard) and the z-axis in the vehicle's down
direction.
Examples
--------
{0}
See also
--------
FrameE, FrameL, FrameN
""".format(_examples.get_examples([2]))
def __init__(self, position, yaw=0, pitch=0, roll=0, degrees=False):
self.nvector = position.to_nvector()
if degrees:
yaw, pitch, roll = rad(yaw), rad(pitch), rad(roll)
self.yaw = yaw
self.pitch = pitch
self.roll = roll
@property
def R_EN(self):
R_NB = zyx2R(self.yaw, self.pitch, self.roll)
n_EB_E = self.nvector.normal
R_EN = n_E2R_EN(n_EB_E, self.nvector.frame.R_Ee)
return mdot(R_EN, R_NB) # rotation matrix
def _is_equal_to(self, other, rtol=1e-12, atol=1e-14):
return (np.allclose(self.yaw, other.yaw, rtol=rtol, atol=atol)
and np.allclose(self.pitch, other.pitch, rtol=rtol, atol=atol)
and np.allclose(self.roll, other.roll, rtol=rtol, atol=atol)
and np.allclose(self.R_EN, other.R_EN, rtol=rtol, atol=atol)
and self.nvector == other.nvector)
class FrameN(_Common):
__doc__ = """
North-East-Down frame
Parameters
----------
position: ECEFvector, GeoPoint or Nvector object
position of the vehicle (B) which also defines the origin of the local
frame N. The origin is directly beneath or above the vehicle (B), at
Earth's surface (surface of ellipsoid model).
Notes
-----
The Cartesian frame is local and oriented North-East-Down, i.e.,
the x-axis points towards north, the y-axis points towards east (both are
horizontal), and the z-axis is pointing down.
When moving relative to the Earth, the frame rotates about its z-axis
to allow the x-axis to always point towards north. When getting close
to the poles this rotation rate will increase, being infinite at the
poles. The poles are thus singularities and the direction of the
x- and y-axes are not defined here. Hence, this coordinate frame is
NOT SUITABLE for general calculations.
Examples
--------
{0}
See also
--------
FrameE, FrameL, FrameB
""".format(_examples.get_examples([1]))
def __init__(self, position):
nvector = position.to_nvector()
self.nvector = Nvector(nvector.normal, z=0, frame=nvector.frame)
@property
def R_EN(self):
nvector = self.nvector
return n_E2R_EN(nvector.normal, nvector.frame.R_Ee)
def _is_equal_to(self, other, rtol=1e-12, atol=1e-14):
return (np.allclose(self.R_EN, other.R_EN, rtol=rtol, atol=atol)
and self.nvector == other.nvector)
def Pvector(self, pvector):
return Pvector(pvector, frame=self)
class _EuclideanDistance(object):
__doc__ = """Return Euclidean distance between positions A and B
Parameters
----------
n_EA_E, n_EB_E: 3 x n array
n-vector(s) [no unit] of position A and B, decomposed in E.
radius: real scalar
radius of sphere.
Examples
--------
{0}
""".format(_examples.get_examples([5], OO=False))
class _ECEFvector2Nvector(object):
__doc__ = """
Converts Cartesian position vector in meters to n-vector.
Parameters
----------
p_EB_E: 3 x n array
Cartesian position vector(s) from E to B, decomposed in E.
a: real scalar, default WGS-84 ellipsoid.
Semi-major axis of the Earth ellipsoid given in [m].
f: real scalar, default WGS-84 ellipsoid.
Flattening [no unit] of the Earth ellipsoid. If f==0 then spherical
Earth with radius a is used in stead of WGS-84.
R_Ee : 3 x 3 array
rotation matrix defining the axes of the coordinate frame E.
Returns
-------
n_EB_E: 3 x n array
n-vector(s) [no unit] of position B, decomposed in E.
depth: 1 x n array
Depth(s) [m] of system B, relative to the ellipsoid (depth = -height)
Notes
-----
The position of B (typically body) relative to E (typically Earth) is
given into this function as cartesian position vector p_EB_E, in meters.
("ECEF-vector"). The function converts to n-vector, n_EB_E and its
depth, depth.
The calculation is excact, taking the ellipsity of the Earth into account.
It is also non-singular as both n-vector and p-vector are non-singular
(except for the center of the Earth).
The default ellipsoid model used is WGS-84, but other ellipsoids/spheres
might be specified.
Examples
--------
{0}
See also
--------
n_EB_E2p_EB_E, n_EA_E_and_p_AB_E2n_EB_E, n_EA_E_and_n_EB_E2p_AB_E
""".format(_examples.get_examples([3], OO=False))
class _DeltaFromPositionAtoB(object):
__doc__ = """
Return the delta vector from position A to B.
Parameters
----------
n_EA_E, n_EB_E: 3 x n array
n-vector(s) [no unit] of position A and B, decomposed in E.
z_EA, z_EB: 1 x n array
Depth(s) [m] of system A and B, relative to the ellipsoid.
(z_EA = -height, z_EB = -height)
a: real scalar, default WGS-84 ellipsoid.
Semi-major axis of the Earth ellipsoid given in [m].
f: real scalar, default WGS-84 ellipsoid.
Flattening [no unit] of the Earth ellipsoid. If f==0 then spherical
Earth with radius a is used in stead of WGS-84.
R_Ee : 3 x 3 array
rotation matrix defining the axes of the coordinate frame E.
Returns
-------
p_AB_E: 3 x n array
Cartesian position vector(s) from A to B, decomposed in E.
Notes
-----
The n-vectors for positions A (n_EA_E) and B (n_EB_E) are given. The
output is the delta vector from A to B (p_AB_E).
The calculation is excact, taking the ellipsity of the Earth into account.
It is also non-singular as both n-vector and p-vector are non-singular
(except for the center of the Earth).
The default ellipsoid model used is WGS-84, but other ellipsoids/spheres
might be specified.
Examples
--------
{0}
See also
--------
n_EA_E_and_p_AB_E2n_EB_E, p_EB_E2n_EB_E, n_EB_E2p_EB_E
""".format(_examples.get_examples([1], False))
class _GreatCircleDistance(object):
__doc__ = """ Return great circle distance between positions A and B
Parameters
----------
n_EA_E, n_EB_E: 3 x n array
n-vector(s) [no unit] of position A and B, decomposed in E.
radius: real scalar
radius of sphere.
Formulae is given by equation (16) in Gade (2010) and is well
conditioned for all angles.
Examples
--------
{0}
""".format(_examples.get_examples([5], OO=False))
class _MeanHorizontalPosition(object):
__doc__ = """
Return the n-vector of the horizontal mean position.
Parameters
----------
n_EB_E: 3 x n array
n-vectors [no unit] of positions Bi, decomposed in E.
Returns
-------
p_EM_E: 3 x 1 array
n-vector [no unit] of the mean positions of all Bi, decomposed in E.
Examples
--------
{0}
""".format(_examples.get_examples([7], OO=False))
class _ClosestPointOnGreatCircle(object):
__doc__ = """ Return closest point C on great circle path A to position B.
Parameters
----------
path: tuple of 2 n-vectors of 3 x n arrays
2 n-vectors of positions defining path A, decomposed in E.
n_EB_E: 3 x m array
n-vector(s) of position B to find the closest point to.
Returns
-------
n_EC_E: 3 x max(m, n) array
n-vector(s) of closest position C on great circle path A
Examples
--------
{0}
""".format(_examples.get_examples([10], OO=False))
class _Intersect(object):
__doc__ = """Return the intersection(s) between the great circles of the two paths
Parameters
----------
path_a, path_b: tuple of 2 n-vectors
defining path A and path B, respectively.
Path A and B has shape 2 x 3 x n and 2 x 3 x m, respectively.
Returns
-------
n_EC_E : array of shape 3 x max(n, m)
n-vector(s) [no unit] of position C decomposed in E.
point(s) of intersection between paths.
Examples
--------
{0}
""".format(_examples.get_examples([9], OO=False))
class _PositionBFromAzimuthAndDistanceFromPositionA(object):
__doc__ = """
Return position B from azimuth and distance from position A
Parameters
----------
n_EA_E: 3 x n array
n-vector(s) [no unit] of position A decomposed in E.
distance_rad: n, array
great circle distance [rad] from position A to B
azimuth: n array
Angle [rad] the line makes with a meridian, taken clockwise from north.
Returns
-------
n_EB_E: 3 x n array
n-vector(s) [no unit] of position B decomposed in E.
Examples
--------
{0}
""".format(_examples.get_examples([8], OO=False))
class GeoPath(object):
__doc__ = """
Geographical path between two positions in Frame E
Parameters
----------
positionA, positionB: Nvector, GeoPoint or ECEFvector objects
The path is defined by the line between position A and B, decomposed
in E.
Examples
--------
{0}
""".format(_examples.get_examples([5, 6, 9, 10]))
def __init__(self, positionA, positionB):
self.positionA = positionA
self.positionB = positionB
def nvectors(self):
""" Return positionA and positionB as n-vectors
"""
return self.positionA.to_nvector(), self.positionB.to_nvector()
def geo_points(self):
""" Return positionA and positionB as geo-points
"""
return self.positionA.to_geo_point(), self.positionB.to_geo_point()
def ecef_vectors(self):
""" Return positionA and positionB as ECEF-vectors
"""
return self.positionA.to_ecef_vector(), self.positionB.to_ecef_vector()
def nvector_normals(self):
n_EA_E, n_EB_E = self.nvectors()
return n_EA_E.normal, n_EB_E.normal
def _get_average_radius(self):
# n1 = self.positionA.to_nvector()
# n2 = self.positionB.to_nvector()
# n_EM_E = mean_horizontal_position(np.hstack((n1.normal, n2.normal)))
# p_EM_E = n1.frame.Nvector(n_EM_E).to_ecef_vector()
# radius = norm(p_EM_E.pvector, axis=0)
# radius = (norm(p_E1_E.pvector, axis=0) +
# norm(p_E2_E.pvector, axis=0)) / 2
p_E1_E, p_E2_E = self.ecef_vectors()
radius = (p_E1_E.length + p_E2_E.length) / 2
return radius
def cross_track_distance(self, point, method='greatcircle', radius=None):
"""
Return cross track distance from path to point.
Parameters
----------
point: GeoPoint, Nvector or ECEFvector object
position to measure the cross track distance to.
radius: real scalar
radius of sphere in [m]. Default mean Earth radius
method: string
defining distance calculated. Options are:
'greatcircle' or 'euclidean'
Returns
-------
distance: real scalar
distance in [m]
"""
if radius is None:
radius = self._get_average_radius()
path = self.nvector_normals()
n_c = point.to_nvector().normal
return cross_track_distance(path, n_c, method=method, radius=radius)
def track_distance(self, method='greatcircle', radius=None):
"""
Return the distance of the path.
Parameters
----------
method: string
'greatcircle':
'euclidean'
'exact'
radius: real scalar
radius of sphere
"""
if method == 'exact':
point_a, point_b = self.geo_points()
s_ab, _angle1, _angle2 = point_a.distance_and_azimuth(point_b)
return s_ab
if radius is None:
radius = self._get_average_radius()
n_EA_E, n_EB_E = self.nvector_normals()
if method[:2] == "eu":
return euclidean_distance(n_EA_E, n_EB_E, radius)
return great_circle_distance(n_EA_E, n_EB_E, radius)
@deprecate
def intersection(self, path):
"""
Deprecated use intersect instead
"""
return self.intersect(path)
def intersect(self, path):
"""
Return the intersection(s) between the great circles of the two paths
Parameters
----------
path: GeoPath object
path to intersect
Returns
-------
point: GeoPoint
point of intersection between paths
"""
frame = self.positionA.frame
path_a = self.nvector_normals()
path_b = path.nvector_normals()
n_EC_E = intersect(path_a, path_b)
return frame.Nvector(n_EC_E)
def _on_ellipsoid_path(self, point, rtol=1e-6, atol=1e-8):
point_a, point_b = self.geo_points()
distanceAB, azimuth_ab, _azi_ba = point_a.distance_and_azimuth(point_b)
distanceAC, azimuth_ac, _azi_ca = point_a.distance_and_azimuth(point)
return distanceAB >= distanceAC and np.allclose(
azimuth_ab, azimuth_ac, rtol=rtol, atol=atol)
def on_great_circle(self, point, rtol=1e-6, atol=1e-8):
distance = np.abs(self.cross_track_distance(point))
return np.isclose(distance, 0, rtol, atol)
def _on_great_circle_path(self, point, radius=None, rtol=1e-6, atol=1e-8):
if radius is None:
radius = self._get_average_radius()
path = self.nvector_normals()
point_c = point.to_nvector().normal
return on_great_circle_path(path, point_c, radius, rtol, atol)
# pointA, pointB = path
# p_ba = pointB - pointA
# p_ca = point_c - pointA
# is_parallell = np.all(np.isclose(np.cross(p_ba, p_ca, axis=0), 0,
# rtol, atol), axis=0)
# same_direction = (np.all(np.sign(np.dot(p_ba.T, p_ca)) == 1, axis=0) &
# np.all(np.sign(p_ba) == np.sign(p_ca), axis=0))
# return (is_parallell & same_direction &
# norm(p_ba, axis=0) >= norm(p_ca, axis=0))
def on_path(self, point, method='greatcircle', rtol=1e-6, atol=1e-8):
"""
Return True if point is on the path between A and B
Parameters
----------
point : Nvector, GeoPoint or ECEFvector
point to test
method: string
'greatcircle':
'exact'
Examples
--------
>>> import nvector as nv
>>> wgs84 = nv.FrameE(name='WGS84')
>>> pointA = wgs84.GeoPoint(89, 0, degrees=True)
>>> pointB = wgs84.GeoPoint(80, 0, degrees=True)
>>> path = nv.GeoPath(pointA, pointB)
>>> pointC = path.interpolate(0.6).to_geo_point()
>>> path.on_path(pointC)
array([ True], dtype=bool)
>>> pointD = path.interpolate(1.000000001).to_geo_point()
>>> path.on_path(pointD)
array([False], dtype=bool)
>>> pointE = wgs84.GeoPoint(85, 0.0001, degrees=True)
>>> path.on_path(pointE)
array([False], dtype=bool)
>>> pointC = path.interpolate(-2).to_geo_point()
>>> path.on_path(pointC)
array([False], dtype=bool)
>>> path = nv.GeoPath(pointC, pointA)
"""
if method == 'exact':
return self._on_ellipsoid_path(point, rtol=rtol, atol=atol)
return self._on_great_circle_path(point, rtol=rtol, atol=atol)
def closest_point_on_great_circle(self, point):
nvector = point.to_nvector()
path = self.nvector_normals()
n = closest_point_on_great_circle(path, nvector.normal)
return nvector.frame.Nvector(n, nvector.z).to_geo_point()
def closest_point_on_path(self, point):
"""
Returns closest point on great circle path segment to the point.
If the point is within the extent of the segment, the point returned is
on the segment path otherwise, it is the closest endpoint defining the
path segment.
Parameters
----------
point: GeoPoint
point of intersection between paths
Returns
-------
closest_point: GeoPoint
closest point on path segment.
Example
-------
>>> import nvector as nv
>>> wgs84 = nv.FrameE(name='WGS84')
>>> pointA = wgs84.GeoPoint(51., 1., degrees=True)
>>> pointB = wgs84.GeoPoint(51., 2., degrees=True)
>>> pointC = wgs84.GeoPoint(51., 1.9, degrees=True)
>>> path = nv.GeoPath(pointA, pointB)
>>> point = path.closest_point_on_path(pointC)
>>> np.allclose((point.latitude_deg, point.longitude_deg),
... ([51.00038411380564], [1.900003311624411]))
True
>>> np.allclose(GeoPath(pointC, point).track_distance(), 42.67368351)
True
>>> pointD = wgs84.GeoPoint(51.0, 2.1, degrees=True)
>>> pointE = path.closest_point_on_path(pointD) # 51.0000, 002.0000
>>> pointE.latitude_deg, pointE.longitude_deg
(51.0, 2.0)
"""
point_c = self.closest_point_on_great_circle(point)
if self.on_path(point_c):
return point_c
n0 = point.to_nvector().normal
n1, n2 = self.nvector_normals()
radius = self._get_average_radius()
d1 = great_circle_distance(n1, n0, radius)
d2 = great_circle_distance(n2, n0, radius)
if d1 < d2:
return self.positionA.to_geo_point()
return self.positionB.to_geo_point()
def interpolate(self, ti):
"""
Return the interpolated point along the path
Parameters
----------
ti: real scalar
interpolation time assuming position A and B is at t0=0 and t1=1,
respectively.
Returns
-------
point: Nvector
point of interpolation along path
"""
point_a, point_b = self.nvectors()
point_c = point_a + (point_b - point_a) * ti
point_c.normal = unit(point_c.normal, norm_zero_vector=np.nan)
return point_c
class ECEFvector(Pvector):
__doc__ = """
Geographical position given as Cartesian position vector in frame E
Parameters
----------
pvector: 3 x n array
Cartesian position vector(s) [m] from E to B, decomposed in E.
frame: FrameE object
reference ellipsoid. The default ellipsoid model used is WGS84, but
other ellipsoids/spheres might be specified.
Notes
-----
The position of B (typically body) relative to E (typically Earth) is
given into this function as p-vector, p_EB_E relative to the center of the
frame.
Examples
--------
{0}
See also
--------
GeoPoint, ECEFvector, Pvector
""".format(_examples.get_examples([3, 4]))
def __init__(self, pvector, frame=None):
self.pvector = pvector
self.frame = _default_frame(frame)
def change_frame(self, frame):
"""
Converts to Cartesian position vector in another frame
Parameters
----------
frame: FrameB, FrameN or frameL object
local frame M used to convert p_AB_E (position vector from A to B,
decomposed in E) to a cartesian vector p_AB_M decomposed in M.
Returns
-------
p_AB_M: Pvector object
position vector from A to B, decomposed in frame M.
See also
--------
n_EB_E2p_EB_E,
n_EA_E_and_p_AB_E2n_EB_E,
n_EA_E_and_n_EB_E2p_AB_E.
"""
_check_frames(self, frame.nvector)
p_AB_E = self.pvector
p_AB_N = mdot(np.rollaxis(frame.R_EN, 1, 0), p_AB_E[:, None, ...])
return Pvector(p_AB_N.reshape(3, -1), frame=frame)
def to_ecef_vector(self):
return self
def to_geo_point(self):
"""
Converts ECEF-vector to geo-point.
Returns
-------
point: GeoPoint object
containing geodetic latitude and longitude given in [rad or deg]
and depth, z, relative to the ellipsoid (depth = -height).
See also
--------
n_E2lat_lon, n_EB_E2p_EB_E, GeoPoint, Nvector, ECEFvector, Pvector
"""
return self.to_nvector().to_geo_point()
def to_nvector(self):
"""
Converts ECEF-vector to n-vector.
Returns
-------
n_EB_E: Nvector object
n-vector(s) [no unit] of position B, decomposed in E.
Notes
-----
The calculation is excact, taking the ellipsity of the Earth into
account. It is also non-singular as both n-vector and p-vector are
non-singular (except for the center of the Earth).
See also
--------
n_EB_E2p_EB_E, Nvector
"""
frame = self.frame
p_EB_E = self.pvector
R_Ee = frame.R_Ee
n_EB_E, depth = p_EB_E2n_EB_E(p_EB_E, a=frame.a, f=frame.f, R_Ee=R_Ee)
return Nvector(n_EB_E, z=depth, frame=frame)
delta_to = _delta
def __add__(self, other):
_check_frames(self, other)
return ECEFvector(self.pvector + other.pvector, self.frame)
def __sub__(self, other):
_check_frames(self, other)
return ECEFvector(self.pvector - other.pvector, self.frame)
def __neg__(self):
return ECEFvector(-self.pvector, self.frame)
