Unverified Commit e1e06af9 authored by Kelvin Rodriguez's avatar Kelvin Rodriguez Committed by GitHub
Browse files

Merge pull request #129 from amystamile-usgs/sensor-utils

Added Sensor Library Math Utils
parents 1452f34a cae0ae3d
Loading
Loading
Loading
Loading
+269 −0
Original line number Diff line number Diff line
import pyproj
import numpy as np 
from typing import NamedTuple

class Point(NamedTuple):
    x: np.ndarray
    y: np.ndarray
    z: np.ndarray

class LatLon(NamedTuple):
    lat: np.ndarray
    lon: np.ndarray

class Sphere(NamedTuple):
    lat: np.ndarray
    lon: np.ndarray
    radius: np.ndarray

def sep_angle(a_vec, b_vec):
    """
    Parameters
    ----------
    a_vec : Point object (x, y, z)

    b_vec : Point object (x, y, z)

    Returns
    -------
    : np.ndarray
    """
    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):
    """
    Parameters
    ----------
    vec : Point object (x, y, z)

    Returns
    -------
    : np.ndarray
    """
    return np.sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z)

def distance(start, stop):
    """
    Parameters
    ----------
    start : Point object (x, y, z)

    stop : Point object (x, y, z)

    Returns
    -------
    : np.ndarray
    """
    diff = Point(stop.x - start.x, stop.y - start.y, stop.z - start.z)

    return magnitude(diff)

def radians_to_degrees(radian_lat_lon):
    """
    Parameters
    ----------
    radian_lat_lon : LatLon object (lat, lon) in radians

    Returns
    -------
    : LatLon object (lat, lon) in degrees
    """
    degree_lon = radian_lat_lon.lon
    if (degree_lon < 0):
      degree_lon += 2 * np.pi

    degree_lon = np.rad2deg(degree_lon)
    degree_lat = np.rad2deg(radian_lat_lon.lat)
    return LatLon(degree_lat, degree_lon)

def spherical_to_rect(spherical):
    """
    Parameters
    ----------
    spherical : Sphere object (lat, lon, radius)

    Returns
    -------
    : Point object (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):
    """
    Parameters
    ----------
    rectangular : Point object (x, y, z)

    Returns
    -------
    : Sphere object (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):
    """
    Parameters
    ----------
    ground_pt : LatLon object (lat, lon)

    sub_pt : LatLon object (lat, lon)

    Returns
    -------
    : np.ndarray
    """
    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):
    """
    Parameters
    ----------
    a_vec : Point object (x, y, z)

    b_vec : Point object (x, y, z)

    Returns
    -------
    : Point object (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):
    """
    Parameters
    ----------
    vec : Point object (x, y, z)

    Returns
    -------
    : Point object (x, y, z)
    """
    mag = magnitude(vec)
    return vec / mag

def perpendicular_vector(a_vec, b_vec):
    """
    Parameters
    ----------
    a_vec : Point object (x, y, z)

    b_vec : Point object (x, y, z)

    Returns
    -------
    : Point object (x, y, z)
    """
    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):
    """
    Parameters
    ----------
    vec : Point object (x, y, z)

    scalar : np.ndarray

    Returns
    -------
    : Point object (x, y, z)
    """
    return Point(vec.x * scalar, vec.y * scalar, vec.z * scalar)

def reproject(record, semi_major, semi_minor, source_proj, dest_proj, **kwargs):
    """

tests/test_utils.py

0 → 100644
+94 −0
Original line number Diff line number Diff line
import numpy as np
from knoten import utils

def test_sep_angle_right():
  pt1 = utils.Point(1, 0, 0)
  pt2 = utils.Point(0, 1, 0)
  np.testing.assert_array_equal(utils.sep_angle(pt1, pt2), np.pi / 2.0)

def test_sep_angle_acute():
  pt1 = utils.Point(1, 0, 0)
  pt2 = utils.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 = utils.Point(1, 0, 0)
  pt2 = utils.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 = utils.Point(1, 0, 0)
  pt2 = utils.Point(1, 1, 0)
  pt3 = utils.Point(100, 0, 0)
  pt4 = utils.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(utils.Point(1.0, 0.0, 0.0)) == 1.0
  assert utils.magnitude(utils.Point(0.0, 1.0, 0.0)) == 1.0
  assert utils.magnitude(utils.Point(0.0, 0.0, 1.0)) == 1.0

def test_magnitude_nonunit():
  assert utils.magnitude(utils.Point(0.0, 0.0, 0.0)) == 0.0
  assert utils.magnitude(utils.Point(2.0, 1.0, 4.0)) == np.sqrt(21.0)
  np.testing.assert_allclose(utils.magnitude(utils.Point(0.2, 0.1, 0.4)), np.sqrt(0.21), atol=1e-12)

def test_distance():
  assert utils.distance(utils.Point(1.0, 2.0, 3.0), utils.Point(6.0, 5.0, 4.0)) == np.sqrt(35)

def test_spherical_to_rect():
  result = utils.spherical_to_rect(utils.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(utils.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(utils.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(utils.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(utils.Point(1000.0, 0.0, 0.0))
  np.testing.assert_array_equal(result, utils.Sphere(0.0, 0.0, 1000.0))

  result = utils.rect_to_spherical(utils.Point(-1000.0, 0.0, 0.0))
  np.testing.assert_array_equal(result, utils.Sphere(0.0, np.pi, 1000.0))

  result = utils.rect_to_spherical(utils.Point(0.0, 0.0, 1000.0))
  np.testing.assert_array_equal(result, utils.Sphere(np.pi / 2.0, 0.0, 1000.0))

  result = utils.rect_to_spherical(utils.Point(0.0, 0.0, -1000.0))
  np.testing.assert_array_equal(result,  utils.Sphere(np.pi / -2.0, 0.0, 1000.0))

def test_ground_azimuth():
  ground_pt = utils.LatLon(0, -180)
  subsolar_pt = utils.LatLon(0, 90)
  np.testing.assert_array_equal(270.0, utils.ground_azimuth(ground_pt, subsolar_pt))

def test_perpendicular_vector():
  vec_a = utils.Point(6.0, 6.0, 6.0)
  vec_b = utils.Point(2.0, 0.0, 0.0)
  result = utils.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(utils.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 = utils.Point(1.0, 2.0, -3.0)
  scalar = 3.0
  result = utils.Point(3.0, 6.0, -9.0)
  np.testing.assert_array_equal(utils.scale_vector(vec, scalar), result)
 No newline at end of file