From 3bd1bd5bd8be8ac960b3559d89546d4b8972d897 Mon Sep 17 00:00:00 2001
From: Amy Stamile <astamile@usgs.gov>
Date: Wed, 17 Apr 2024 10:54:11 -0700
Subject: [PATCH] Added sensor-library math utils
---
knoten/utils.py | 170 ++++++++++++++++++++++++++++++++++++++++++++
tests/test_utils.py | 100 ++++++++++++++++++++++++++
2 files changed, 270 insertions(+)
create mode 100644 tests/test_utils.py
diff --git a/knoten/utils.py b/knoten/utils.py
index 121721a..5920e38 100644
--- a/knoten/utils.py
+++ b/knoten/utils.py
@@ -1,4 +1,174 @@
import pyproj
+import numpy as np
+from collections import namedtuple
+
+def sep_angle(a_pt, b_pt, c_pt):
+ return sep_angle(a_pt - b_pt, c_pt - b_pt)
+
+def sep_angle(a_vec, b_vec):
+ dot_prod = a_vec.x * b_vec.x + a_vec.y * b_vec.y + a_vec.z * b_vec.z
+ dot_prod /= magnitude(a_vec) * magnitude(b_vec)
+
+ if(dot_prod >= 1.0): return 0.0
+ if(dot_prod <= -1.0): return np.pi
+
+ return np.arccos(dot_prod)
+
+def magnitude(vec):
+ return np.sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z)
+
+def distance(start, stop):
+ Point = namedtuple("Point", 'x, y, z')
+ diff = Point(stop.x - start.x, stop.y - start.y, stop.z - start.z)
+
+ return magnitude(diff)
+
+def radiansToDegrees(radian_lat_lon):
+ LatLon = namedtuple("LatLon", 'lat lon')
+
+ degree_lon = radian_lat_lon.lon
+ if (degree_lon < 0):
+ degree_lon += 2 * np.pi
+
+ degree_lon = np.rad2deg(degree_lon)
+ degreeLat = np.rad2deg(radian_lat_lon.lat)
+ return LatLon(degreeLat, degree_lon)
+
+def spherical_to_rect(spherical):
+ Point = namedtuple("Point", 'x, y, z')
+
+ x = spherical.radius * np.cos(spherical.lat) * np.cos(spherical.lon)
+ y = spherical.radius * np.cos(spherical.lat) * np.sin(spherical.lon)
+ z = spherical.radius * np.sin(spherical.lat)
+
+ return Point(x, y, z)
+
+def rect_to_spherical(rectangular):
+ Sphere = namedtuple("Sphere", 'lat, lon, radius')
+
+ rad = magnitude(rectangular)
+ if (rad < 1e-15):
+ return Sphere(0.0, 0.0, 0.0)
+
+ return Sphere(
+ np.arcsin(rectangular.z / rad),
+ np.arctan2(rectangular.y, rectangular.x),
+ rad
+ )
+
+def ground_azimuth(ground_pt, sub_pt):
+ LatLon = namedtuple("LatLon", 'lat lon')
+
+ if (ground_pt.lat >= 0.0):
+ a = (90.0 - sub_pt.lat) * np.pi / 180.0
+ b = (90.0 - ground_pt.lat) * np.pi / 180.0
+ else:
+ a = (90.0 + sub_pt.lat) * np.pi / 180.0
+ b = (90.0 + ground_pt.lat) * np.pi / 180.0
+
+ cs = LatLon(0, sub_pt.lon)
+ cg = LatLon(0, ground_pt.lon)
+
+ if (cs.lon > cg.lon):
+ if ((cs.lon - cg.lon) > 180.0):
+ while ((cs.lon - cg.lon) > 180.0):
+ cs = LatLon(0, cs.lon - 360.0)
+ if (cg.lon > cs.lon):
+ if ((cg.lon-cs.lon) > 180.0):
+ while ((cg.lon-cs.lon) > 180.0):
+ cg = LatLon(0, cg.lon - 360.0)
+
+ if (sub_pt.lat > ground_pt.lat):
+ if (cs.lon < cg.lon):
+ quad = 2
+ else:
+ quad = 1
+ elif (sub_pt.lat < ground_pt.lat):
+ if (cs.lon < cg.lon):
+ quad = 3
+ else:
+ quad = 4
+ else:
+ if (cs.lon > cg.lon):
+ quad = 1
+ elif (cs.lon < cg.lon):
+ quad = 2
+ else:
+ return 0.0
+
+ C = (cg.lon - cs.lon) * np.pi / 180.0
+ if (C < 0):
+ C = -C
+
+ c = np.arccos(np.cos(a) * np.cos(b) + np.sin(a) * np.sin(b) * np.cos(C))
+
+ azimuth = 0.0
+
+ if (np.sin(b) == 0.0 or np.sin(c) == 0.0):
+ return azimuth
+
+ intermediate = (np.cos(a) - np.cos(b) * np.cos(c)) / (np.sin(b) * np.sin(c))
+ if (intermediate < -1.0):
+ intermediate = -1.0
+ elif (intermediate > 1.0):
+ intermediate = 1.0
+
+ A = np.arccos(intermediate) * 180.0 / np.pi
+
+ if (ground_pt.lat >= 0.0):
+ if (quad == 1 or quad == 4):
+ azimuth = A
+ elif (quad == 2 or quad == 3):
+ azimuth = 360.0 - A
+ else:
+ if (quad == 1 or quad == 4):
+ azimuth = 180.0 - A
+ elif (quad == 2 or quad == 3):
+ azimuth = 180.0 + A
+ return azimuth
+
+def crossProduct(a_vec, b_vec):
+ Point = namedtuple("Point", 'x, y, z')
+
+ x = a_vec.y * b_vec.z - a_vec.z * b_vec.y
+ y = a_vec.z * b_vec.x - a_vec.x * b_vec.z
+ z = a_vec.x * b_vec.y - a_vec.y * b_vec.x
+ return Point(x, y, z)
+
+
+def unit_vector(vec):
+ mag = magnitude(vec)
+ return vec / mag
+
+def perpendicular_vector(a_vec, b_vec):
+ if (magnitude(a_vec) == 0):
+ return b_vec
+
+ a_norm = unit_vector(a_vec)
+ b_norm = unit_vector(b_vec)
+
+ angle = a_norm * b_norm
+ a_mag = magnitude(a_vec)
+ mag_p = angle * a_mag
+
+ p = b_norm * mag_p
+ q = a_vec - p
+
+ return q
+
+def scale_vector(vec, scalar):
+ Point = namedtuple("Point", 'x, y, z')
+
+ return Point(vec.x * scalar, vec.y * scalar, vec.z * scalar)
+
+def matrixVecProduct(mat, vec):
+ Point = namedtuple("Point", 'x, y, z')
+
+ x = mat.a.x * vec.x + mat.a.y * vec.y + mat.a.z * vec.z
+ y = mat.b.x * vec.x + mat.b.y * vec.y + mat.b.z * vec.z
+ z = mat.c.x * vec.x + mat.c.y * vec.y + mat.c.z * vec.z
+
+ return Point(x, y, z)
def reproject(record, semi_major, semi_minor, source_proj, dest_proj, **kwargs):
"""
diff --git a/tests/test_utils.py b/tests/test_utils.py
new file mode 100644
index 0000000..8547c0e
--- /dev/null
+++ b/tests/test_utils.py
@@ -0,0 +1,100 @@
+import numpy as np
+from knoten import utils
+from collections import namedtuple
+
+Point = namedtuple("Point", 'x, y, z')
+Sphere = namedtuple("Sphere", 'lat, lon, radius')
+
+def test_sep_angle_right():
+ pt1 = Point(1, 0, 0)
+ pt2 = Point(0, 1, 0)
+ np.testing.assert_array_equal(utils.sep_angle(pt1, pt2), np.pi / 2.0)
+
+def test_sep_angle_acute():
+ pt1 = Point(1, 0, 0)
+ pt2 = Point(1, 1, 0)
+ np.testing.assert_allclose(utils.sep_angle(pt1, pt2), np.pi / 4.0, atol=1e-12)
+
+def test_sep_angle_obtuse():
+ pt1 = Point(1, 0, 0)
+ pt2 = Point(-1, 1, 0)
+ np.testing.assert_array_equal(utils.sep_angle(pt1, pt2), 3.0 * np.pi / 4.0)
+
+def test_sep_angle_normalization():
+ pt1 = Point(1, 0, 0)
+ pt2 = Point(1, 1, 0)
+ pt3 = Point(100, 0, 0)
+ pt4 = Point(100, 100, 0)
+ np.testing.assert_array_equal(utils.sep_angle(pt1, pt2), utils.sep_angle(pt3, pt4))
+
+def test_magnitude_unit():
+ assert utils.magnitude(Point(1.0, 0.0, 0.0)) == 1.0
+ assert utils.magnitude(Point(0.0, 1.0, 0.0)) == 1.0
+ assert utils.magnitude(Point(0.0, 0.0, 1.0)) == 1.0
+
+def test_magnitude_nonunit():
+ assert utils.magnitude(Point(0.0, 0.0, 0.0)) == 0.0
+ assert utils.magnitude(Point(2.0, 1.0, 4.0)) == np.sqrt(21.0)
+ np.testing.assert_allclose(utils.magnitude(Point(0.2, 0.1, 0.4)), np.sqrt(0.21), atol=1e-12)
+
+def test_distance():
+ assert utils.distance(Point(1.0, 2.0, 3.0), Point(6.0, 5.0, 4.0)) == np.sqrt(35)
+
+def test_spherical_to_rect():
+ result = utils.spherical_to_rect(Sphere(0.0, 0.0, 1000.0))
+ np.testing.assert_allclose(result.x, 1000.0, atol=1e-12)
+ np.testing.assert_allclose(result.y, 0.0, atol=1e-12)
+ np.testing.assert_allclose(result.z, 0.0, atol=1e-12)
+
+ result = utils.spherical_to_rect(Sphere(0.0, np.pi, 1000.0))
+ np.testing.assert_allclose( result.x, -1000.0, atol=1e-12)
+ np.testing.assert_allclose( result.y, 0.0, atol=1e-12)
+ np.testing.assert_allclose( result.z, 0.0, atol=1e-12)
+
+ result = utils.spherical_to_rect(Sphere(np.pi / 2.0, 0.0, 1000.0))
+ np.testing.assert_allclose( result.x, 0.0, atol=1e-12)
+ np.testing.assert_allclose( result.y, 0.0, atol=1e-12)
+ np.testing.assert_allclose( result.z, 1000.0, atol=1e-12)
+
+ result = utils.spherical_to_rect(Sphere(np.pi / -2.0, 0.0, 1000.0))
+ np.testing.assert_allclose( result.x, 0.0, atol=1e-12)
+ np.testing.assert_allclose( result.y, 0.0, atol=1e-12)
+ np.testing.assert_allclose( result.z, -1000.0, atol=1e-12)
+
+def test_rect_to_spherical():
+ result = utils.rect_to_spherical(Point(1000.0, 0.0, 0.0))
+ np.testing.assert_array_equal(result, Sphere(0.0, 0.0, 1000.0))
+
+ result = utils.rect_to_spherical(Point(-1000.0, 0.0, 0.0))
+ np.testing.assert_array_equal(result, Sphere(0.0, np.pi, 1000.0))
+
+ result = utils.rect_to_spherical(Point(0.0, 0.0, 1000.0))
+ np.testing.assert_array_equal(result, Sphere(np.pi / 2.0, 0.0, 1000.0))
+
+ result = utils.rect_to_spherical(Point(0.0, 0.0, -1000.0))
+ np.testing.assert_array_equal(result, Sphere(np.pi / -2.0, 0.0, 1000.0))
+
+def test_ground_azimuth():
+ LatLon = namedtuple("LatLon", "lat lon")
+
+ ground_pt = LatLon(0, -180)
+ subsolar_pt = LatLon(0, 90)
+ np.testing.assert_array_equal(270.0, utils.ground_azimuth(ground_pt, subsolar_pt))
+
+def test_perpendicular_vector():
+ vec_a = Point(6.0, 6.0, 6.0)
+ vec_b = Point(2.0, 0.0, 0.0)
+ result = Point(0.0, 6.0, 6.0)
+ np.testing.assert_array_equal(utils.perpendicular_vector(vec_a, vec_b), result)
+
+def test_unit_vector():
+ result = utils.unit_vector(Point(5.0, 12.0, 0.0))
+ np.testing.assert_allclose(result[0], 0.384615, atol=1e-6)
+ np.testing.assert_allclose(result[1], 0.923077, atol=1e-6)
+ np.testing.assert_array_equal(result[2], 0.0)
+
+def test_scale_vector():
+ vec = Point(1.0, 2.0, -3.0)
+ scalar = 3.0
+ result = Point(3.0, 6.0, -9.0)
+ np.testing.assert_array_equal(utils.scale_vector(vec, scalar), result)
\ No newline at end of file
--
GitLab