def test_Ex9_intersection(): # Two paths A and B are given by two pairs of positions: # Enter elements as lat/long in deg: n_EA1_E = lat_lon2n_E(rad(10), rad(20)) n_EA2_E = lat_lon2n_E(rad(30), rad(40)) n_EB1_E = lat_lon2n_E(rad(50), rad(60)) n_EB2_E = lat_lon2n_E(rad(70), rad(80)) # Find the intersection between the two paths, n_EC_E: n_EC_E_tmp = unit(np.cross(np.cross(n_EA1_E, n_EA2_E, axis=0), np.cross(n_EB1_E, n_EB2_E, axis=0), axis=0)) # n_EC_E_tmp is one of two solutions, the other is -n_EC_E_tmp. Select # the one that is closet to n_EA1_E, by selecting sign from the dot # product between n_EC_E_tmp and n_EA1_E: n_EC_E = np.sign(np.dot(n_EC_E_tmp.T, n_EA1_E)) * n_EC_E_tmp # When displaying the resulting position for humans, it is more # convenient to see lat, long: lat_EC, long_EC = n_E2lat_lon(n_EC_E) msg = 'Ex9, Intersection: lat, long = {} {} deg' print(msg.format(deg(lat_EC), deg(long_EC))) assert_array_almost_equal(deg(lat_EC), 40.31864307) assert_array_almost_equal(deg(long_EC), 55.90186788)
def plot_mean_position(): """ Example ------- >>> plot_mean_position() Ex7, Average lat=[ 67.23615295], lon=[-6.91751117] """ positions = np.array([ (90, 0), (60, 10), (50, -20), ]) lats, lons = positions.T nvecs = lat_lon2n_E(rad(lats), rad(lons)) # Find the horizontal mean position: n_EM_E = unit(np.sum(nvecs, axis=1).reshape((3, 1))) lat, lon = n_E2lat_lon(n_EM_E) lat, lon = deg(lat), deg(lon) print(('Ex7, Average lat={0}, lon={1}'.format(lat, lon))) map1 = Basemap(projection='ortho', lat_0=int(lat), lon_0=int(lon), resolution='l') plot_world(map1) x, y = map1(lon, lat) map1.scatter(x, y, linewidth=5, marker='o', color='r') x1, y1 = map1(lons, lats) # print(len(lons), x1, y1) map1.scatter(x1, y1, linewidth=5, marker='o', color='k') plt.title('Figure of mean position (red dot) compared to positions ' 'A, B, and C (black dots).')
def test_Ex10_cross_track_distance(): # Position A1 and A2 and B are given as n_EA1_E, n_EA2_E, and n_EB_E: # Enter elements as lat/long in deg: n_EA1_E = lat_lon2n_E(rad(0), rad(0)) n_EA2_E = lat_lon2n_E(rad(10), rad(0)) n_EB_E = lat_lon2n_E(rad(1), rad(0.1)) radius = 6371e3 # m, mean Earth radius # Find the cross track distance from path A to position B. # SOLUTION: # Find the unit normal to the great circle: c_E = unit(np.cross(n_EA1_E, n_EA2_E, axis=0)) # Find the great circle cross track distance: s_xt = -np.arcsin(np.dot(c_E.T, n_EB_E)) * radius # Find the Euclidean cross track distance: d_xt = -np.dot(c_E.T, n_EB_E) * radius msg = 'Ex10, Cross track distance = {} m, Euclidean = {} m' print(msg.format(s_xt, d_xt)) assert_array_almost_equal(s_xt, 11117.79911015) assert_array_almost_equal(d_xt, 11117.79346741)
def test_Ex10_cross_track_distance(): # Position A1 and A2 and B are given as n_EA1_E, n_EA2_E, and n_EB_E: # Enter elements as lat/long in deg: n_EA1_E = lat_lon2n_E(rad(0), rad(0)) n_EA2_E = lat_lon2n_E(rad(10), rad(0)) n_EB_E = lat_lon2n_E(rad(1), rad(0.1)) r_Earth = 6371e3 # m, mean Earth radius # Find the cross track distance from path A to position B. # SOLUTION: # Find the unit normal to the great circle: c_E = unit(np.cross(n_EA1_E, n_EA2_E, axis=0)) # Find the great circle cross track distance: s_xt = (np.arccos(np.dot(c_E.T, n_EB_E)) - np.pi / 2) * r_Earth # Find the Euclidean cross track distance: d_xt = -np.dot(c_E.T, n_EB_E) * r_Earth msg = 'Ex10, Cross track distance = {} m, Euclidean = {} m' print(msg.format(s_xt, d_xt)) assert_array_almost_equal(s_xt, 11117.79911015) assert_array_almost_equal(d_xt, 11117.79346741)
def plot_mean_position(): positions = np.array([(90, 0), (60, 10), (50, -20), ]) lats, lons = positions.T nvecs = lat_lon2n_E(rad(lats), rad(lons)) # Find the horizontal mean position: n_EM_E = unit(np.sum(nvecs, axis=1).reshape((3, 1))) lat, lon = n_E2lat_lon(n_EM_E) lat, lon = deg(lat), deg(lon) print('Ex7, Average lat={0}, lon={1}'.format(lat, lon)) map1 = Basemap(projection='ortho', lat_0=int(lat), lon_0=int(lon), resolution='l') plot_world(map1) x, y = map1(lon, lat) map1.scatter(x, y, linewidth=5, marker='o', color='r') x1, y1 = map1(lons, lats) print(len(lons), x1, y1) map1.scatter(x1, y1, linewidth=5, marker='o', color='k') plt.title('Figure of mean position (red dot) compared to positions ' 'A, B, and C (black dots).')
def plot_mean_position(): """ Example ------- >>> plot_mean_position() Ex7, Average lat=67.2, lon=-6.9 >>> plt.show() # doctest: +SKIP >>> plt.close() """ positions = np.array([ (90, 0), (60, 10), (50, -20), ]) lats, lons = np.transpose(positions) nvecs = lat_lon2n_E(rad(lats), rad(lons)) # Find the horizontal mean position: n_EM_E = unit(np.sum(nvecs, axis=1).reshape((3, 1))) lat, lon = n_E2lat_lon(n_EM_E) lat, lon = deg(lat), deg(lon) print('Ex7, Average lat={0:2.1f}, lon={1:2.1f}'.format(lat[0], lon[0])) plotter = _init_plotter(lat, lon) plotter(lon, lat, linewidth=5, marker='o', color='r') plotter(lons, lats, linewidth=5, marker='o', color='k') plt.title('Figure of mean position (red dot) compared to \npositions ' 'A, B, and C (black dots).')
def test_Ex2_B_and_delta_in_frame_B_to_C_in_frame_E(): # delta vector from B to C, decomposed in B is given: # A custom reference ellipsoid is given (replacing WGS-84): wgs72 = FrameE(name="WGS72") # Position and orientation of B is given 400m above E: n_EB_E = wgs72.Nvector(unit([[1], [2], [3]]), z=-400) frame_B = FrameB(n_EB_E, yaw=10, pitch=20, roll=30, degrees=True) p_BC_B = frame_B.Pvector(np.r_[3000, 2000, 100].reshape((-1, 1))) p_BC_E = p_BC_B.to_ecef_vector() p_EB_E = n_EB_E.to_ecef_vector() p_EC_E = p_EB_E + p_BC_E pointC = p_EC_E.to_geo_point() lat_EC, long_EC = pointC.latitude_deg, pointC.longitude_deg z_EC = pointC.z # Here we also assume that the user wants output height (= - depth): msg = "Ex2, Pos C: lat, long = {},{} deg, height = {} m" print(msg.format(lat_EC, long_EC, -z_EC)) assert_array_almost_equal(lat_EC, 53.32637826) assert_array_almost_equal(long_EC, 63.46812344) assert_array_almost_equal(z_EC, -406.00719607)
def test_Ex2_B_and_delta_in_frame_B_to_C_in_frame_E(): # delta vector from B to C, decomposed in B is given: # A custom reference ellipsoid is given (replacing WGS-84): wgs72 = FrameE(name='WGS72') # Position and orientation of B is given 400m above E: n_EB_E = wgs72.Nvector(unit([[1], [2], [3]]), z=-400) frame_B = FrameB(n_EB_E, yaw=10, pitch=20, roll=30, degrees=True) p_BC_B = frame_B.Pvector(np.r_[3000, 2000, 100].reshape((-1, 1))) p_BC_E = p_BC_B.to_ecef_vector() p_EB_E = n_EB_E.to_ecef_vector() p_EC_E = p_EB_E + p_BC_E pointC = p_EC_E.to_geo_point() lat_EC, lon_EC = pointC.latitude_deg, pointC.longitude_deg z_EC = pointC.z # Here we also assume that the user wants output height (= - depth): msg = 'Ex2, Pos C: lat, long = {},{} deg, height = {} m' print(msg.format(lat_EC, lon_EC, -z_EC)) assert_array_almost_equal(lat_EC, 53.32637826) assert_array_almost_equal(lon_EC, 63.46812344) assert_array_almost_equal(z_EC, -406.00719607)
def test_Ex9_intersect(): # Two paths A and B are given by two pairs of positions: # Enter elements as lat/long in deg: n_EA1_E = lat_lon2n_E(rad(10), rad(20)) n_EA2_E = lat_lon2n_E(rad(30), rad(40)) n_EB1_E = lat_lon2n_E(rad(50), rad(60)) n_EB2_E = lat_lon2n_E(rad(70), rad(80)) # Find the intersection between the two paths, n_EC_E: n_EC_E_tmp = unit( np.cross(np.cross(n_EA1_E, n_EA2_E, axis=0), np.cross(n_EB1_E, n_EB2_E, axis=0), axis=0)) # n_EC_E_tmp is one of two solutions, the other is -n_EC_E_tmp. Select # the one that is closet to n_EA1_E, by selecting sign from the dot # product between n_EC_E_tmp and n_EA1_E: n_EC_E = np.sign(np.dot(n_EC_E_tmp.T, n_EA1_E)) * n_EC_E_tmp # When displaying the resulting position for humans, it is more # convenient to see lat, long: lat_EC, long_EC = n_E2lat_lon(n_EC_E) msg = 'Ex9, Intersection: lat, long = {} {} deg' print(msg.format(deg(lat_EC), deg(long_EC))) assert_array_almost_equal(deg(lat_EC), 40.31864307) assert_array_almost_equal(deg(long_EC), 55.90186788)
def test_Ex6_interpolated_position(): # Position B at time t0 and t2 is given as n_EB_E_t0 and n_EB_E_t1: # Enter elements as lat/long in deg: n_EB_E_t0 = lat_lon2n_E(rad(89), rad(0)) n_EB_E_t1 = lat_lon2n_E(rad(89), rad(180)) # The times are given as: t0 = 10 t1 = 20 ti = 16 # time of interpolation # Find the interpolated position at time ti, n_EB_E_ti # SOLUTION: # Using standard interpolation: n_EB_E_ti = unit(n_EB_E_t0 + (ti - t0) * (n_EB_E_t1 - n_EB_E_t0) / (t1 - t0)) # When displaying the resulting position for humans, it is more # convenient to see lat, long: lat_EB_ti, long_EB_ti = n_E2lat_lon(n_EB_E_ti) msg = 'Ex6, Interpolated position: lat, long = {} {} deg' print(msg.format(deg(lat_EB_ti), deg(long_EB_ti))) assert_array_almost_equal(deg(lat_EB_ti), 89.7999805) assert_array_almost_equal(deg(long_EB_ti), 180.)
def find_c_point_from_precalc(to_point, point1, point2, c12, p1h, p2h, dp1p2): tpn = to_point.nv ctp = cross(tpn, c12, axis=0) try: c = unit(cross(ctp, c12, axis=0)) except Exception: print((to_point, point1, point2)) raise sutable_c = None for co in (c, 0 - c): co_rs = co.reshape((3, )) dp1co = arccos(arccos_limit(dot(p1h, co_rs))) dp2co = arccos(arccos_limit(dot(p2h, co_rs))) if abs(dp1co + dp2co - dp1p2) < 0.000001: sutable_c = co break if sutable_c is not None: c_point_lat, c_point_lng = n_E2lat_lon(sutable_c) c_point = Point(lat=rad2deg(c_point_lat[0]), lng=rad2deg(c_point_lng[0])) c_dist = distance(to_point, c_point) else: c_dist, c_point = min( ((distance(to_point, p), p) for p in (point1, point2)), key=itemgetter(0)) return find_c_point_result(c_dist, c_point)
def test_compare_B_frames(self): E = FrameE(name="WGS84") E2 = FrameE(name="WGS72") n_EB_E = E.Nvector(unit([[1], [2], [3]]), z=-400) B = FrameB(n_EB_E, yaw=10, pitch=20, roll=30, degrees=True) self.assertEqual(B, B) self.assertNotEqual(B, E) B2 = FrameB(n_EB_E, yaw=1, pitch=20, roll=30, degrees=True) self.assertNotEqual(B, B2) B3 = FrameB(n_EB_E, yaw=10, pitch=20, roll=30, degrees=True) self.assertEqual(B, B3) n_EC_E = E.Nvector(unit([[1], [2], [2]]), z=-400) B4 = FrameB(n_EC_E, yaw=10, pitch=20, roll=30, degrees=True) self.assertNotEqual(B, B4)
def test_compare_B_frames(self): E = FrameE(name='WGS84') E2 = FrameE(name='WGS72') n_EB_E = E.Nvector(unit([[1], [2], [3]]), z=-400) B = FrameB(n_EB_E, yaw=10, pitch=20, roll=30, degrees=True) self.assertEqual(B, B) self.assertNotEqual(B, E) B2 = FrameB(n_EB_E, yaw=1, pitch=20, roll=30, degrees=True) self.assertNotEqual(B, B2) B3 = FrameB(n_EB_E, yaw=10, pitch=20, roll=30, degrees=True) self.assertEqual(B, B3) n_EC_E = E.Nvector(unit([[1], [2], [2]]), z=-400) B4 = FrameB(n_EC_E, yaw=10, pitch=20, roll=30, degrees=True) self.assertNotEqual(B, B4) n_ED_E = E2.Nvector(unit([[1], [2], [3]]), z=-400) B5 = FrameB(n_ED_E, yaw=10, pitch=20, roll=30, degrees=True) self.assertNotEqual(B, B5)
def find_closest_point_pair(points, to_point, req_min_dist=20, stop_after_dist=50): tpn = to_point.nv min_distance = None min_point_pair = None min_c_point = None for point1, point2 in pairs(points): p1 = point1.nv p2 = point2.nv c12 = cross(p1, p2, axis=0) ctp = cross(tpn, c12, axis=0) c = unit(cross(ctp, c12, axis=0)) p1h = p1.reshape((3, )) p2h = p2.reshape((3, )) dp1p2 = arccos(dot(p1h, p2h)) sutable_c = None for co in (c, 0 - c): co_rs = co.reshape((3, )) dp1co = arccos(dot(p1h, co_rs)) dp2co = arccos(dot(p2h, co_rs)) if abs(dp1co + dp2co - dp1p2) < 0.000001: sutable_c = co break if sutable_c is not None: c_point_lat, c_point_lng = n_E2lat_lon(sutable_c) c_point = Point(lat=rad2deg(c_point_lat[0]), lng=rad2deg(c_point_lng[0])) c_dist = distance(to_point, c_point) else: c_dist, c_point = min( ((distance(to_point, p), p) for p in (point1, point2))) if min_distance is None or c_dist < min_distance: min_distance = c_dist min_point_pair = (point1, point2) min_c_point = c_point if min_distance < req_min_dist and c_dist > stop_after_dist: break return min_point_pair, min_c_point, min_distance
def test_Ex7_mean_position(): # Three positions A, B and C are given: # Enter elements as lat/long in deg: n_EA_E = lat_lon2n_E(rad(90), rad(0)) n_EB_E = lat_lon2n_E(rad(60), rad(10)) n_EC_E = lat_lon2n_E(rad(50), rad(-20)) # Find the horizontal mean position: n_EM_E = unit(n_EA_E + n_EB_E + n_EC_E) # The result is best viewed with a figure that shows the n-vectors # relative to an Earth-model: print('Ex7, See figure') # plot_Earth_figure(n_EA_E,n_EB_E,n_EC_E,n_EM_E) assert_array_almost_equal(n_EM_E.ravel(), [0.384117, -0.046602, 0.922107]) # Alternatively: n_EM_E = mean_horizontal_position(np.hstack((n_EA_E, n_EB_E, n_EC_E))) assert_array_almost_equal(n_EM_E.ravel(), [0.384117, -0.046602, 0.922107])
def test_Ex2_B_and_delta_in_frame_B_to_C_in_frame_E(): # delta vector from B to C, decomposed in B is given: p_BC_B = np.r_[3000, 2000, 100].reshape((-1, 1)) # Position and orientation of B is given: n_EB_E = unit([[1], [2], [3]]) # unit to get unit length of vector z_EB = -400 R_NB = zyx2R(rad(10), rad(20), rad(30)) # the three angles are yaw, pitch, and roll # A custom reference ellipsoid is given (replacing WGS-84): a, f = 6378135, 1.0 / 298.26 # (WGS-72) # Find the position of C. # SOLUTION: # Step1: Find R_EN: R_EN = n_E2R_EN(n_EB_E) # Step2: Find R_EB, from R_EN and R_NB: R_EB = np.dot(R_EN, R_NB) # Note: closest frames cancel # Step3: Decompose the delta vector in E: p_BC_E = np.dot(R_EB, p_BC_B) # no transpose of R_EB, since the vector is in B # Step4: Find the position of C, using the functions that goes from one # position and a delta, to a new position: n_EC_E, z_EC = n_EA_E_and_p_AB_E2n_EB_E(n_EB_E, p_BC_E, z_EB, a, f) # When displaying the resulting position for humans, it is more # convenient to see lat, long: lat_EC, long_EC = n_E2lat_lon(n_EC_E) # Here we also assume that the user wants output height (= - depth): msg = 'Ex2, Pos C: lat, long = {},{} deg, height = {} m' print(msg.format(deg(lat_EC), deg(long_EC), -z_EC)) assert_array_almost_equal(deg(lat_EC), 53.32637826) assert_array_almost_equal(deg(long_EC), 63.46812344) assert_array_almost_equal(z_EC, -406.00719607)
def interpolate(path, ti): """ Return the interpolated point along the path Parameters ---------- path: tuple of n-vectors (positionA, po) 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 """ n_EB_E_t0, n_EB_E_t1 = path n_EB_E_ti = unit(n_EB_E_t0 + ti * (n_EB_E_t1 - n_EB_E_t0), norm_zero_vector=nan) return n_EB_E_ti
def test_small_and_large_cross_track_distance(self): radius = 6371e3 # m, mean Earth radius n_EA1_E = lat_lon2n_E(rad(5), rad(10)) n_EA2_E = lat_lon2n_E(rad(10), rad(10)) n_EB0_E = lat_lon2n_E(rad(7), rad(10.1)) path = (n_EA1_E, n_EA2_E) n_EB1_E = closest_point_on_great_circle(path, n_EB0_E) for s_xt0 in [np.pi / 3 * radius, 10., 0.1, 1e-4, 1e-8]: distance_rad = s_xt0 / radius n_EB_E = n_EA_E_distance_and_azimuth2n_EB_E( n_EB1_E, distance_rad, np.pi / 2) n_EB2_E = closest_point_on_great_circle(path, n_EB_E) s_xt = great_circle_distance(n_EB1_E, n_EB_E, radius) c_E = unit(np.cross(n_EA1_E, n_EA2_E, axis=0)) s_xt2 = (np.arccos(np.dot(c_E.T, n_EB_E)) - np.pi / 2) * radius s_xt3 = cross_track_distance(path, n_EB_E, method='greatcircle', radius=radius) s_xt4 = np.arctan2( -np.dot(c_E.T, n_EB_E), np.linalg.norm(np.cross(c_E, n_EB_E, axis=0), axis=0)) * radius assert_array_almost_equal(n_EB2_E, n_EB1_E) assert_array_almost_equal(s_xt, s_xt0) assert_array_almost_equal(s_xt2, s_xt0) assert_array_almost_equal(s_xt3, s_xt0) assert_array_almost_equal(s_xt4, s_xt0) rtol = 10**(-min(9 + np.log10(s_xt0), 15)) self.assertTrue(np.abs(s_xt - s_xt0) / s_xt0 < rtol, 's_xt fails') self.assertTrue( np.abs(s_xt2 - s_xt0) / s_xt0 < rtol, 's_xt2 fails') self.assertTrue( np.abs(s_xt3 - s_xt0) / s_xt0 < rtol, 's_xt3 fails') self.assertTrue( np.abs(s_xt4 - s_xt0) / s_xt0 < rtol, 's_xt4 fails')
def getMiddlePointFromList(positions): n_EM_E = nv.unit(sum(positions)) lat, lon = nv.n_E2lat_lon(n_EM_E) lat, lon = degrees(lat[0]), degrees(lon[0]) return lat, lon