diff --git a/event/algorithm/coordreg/EllipsoidParametric.py b/event/algorithm/coordreg/EllipsoidParametric.py index c9715c4..c9599b3 100644 --- a/event/algorithm/coordreg/EllipsoidParametric.py +++ b/event/algorithm/coordreg/EllipsoidParametric.py @@ -94,14 +94,22 @@ class EllipsoidParametric: """ # rotation matrix + phi = np.pi/2 - ellipsoid.pitch + theta = ellipsoid.yaw + np.pi/2 + phi = np.deg2rad(3.834) + theta = -np.deg2rad(-77+90) + phi = ellipsoid.pitch theta = ellipsoid.yaw - phi = 0 - theta = 0 + # R = np.array([ + # [np.cos(phi)*np.cos(theta), -np.sin(phi)*np.cos(theta), np.sin(theta)], + # [np.sin(phi), np.cos(phi), 0], + # [-np.cos(phi)*np.sin(theta), np.sin(phi)*np.sin(theta), np.cos(theta)] + # ]) R = np.array([ - [np.cos(phi)*np.cos(theta), -np.sin(phi)*np.cos(theta), np.sin(theta)], - [np.sin(phi), np.cos(phi), 0], - [-np.cos(phi)*np.sin(theta), np.sin(phi)*np.sin(theta), np.cos(theta)] + [np.cos(theta), -np.sin(theta)*np.cos(phi), np.sin(theta)*np.sin(phi)], + [np.sin(theta), np.cos(theta)*np.cos(phi), -np.cos(theta)*np.sin(phi)], + [0, np.sin(phi), np.cos(phi)] ]) # rotation matrix normal @@ -129,8 +137,8 @@ class EllipsoidParametric: #r_1 = np.dot(r, np.dot(R, R2)) + ellipsoid.midpoint #r_1 = np.dot(r, R) + ellipsoid.midpoint - #r_1 = np.dot(r, R) - r_1 = r + r_1 = np.dot(r, R) + #r_1 = r a, b, c = Geometry.ecef2lla( ellipsoid.midpoint[0], ellipsoid.midpoint[1], ellipsoid.midpoint[2]) diff --git a/event/algorithm/geometry/Geometry.py b/event/algorithm/geometry/Geometry.py index 017547f..d904f44 100644 --- a/event/algorithm/geometry/Geometry.py +++ b/event/algorithm/geometry/Geometry.py @@ -10,15 +10,16 @@ class Geometry: """ @class Geometry @brief A class to store geometric functions. + @details Assumes WGS-84 ellipsoid for all functions. """ - def __init__(self, f1, f2, name): + def __init__(self): """ @brief Constructor for the Geometry class. """ - def lla2ecef(latitude, longitude, altitude): + def lla2ecef(lat, lon, alt): # WGS84 constants a = 6378137.0 # semi-major axis in meters @@ -26,8 +27,8 @@ class Geometry: e = 0.081819190842622 # Convert latitude and longitude to radians - lat_rad = math.radians(latitude) - lon_rad = math.radians(longitude) + lat_rad = math.radians(lat) + lon_rad = math.radians(lon) # Calculate the auxiliary values cos_lat = math.cos(lat_rad) @@ -35,9 +36,9 @@ class Geometry: N = a / math.sqrt(1 - f * (2 - f) * sin_lat**2) # Calculate ECEF coordinates - ecef_x = (N + altitude) * cos_lat * math.cos(lon_rad) - ecef_y = (N + altitude) * cos_lat * math.sin(lon_rad) - ecef_z = ((1-(e**2)) * N + altitude) * sin_lat + ecef_x = (N + alt) * cos_lat * math.cos(lon_rad) + ecef_y = (N + alt) * cos_lat * math.sin(lon_rad) + ecef_z = ((1-(e**2)) * N + alt) * sin_lat return ecef_x, ecef_y, ecef_z @@ -134,4 +135,51 @@ class Geometry: u = math.cos(lon) * t - math.sin(lon) * east v = math.sin(lon) * t + math.cos(lon) * east - return u, v, w \ No newline at end of file + return u, v, w + + def ecef2enu(x, y, z, lat, lon, alt): + + """ + @brief From observer to target, ECEF => ENU. + @param x (float): Target x ECEF coordinate (m). + @param y (float): Target y ECEF coordinate (m). + @param z (float): Target z ECEF coordinate (m). + @param lat (float): Observer geodetic latitude (deg). + @param lon (float): Observer geodetic longitude (deg). + @param alt (float): Observer geodetic altituder (m). + @return east (float): Target east ENU coordinate (m). + @return north (float): Target north ENU coordinate (m). + @return up (float): Target up ENU coordinate (m). + """ + + x0, y0, z0 = Geometry.lla2ecef(lat, lon, alt) + return Geometry.uvw2enu(x - x0, y - y0, z - z0, lat, lon) + + def uvw2enu(u, v, w, lat, lon): + + """ + @brief + @param u (float): Shifted ECEF coordinate (m). + @param v (float): Shifted ECEF coordinate (m). + @param w (float): Shifted ECEF coordinate (m). + @param lat (float): Observer geodetic latitude (deg). + @param lon (float): Observer geodetic longitude (deg). + @param e (float): Target east ENU coordinate (m). + @param n (float): Target north ENU coordinate (m). + @param u (float): Target up ENU coordinate (m). + """ + + lat = math.radians(lat) + lon = math.radians(lon) + + cos_lat = math.cos(lat) + sin_lat = math.sin(lat) + cos_lon = math.cos(lon) + sin_lon = math.sin(lon) + + t = cos_lon * u + sin_lon * v + e = -sin_lon * u + cos_lon * v + u = cos_lat * t + sin_lat * w + n = -sin_lat * t + cos_lat * w + + return e, n, u \ No newline at end of file diff --git a/event/data/Ellipsoid.py b/event/data/Ellipsoid.py index 8be519c..c747265 100644 --- a/event/data/Ellipsoid.py +++ b/event/data/Ellipsoid.py @@ -4,6 +4,7 @@ """ import math +from algorithm.geometry.Geometry import Geometry class Ellipsoid: @@ -29,21 +30,14 @@ class Ellipsoid: # dependent members self.midpoint = [(f1[0]+f2[0])/2, (f1[1]+f2[1])/2, (f1[2]+f2[2])/2] - vector = (f2[0]-f1[0], f2[1]-f1[1], f2[2]-f1[2]) - self.yaw = math.atan2(vector[1], vector[0]) - self.pitch = math.atan2(vector[2], - math.sqrt(vector[0]**2 + vector[1]**2)) + midpoint_lla = Geometry.ecef2lla( + self.midpoint[0], self.midpoint[1], self.midpoint[2]) + vector_enu = Geometry.ecef2enu(f1[0], f1[1], f1[2], + midpoint_lla[0], midpoint_lla[1], midpoint_lla[2]) + self.yaw = -math.atan2(vector_enu[1], vector_enu[0])-math.pi/2 + self.pitch = math.atan2(vector_enu[2], + math.sqrt(vector_enu[0]**2 + vector_enu[1]**2)) self.distance = math.sqrt( (f2[0] - f1[0])**2 + (f2[1] - f1[1])**2 + (f2[2] - f1[2])**2) - - # rotate to normal plane on WGS-84 - length = math.sqrt( - self.midpoint[0]**2 + - self.midpoint[1]**2 + - self.midpoint[2]**2 - ) - vector = [x / length for x in self.midpoint] - self.pitch_plane = math.asin(-vector[1]) - self.yaw_plane = math.atan2(-vector[0], -vector[2]) diff --git a/event/event.py b/event/event.py index 6004e53..8abea4b 100644 --- a/event/event.py +++ b/event/event.py @@ -123,7 +123,7 @@ async def event(): [x_rx, y_rx, z_rx], 'radar4.30hours.dev' ) - pointsEcef = ellipsoidParametric.sample(ellipsoid, 25000, 15) + pointsEcef = ellipsoidParametric.sample(ellipsoid, 6000, 15) pointsLla = [] for point in pointsEcef: lat, lon, alt = Geometry.ecef2lla(point[0], point[1], point[2])