C++ 旋转矩阵 转换 四元数 详细代码

在C++中，矩阵旋转通常涉及到二维数组的操作，而四元数是一种用于表示三维空间旋转的数学工具，它们可以方便地处理旋转变换。下面分别给出如何使用矩阵和四元数来进行旋转的简单示例。

**1. 矩阵旋转（二维向量顺时针旋转90度）**
假设我们有一个二维矩阵，代表一个点的坐标，我们可以创建一个函数来实现矩阵的顺时针旋转：

#include <vector>

// 逆时针旋转矩阵90度
void rotateMatrix(std::vector<std::vector<int>>& matrix) {
    int n = matrix.size();
    for (int layer = 0; layer < n / 2; ++layer) {
        for (int i = layer; i < n - layer - 1; ++i) {
            // 交换行和列
            std::swap(matrix[i][layer], matrix[layer][n - i - 1]);
            // 对角线元素交换
            std::swap(matrix[i][n - 1 - i], matrix[n - 1 - i][i]);
        }
    }
}

// 示例矩阵
std::vector<std::vector<int>> matrix = {{1, 2}, {3, 4}};
rotateMatrix(matrix);

**2. 四元数旋转**
四元数旋转需要`Quaternion`类来操作。首先，你需要包含`quaternion.h`文件，然后定义一个旋转并应用到矩阵上的函数：

#include <quaternion/quaternion.h>
#include <Eigen/Dense>

// 使用四元数旋转矩阵
void rotateWithQuaternion(Eigen::MatrixXd& matrix, Quaternion q) {
    Eigen::Matrix4f rotationMatrix;
    q.toRotationMatrix(rotationMatrix);
    matrix = rotationMatrix * matrix * rotationMatrix.transpose(); // 两次乘法，一次是对称操作
}

// 示例四元数
Quaternion q(1, 0, 0, 0); // 这是一个纯旋转（绕原点），无平移

// 示例矩阵
Eigen::MatrixXd matrix(3, 3); // 任意3x3矩阵
rotateWithQuaternion(matrix, q);

注意，这里的示例仅展示了基本的旋转概念，实际使用时可能需要根据具体需求调整代码细节，并处理边界情况。