当前位置：首页 > news >正文

绕任一向量旋转矩阵计算思考与实现

news 来源：原创 2024/11/15 0:47:11

欢迎关注更多精彩
关注我，学习常用算法与数据结构，一题多解，降维打击。

问题提出

请添加图片描述

如图所示，在空间中有一向量A，问点O绕A方向逆时针旋转角度α的矩阵如何表示。

问题分析

问题化规

直接去构造一个矩阵是比较困难的。
我们知道绕X，Y，Z三个方向的旋转矩阵是可以直接给出的。分别如下。
角度根据右手定则绕各轴逆时针旋转θ

$绕X轴表示为：X(\theta)=\begin{bmatrix}1&0&0\\0&cos\theta&-sin\theta\\0&sin\theta&cos\theta\end{bmatrix}$

$绕Y轴表示为：Y(\theta)=\begin{bmatrix}cos\theta&0&sin\theta\\0&1&0\\－sin\theta&0&cos\theta\end{bmatrix}$

$绕Z轴表示为：Z(\theta)=\begin{bmatrix}cos\theta&-sin\theta&0\\sin\theta&cos\theta&0\\0&0&1\end{bmatrix}$

一个直观的想法就是先把向量A转到与X轴相同的方向。
也就是沿着A与X叉乘方向旋转β，如图所示。

请添加图片描述

图中向量M分别与向量A，向量X垂直，可知向量M处于平面YOZ中。

设上述旋转为RM

那么O点最终结果可以表示如下

$O'=RM^{-1}\cdot X(\alpha) \cdot RM\cdot O$

由于向量M并不与X，Y，Z轴中任意一轴平行，所以还是不好直接给出RM表达式。

但是向量M处于平面YOZ，可以行将向量M旋转到与Y轴平行，再按照上述同理操作。设旋转到Y轴矩阵为RY。

$RM=RY^{-1}\cdot Y(-\beta)\cdot RY$

$\gamma)^{-1}\cdot Y(-\beta)\cdot X(- \gamma)$

致此，所有旋转都转化成和X轴，Y轴相关的旋转。

求解过程

RY计算。

RY的作用是把M转到与Y轴相同位置。

M可以由A与X叉乘得到

$设 M ＝ (0, M y, M z), 由于 M 处于 Y OZ 平面，可知 M x = 0$

请添加图片描述
由上图可以知

$RY=X(-\gamma)=X(\gamma)^T$

$=\begin{bmatrix}1&0&0\\0&cos\gamma&-sin\gamma\\0&sin\gamma&cos\gamma\end{bmatrix}^T$

$=\begin{bmatrix}1&0&0\\0&cos\gamma&sin\gamma\\0&－sin\gamma&cos\gamma\end{bmatrix}$

$=\begin{bmatrix}1&0&0\\0&My&Mz\\0&－Mz&My\end{bmatrix}$

$RY^{-1}=RY^T$

有了RY后，可以先将A乘上RY。这样A就会被旋转到平面ZOX上来。

$RY\cdot A = (Ax',0, Az')$

同理，

$RX=Y(-\beta) = Y(\beta)^T$

$\begin{bmatrix}cos\beta&0&-sin\beta\\0&1&0\\sin\beta&0&cos\beta\end{bmatrix}$

$\begin{bmatrix}Ax'&0&Az'\\0&1&0\\－Az'&0&Ax'\end{bmatrix}$

旋转后的P’

$P'=RY^{-1}\cdot RX^{-1}\cdot X(\alpha)\cdot RX\cdot RY\cdot P$

代码实现

代码链接点击前往
代码链接点击前往
代码链接点击前往


namespace acamcad {
    const double pi = acos(-1);
    using Point = Eigen::Vector3d;
    class RigidRTMatrix {
    private:
        Eigen::Matrix3d mat;
        Eigen::Vector3d trans;
        
    public:
        RigidRTMatrix(Point start, Point end, double theta) {
            cout << "generate RigidRTMatrix 2" << endl;
            Eigen::Vector3d v = end - start;
            cout << "v:" << v << endl;
            cout << "angle:" << theta << endl;
            assert(!v.isZero());
            // Point::Zero();
            v.normalize();
            Eigen::Vector3d X(1,0,0);
            Eigen::Vector3d m = v.cross(X);
            // todo m=0时特殊处理
            if (m.isZero()) {
                if (v.dot(X) > 0) m = { 0,1,0 }; // 直接等于Y轴
                else m = { 0,-1,0 }; // 等于Y轴的反轴
            }

            auto RY = GetRY(m);
            // 将v 旋转至ZOX 平面。
            auto vZOX = RY * v;
            auto RX = GetRX(vZOX);

            auto Xrotate = GetXRotate(theta);

            mat = RY.transpose() * RX.transpose() * Xrotate * RX * RY;
            cout << "mat create :" << mat << endl;
        }
        
        RigidRTMatrix() {

        }
        
        // 给定YOX平面上的单位M向量，将其旋转到Y轴上。
        Eigen::Matrix3d GetRY(Eigen::Vector3d m) {
            assert(!m.isZero());
            m.normalize();
            Eigen::Matrix3d RY;
            RY.setIdentity();
            RY(1, 1) = m.y();
            RY(1, 2) = m.z();
            RY(2, 1) = -m.z();
            RY(2, 2) = m.y();
            return RY;
        }

        // 给定ZOX平面上的单位M向量，将其旋转到X轴上。
        Eigen::Matrix3d GetRX(Eigen::Vector3d m) {
            assert(!m.isZero());
            m.normalize();
            Eigen::Matrix3d RX;
            RX.setIdentity();
            RX(0, 0) = m.x();
            RX(0, 2) = m.z();
            RX(2, 0) = -m.z();
            RX(2, 2) = m.x();
            return RX;
        }

        // 给定ZOX平面上的单位M向量，将其旋转到X轴上。
        Eigen::Matrix3d GetXRotate(double theta) {
            double rad = theta / 180 * pi;
            Eigen::Matrix3d X;
            X.setIdentity();
            X(1, 1) = cos(rad);
            X(1, 2) = -sin(rad);
            X(2, 1) = sin(rad);
            X(2, 2) = cos(rad);
            return X;
        }

        double angleMod(double theta) {
            while (theta < -180)theta += 360;
            while (theta > 180)theta -= 360;
            return theta;
        }

        Point Trans(Point &a) {
            return mat * a;
        }

        friend static RigidRTMatrix operator*(RigidRTMatrix& a, RigidRTMatrix& b) {
            RigidRTMatrix multi;
            multi.mat = a.mat * b.mat;
            return multi;
        }
    };
}