Commit 3bd1bd5b authored by Amy Stamile's avatar Amy Stamile
Browse files

Added sensor-library math utils

parent e659caf8

knoten/utils.py

+170 −0
Original line number Diff line number Diff line 
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

0 → 100644
+100 −0
Original line number Diff line number Diff line 
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