如何找到将X、Y、Z向量对准另一个坐标系的角度。

Question

我正在尝试旋转一组3D点（形成一个矩形对象），使用 GLMeshItem 在pyqtgraph和opengl中。然而，我在做这件事时遇到了麻烦。

我需要将对象的轴 (T_X, T_Y, T_Z) 对齐轴 (Q_X, Q_Y, Q_Z) 。Q轴是由四元组计算出来的向量。

重现步骤：1.由四元数制作旋转矩阵2. 矩阵乘法来定义我要旋转的Q轴3. 将对象原点转化为Q轴4. 制作单位向量并计算X、Y、Z轴对之间的角度5. 旋转X轴和Z轴之间的差值

你能帮助我吗？

示例代码。

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_())

Answer 1

试试这个： another = verts + 2.0 * np.cross(Q_Quat[1:4], np.cross(Q_Quat[1:4], verts) + Q_Quat[0] * verts)

Answer 2

几何体 -- 你有3个点：mpo是meshPointOrigin mpe是meshPointEnd hp是headPoint -- 你有2个向量，V1=(mpe-mpo)从mpo到mpe V2=(hp-mpo)从mpo到hp -- 你想把V1旋转到V2。V1=(mpe-mpo)从mpo到mpe V2=(hp-mpo)从mpo到hp -- 你想把V1旋转到V2。单位法向量是VN=(V1交V2).normalize() 角度的余弦是V1点V2 从这里你可以做四元组。

kcos = sqrt((1+cosine)/2) (half-angle formula)  
ksin = sqrt(1-ksin*ksin)   
quaternionWXYZ = (kcos, ksin*VN.x, ksin*VN.y, ksin*VN.z)

如果V1的长度和V2的长度不一样，你就得把头按比例放大。(编辑：这不会使头向左转吧？这可能只是一个开始。)

Answer 3

创建 4x4均匀变换矩阵。 A,B 代表你的两个坐标系
如果你有了原点和xis基向量，只需将其输入到矩阵中...

计算变换 C 兑换 A 到 B

             A * C =             B // Inverse(A)*
Inverse(A) * A * C = Inverse(A)* B
                 C = Inverse(A)* B

就是这样...

如何找到将X、Y、Z向量对准另一个坐标系的角度。

问题描述投票：1回答：1

1个回答

最新问题

如何找到将X、Y、Z向量对准另一个坐标系的角度。

问题描述 投票：1回答：1

1个回答

最新问题

问题描述投票：1回答：1