问题
I am trying to rotate a group of 3D points (forming a rectangular object) created using GLMeshItem in pyqtgraph and opengl. However, I am having trouble doing this.
I need to align the axes of the object (T_X, T_Y, T_Z) to the axes (Q_X, Q_Y, Q_Z). The Q axes are vectors calculated from a quaternion.
Steps to reproduce: 1. Make a rotation matrix from the quaternion 2. Matrix multiplication to define the Q axes i want to rotate to 3. Translate the object origin to Q. 4. Make unit vectors and calculate the angle between the pairs of X, Y, Z axes 5. Rotate the difference between X and Z axis
Can you help me?
Sample code:
import numpy as np
import sys
from PyQt5.QtWidgets import QApplication, QHBoxLayout, QWidget
import pyqtgraph as pg
import pyqtgraph.opengl as gl
from pyqtgraph import Vector as VC
class MainWindow(QWidget):
def __init__(self):
super(MainWindow, self).__init__()
def quaternion_to_rotmat(point):
qw = point[0]
qx = point[1]
qy = point[2]
qz = point[3]
RotMat = np.array([[1 - 2 * (qy ** 2) - 2 * (qz ** 2),
2 * qx * qy - 2 * qz * qw,
2 * qx * qz + 2 * qy * qw],
[2 * qx * qy + 2 * qz * qw,
1 - 2 * (qx ** 2) - 2 * (qz ** 2),
2 * qy * qz - 2 * qx * qw],
[2 * qx * qz - 2 * qy * qw,
2 * qy * qz + 2 * qx * qw,
1 - 2 * (qx ** 2) - (2 * qy ** 2)]])
return RotMat
self.setFixedSize(1000, 700)
self.graphLayout = QHBoxLayout()
# Set camera
w = gl.GLViewWidget()
# Group of points defining the rectangle object
verts = np.array([(-1.0, -1.0, 0.0),
(1.0, -1.0, 0.0),
(-1.0, 1.0, 0.0),
(1.0, 1.0, 0.0),
(-1.2987148761749268, -1.3632668256759644, -0.16066408157348633),
(-1.2987148761749268, -1.3632668256759644, 7.678848743438721),
(-1.2987148761749268, 1.3632668256759644, -0.16066408157348633),
(-1.2987148761749268, 1.3632668256759644, 7.678848743438721),
(1.2987148761749268, -1.3632668256759644, -0.16066408157348633),
(1.2987148761749268, -1.3632668256759644, 7.678848743438721),
(1.2987148761749268, 1.3632668256759644, -0.16066408157348633),
(1.2987148761749268, 1.3632668256759644, 7.678848743438721),
(-1.0, -1.0, 7.536437511444092),
(1.0, -1.0, 7.536437511444092),
(-1.0, 1.0, 7.536437511444092),
(1.0, 1.0, 7.536437511444092)])
faces = np.array([(1, 2, 0), (1, 3, 2), (5, 6, 4),
(7, 10, 6), (11, 8, 10), (9, 4, 8),
(10, 4, 6), (7, 9, 11), (5, 7, 6),
(7, 11, 10), (11, 9, 8), (9, 5, 4),
(10, 8, 4), (7, 5, 9), (13, 14, 12),
(13, 15, 14)])
colors = np.array([[1, 1, 1, 1] for i in range(len(faces))])
# Object origin (should rotate around this position)
T_Pos = (verts[0] + verts[3]) / 2
# Q position
Q_Pos = np.array([-13.5708862, 1.1735056, 107.5772339])
# Q quaternion (W, X, Y, Z)
Q_Quat = np.array([0.547013, 0.593053, -0.543852, -0.230846])
# Find the rotation matrix of Q quaternion
rotMat = quaternion_to_rotmat(Q_Quat)
# Matrix multiplication
r1 = np.matmul(rotMat, np.array([1, 0, 0]))
r2 = np.matmul(rotMat, np.array([0, 1, 0]))
r3 = np.matmul(rotMat, np.array([0, 0, 1]))
# Define new points - Multiply by 25 to visualize the axis in openGL
Q_X = np.array([Q_Pos[0] + r1[0] * 25, Q_Pos[1] + r1[1] * 25, Q_Pos[2] + r1[2] * 25])
Q_Y = np.array([Q_Pos[0] + r2[0] * 25, Q_Pos[1] + r2[1] * 25, Q_Pos[2] + r2[2] * 25])
Q_Z = np.array([Q_Pos[0] + r3[0] * 25, Q_Pos[1] + r3[1] * 25, Q_Pos[2] + r3[2] * 25])
Q_Line_X = np.array([Q_Pos, Q_X])
Q_Line_Y = np.array([Q_Pos, Q_Y])
Q_Line_Z = np.array([Q_Pos, Q_Z])
Q_Vec_X = Q_Pos-Q_X
Q_Vec_Y = Q_Pos-Q_Y
Q_Vec_Z = Q_Pos-Q_Z
# Camera settings
w.setCameraPosition(distance=90, azimuth=-2)
w.opts['center'] = VC(Q_Pos)
# Add object to window
self.object = gl.GLMeshItem(vertexes=verts, faces=faces, faceColors=colors, smooth=False, shader='shaded', glOptions='opaque')
w.addItem(self.object)
# Add visualization of Q positions
sphere = gl.MeshData.sphere(rows=10, cols=20, radius=[1])
self.P_Point = gl.GLMeshItem(meshdata=sphere, smooth=True, color=(1, 0, 1, 0.2), shader="balloon", glOptions="additive")
w.addItem(self.P_Point)
tr1 = pg.Transform3D()
tr1.translate(*Q_Pos)
self.P_Point.setTransform(tr1)
# Translated object origin (should rotate around this position)
# Translate T to Q
posDiff = Q_Pos - T_Pos
verts = posDiff-verts
T_Pos_base = (verts[0] + verts[3]) / 2
T_Pos_baseX = T_Pos_base - np.array([T_Pos_base[0] + 10, T_Pos_base[1], T_Pos_base[2]])
T_Pos_baseY = T_Pos_base - np.array([T_Pos_base[0], T_Pos_base[1] + 10, T_Pos_base[2]])
T_Pos_baseZ = T_Pos_base - np.array([T_Pos_base[0], T_Pos_base[1], T_Pos_base[2] + 10])
unit_TX = T_Pos_baseX / np.linalg.norm(T_Pos_baseX)
unit_TY = T_Pos_baseY / np.linalg.norm(T_Pos_baseY)
unit_TZ = T_Pos_baseZ / np.linalg.norm(T_Pos_baseZ)
unit_QX = Q_Vec_X / np.linalg.norm(Q_Vec_X)
unit_QY = Q_Vec_Y / np.linalg.norm(Q_Vec_Y)
unit_QZ = Q_Vec_Z / np.linalg.norm(Q_Vec_Z)
dotX = np.dot(unit_TX, unit_QX)
dotY = np.dot(unit_TY, unit_QY)
dotZ = np.dot(unit_TZ, unit_QZ)
angleX = np.rad2deg(np.arccos(dotX))
angleY = np.rad2deg(np.arccos(dotY))
angleZ = np.rad2deg(np.arccos(dotZ))
# Visualization of T axes
T_Pos_X = [T_Pos[0]+10, T_Pos[1], T_Pos[2]]
self.T_Plot_X = gl.GLLinePlotItem(pos=np.array([T_Pos, T_Pos_X]), color=(1,0,0,1), width=1, antialias=False)
w.addItem(self.T_Plot_X)
T_Pos_Y = [T_Pos[0], T_Pos[1]+10, T_Pos[2]]
self.T_Plot_Y = gl.GLLinePlotItem(pos=np.array([T_Pos, T_Pos_Y]), color=(0,1,0,1), width=1, antialias=False)
w.addItem(self.T_Plot_Y)
T_Pos_Z = [T_Pos[0], T_Pos[1], T_Pos[2]+10]
self.T_Plot_Z = gl.GLLinePlotItem(pos=np.array([T_Pos, T_Pos_Z]), color=(0,0,1,1), width=1, antialias=False)
w.addItem(self.T_Plot_Z)
# Visualization of Q axes
self.Q_Plot_X = gl.GLLinePlotItem(pos=np.array(Q_Line_X), color=(1,0,0,1), width=1, antialias=False)
w.addItem(self.Q_Plot_X)
self.Q_Plot_Y = gl.GLLinePlotItem(pos=np.array(Q_Line_Y), color=(0,1,0,1), width=1, antialias=False)
w.addItem(self.Q_Plot_Y)
self.Q_Plot_Z = gl.GLLinePlotItem(pos=np.array(Q_Line_Z), color=(0,0,1,1), width=1, antialias=False)
w.addItem(self.Q_Plot_Z)
tr1 = pg.Transform3D()
tr1.translate(*Q_Pos)
tr1.rotate(-angleX, 0, 0, 1)
tr1.rotate(angleZ, 1, 0, 0)
self.T_Plot_X.setTransform(tr1)
self.T_Plot_Y.setTransform(tr1)
self.T_Plot_Z.setTransform(tr1)
tr5 = pg.Transform3D()
tr5.translate(*Q_Pos)
tr5.rotate(-angleX, 0, 0, 1)
tr5.rotate(angleZ, 1, 0, 0)
self.object.setTransform(tr5)
self.graphLayout.addWidget(w)
self.setLayout(self.graphLayout)
if __name__ == '__main__':
app = QApplication(sys.argv)
ex = MainWindow()
ex.show()
sys.exit(app.exec_())
回答1:
Try out this: another = verts + 2.0 * np.cross(Q_Quat[1:4], np.cross(Q_Quat[1:4], verts) + Q_Quat[0] * verts)
回答2:
The geometry -- you have 3 points:
mpo is meshPointOrigin
mpe is meshPointEnd
hp is headPoint
-- You have 2 vectors:
V1=(mpe-mpo) from mpo to mpe
V2=(hp-mpo) from mpo to hp
-- You want to rotate V1 to V2.
The unit normal vector is VN = (V1 cross V2).normalize()
The cosine of the angle is V1 dot V2
From here you could make the quaternion:
kcos = sqrt((1+cosine)/2) (half-angle formula)
ksin = sqrt(1-ksin*ksin)
quaternionWXYZ = (kcos, ksin*VN.x, ksin*VN.y, ksin*VN.z)
If the length of V1 is different than V2 you would have to scale the head.
(EDIT: This doesn't turn the head left/right, does it? This may be just a start.)
回答3:
create 4x4 homogenuous transform matrices
A,Brepresenting your 2 coordinate systemsif you have origin and xis basis vectors its just a matter of feeding it to matrix ...
compute the transformation
Cthat convertsAtoBA * C = B // Inverse(A)* Inverse(A) * A * C = Inverse(A)* B C = Inverse(A)* B
And that is it ...
来源：https://stackoverflow.com/questions/61540785/how-to-find-the-angles-to-align-x-y-z-vectors-to-another-coordinate-system