Added sensor-library math utils

knoten/utils.py

 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):
     """

tests/test_utils.py

+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