从某一点出发沿任意一方向旋转矩阵计算思考与实现-Toy模板网

这篇具有很好参考价值的文章主要介绍了从某一点出发沿任意一方向旋转矩阵计算思考与实现。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

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

上期讲到
绕任一向量旋转矩阵计算思考与实现
点击前往
点击前往

问题提出

之前讲到绕任一向量旋转矩阵实现，原来的向量都是从原点出发，现在把出发点改变成任意一点。

从某一点出发沿任意一方向旋转矩阵计算思考与实现

除了需要计算旋转矩阵，还可以支持2个旋转矩阵的乘法操作。就是2个旋转矩阵依次作用以后的一个综合结果。

问题分析

从某点出发沿某一方向旋转实现

从原点出发的旋转是已经实现的，我的思路是把坐标系统一平移一个(-x0, -y0,-z0)，然后作用旋转矩阵，最后再平移回去。

用公式表示, RT1表示旋转矩阵，O1表示出发点。

$RT1\cdot(P-O1)+O1 （1）$

既我们只要用一个旋转矩阵RT加一个出发点O就可以表示这个旋转动作。那么如果是2个旋转动作叠加怎么去用一个旋转动作来表示呢。

旋转相乘

有一种方法是用奇次矩阵来表示一个平移和旋转的组合，然后矩阵可以相乘，但是这种方式需要用到4*4的矩阵效率上没有3*3的矩阵来得高。

我这边通过公式推导得到一种新的表示方法。

首先对式子（1）进行展开再合并同类项得到

$P'=RT1\cdot P+(I-RT1)\cdot O1$

$(I-RT1)\cdot O1 是一个常量，$

$RT\cdot P+Q(Q是一个常量)的形式，$

$就可用一个矩阵加一个出发点的形式来表示。$

$出发点O=(I-RT)^{-1}Q$

现在有第二个旋转RT2，O2作用于P’ 得到

$P''=RT2\cdot P' + (I-RT2)\cdot O2$

$=RT2\cdot RT1 \cdot P + RT2\cdot (I-RT1)\cdot O1 + (I-RT2)\cdot O2$

$RT2\cdot RT1$

$(I-RT)^{-1}\cdot [RT2\cdot (I-RT1)\cdot O1 + (I-RT2)\cdot O2]$

这样，我们就得了两个刚体变换的综合变换。

但是上面的方法有一个问题，当(I-RT)不可逆时，那么该公式失效。
我的理解是，当(I-RT)不可逆时，O并不是没有解，而是有很多个解。

那么我们要另选他法。

再观察（1）式，其实我们并不需要求出O，只要求出Q就可以了。

$RT2\cdot RT1, Q=RT2\cdot Q_1 + Q_2$

这个方法与奇次矩阵相比，计算量小很多。

代码实现

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


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;

            trans = (Eigen::Matrix3d::Identity() - mat) * start; // 存储总体位移
        }
        
        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 + trans;
        }

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