first commit

lib/math_utils.py

@@ -0,0 +1,367 @@
+import math
+import numpy as np
+
+
+def rads_to_degs(rads):
+    """Convert an angle from radians to degrees"""
+    return 180 * rads / math.pi
+
+def distance_of_point_and_plane(origin, x_axis, y_axis, point):
+    # 确保输入是 numpy 数组
+    origin = np.array(origin)
+    x_axis = np.array(x_axis)
+    y_axis = np.array(y_axis)
+    point = np.array(point)
+    normal_vector = np.cross(x_axis, y_axis)
+    distance = np.dot(point - origin, normal_vector)
+    return np.abs(distance)
+
+def is_point_on_plane(origin, x_axis, y_axis, point, tolerance=1e-10):
+    """
+    判断一个3D点是否在给定的平面上
+    
+    参数:
+    origin - 平面的原点坐标，形状为 (3,)
+    x_axis - 平面X轴的方向向量，形状为 (3,)
+    y_axis - 平面Y轴的方向向量，形状为 (3,)
+    point - 要检查的3D点，形状为 (3,)
+    tolerance - 容忍度，默认值为 1e-10
+    
+    返回:
+    如果点在平面上返回 True，否则返回 False
+    """
+    # 确保输入是 numpy 数组
+    origin = np.array(origin)
+    x_axis = np.array(x_axis)
+    y_axis = np.array(y_axis)
+    point = np.array(point)
+    
+    # 计算法向量
+    normal_vector = np.cross(x_axis, y_axis)
+    
+    # 计算点到平面的距离
+    distance = np.dot(point - origin, normal_vector)
+    
+    # 判断距离是否在容忍度范围内
+    return np.abs(distance) < tolerance
+
+
+def number_to_pi_string(number):
+    """
+    将数字转换为带有 np.pi 的字符串表示形式
+    
+    参数:
+    number - 输入数字，可以是 np.pi 的倍数
+    
+    返回:
+    带有 np.pi 的字符串表示形式
+    """
+    # 定义一个容忍度来比较浮点数
+    tolerance = 1e-10
+
+    # 预定义一些常见的 π 的倍数及其对应的字符串表示
+    pi_factors = {
+        np.pi: 'np.pi',
+        np.pi / 2: 'np.pi/2',
+        np.pi / 3: 'np.pi/3',
+        np.pi / 4: 'np.pi/4',
+        np.pi / 6: 'np.pi/6',
+        2 * np.pi: '2*np.pi',
+        3 * np.pi / 2: '3*np.pi/2',
+        3 * np.pi / 4: '3*np.pi/4',
+        5 * np.pi / 6: '5*np.pi/6',
+        5 * np.pi / 3: '5*np.pi/3',
+        7 * np.pi / 6: '7*np.pi/6',
+        4 * np.pi / 3: '4*np.pi/3',
+    }
+    
+    # 检查输入数字是否接近这些常见的 π 倍数
+    for key, value in pi_factors.items():
+        if np.abs(np.abs(number) - key) < tolerance:
+            return value if number > 0 else '-' + value
+    
+    # 如果数字不在预定义的 π 倍数中，返回原数字
+    return str(number.round(6))
+
+def are_parallel(v1, v2, tol=1e-10):
+    # 计算叉积
+    cross_product = np.cross(v1, v2)
+    # 判断叉积是否接近于零向量
+    return np.all(np.abs(cross_product) < tol)
+
+def find_circle_center_and_radius(start_point, end_point, mid_point):    
+    # Calculate midpoints of the chords
+    mid_point_start_end = (start_point + end_point) / 2
+    mid_point_start_mid = (start_point + mid_point) / 2
+    
+    # Calculate direction vectors of the chords
+    direction_start_end = end_point - start_point
+    direction_start_mid = mid_point - start_point
+    
+    # Calculate perpendicular direction vectors
+    perp_start_end = np.array([-direction_start_end[1], direction_start_end[0]])
+    perp_start_mid = np.array([-direction_start_mid[1], direction_start_mid[0]])
+    
+    # Solve for the intersection of the perpendicular bisectors
+    A = np.array([perp_start_end, -perp_start_mid]).T
+    b = mid_point_start_mid - mid_point_start_end
+    
+    # Solve the linear system
+    t, s = np.linalg.solve(A, b)
+    
+    # Calculate the center
+    center = mid_point_start_end + t * perp_start_end
+    
+    # Calculate the radius
+    radius = np.linalg.norm(center - start_point)
+    
+    return center, radius
+
+def rotate_vector(vector, axis, angle):
+    """
+    将一个3D向量绕指定轴逆时针旋转给定角度
+    
+    参数:
+    vector - 要旋转的3D向量，形状为 (3,)
+    axis - 旋转轴，形状为 (3,)
+    angle - 旋转角度（弧度）
+
+    返回:
+    旋转后的3D向量，形状为 (3,)
+    """
+    # 确保输入是 numpy 数组
+    vector = np.array(vector)
+    axis = np.array(axis)
+    
+    # 计算单位轴向量
+    axis = axis / np.linalg.norm(axis)
+    
+    # 计算旋转矩阵的各个分量
+    cos_theta = np.cos(angle)
+    sin_theta = np.sin(angle)
+    cross_product = np.cross(axis, vector)
+    dot_product = np.dot(axis, vector)
+    
+    # 计算旋转后的向量
+    rotated_vector = (vector * cos_theta + 
+                      cross_product * sin_theta + 
+                      axis * dot_product * (1 - cos_theta))
+    
+    return rotated_vector
+
+def calculate_rotation_angle(v1, v2, axis):
+    """
+    计算从向量 v1 到向量 v2 绕给定轴的逆时针旋转角度
+
+    参数:
+    v1 - 初始向量，形状为 (3,)
+    v2 - 旋转后的向量，形状为 (3,)
+    axis - 旋转轴，形状为 (3,)
+
+    返回:
+    旋转角度（弧度），范围 (-pi, pi]
+    """
+    # 确保输入是 numpy 数组
+    v1 = np.array(v1)
+    v2 = np.array(v2)
+    axis = np.array(axis)
+    
+    # 计算单位轴向量
+    axis = axis / np.linalg.norm(axis)
+    
+    # 计算点积
+    dot_product = np.dot(v1, v2)
+    
+    # 计算叉积
+    cross_product = np.cross(v1, v2)
+    
+    # 计算叉积在旋转轴上的投影长度
+    projection_length = np.dot(cross_product, axis)
+    
+    # 计算向量的范数
+    norm_v1 = np.linalg.norm(v1)
+    norm_v2 = np.linalg.norm(v2)
+    
+    # 计算角度的余弦值和正弦值
+    cos_theta = dot_product / (norm_v1 * norm_v2)
+    sin_theta = projection_length / (norm_v1 * norm_v2)
+    
+    # 使用 arctan2 计算角度
+    angle = np.arctan2(sin_theta, cos_theta)
+    
+    return angle
+
+def map_2d_to_3d(origin, x_axis, y_axis, point):
+    u, v = point
+    return origin + u * x_axis + v * y_axis
+
+def map_3d_to_2d(origin, x_axis, y_axis, point_3d):
+    """
+    将三维空间中的点转换为二维平面上的点
+    
+    参数:
+    origin - 原点坐标 (Ox, Oy, Oz)，形状为 (3,)
+    x_axis - X轴向量 (Xx, Xy, Xz)，形状为 (3,)
+    y_axis - Y轴向量 (Yx, Yy, Yz)，形状为 (3,)
+    point_3d - 三维空间中的点 (Px, Py, Pz)，形状为 (3,)
+
+    返回:
+    二维平面上的点 (u, v)，形状为 (2,)
+    """
+    # 确保输入是 numpy 数组
+    origin = np.array(origin)
+    x_axis = np.array(x_axis)
+    y_axis = np.array(y_axis)
+    point_3d = np.array(point_3d)
+    
+    # 构建矩阵 A 和向量 b
+    A = np.vstack([x_axis, y_axis]).T
+    b = point_3d - origin
+    
+    # 求解线性方程组 Ax = b
+    uv = np.linalg.lstsq(A, b, rcond=None)[0]
+    
+    return uv
+
+
+def unit_vector(vector):
+    """
+    计算给定向量的单位向量
+    
+    参数:
+    vector - 输入向量，形状为 (n,)
+
+    返回:
+    单位向量，形状为 (n,)
+    """
+    # 计算向量的范数
+    norm = np.linalg.norm(vector)
+    if norm == 0:
+        raise ValueError("零向量没有单位向量")
+    # 计算单位向量
+    unit_vector = vector / norm
+    return unit_vector
+
+
+def find_n_from_x_and_y(x, y):
+    """
+    Given vectors x and y, find a vector n such that y = n × x.
+    Assumes that n is orthogonal to x.
+    
+    Parameters:
+    x (numpy array): The vector x.
+    y (numpy array): The vector y.
+    
+    Returns:
+    numpy array: The vector n.
+    """
+    # Step 1: Compute the cross product of x and y to get n'
+    n_prime = np.cross(x, y)
+    
+    # Step 2: Normalize n' to get the unit vector
+    n_prime_unit = n_prime / np.linalg.norm(n_prime)
+    
+    # Step 3: Determine the correct sign of n_prime_unit
+    # To ensure y = n × x, we should check if the direction is correct
+    if np.allclose(np.cross(n_prime_unit, x), y):
+        n = n_prime_unit
+    else:
+        n = -n_prime_unit
+    
+    return n
+
+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