c++ 知道旋轉平移變換前后矩陣向量值求旋轉變換矩陣c++/c#代碼知道兩個向量求他們的旋轉變換矩陣

本文轉載自查看原文 2019-12-29 15:53 1548 C/C++

原文作者：aircraft

原文鏈接：https://www.cnblogs.com/DOMLX/p/12115244.html

知道旋轉前后矩陣向量值如何去求旋轉矩陣R 的c++/c#代碼？？？

因為需要用到矩陣處理庫所以需要先配置

一、Eigen庫的配置(VS2017)

Eigen庫下載： http://eigen.tuxfamily.org/index.php?title=Main_Page
下載文件並解壓：

然后在自己的VS工程屬性中的這個附加包含進去

注意看清楚了是D:\Dependencies\eigen-eigen\eigen-eigen; 前面部分是你們自己的路徑后面的這個eigen-eigen\eigen-eigen; 代表的意思解壓是點擊進去選擇里面那個名字跟外面一樣的

二、實現代碼

c++代碼：

#include <cmath>
#include <iostream>
#include "Eigen/Dense"  
#include "Eigen/LU"  
#include "Eigen/Core"  
#define PI 3.1415926

//計算旋轉角
double calculateAngle(const Eigen::Vector3d &vectorBefore, const Eigen::Vector3d &vectorAfter)
{
    double ab, a1, b1, cosr;
    ab = vectorBefore.x()*vectorAfter.x() + vectorBefore.y()*vectorAfter.y() + vectorBefore.z()*vectorAfter.z();
    a1 = sqrt(vectorBefore.x()*vectorBefore.x() + vectorBefore.y()*vectorBefore.y() + vectorBefore.z()*vectorBefore.z());
    b1 = sqrt(vectorAfter.x()*vectorAfter.x() + vectorAfter.y()*vectorAfter.y() + vectorAfter.z()*vectorAfter.z());
    cosr = ab / a1 / b1;
    return (acos(cosr) * 180 / PI);
}
//計算旋轉軸
inline Eigen::Vector3d calculateRotAxis(const Eigen::Vector3d &vectorBefore, const Eigen::Vector3d &vectorAfter)
{
    return Eigen::Vector3d(vectorBefore.y()*vectorAfter.z() - vectorBefore.z()*vectorAfter.y(), \
        vectorBefore.z()*vectorAfter.y() - vectorBefore.x()*vectorAfter.z(), \
        vectorBefore.x()*vectorAfter.y() - vectorBefore.y()*vectorAfter.x());
}
//計算旋轉矩陣
void rotationMatrix(const Eigen::Vector3d &vectorBefore, const Eigen::Vector3d &vectorAfter, Eigen::Matrix3d &rotMatrix)
{
    Eigen::Vector3d vector = calculateRotAxis(vectorBefore, vectorAfter);
    double angle = calculateAngle(vectorBefore, vectorAfter);
    Eigen::AngleAxisd rotationVector(angle, vector.normalized());
    Eigen::Matrix3d rotationMatrix = Eigen::Matrix3d::Identity();
    rotMatrix =  rotationVector.toRotationMatrix();//所求旋轉矩陣
}

int main()
{
    Eigen::Matrix3d rotMatrix;
    

    Eigen::Vector3d vectorBefore(0,0,1);
    Eigen::Vector3d vectorAfter(1,0,0);
    rotationMatrix(vectorBefore, vectorAfter, rotMatrix);
    std::cout << rotMatrix << std::endl;
    system("pause");
    return 0;
}

打印結果：

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;
}