"""Assorted utilities for math"""
import math
import numpy as np
# Math
[docs]
def vec(p1, p2):
"""vector from p1 to p2"""
if type(p1) != np.ndarray:
p1 = np.array(p1)
if type(p2) != np.ndarray:
p2 = np.array(p2)
return p2 - p1
[docs]
def proj(vec1, vec2, scalar=False):
# Project vec1 onto vec2. Returns a vector in the direction of vec2.
scale = np.dot(vec1, vec2) / np.linalg.norm(vec2)
if scalar:
return scale
else:
return vec2 * scale
[docs]
def R_x(th):
return np.array(
[
1,
0,
0,
0,
0,
np.cos(th),
-np.sin(th),
0,
0,
np.sin(th),
np.cos(th),
0,
0,
0,
0,
1,
]
).reshape(4, 4)
[docs]
def R_y(th):
return np.array(
[
np.cos(th),
0,
np.sin(th),
0,
0,
1,
0,
0,
-np.sin(th),
0,
np.cos(th),
0,
0,
0,
0,
1,
]
).reshape(4, 4)
[docs]
def R_z(th):
return np.array(
[
np.cos(th),
-np.sin(th),
0,
0,
np.sin(th),
np.cos(th),
0,
0,
0,
0,
1,
0,
0,
0,
0,
1,
]
).reshape(4, 4)
[docs]
def T(dx, dy, dz):
return np.array([1, 0, 0, dx, 0, 1, 0, dy, 0, 0, 1, dz, 0, 0, 0, 1]).reshape(4, 4)
[docs]
def to_radians(th):
return th * np.pi / 180
[docs]
def R_between(v1, v2):
if len(v1) != 3 or len(v2) != 3:
raise ValueError("Only applicable to 3D vectors!")
v = np.cross(v1, v2)
c = np.dot(v1, v2)
s = np.linalg.norm(v)
I = np.identity(3)
vX = np.array([0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0]).reshape(3, 3)
R = I + vX + np.matmul(vX, vX) * ((1 - c) / (s**2))
return R
[docs]
def approx_equal(v1, v2, epsilon=1e-6):
if len(v1) != len(v2):
return False
for i in range(len(v1)):
if abs(v1[i] - v2[i]) > epsilon:
return False
return True
[docs]
def euclidean_dist(p1, p2):
return math.sqrt(sum([(a - b) ** 2 for a, b in zip(p1, p2)]))
