3d数学基础-旋转矩阵-C++代码实现

#include <iostream.h>

#include <math.h>

#include <assert.h>

const float kPi = 3.1415926f;

const float k2Pi = kPi*2.0f;

const float kPiover2 = kPi/2.0f;

//数学工具，计算Sin, Cos. theta是角度

void sinCos(float* returnSin, float* returnCos, float theta)

{

*returnSin = sin(theta);

*returnCos = cos(theta);

}

//如果一个数很小，我们就把它置为0

float to_zero(float n)

{

return((abs(n)<0.00001)?0:n);

}

class Vector3

{

public:

double x, y, z; //在数学上的表示 [x,y,z] ， 这里的我们定义为成员变量

public:

Vector3(){}

Vector3(const Vector3& a) : x(a.x), y(a.y), z(a.z){}

Vector3(double a, double b, double c) : x(a), y(b), z(c){}

void zero() //设置为零向量

{

x = y = z = 0;

}

Vector3 operator-() const //负向量

{

return Vector3(-x, -y, -z);

}

Vector3 operator*(float a) const //标量必须在右边

{

return Vector3(x*a, y*a, z*a);

}

Vector3 operator*=(float a) //标量必须在右边

{

x*=a; y*=a; z*=a;

return *this;

}

Vector3 operator/(float a) const //标量必须在右边

{

float oneOverA = 1.0f/a; //计算机中的除法速度比较慢

return Vector3(x*oneOverA, y*oneOverA, z*oneOverA); //目的是速度快一点

}

Vector3 operator/=(float a) //标量必须在右边

{

float oneOverA = 1.0f/a;

x*=oneOverA; y*=oneOverA; z*=oneOverA;

return *this;

}

//向量的加法，两个向量相加

Vector3 operator+(const Vector3& a) const

{

return Vector3(x+a.x, y+a.y, z+a.z);

}

//向量的减法，两个向量相减

Vector3 operator-(const Vector3& a) const

{

return Vector3(x-a.x, y-a.y, z-a.z);

}

//向量的加法，另一种方式

Vector3 operator+=(const Vector3& a)

{

x+=a.x; y+=a.y; z+=a.z;

return *this;

}

//向量的减法，另一种方式

Vector3 operator-=(const Vector3& a)

{

x-=a.x; y-=a.y; z-=a.z;

return *this;

}

//向量的标准化操作，单位向量

void normalize()

{

float magSq = x*x + y*y + z*z;

if(magSq>0.0f)

{

float oneOverMag = 1.0f/sqrt(magSq);

x *= oneOverMag;

y *= oneOverMag;

z *= oneOverMag;

}

}

//点乘

float operator*(const Vector3& a) const

{

return x*a.x + y*a.y + z*a.z;

}

};

/*

如果将数据定义为私有，需要使用友元

public:

friend Matrix3x3& operator*(const Matrix3x3& a, const Matrix3x3& b);

friend Vector3& operator*(const Vector3&p, const Matrix3x3& m);

friend void print_m(Matrix3x3 m);

//叉乘

friend Vector3 crossProduct(const Vector3& a, const Vector3& b);

friend void print_v(Vector3 v);

friend double vectorMag(const Vector3& a);

friend Vector3 operator*(float k, const Vector3& v); //定义标量的左乘

friend float distance(const Vector3& a, const Vector3& b); //两点之间的距离

*/

double vectorMag(const Vector3& a) //计算模

{

return sqrt(a.x*a.x + a.y*a.y + a.z*a.z);

}

void print_v(Vector3 v)

{

//由于这是float型，有时候会出现很小的数，如果数太小，我们就用0来代替

cout<<"["<<to_zero(v.x)<<", "<<to_zero(v.y)<<", "<<to_zero(v.z)<<"]"<<endl;

}

Vector3 operator*(float k, const Vector3& v)

{

return Vector3(k*v.x, k*v.y, k*v.z);

}

float distance(const Vector3& a, const Vector3& b)

