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)