100 lines
2.8 KiB
Python
100 lines
2.8 KiB
Python
import numpy as np
|
|
|
|
|
|
def compute_face_normal(corners: np.ndarray) -> np.ndarray:
|
|
"""
|
|
Compute the normal vector of a face defined by its corners.
|
|
Assumes corners are in order (e.g., clockwise or counter-clockwise).
|
|
|
|
Args:
|
|
corners: (N, 3) array of corner coordinates.
|
|
|
|
Returns:
|
|
(3,) normalized normal vector.
|
|
"""
|
|
if corners.shape[0] < 3:
|
|
raise ValueError("At least 3 corners are required to compute a normal.")
|
|
|
|
# Use the cross product of two edges
|
|
v1 = corners[1] - corners[0]
|
|
v2 = corners[2] - corners[0]
|
|
|
|
normal = np.cross(v1, v2)
|
|
norm = np.linalg.norm(normal)
|
|
|
|
if norm < 1e-10:
|
|
raise ValueError("Corners are collinear or degenerate; cannot compute normal.")
|
|
|
|
return normal / norm
|
|
|
|
|
|
def rotation_align_vectors(from_vec: np.ndarray, to_vec: np.ndarray) -> np.ndarray:
|
|
"""
|
|
Compute the 3x3 rotation matrix that aligns from_vec to to_vec.
|
|
|
|
Args:
|
|
from_vec: (3,) source vector.
|
|
to_vec: (3,) target vector.
|
|
|
|
Returns:
|
|
(3, 3) rotation matrix.
|
|
"""
|
|
# Normalize inputs
|
|
a = from_vec / np.linalg.norm(from_vec)
|
|
b = to_vec / np.linalg.norm(to_vec)
|
|
|
|
v = np.cross(a, b)
|
|
c = np.dot(a, b)
|
|
s = np.linalg.norm(v)
|
|
|
|
# Handle parallel case
|
|
if s < 1e-10:
|
|
if c > 0:
|
|
return np.eye(3)
|
|
else:
|
|
# Anti-parallel case: 180 degree rotation around an orthogonal axis
|
|
# Find an orthogonal axis
|
|
if abs(a[0]) < 0.9:
|
|
ortho = np.array([1, 0, 0])
|
|
else:
|
|
ortho = np.array([0, 1, 0])
|
|
|
|
axis = np.cross(a, ortho)
|
|
axis /= np.linalg.norm(axis)
|
|
|
|
# Rodrigues formula for 180 degrees
|
|
K = np.array(
|
|
[[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]], [-axis[1], axis[0], 0]]
|
|
)
|
|
return np.eye(3) + 2 * (K @ K)
|
|
|
|
# General case using Rodrigues' rotation formula
|
|
# R = I + [v]_x + [v]_x^2 * (1-c)/s^2
|
|
vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
|
|
|
|
R = np.eye(3) + vx + (vx @ vx) * ((1 - c) / (s**2))
|
|
return R
|
|
|
|
|
|
def apply_alignment_to_pose(T: np.ndarray, R_align: np.ndarray) -> np.ndarray:
|
|
"""
|
|
Apply an alignment rotation to a 4x4 pose matrix.
|
|
The alignment is applied in the global frame (pre-multiplication of rotation).
|
|
|
|
Args:
|
|
T: (4, 4) homogeneous transformation matrix.
|
|
R_align: (3, 3) alignment rotation matrix.
|
|
|
|
Returns:
|
|
(4, 4) aligned transformation matrix.
|
|
"""
|
|
if T.shape != (4, 4):
|
|
raise ValueError(f"Expected 4x4 matrix, got {T.shape}")
|
|
if R_align.shape != (3, 3):
|
|
raise ValueError(f"Expected 3x3 matrix, got {R_align.shape}")
|
|
|
|
T_align = np.eye(4)
|
|
T_align[:3, :3] = R_align
|
|
|
|
return T_align @ T
|