在Python和OpenGL中使用四元数旋转一组3D点

Question

我正在尝试旋转在pyqtgraph和opengl中使用GLMeshItem创建的一组3D点（形成一个矩形对象）。但是，我在执行此操作时遇到了麻烦。

我知道物体的原点（T）和终点（S）。我也知道我想平移并旋转到（Q和P）的点的位置，以及它们的旋转（由四元数定义）。

请参见图像以进行可视化。

[首先，我将T转换为P。然后，我将四元数旋转矩阵与每个轴进行矩阵乘法，以定义要旋转的轴（请参见下图）。现在，我想旋转对象，以便X（红色轴），Y（绿色轴）和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

        # P and Q positions
        P_Pos = np.array([-0.7359945, -3.3383354, 103.5234863])
        Q_Pos = np.array([-0.6073777, -11.3798523, 102.1110168])

        # P and Q quaternions (W, X, Y, Z)
        P_Quat = np.array([0.515479, -0.492095, 0.564883, -0.415971])
        Q_Quat = np.array([0.535875, -0.470614, 0.543981, -0.442092])

        # Translate T to P
        posDiff = P_Pos - T_Pos
        verts = posDiff-verts

        # 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])

        # Camera settings
        w.setCameraPosition(distance=90, azimuth=-2)
        w.opts['center'] = VC(P_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 P and Q positions
        sphere = gl.MeshData.sphere(rows=10, cols=20, radius=[1])

        self.neckPoint = gl.GLMeshItem(meshdata=sphere, smooth=True, color=(1, 0, 0, 0.2), shader="balloon", glOptions="additive")
        w.addItem(self.neckPoint)
        tr2 = pg.Transform3D()
        tr2.translate(*P_Pos)
        self.neckPoint.setTransform(tr2)

        self.headPoint = gl.GLMeshItem(meshdata=sphere, smooth=True, color=(1, 0, 1, 0.2), shader="balloon", glOptions="additive")
        w.addItem(self.headPoint)
        tr1 = pg.Transform3D()
        tr1.translate(*Q_Pos)
        self.headPoint.setTransform(tr1)

        # Translated object origin (should rotate around this position)
        T_Pos = (verts[0] + verts[3]) / 2

        # 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)

        # How to find the rotation from T to Q and rotate the object?

        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

几何-您有3个点：MPO是meshPointOriginmpe是meshPointEndhp是headPoint-您有2个向量：V1 =（MPE-MPO）从MPO到MPEV2 =（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的长度不同，则必须缩放磁头。（编辑：这不会左右转动头，对吗？这可能只是一个开始。）

在Python和OpenGL中使用四元数旋转一组3D点

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

1个回答

最新问题

在Python和OpenGL中使用四元数旋转一组3D点

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

1个回答

最新问题

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