根據兩個向量計算它們之間的旋轉矩陣

一、簡介

　　本文主要介紹通過給定的兩個空間向量，計算出從一個向量旋轉到另一個向量的旋轉矩陣，

二、步驟

　　① 假設兩個向量分別為vectorBefore(x1,y1,z1), vectorAfter(x2,y2,z2)，將這兩個向量轉為單位向量，

　　　 得到 va = normalize(vectorBefore), vb = normalize(vectorAfter)

　　② vs = vb × va, 叉乘得到旋轉軸vs

　　③ v = normalize(vs), vs轉為單位向量得到v

　　④ ca = vb · va, 點乘得到旋轉角的余弦值 ca, 即cos(angle)

　　⑤ vt = v * scale, 對v進行縮放，方便后面計算, scale = 1 - ca

　　⑥ 旋轉矩陣rm為 [3,3]矩陣, 計算原理為羅德里格旋轉公式(Rodrigues' rotation formula)

　　　 rm[0,0] = vt.x * v.x + ca　　　 

 　　　rm[1,1] = vt.y * v.y + ca

　　　 rm[2,2] = vt.z * v.z + ca

　　　 vt.x *= v.y

　　　 vt.z *= v.x

　　　 vt.y *= v.z

　　　 rm[0,1] = vt.x - vs.z

　　　 rm[0,2] = vt.z - vs.y

　　　 rm[1,0] = vt.x - vs.z

　　　 rm[1,2] = vt.y - vs.x

　　　 rm[2,0] = vt.z - vs.y

　　　 rm[2,1] = vt.y - vs.x

三、效果

　　紅色為旋轉前的平面，藍色為旋轉后的實際平面， 綠色是對紅色平面應用計算的旋轉矩陣后得到的平面，所以綠色與藍色應在同一平面內，

四、代碼

public struct Vec3
        {
            public float x;
            public float y;
            public float z;
            public Vec3(float X, float Y, float Z)
            {
                x = X;
                y = Y;
                z = Z;
            }         
        }

float[,] getRotationMatrix(Vec3 vectorBefore, Vec3 vectorAfter)
        {
            Vec3 vb = Normalize(vectorBefore);
            Vec3 va = Normalize(vectorAfter);

            Vec3 vs = CrossProduct(vb, va);
            Vec3 v = Normalize(vs);
            float ca = DotProduct(vb, va);

            float scale = 1 - ca;
            Vec3 vt = new Vec3(v.x * scale, v.y * scale, v.z * scale);

            float[,] rotationMatrix = new float[3,3];

            rotationMatrix[0, 0] = vt.x * v.x + ca;
            rotationMatrix[1, 1] = vt.y * v.y + ca;
            rotationMatrix[2, 2] = vt.z * v.z + ca;
            vt.x *= v.y;
            vt.z *= v.x;
            vt.y *= v.z;

            rotationMatrix[0, 1] = vt.x - vs.z;
            rotationMatrix[0, 2] = vt.z + vs.y;
            rotationMatrix[1, 0] = vt.x + vs.z;
            rotationMatrix[1, 2] = vt.y - vs.x;
            rotationMatrix[2, 0] = vt.z - vs.y;
            rotationMatrix[2, 1] = vt.y + vs.x;

            return rotationMatrix;
        }

        Vec3 CrossProduct(Vec3 a, Vec3 b)
        {
            Vec3 c = new Vec3()
            {
                x = a.y * b.z - a.z * b.y,
                y = a.z * b.x - a.x * b.z,
                z = a.x * b.y - a.y * b.x
            };

            return c;
        }

        float DotProduct(Vec3 a, Vec3 b)
        {
            return a.x * b.x + a.y * b.y + a.z * b.z;
        }

        Vec3 Normalize(Vec3 v)
        {
            float frt = (float)Math.Sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
            if (frt == 0.0f)
            {
                return new Vec3(0, 0, 0);
            }
            else
            {
                return new Vec3(v.x / frt, v.y / frt, v.z / frt);
            }
        }

五、呼叫庫

　　以上是通過自己計算得出結果，其實eigen庫中有輸入兩個向量得出旋轉矩陣的介面，非常簡單，

　　 Eigen::Matrix3d rotMatrix;
    Eigen::Vector3d vectorBef(vectorBefore[0], vectorBefore[1], vectorBefore[2]);
    Eigen::Vector3d vectorAft(vectorAfter[0], vectorAfter[1], vectorAfter[2]);

    rotMatrix = Eigen::Quaterniond::FromTwoVectors(vectorBef, vectorAft).toRotationMatrix();

標籤：其他

上一篇：Couverture for mac(彈簧和緩動曲線編輯器)

下一篇：Nevercenter CameraBag Photo for Mac(照片濾鏡軟體)