first commit

lib/math_utils.py

@@ -0,0 +1,104 @@
+import math
+import numpy as np
+
+
+def rads_to_degs(rads):
+    """Convert an angle from radians to degrees"""
+    return 180 * rads / math.pi
+
+
+def angle_from_vector_to_x(vec):
+    """computer the angle (0~2pi) between a unit vector and positive x axis"""
+    angle = 0.0
+    # 2 | 1
+    # -------
+    # 3 | 4
+    if vec[0] >= 0:
+        if vec[1] >= 0:
+            # Qadrant 1
+            angle = math.asin(vec[1])
+        else:
+            # Qadrant 4
+            angle = 2.0 * math.pi - math.asin(-vec[1])
+    else:
+        if vec[1] >= 0:
+            # Qadrant 2
+            angle = math.pi - math.asin(vec[1])
+        else:
+            # Qadrant 3
+            angle = math.pi + math.asin(-vec[1])
+    return angle
+
+
+def cartesian2polar(vec, with_radius=False):
+    """convert a vector in cartesian coordinates to polar(spherical) coordinates"""
+    vec = vec.round(6)
+    norm = np.linalg.norm(vec)
+    theta = np.arccos(vec[2] / norm) # (0, pi)
+    phi = np.arctan(vec[1] / (vec[0] + 1e-15)) # (-pi, pi) # FIXME: -0.0 cannot be identified here
+    if not with_radius:
+        return np.array([theta, phi])
+    else:
+        return np.array([theta, phi, norm])
+
+
+def polar2cartesian(vec):
+    """convert a vector in polar(spherical) coordinates to cartesian coordinates"""
+    r = 1 if len(vec) == 2 else vec[2]
+    theta, phi = vec[0], vec[1]
+    x = r * np.sin(theta) * np.cos(phi)
+    y = r * np.sin(theta) * np.sin(phi)
+    z = r * np.cos(theta)
+    return np.array([x, y, z])
+
+
+def rotate_by_x(vec, theta):
+    mat = np.array([[1, 0, 0],
+                    [0, np.cos(theta), -np.sin(theta)],
+                    [0, np.sin(theta), np.cos(theta)]])
+    return np.dot(mat, vec)
+
+
+def rotate_by_y(vec, theta):
+    mat = np.array([[np.cos(theta), 0, np.sin(theta)],
+                    [0, 1, 0],
+                    [-np.sin(theta), 0, np.cos(theta)]])
+    return np.dot(mat, vec)
+
+
+def rotate_by_z(vec, phi):
+    mat = np.array([[np.cos(phi), -np.sin(phi), 0],
+                    [np.sin(phi), np.cos(phi), 0],
+                    [0, 0, 1]])
+    return np.dot(mat, vec)
+
+
+def polar_parameterization(normal_3d, x_axis_3d):
+    """represent a coordinate system by its rotation from the standard 3D coordinate system
+
+    Args:
+        normal_3d (np.array): unit vector for normal direction (z-axis)
+        x_axis_3d (np.array): unit vector for x-axis
+
+    Returns:
+        theta, phi, gamma: axis-angle rotation 
+    """
+    normal_polar = cartesian2polar(normal_3d)
+    theta = normal_polar[0]
+    phi = normal_polar[1]
+
+    ref_x = rotate_by_z(rotate_by_y(np.array([1, 0, 0]), theta), phi)
+
+    gamma = np.arccos(np.dot(x_axis_3d, ref_x).round(6))
+    if np.dot(np.cross(ref_x, x_axis_3d), normal_3d) < 0:
+        gamma = -gamma
+    return theta, phi, gamma
+
+
+def polar_parameterization_inverse(theta, phi, gamma):
+    """build a coordinate system by the given rotation from the standard 3D coordinate system"""
+    normal_3d = polar2cartesian([theta, phi])
+    ref_x = rotate_by_z(rotate_by_y(np.array([1, 0, 0]), theta), phi)
+    ref_y = np.cross(normal_3d, ref_x)
+    x_axis_3d = ref_x * np.cos(gamma) + ref_y * np.sin(gamma)
+    return normal_3d, x_axis_3d