旋转矩阵和编程

旋转矩阵

一、数学推导

如何描述三维空间中刚体的旋转，是个有趣的问题。具体地说，就是刚体上的任意一个点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、产生单个轴的旋转矩阵 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;

}

同理，按照以上原理可以很容易写出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;