I have two lines represented as 3D vectors with their individual start and end points. The red line is fixed on a base (RV-O, red vector origin) and can perform a full rotation between 0° and 360° in anticlockwise z direction . After a rotation about its origin using a given direction vector and angle, its end point RV-E becomes the new position RV-E'.
The orange line is connected on its origin point OV-O with the RV-E' and every rotation applied on the RV-E rotates it also in the same direction. Afterwards, it can independently rotate in y direction between -90° and 90°and becomes the new position then OV-E', as shown on the left side.
If I apply a ~70° rotation as shown on the middle (Rotation of RV-E) on the red line RV-E, the new positions of the red RV-E' and orange line OV-E' are fully correct.
But my problem begins after that. The applied rotation on the OV-E' in y direction to become a new OV-E'' brings the line in a correct direction indeed, but with some offset OV offset-E''.
I implemented this in python. The 3D rotation matrix that I use looks like follows:
def rotate_line(self, axis_direction, origin, point_to_rotate, angle):
angle = np.radians(angle)
a, b, c = origin
x, y, z = point_to_rotate
if axis_direction == 0:
u, v, w = [1.0, 0.0, 0.0] # Rotate in x direction
elif axis_direction == 1:
u, v, w = [0.0, 1.0, 0.0] # Rotate in y direction
elif axis_direction == 2:
u, v, w = [-0.1027, -0.1525, 0.9829] # Rotate in z direction
cos = np.cos(angle)
sin = np.sin(angle)
qx = (a*(v**2 + w**2) -u*(b*v + c*w - u*x - v*y - w*z)) * (1-cos) + x*cos + (-c*v + b*w - w*y + v*z) * sin
qy = (b*(u**2 + w**2) -v*(a*u + c*w - u*x - v*y - w*z)) * (1-cos) + y*cos + (c*u - a*w + w*x - u*z) * sin
qz = (c*(u**2 + v**2) -w*(a*u + b*v - u*x - v*y - w*z)) * (1-cos) + z*cos + (-b*u + a*v - v*x + u*y) * sin
return qx, qy, qz
I guess my problem is with the directions. I know the exact direction unit vector of the base. Therefore I can apply it for the rotations in z direction. But for rotations in x and y directions, I simply set u, v, w = [1.0, 0.0, 0.0] and u, v, w = [0.0, 1.0, 0.0] which probably causes these offsets. My second problem is the shifted axes after applied rotations on a line. At the begin e.g. the y axis is on its default position, after rotation it changes based on the new position of the line. Therefore my if conditions if axis_direction == does not correct anymore for another planned rotation.
I would appreciate any help
Edit: Some code is added
self.red_line_rot_axis = 2 # rotation direction, z axis
self.orange_line_rot_axis = 1 # rotation direction, y axis
def visualize_red_line(self):
self.red_line_origin = [-11.833, -173.2017, 1064.0267]
self.red_line = vtk.vtkLineSource()
self.red_line.SetPoint1(origin[0], origin[1], origin[2])
self.red_line.SetPoint2(self.red_line.GetPoint1()[0],
self.red_line.GetPoint1()[1] + 40, self.red_line.GetPoint1()[2])
self.red_line_origin = self.red_line.GetPoint1()
self.red_line_curr_pos = self.red_line.GetPoint2()
self.red_line_mapper = vtk.vtkPolyDataMapper()
self.red_line_mapper.SetInputConnection(
self.red_line.GetOutputPort())
self.red_line_actor = vtk.vtkActor()
self.red_line_actor.SetMapper(self.red_line_mapper)
self.red_line_actor.GetProperty().SetColor(1.0, 0.0, 0.0)
self.red_line_actor.GetProperty().SetLineWidth(2)
self.renderer.AddActor(self.red_line_actor)
def visualize_orange_line(self):
self.orange_line = vtk.vtkLineSource()
self.orange_line.SetPoint1(self.red_line_curr_pos)
self.orange_line.SetPoint2(self.red_line_curr_pos[0],
self.red_line_curr_pos[1] + 50, self.red_line_curr_pos[2])
self.orange_line_origin = self.orange_line.GetPoint1()
self.orange_line_curr_pos = self.orange_line.GetPoint2()
self.orange_line_mapper = vtk.vtkPolyDataMapper()
self.orange_line_mapper.SetInputConnection(
self.orange_line.GetOutputPort())
self.orange_line_actor = vtk.vtkActor()
self.orange_line_actor.SetMapper(self.orange_line_mapper)
self.orange_line_actor.GetProperty().SetColor(0.9, 0.6, 0.1)
self.orange_line_actor.GetProperty().SetLineWidth(2)
self.renderer.AddActor(self.orange_line_actor)
# Begin with the rotation after representing both lines
rotate_red_line(self)
# Rotate red and orange line between 0° and x° with 1° increment each time about the origin of red line
def rotate_red_line(self):
min_angle = 0.0
max_angle = 50.0 #260 #360
curr_angle = 1
for idx in np.arange(min_angle, max_angle, curr_angle):
rotated_red_line_pos = np.array(rotate_line(self, self.red_line_rot_axis, self.red_line_origin , self.red_line_curr_pos, curr_angle))
rotated_orange_line_pos = np.array(rotate_line(self, self.red_line_rot_axis, self.red_line_origin , self.orange_line_curr_pos, curr_angle))
# Visualize the rotation
update_red_line_pos(self, rotated_red_line_pos, rotated_orange_line_pos)
if curr_angle == max_angle:
# Rotate the orange line afterwards
rotate_orange_line(self, rotated_red_line_pos, rotated_orange_line_pos)
curr_angle = np.round(curr_angle + 1, 2)
# Update the positions of the lines after rotation
def update_red_line_pos(self, rotated_red_line_pos, rotated_orange_line_pos,):
self.red_line.SetPoint2(rotated_red_line_pos)
self.orange_line.SetPoint1(rotated_red_line_pos)
self.orange_line.SetPoint2(rotated_orange_line_pos)
# Rotate orange line between -20° and x° with 1° increment each time about the origin of orange line
# The origin of the orange line is the new position of the red line
def rotate_orange_line(self, rotated_red_line_pos, rotated_orange_line_pos):
min_angle = -20.0
max_angle = 180.0
curr_angle = 1
for idx in np.arange(min_angle, max_angle, curr_angle):
rotated_orange_line_pos = np.array(rotate_line(self, self.orange_line_rot_axis, rotated_red_line_pos , rotated_orange_line_pos, curr_angle))
# Visualize the rotation
update_orange_line_pos(self, rotated_orange_line_pos)
curr_angle = np.round(curr_angle + 1, 2)
# Update the position of the orange line after rotation
def update_orange_line_pos(self, rotated_orange_line_pos):
self.orange_line.SetPoint2(rotated_orange_line_pos)
The image of rotation of a point $E$ about an axis that passes through point $O$
is given by
$E' = O + R (E - O) = (I - R) O + R E $
If the unit vector of the axis is a = (u, v, w) as in your code, then
$R = a a^T + (I - a a^T) \cos \theta + S_a \sin \theta $
where
$S_a = \begin{bmatrix} 0 && - w && v \\ w && 0 && -u \\ -v && u && 0 \end{bmatrix}$
As per our discussion on the MSE chat, I realized that you want to rotate both line segment (the red and the orange) about the 'z' axis (which is not really the z axis, but an arbitrary axis), and then you want rotate only the orange line segment about the rotated Y axis resulting after rotation about your arbitrary 'z' axis. You mentioned that you don't have any problems with the first rotation, and that the problem is with the second rotation. I am going to re-label the endpoints of the red segment as $A$ and $B$ respectively, and the orange segment as $B$ and $C$, (Because the origin of the orange segment is the end of the red segment). After the first rotation, the images are
$\large A' = A + R_z (A - A) = A $
$\large B' = A + R_z (B - A ) $
$\large C' = A + R_z (C - A) $
Now for the second rotation, which is about the rotated $Y$ axis passing though the point $B'$
$\large B'' = B' + R_{y'} (B' - B') = B' $
$\large C'' = B' + R_{y'} (C' - B') = B' + R_{y'} R_z (C - B) $
Once can either calculate the axis $y'$ by rotating the $y$ axis about the $z$ axis, or make use of the fact that
$\large R_{y'} R_z = \left( R_z R_y R_z^T \right) R_z = R_z R_y $
Hence,
$\large C'' = B' + R_z R_y ( C - B ) = A + R_z (B - A) + R_z R_y (C - B ) $
Make sure, when implementing the rotations, that you correctly specify the point where the axis of rotation passes.