Skip to content
Snippets Groups Projects
Commit 3bd1bd5b authored by Amy Stamile's avatar Amy Stamile
Browse files

Added sensor-library math utils

parent e659caf8
No related branches found
No related tags found
No related merge requests found
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):
"""
......
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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment