从机器人到游戏引擎：用Eigen库搞定C++中的3D数学（附完整代码示例）
在计算机图形学、机器人学和游戏开发中，3D数学是不可或缺的基础。无论是计算机器人末端执行器的位姿，还是实现3D相机的变换，亦或是进行刚体运动的模拟，都离不开矩阵运算和几何变换。而Eigen库作为C++中高效线性代数运算的利器，为这些领域提供了强大的支持。
1. Eigen库简介与安装
Eigen是一个开源的C++模板库，专注于线性代数、矩阵和向量运算。它完全由头文件组成，无需编译即可使用，这使得它在项目中集成变得异常简单。Eigen支持固定大小和动态大小的矩阵，并提供了丰富的线性代数运算功能。
在Linux系统下安装Eigen非常简单：
sudo apt-get install libeigen3-dev对于CMake项目，只需在CMakeLists.txt中添加：
find_package(Eigen3 REQUIRED) include_directories(${EIGEN3_INCLUDE_DIR})Eigen的一个显著特点是它的表达式模板技术，这使得它能够在不牺牲性能的情况下提供直观的数学表达式语法。例如，你可以直接写出像MatrixXd C = A * B + D;这样的代码，而Eigen会在编译时优化这些操作。
2. Eigen中的矩阵与向量操作
Eigen提供了多种矩阵和向量类型，从固定大小的小矩阵到动态大小的矩阵都有支持。以下是一些基本操作示例：
#include <Eigen/Dense> #include <iostream> int main() { // 动态大小矩阵 Eigen::MatrixXd m(2,2); m << 1, 2, 3, 4; // 向量 Eigen::Vector3d v(1, 2, 3); // 矩阵乘法 Eigen::MatrixXd result = m * m; std::cout << "Matrix m:\n" << m << std::endl; std::cout << "Vector v:\n" << v << std::endl; std::cout << "m * m:\n" << result << std::endl; return 0; }Eigen还支持各种初始化方式：
// 零矩阵 Eigen::Matrix3d zero_matrix = Eigen::Matrix3d::Zero(); // 单位矩阵 Eigen::Matrix4d identity_matrix = Eigen::Matrix4d::Identity(); // 随机矩阵 Eigen::MatrixXd random_matrix = Eigen::MatrixXd::Random(3,3);3. 3D几何变换实战
Eigen的几何模块提供了处理3D空间变换的强大工具。让我们看一个完整的例子，展示如何用Eigen实现3D物体的旋转和平移。
3.1 旋转表示与转换
在3D空间中，旋转有多种表示方式，Eigen支持所有这些表示：
// 旋转向量（轴角表示） Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1)); // 转换为旋转矩阵 Eigen::Matrix3d rotation_matrix = rotation_vector.toRotationMatrix(); // 转换为四元数 Eigen::Quaterniond quaternion(rotation_vector); std::cout << "Rotation matrix:\n" << rotation_matrix << std::endl; std::cout << "Quaternion:\n" << quaternion.coeffs().transpose() << std::endl;3.2 组合变换
在实际应用中，我们经常需要组合多个变换。Eigen的Transform类让这变得简单：
// 创建一个等距变换（旋转+平移） Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); // 设置旋转部分 transform.rotate(rotation_vector); // 设置平移部分 transform.translate(Eigen::Vector3d(1,0,0)); // 应用变换到一个点 Eigen::Vector3d point(0,1,0); Eigen::Vector3d transformed_point = transform * point; std::cout << "Original point: " << point.transpose() << std::endl; std::cout << "Transformed point: " << transformed_point.transpose() << std::endl;4. 机器人学中的应用案例
让我们看一个实际的机器人学应用：计算机器人末端执行器的位姿。假设我们有一个简单的机械臂，由两个连杆组成：
// 定义连杆长度 const double l1 = 0.5; // 第一段连杆长度 const double l2 = 0.3; // 第二段连杆长度 // 关节角度 double theta1 = M_PI/4; // 第一个关节角度 double theta2 = M_PI/6; // 第二个关节角度 // 创建变换矩阵 Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity(); T1.rotate(Eigen::AngleAxisd(theta1, Eigen::Vector3d::UnitZ())); T1.pretranslate(Eigen::Vector3d(l1,0,0)); Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity(); T2.rotate(Eigen::AngleAxisd(theta2, Eigen::Vector3d::UnitZ())); T2.pretranslate(Eigen::Vector3d(l2,0,0)); // 计算末端执行器位姿 Eigen::Isometry3d end_effector_pose = T1 * T2; std::cout << "End effector position: " << end_effector_pose.translation().transpose() << std::endl; std::cout << "End effector orientation:\n" << end_effector_pose.rotation() << std::endl;5. 游戏开发中的3D相机实现
在游戏开发中，3D相机是一个核心组件。让我们看看如何用Eigen实现一个简单的第一人称相机：
class FirstPersonCamera { public: FirstPersonCamera() { position = Eigen::Vector3d(0,0,0); yaw = 0; pitch = 0; updateVectors(); } void move(const Eigen::Vector3d& movement) { position += movement; } void rotate(double deltaYaw, double deltaPitch) { yaw += deltaYaw; pitch += deltaPitch; updateVectors(); } Eigen::Matrix4d getViewMatrix() const { Eigen::Matrix4d view = Eigen::Matrix4d::Identity(); Eigen::Vector3d center = position + front; Eigen::Vector3d up = this->up; view.block<3,3>(0,0) = Eigen::Quaterniond::FromTwoVectors( Eigen::Vector3d::UnitZ(), front).toRotationMatrix(); view.block<3,1>(0,3) = -position; return view; } private: void updateVectors() { front = Eigen::Vector3d( cos(yaw) * cos(pitch), sin(yaw) * cos(pitch), sin(pitch) ).normalized(); Eigen::Vector3d right = Eigen::Vector3d::UnitZ().cross(front).normalized(); up = front.cross(right).normalized(); } Eigen::Vector3d position; double yaw, pitch; Eigen::Vector3d front, up; };6. 性能优化技巧
虽然Eigen已经做了很多优化，但在性能关键的应用中，我们还可以采取一些额外措施：
- 使用固定大小矩阵：对于小矩阵（通常小于16x16），使用固定大小矩阵可以避免动态内存分配并启用循环展开优化。
// 固定大小矩阵 Eigen::Matrix3d fixed_matrix; // 等同于 float fixed_matrix[9];- 避免临时对象：使用Eigen的noalias()来避免不必要的临时对象创建。
// 不好的写法：会创建临时对象 matrix = matrix * other_matrix; // 好的写法：使用noalias() matrix.noalias() = matrix * other_matrix;利用SIMD指令：Eigen会自动使用SIMD指令（如SSE、AVX）来加速运算。确保你的编译器启用了相应的优化标志（如-march=native）。
延迟求值：Eigen的表达式模板技术会自动合并多个操作，减少中间结果的计算。
// 这些操作会被合并，不会产生临时矩阵 result = 2 * (matrix1 + matrix2).transpose();7. 完整示例：刚体运动模拟
让我们用一个完整的例子来展示Eigen在刚体运动模拟中的应用：
#include <Eigen/Dense> #include <Eigen/Geometry> #include <iostream> #include <vector> class RigidBody { public: RigidBody(double mass, const Eigen::Matrix3d& inertia) : mass(mass), inertia(inertia), position(Eigen::Vector3d::Zero()), velocity(Eigen::Vector3d::Zero()), orientation(Eigen::Quaterniond::Identity()), angular_velocity(Eigen::Vector3d::Zero()) {} void applyForce(const Eigen::Vector3d& force, const Eigen::Vector3d& point) { // 计算力和扭矩 total_force += force; total_torque += (point - position).cross(force); } void update(double dt) { // 更新线性运动 velocity += total_force / mass * dt; position += velocity * dt; // 更新旋转运动 Eigen::Vector3d angular_acceleration = inertia.inverse() * total_torque; angular_velocity += angular_acceleration * dt; // 更新方向 double angle = angular_velocity.norm() * dt; if (angle > 1e-6) { Eigen::AngleAxisd rotation(angle, angular_velocity.normalized()); orientation = rotation * orientation; } // 重置力和扭矩 total_force = Eigen::Vector3d::Zero(); total_torque = Eigen::Vector3d::Zero(); } Eigen::Vector3d getPosition() const { return position; } Eigen::Quaterniond getOrientation() const { return orientation; } private: double mass; Eigen::Matrix3d inertia; Eigen::Vector3d position; Eigen::Vector3d velocity; Eigen::Quaterniond orientation; Eigen::Vector3d angular_velocity; Eigen::Vector3d total_force; Eigen::Vector3d total_torque; }; int main() { // 创建一个刚体，质量为1，惯性矩阵为单位矩阵 Eigen::Matrix3d inertia = Eigen::Matrix3d::Identity(); RigidBody body(1.0, inertia); // 模拟参数 double dt = 0.01; int steps = 1000; // 记录轨迹 std::vector<Eigen::Vector3d> trajectory; // 模拟循环 for (int i = 0; i < steps; ++i) { // 应用重力 body.applyForce(Eigen::Vector3d(0, 0, -9.8), body.getPosition()); // 应用一些随机扭矩 if (i % 100 == 0) { body.applyForce(Eigen::Vector3d(1, 0, 0), body.getPosition() + Eigen::Vector3d(0, 0.1, 0)); } // 更新刚体状态 body.update(dt); // 记录位置 trajectory.push_back(body.getPosition()); } // 输出最后的位置和方向 std::cout << "Final position: " << body.getPosition().transpose() << std::endl; std::cout << "Final orientation:\n" << body.getOrientation().matrix() << std::endl; return 0; }这个例子展示了如何使用Eigen来实现一个简单的刚体物理模拟，包括力和扭矩的应用、位置和方向的更新等。Eigen的几何模块使得处理3D旋转变得直观而高效。
