I am trying to draw an object defined by several vertices and then rotate it by defining a face and a vector that I want that face to be normal to. It works fine if I choose a vector that is parallel to the x, y, or z axis but anything different than that makes it skewed. In my code below, line 104 is the vector that makes it skewed if you want to test. Does anyone know why this is happening? I feel there is some offset that needs to be applied but I have no idea how to calculate it.
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
import numpy as np
# Create a new figure with 3D projection
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
def rotate_shape_to_normal(vertices, face_indices, target_normal):
"""
Rotate vertices so that the face specified by face_indices becomes normal to target_normal.
:param vertices: List or numpy array of 3D points defining the shape
:param face_indices: Indices of the vertices that form the face to align
:param target_normal: The vector to which the face should be normal
:return: Rotated vertices
"""
vertices = np.array(vertices) # Convert to numpy array if it isn't already
face_vertices = vertices[face_indices]
# Calculate the normal vector of the face
v1 = face_vertices[1] - face_vertices[0]
v2 = face_vertices[2] - face_vertices[0]
face_normal = np.cross(v1, v2)
print(face_normal)
ax.quiver(*face_vertices[0], *face_normal, color='r', length=2)
ax.quiver(*face_vertices[0], *target_normal, color='b', length=2)
face_normal = face_normal / np.linalg.norm(face_normal) # Normalize
# Normalize target_normal for consistency
target_normal = target_normal / np.linalg.norm(target_normal)
# Compute the rotation axis (perpendicular to both vectors)
rotation_axis = np.cross(face_normal, target_normal)
rotation_axis = rotation_axis / np.linalg.norm(rotation_axis)# if np.linalg.norm(rotation_axis) > 0 else np.array([1, 0, 0]) # Default to x-axis if parallel
print("rotation_axis", rotation_axis)
# Compute the angle between the vectors
cos_theta = np.dot(face_normal, target_normal)
print("cos_theta", np.degrees(cos_theta))
theta = np.arccos(np.clip(cos_theta, -1.0, 1.0)) # Clip to avoid floating-point issues
print("theta", np.degrees(theta))
# Check if vectors are already aligned or opposite
if np.isclose(theta, 0) or np.isclose(theta, np.pi):
return vertices # No rotation needed
# Rodrigues' rotation formula
K = np.array([
[0, -rotation_axis[2], rotation_axis[1]],
[rotation_axis[2], 0, -rotation_axis[0]],
[-rotation_axis[1], rotation_axis[0], 0]
])
rotation_matrix = np.sin(theta) * K + (1 - np.cos(theta)) * np.outer(rotation_axis, rotation_axis)
# Apply the rotation to all vertices
return np.dot(vertices, rotation_matrix.T)
# Define the vertices for a simple satellite model:
vertices = np.array([
# Main body (cube)
(0, 0, 0), # 0 - Base, front-left
(1, 0, 0), # 1 - Base, front-right
(1, 1, 0), # 2 - Base, back-right
(0, 1, 0), # 3 - Base, back-left
(0, 0, 1), # 4 - Top, front-left
(1, 0, 1), # 5 - Top, front-right
(1, 1, 1), # 6 - Top, back-right
(0, 1, 1), # 7 - Top, back-left
# Solar panels
# Left panel
(0.5, .25, -1.5), # 8
(0.5, .75, -1.5), # 9
(0.5, .75, 0), # 10
(0.5, .25, 0), # 11
# Right panel
(0.5, .25, 2.5), # 12
(0.5, .75, 2.5), # 13
(0.5, .75, 1), # 14
(0.5, .25, 1) # 15
])
# Translate spacecraft to origin
vertices = [(x - .5, y - .5, z - .5) for x, y, z in vertices]
# Original points
ax.scatter(vertices[5][0], vertices[5][1],vertices[5][2], color='green', s=25)
ax.scatter(vertices[1][0], vertices[1][1],vertices[1][2], color='green', s=25)
ax.scatter(vertices[4][0], vertices[4][1],vertices[4][2], color='green', s=25)
# Rotate spacecraft to point at specified vector
face_to_align = [5, 1, 4] # Indexes of points that define the face I want to be normal to the specified vector (-Y)
#vector = np.array([0, 0, 1]) # This rotation doesn't skew anything
vector = np.array([0.56167836, 0.76075023, 0.32523301]) # This rotation skews everything
vertices = rotate_shape_to_normal(vertices, face_to_align, vector)
# Rotated points
ax.scatter(vertices[5][0], vertices[5][1],vertices[5][2], color='blue', s=25)
ax.scatter(vertices[1][0], vertices[1][1],vertices[1][2], color='blue', s=25)
ax.scatter(vertices[4][0], vertices[4][1],vertices[4][2], color='blue', s=25)
# Faces for the cube
cube_faces = [
[vertices[0], vertices[1], vertices[5], vertices[4]], # (-Y)
[vertices[1], vertices[2], vertices[6], vertices[5]], # (+X)
[vertices[2], vertices[3], vertices[7], vertices[6]], # (+Y)
[vertices[3], vertices[0], vertices[4], vertices[7]], # (-X)
[vertices[0], vertices[1], vertices[2], vertices[3]], # (-Z)
[vertices[4], vertices[5], vertices[6], vertices[7]], # (+Z)
]
# Solar panel faces - note these are just rectangles
left_panel = [vertices[8], vertices[9], vertices[10], vertices[11]]
right_panel = [vertices[12], vertices[13], vertices[14], vertices[15]]
# Combine all faces
faces = cube_faces + [left_panel, right_panel]
# Create Poly3DCollection
poly3d = Poly3DCollection(faces, alpha=0.7)
poly3d.set_edgecolor('k')
# Set face color for different parts
poly3d.set_facecolor([[1, 0, 0], [.7, .7, .7], [.7, .7, .7], [.7, .7, .7], [.7, .7, .7], [.7, .7, .7], [0, 0.5, 1], [0, 0.5, 1]])
ax.add_collection3d(poly3d)
# Set the aspect ratio to ensure it looks like a cube
ax.set_box_aspect((1, 1, 1))
# Remove axes for a cleaner look
#ax.set_axis_off()
# Set limits to see everything
ax.set_xlim(-1, 2)
ax.set_ylim(-1, 2)
ax.set_zlim(-1, 2)
# Origin
ax.scatter(0, 0, 0, color='red', s=25)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
# Adjust view for better visualization
ax.view_init(elev=20., azim=-45)
plt.show()
I am trying to draw an object defined by several vertices and then rotate it by defining a face and a vector that I want that face to be normal to. It works fine if I choose a vector that is parallel to the x, y, or z axis but anything different than that makes it skewed. In my code below, line 104 is the vector that makes it skewed if you want to test. Does anyone know why this is happening? I feel there is some offset that needs to be applied but I have no idea how to calculate it.
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
import numpy as np
# Create a new figure with 3D projection
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
def rotate_shape_to_normal(vertices, face_indices, target_normal):
"""
Rotate vertices so that the face specified by face_indices becomes normal to target_normal.
:param vertices: List or numpy array of 3D points defining the shape
:param face_indices: Indices of the vertices that form the face to align
:param target_normal: The vector to which the face should be normal
:return: Rotated vertices
"""
vertices = np.array(vertices) # Convert to numpy array if it isn't already
face_vertices = vertices[face_indices]
# Calculate the normal vector of the face
v1 = face_vertices[1] - face_vertices[0]
v2 = face_vertices[2] - face_vertices[0]
face_normal = np.cross(v1, v2)
print(face_normal)
ax.quiver(*face_vertices[0], *face_normal, color='r', length=2)
ax.quiver(*face_vertices[0], *target_normal, color='b', length=2)
face_normal = face_normal / np.linalg.norm(face_normal) # Normalize
# Normalize target_normal for consistency
target_normal = target_normal / np.linalg.norm(target_normal)
# Compute the rotation axis (perpendicular to both vectors)
rotation_axis = np.cross(face_normal, target_normal)
rotation_axis = rotation_axis / np.linalg.norm(rotation_axis)# if np.linalg.norm(rotation_axis) > 0 else np.array([1, 0, 0]) # Default to x-axis if parallel
print("rotation_axis", rotation_axis)
# Compute the angle between the vectors
cos_theta = np.dot(face_normal, target_normal)
print("cos_theta", np.degrees(cos_theta))
theta = np.arccos(np.clip(cos_theta, -1.0, 1.0)) # Clip to avoid floating-point issues
print("theta", np.degrees(theta))
# Check if vectors are already aligned or opposite
if np.isclose(theta, 0) or np.isclose(theta, np.pi):
return vertices # No rotation needed
# Rodrigues' rotation formula
K = np.array([
[0, -rotation_axis[2], rotation_axis[1]],
[rotation_axis[2], 0, -rotation_axis[0]],
[-rotation_axis[1], rotation_axis[0], 0]
])
rotation_matrix = np.sin(theta) * K + (1 - np.cos(theta)) * np.outer(rotation_axis, rotation_axis)
# Apply the rotation to all vertices
return np.dot(vertices, rotation_matrix.T)
# Define the vertices for a simple satellite model:
vertices = np.array([
# Main body (cube)
(0, 0, 0), # 0 - Base, front-left
(1, 0, 0), # 1 - Base, front-right
(1, 1, 0), # 2 - Base, back-right
(0, 1, 0), # 3 - Base, back-left
(0, 0, 1), # 4 - Top, front-left
(1, 0, 1), # 5 - Top, front-right
(1, 1, 1), # 6 - Top, back-right
(0, 1, 1), # 7 - Top, back-left
# Solar panels
# Left panel
(0.5, .25, -1.5), # 8
(0.5, .75, -1.5), # 9
(0.5, .75, 0), # 10
(0.5, .25, 0), # 11
# Right panel
(0.5, .25, 2.5), # 12
(0.5, .75, 2.5), # 13
(0.5, .75, 1), # 14
(0.5, .25, 1) # 15
])
# Translate spacecraft to origin
vertices = [(x - .5, y - .5, z - .5) for x, y, z in vertices]
# Original points
ax.scatter(vertices[5][0], vertices[5][1],vertices[5][2], color='green', s=25)
ax.scatter(vertices[1][0], vertices[1][1],vertices[1][2], color='green', s=25)
ax.scatter(vertices[4][0], vertices[4][1],vertices[4][2], color='green', s=25)
# Rotate spacecraft to point at specified vector
face_to_align = [5, 1, 4] # Indexes of points that define the face I want to be normal to the specified vector (-Y)
#vector = np.array([0, 0, 1]) # This rotation doesn't skew anything
vector = np.array([0.56167836, 0.76075023, 0.32523301]) # This rotation skews everything
vertices = rotate_shape_to_normal(vertices, face_to_align, vector)
# Rotated points
ax.scatter(vertices[5][0], vertices[5][1],vertices[5][2], color='blue', s=25)
ax.scatter(vertices[1][0], vertices[1][1],vertices[1][2], color='blue', s=25)
ax.scatter(vertices[4][0], vertices[4][1],vertices[4][2], color='blue', s=25)
# Faces for the cube
cube_faces = [
[vertices[0], vertices[1], vertices[5], vertices[4]], # (-Y)
[vertices[1], vertices[2], vertices[6], vertices[5]], # (+X)
[vertices[2], vertices[3], vertices[7], vertices[6]], # (+Y)
[vertices[3], vertices[0], vertices[4], vertices[7]], # (-X)
[vertices[0], vertices[1], vertices[2], vertices[3]], # (-Z)
[vertices[4], vertices[5], vertices[6], vertices[7]], # (+Z)
]
# Solar panel faces - note these are just rectangles
left_panel = [vertices[8], vertices[9], vertices[10], vertices[11]]
right_panel = [vertices[12], vertices[13], vertices[14], vertices[15]]
# Combine all faces
faces = cube_faces + [left_panel, right_panel]
# Create Poly3DCollection
poly3d = Poly3DCollection(faces, alpha=0.7)
poly3d.set_edgecolor('k')
# Set face color for different parts
poly3d.set_facecolor([[1, 0, 0], [.7, .7, .7], [.7, .7, .7], [.7, .7, .7], [.7, .7, .7], [.7, .7, .7], [0, 0.5, 1], [0, 0.5, 1]])
ax.add_collection3d(poly3d)
# Set the aspect ratio to ensure it looks like a cube
ax.set_box_aspect((1, 1, 1))
# Remove axes for a cleaner look
#ax.set_axis_off()
# Set limits to see everything
ax.set_xlim(-1, 2)
ax.set_ylim(-1, 2)
ax.set_zlim(-1, 2)
# Origin
ax.scatter(0, 0, 0, color='red', s=25)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
# Adjust view for better visualization
ax.view_init(elev=20., azim=-45)
plt.show()
1 Answer
Reset to default 2A rotation matrix is an orthogonal matrix. That means that its inverse is the same as its transpose, or, equivalently, that all rows or all columns are orthonormal (i.e. unit length and orthogonal to each other).
If you print out your rotation matrix you get
[[ 0.06007468 0.56167836 -0.10374915]
[-0.56167836 0. -0.32523301]
[-0.10374915 0.32523301 0.17917509]]
This clearly isn't orthogonal (none of the rows or columns as vectors have unit length; row 1 isn't perpendicular to row 2; etc.).
Here's an alternative rotation matrix routine:
def rotMat( a, theta ): # rotation matrix for a rotation of angle theta about axis a
R = np.zeros( ( 3, 3 ) )
n = a / np.linalg.norm( a )
C = np.cos( theta )
S = np.sin( theta )
R[0,0] = C + n[0] * n[0] * ( 1.0 - C )
R[0,1] = n[0] * n[1] * ( 1.0 - C ) - n[2] * S
R[0,2] = n[0] * n[2] * ( 1.0 - C ) + n[1] * S
R[1,1] = C + n[1] * n[1] * ( 1.0 - C )
R[1,2] = n[1] * n[2] * ( 1.0 - C ) - n[0] * S
R[1,0] = n[1] * n[0] * ( 1.0 - C ) + n[2] * S
R[2,2] = C + n[2] * n[2] * ( 1.0 - C )
R[2,0] = n[2] * n[0] * ( 1.0 - C ) - n[1] * S
R[2,1] = n[2] * n[1] * ( 1.0 - C ) + n[0] * S
return R
You can call it in your code with
rotation_matrix = rotMat( rotation_axis, theta )
Then I think your cuboid is OK.