Source code for pomdp_py.utils.math

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