mirror of
https://github.com/Yuki-Kokomi/OpenECAD_Project.git
synced 2026-02-04 00:33:22 -05:00
368 lines
10 KiB
Python
368 lines
10 KiB
Python
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
|