How to find the angles to align X, Y, Z vectors to another coordinate system

Question

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

score 1 · Accepted Answer · answered May 08 '20 at 06:56

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)

answered May 08 '20 at 06:56

Masilila

504
4
9

Thanks! Got it to work using the following loop for all verts arr = np.empty((0, 3), int) for i in range(len(verts)): another = verts[i] + 2.0 * np.cross(quat[1:4], np.cross(quat[1:4], verts[i]) + quat[0] * verts[i]) arr = np.append(arr, np.array([another]), axis=0) tr = pg.Transform3D() tr.translate(*pos) – Frederik Petri May 08 '20 at 07:01

dcromley · Answer 2 · 2020-05-01T15:16:11.603

0

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

edited May 01 '20 at 15:16

answered May 01 '20 at 15:10

dcromley

1,373
1
8
23

I tried implementing your suggestion however, I am not sure what exactly to do. How do I use the quaternionWXYZ to rotate the mesh? – Frederik Petri May 03 '20 at 14:59
@Frederik Petri I"m sorry, I don't do python or opengl. I do javascript/three.js. Surely there must be some kind of "applyQuaternion" function. – dcromley May 04 '20 at 04:51

score 0 · Answer 3 · answered May 06 '20 at 07:38

0

create 4x4 homogenuous transform matrices A,B representing your 2 coordinate systems

if you have origin and xis basis vectors its just a matter of feeding it to matrix ...

compute the transformation C that converts A to B

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

And that is it ...

answered May 06 '20 at 07:38

Spektre

49,595
11
110
380

Hi @Spektre. Thanks for your answer. I am not practiced in mathematics or Python, however, I read in the link you posted in section 6, that "If you want glRotate then you should use quaternions instead because that is rotation around axis not by 3 angles!". So I dont know if the 4x4 homogenuous transform matrices will work, as I am using glRotate? – Frederik Petri May 06 '20 at 07:51
@FrederikPetri glRotate will rotate the 4x4 matrix ... what math it use inside is irrelevant... if you already have your 2 coordinate systems you always can create their matrices. And aligning is much easier on those ... if you have just rotational matrices (from quaternions) you still can make 4x4 transform matrix you just need to know the origin (translation) if not you can use `(0,0,0)` and remedy the offset latter – Spektre May 06 '20 at 08:09
Hi @Spektre - so I have the 4x4 matrixes for A and B. I am not quite sure what to do next - I am using rotate(angle, x, y, z). I think its the same as glRotate. Can you explain a bit more how to compute the transformation C? And then how to apply the rotate(angle, x, y, z)? Sorry for my bad understanding:) – Frederik Petri May 06 '20 at 08:20
@FrederikPetri compute `C = Inverse(A)* B` and then simply when you want to transform your points from `A` to `B` just multiply the "actual" `A` matrix by `C` that is all – Spektre May 06 '20 at 08:22
@FrederikPetri maybe this [Problem superimposing and aligning 3D triangles](https://stackoverflow.com/a/52163563/2521214) will clear things out for you ... but its one level higher problem as the matrices are not known there. – Spektre May 06 '20 at 08:25

How to find the angles to align X, Y, Z vectors to another coordinate system

3 Answers3