根據旋轉前后的向量值求旋轉矩陣

如果已知旋轉前后的一向量的變化，那么該如何求這個旋轉矩陣呢？本篇結合Rodrigues' rotation formula，介紹一下該旋轉矩陣的求法。

1.旋轉角度

已知旋轉前向量為P, 旋轉后變為Q。由點積定義可知：

可推出P，Q之間的夾角為：

2. 旋轉軸

由1中可知，旋轉角所在的平面為有P和Q所構成的平面，那么旋轉軸必垂直該平面。

假定旋轉前向量為a(a1, a2, a3)， 旋轉后向量為b（b1, b2, b3)。由叉乘定義得：

所以旋轉軸c(c1, c2, c3)為：

3.  羅德里格旋轉公式(Rodrigues' rotation formula)

3.1 公式

已知單位向量 ， 將它旋轉θ角。由羅德里格旋轉公式，可知對應的旋轉矩陣 ：

其中I是3x3的單位矩陣，

是叉乘中的反對稱矩陣r：

3.2 公式證明

假設在坐標系(x, y, z）中，向量v=ax+by+cz，v繞z軸逆時針旋轉θ角后得到新的向量v’。

根據2維（x,y)面上的旋轉公式可得：

推出：

已知：

將上式帶入v’的公式：

  將cz替換掉，可得：

將上式中的叉乘表示為反對稱矩陣得：

另外：

最終可以推出：

上式即為羅德里格旋轉公式。

 

4. 求旋轉矩陣

根據旋轉前后的兩個向量值，使用上面的方法，先求出旋轉角度和旋轉軸，然后用羅德里格旋轉公式即可求出對應的旋轉矩陣。

C#的實現代碼如下：

void Calculation(double[] vectorBefore, double[] vectorAfter)
{
    double[] rotationAxis;
    double rotationAngle;
    double[,] rotationMatrix;
    rotationAxis = CrossProduct(vectorBefore, vectorAfter);
    rotationAngle = Math.Acos(DotProduct(vectorBefore, vectorAfter) / Normalize(vectorBefore) / Normalize(vectorAfter));
    rotationMatrix = RotationMatrix(rotationAngle, rotationAxis);
}

double[] CrossProduct(double[] a, double[] b)
{
    double[] c = new double[3];

    c[0] = a[1] * b[2] - a[2] * b[1];
    c[1] = a[2] * b[0] - a[0] * b[2];
    c[2] = a[0] * b[1] - a[1] * b[0];

    return c;
}

double DotProduct(double[] a, double[] b)
{
    double result;
    result = a[0] * b[0] + a[1] * b[1] + a[2] * b[2];

    return result;
}

double Normalize(double[] v)
{
    double result;

    result = Math.Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);

    return result;
}

double[,] RotationMatrix(double angle, double[] u)
{
    double norm = Normalize(u);
    double[,] rotatinMatrix = new double[3,3];
    
    u[0] = u[0] / norm;
    u[1] = u[1] / norm;
    u[2] = u[2] / norm;

    rotatinMatrix[0, 0] = Math.Cos(angle) + u[0] * u[0] * (1 - Math.Cos(angle));
    rotatinMatrix[0, 0] = u[0] * u[1] * (1 - Math.Cos(angle) - u[2] * Math.Sin(angle));
    rotatinMatrix[0, 0] = u[1] * Math.Sin(angle) + u[0] * u[2] * (1 - Math.Cos(angle));

    rotatinMatrix[0, 0] = u[2] * Math.Sin(angle) + u[0] * u[1] * (1 - Math.Cos(angle));
    rotatinMatrix[0, 0] = Math.Cos(angle) + u[1] * u[1] * (1 - Math.Cos(angle));
    rotatinMatrix[0, 0] = -u[0] * Math.Sin(angle) + u[1] * u[2] * (1 - Math.Cos(angle));
      
    rotatinMatrix[0, 0] = -u[1] * Math.Sin(angle) + u[0] * u[2] * (1 - Math.Cos(angle));
    rotatinMatrix[0, 0] = u[0] * Math.Sin(angle) + u[1] * u[2] * (1 - Math.Cos(angle));
    rotatinMatrix[0, 0] = Math.Cos(angle) + u[2] * u[2] * (1 - Math.Cos(angle));

    return rotatinMatrix;
}