从机器人到游戏引擎：用Eigen库搞定C++中的3D数学（附完整代码示例）

从机器人到游戏引擎用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.block3,3(0,0) Eigen::Quaterniond::FromTwoVectors( Eigen::Vector3d::UnitZ(), front).toRotationMatrix(); view.block3,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来加速运算。确保你的编译器启用了相应的优化标志如-marchnative。延迟求值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::vectorEigen::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旋转变得直观而高效。