{

//float dx = a.x - b.x;

//float dy = a.y - b.y;

//float dz = a.z - b.z;

//return sqrt(dx*dx + dy*dy + dz*dz);

//以上代码可以用一句代替

return vectorMag(a-b);

}

Vector3 crossProduct(const Vector3& a, const Vector3& b)

{

return Vector3

(

a.y*b.z - a.z*b.y,

a.z*b.x - a.x*b.z,

a.x*b.y - a.y*b.x

);

}

//一个3*3的旋转矩阵，绕X轴旋转，绕Y轴旋转，绕Z轴旋转

class Matrix3x3

{

public:

float m11, m12, m13;

float m21, m22, m23;

float m31, m32, m33;

//axis是1时，绕X轴旋转。为2时，绕Y轴旋转。为3时，绕Z轴旋转

void setRotate(int axis, float theta)

{

float s, c;

sinCos(&s, &c, theta);

switch(axis)

{

case 1:

m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;

m21 = 0.0f; m22 = c; m23 = s;

m31 = 0.0f; m32 = -s; m33 = c;

break;

case 2:

m11 = c; m12 = 0.0f; m13 = -s;

m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;

m31 = s; m32 = 0.0f; m33 = c;

break;

case 3:

m11 = c; m12 = s; m13 = 0.0f;

m21 = -s; m22 = c; m23 = 0.0f;

m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;

break;

default:

assert(false);

}

}

};

//矩阵*矩阵

Matrix3x3& operator*(const Matrix3x3& a, const Matrix3x3& b)

{

Matrix3x3 r;

r.m11 = a.m11*b.m11 + a.m12*b.m21 + a.m13*b.m31;

r.m12 = a.m11*b.m12 + a.m12*b.m22 + a.m13*b.m32;

r.m13 = a.m11*b.m13 + a.m12*b.m23 + a.m13*b.m33;

r.m21 = a.m21*b.m11 + a.m22*b.m21 + a.m23*b.m31;

r.m22 = a.m21*b.m12 + a.m22*b.m22 + a.m23*b.m32;

r.m23 = a.m21*b.m13 + a.m22*b.m23 + a.m23*b.m33;

r.m31 = a.m31*b.m11 + a.m32*b.m21 + a.m33*b.m31;

r.m32 = a.m31*b.m12 + a.m32*b.m22 + a.m33*b.m32;

r.m33 = a.m31*b.m13 + a.m32*b.m23 + a.m33*b.m33;

return r;

}

//向量*矩阵

Vector3 operator*(const Vector3& p, const Matrix3x3& m)

{

return Vector3(

p.x*m.m11 + p.y*m.m21 + p.z*m.m31,

p.x*m.m12 + p.y*m.m22 + p.z*m.m32,

p.x*m.m13 + p.y*m.m23 + p.z*m.m33

);

}

//矩阵*矩阵的另一种方式

Matrix3x3& operator*=(Matrix3x3& a, const Matrix3x3& b)

{

a = a*b;

return a;

}

//向量*矩阵的另一种方式

Vector3 operator*=(Vector3& p, const Matrix3x3& m)

{

p = p*m;

return p;

}

//输出一个矩阵

void print_m(Matrix3x3 m)

{

cout<<to_zero(m.m11)<<"\t"<<to_zero(m.m12)<<"\t"<<to_zero(m.m13)<<endl;

cout<<to_zero(m.m21)<<"\t"<<to_zero(m.m22)<<"\t"<<to_zero(m.m23)<<endl;

cout<<to_zero(m.m31)<<"\t"<<to_zero(m.m32)<<"\t"<<to_zero(m.m33)<<endl;

}

int main()

{

cout<<"hello, 线性变换 - 旋转！"<<endl;

Vector3 a(10, 0, 0), b;

Matrix3x3 M; //M是一个旋转矩阵

M.setRotate(3, kPiover2); //绕Z轴，旋转90度

b = a*M; //作用旋转矩阵，将a向量变换为b向量

print_m(M); //输出这个转换矩阵

print_v(b);

M.setRotate(3, kPi);

b = a*M;

print_v(b);

return 0;

}