旋转矩阵一、数学推导如何描述三维空间中刚体的旋转，是个有趣的问题。

具体地说，就是刚体上的任意一个点P(x, y, z)围绕过原点的轴(i, j, k)旋转θ，求旋转后的点)',','(z y x P 。

可以用下面的表达式表达：⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡z y x R z y x ''' 那么绕x 、y 、z 轴旋转θ角的矩阵为：那么绕x 轴旋转θ，绕y 轴旋转ω，绕z 轴旋转ϕ的矩阵可以表示为：)()()(ϕωθz y x R R R R ⋅⋅=二、代码实现1、产生单个轴的旋转矩阵 Matrix3RotateX(float angleX ){Matrix3 mMatrix3;/* 将角度转换为弧度 */angleX = angleX /(180/3.14159f);/* 绕x 轴的旋转矩阵 */mMatrix3.m[0][0] = 1.0f;mMatrix3.m[0][1] = 0.0f;mMatrix3.m[0][2] = 0.0f;mMatrix3.m[1][0] = 0.0f;mMatrix3.m[1][1] = cos(angleX);mMatrix3.m[1][2] =-sin(angleX);mMatrix3.m[2][0] = 0.0f;mMatrix3.m[2][1] = sin(angleX );mMatrix3.m[2][2] = cos(angleX );return mMatrix3;}同理，按照以上原理可以很容易写出RotateX、RotateY、RotateZ。

2、旋转矩阵相乘前面的步骤我们已经得到的三个旋转矩阵，为了得到旋转矩阵R，我们将Rx、Ry和Rz相乘，这里我按照矩阵相乘的法则写了两个3*3的矩阵相乘的函数。

/* 定义两个3*3的矩阵相乘 */Matrix3 Matrix3Multiplication(Matrix3mMatrix1,Matrix3mMatrix2){Matrix3 mResult;int i,j;/*按照矩阵相乘的法则进行计算*/for(i=0;i<3;i++){for(j=0;j<3;j++){mResult.m[i][j] = mMatrix1.m[i][0]*mMatrix2.m[0][j] +mMatrix1.m[i][1]*mMatrix2.m[1][j] +mMatrix1.m[i][2]*mMatrix2.m[2][j]; }}return mResult;}通过这个函数我们可以得到绕任意轴的旋转矩阵：/* 通过给定绕XYZ轴的量产生旋转矩阵 */Matrix3 Rotate(float angleX,float angleY,float angleZ){Matrix3 m;/*依次按照绕x轴 y轴 z轴进行旋转 *//* 相应矩阵变换为Rz*Ry*Rx */m = Matrix3Multiplication(Matrix3Multiplication(RotateZ(angleZ),RotateY(angleY)),RotateX(angleX));return m;}3、得到旋转后的坐标得到旋转矩阵后，P’就非常容易求解了，其本质就是一个3*3的矩阵和一个3*1的向量相乘的问题。

得到一下代码：/* 给定旋转矩阵以及向量，返回旋转后的向量*/Vector3 ComputeRotate(Matrix3mMatrix,Vector3vec){Vector3 mResult;int j;for(j=0;j<3;j++){mResult.v[j] = mMatrix.m[j][0]*vec.v[0] +mMatrix.m[j][1]*vec.v[1] +mMatrix.m[j][2]*vec.v[2];}return mResult;}4、代码使用示例Vector3 Loc,Loc1; // 创建变量Loc.v[0] = 0;Loc.v[1] = 5;Loc.v[2] = 0; // 给定初始坐标(向量)为（0,5,0）float aX = Roll; // 绕x轴旋转量float aY = Pitch; // 绕y轴旋转量float aZ = Yaw; // 绕z轴旋转量Loc1 = ComputeRotate(Rotate(aX,aY,aZ),Loc); // 计算旋转后的结果//输出结果printf("X = %fY=%f Z =%fY=%f P =%f R=%f\n",Loc1.v[0],Loc1.v[1],Loc1.v[2],Yaw,Pitch,Roll); 5、附录1、源代码/******************************************************************************************** 文件名： Rotate.cpp* 功能：给定绕rx,ry,rz 轴的量以及向量(x,y,z)，创建旋转矩阵并求出旋转后的向量（x1,y1,z1）* 时间： 2016/10/8* 作者：詹力* 范例：Vector3 Loc,Loc1; // 创建变量Loc.v[0] = 0;Loc.v[1] = 5;Loc.v[2] = 0; // 给定初始坐标(向量)为（0,5,0）float aX = Roll; // 绕x轴旋转量float aY = Pitch; // 绕y轴旋转量float aZ = Yaw; // 绕z轴旋转量Loc1 = ComputeRotate(Rotate(aX,aY,aZ),Loc); // 计算旋转后的结果//输出结果printf("X = %fY=%f Z =%fY=%f P =%f R=%f\n",Loc1.v[0],Loc1.v[1],Loc1.v[2],Yaw,Pitch,Roll); *******************************************************************************************/#include"stdafx.h"#include"math.h"#include"Rotate.h"/* 产生绕x轴的旋转矩阵 */Matrix3 RotateX(float angleX){Matrix3 mMatrix3;/* 将角度转换为弧度 */angleX = angleX/(180/3.14159f);/* 绕x轴的旋转矩阵 */mMatrix3.m[0][0] = 1.0f;mMatrix3.m[0][1] = 0.0f;mMatrix3.m[0][2] = 0.0f;mMatrix3.m[1][0] = 0.0f;mMatrix3.m[1][1] = cos(angleX);mMatrix3.m[1][2] =-sin(angleX);mMatrix3.m[2][0] = 0.0f;mMatrix3.m[2][1] = sin(angleX);mMatrix3.m[2][2] = cos(angleX);return mMatrix3;}/* 产生绕y轴的旋转矩阵 */Matrix3 RotateY(float angleY){Matrix3 mMatrix3;/* 将角度转换为弧度 */angleY = angleY/(180/3.14159f);/* 绕x轴的旋转矩阵 */mMatrix3.m[0][0] = cos(angleY);mMatrix3.m[0][1] = 0.0f;mMatrix3.m[0][2] = sin(angleY);mMatrix3.m[1][0] = 0.0f;mMatrix3.m[1][1] = 1.0f;mMatrix3.m[1][2] = 0.0f;mMatrix3.m[2][0] =-sin(angleY);mMatrix3.m[2][1] = 0.0f;mMatrix3.m[2][2] = cos(angleY);return mMatrix3;}/* 产生绕z轴的旋转矩阵 */Matrix3 RotateZ(float angleZ){Matrix3 mMatrix3;/* 将角度转换为弧度 */angleZ = angleZ/(180/3.14159f);/* 绕x轴的旋转矩阵 */mMatrix3.m[0][0] = cos(angleZ);mMatrix3.m[0][1] =-sin(angleZ);mMatrix3.m[0][2] = 0.0f;mMatrix3.m[1][0] = sin(angleZ);mMatrix3.m[1][1] = cos(angleZ);mMatrix3.m[1][2] = 0.0f;mMatrix3.m[2][0] = 0.0f;mMatrix3.m[2][1] = 0.0f;mMatrix3.m[2][2] = 1.0f;return mMatrix3;}/* 定义两个3*3的矩阵相乘 */Matrix3 Matrix3Multiplication(Matrix3mMatrix1,Matrix3mMatrix2){Matrix3 mResult;int i,j;/*按照矩阵相乘的法则进行计算*/for(i=0;i<3;i++){for(j=0;j<3;j++){mResult.m[i][j] = mMatrix1.m[i][0]*mMatrix2.m[0][j] +mMatrix1.m[i][1]*mMatrix2.m[1][j] +mMatrix1.m[i][2]*mMatrix2.m[2][j];}}return mResult;}/* 通过给定绕XYZ轴的量产生旋转矩阵 */Matrix3 Rotate(float angleX,float angleY,float angleZ){Matrix3 m;/*依次按照绕x轴 y轴 z轴进行旋转 *//* 相应矩阵变换为Rz*Ry*Rx */m = Matrix3Multiplication(Matrix3Multiplication(RotateZ(angleZ),RotateY(angleY)), RotateX(angleX));return m;}/* 给定旋转矩阵以及向量，返回旋转后的向量*/Vector3 ComputeRotate(Matrix3mMatrix,Vector3vec){Vector3 mResult;int j;for(j=0;j<3;j++){mResult.v[j] = mMatrix.m[j][0]*vec.v[0] +mMatrix.m[j][1]*vec.v[1] +mMatrix.m[j][2]*vec.v[2];}return mResult;}void DisplayMatrix(Matrix3mMatrix){printf("%f %f %f\n",mMatrix.m[0][0],mMatrix.m[0][1],mMatrix.m[0][2]);printf("%f %f %f\n",mMatrix.m[1][0],mMatrix.m[1][1],mMatrix.m[1][2]);printf("%f %f %f\n",mMatrix.m[2][0],mMatrix.m[2][1],mMatrix.m[2][2]); }2、头文件#ifndef _ROTATE_H_#define_ROTATE_H_typedefstruct_Matrix3{float m[3][3];}Matrix3;typedefstruct_Vector3{float v[3];}Vector3;/* 产生绕x轴的旋转矩阵 */Matrix3 RotateX(float angleX);/* 显示矩阵数据 */void DisplayMatrix(Matrix3 mMatrix);/* 产生绕x轴的旋转矩阵 */Matrix3 RotateX(float angleX);/* 产生绕y轴的旋转矩阵 */Matrix3 RotateY(float angleY);/* 产生绕z轴的旋转矩阵 */Matrix3 RotateZ(float angleZ);/* 定义两个3*3的矩阵相乘 */Matrix3 Matrix3Multiplication(Matrix3 mMatrix1,Matrix3 mMatrix2);/* 通过给定绕XYZ轴的量产生旋转矩阵 */Matrix3 Rotate(float angleX,float angleY,float angleZ);/* 给定旋转矩阵以及向量，返回旋转后的向量*/Vector3 ComputeRotate(Matrix3 mMatrix,Vector3 vec);#endif。