向量绕任意给定轴向旋转

 1 void rotate( double degrees, const vector3df& axe, vector3df& n)
 2 {
 3     vector3df dir(axe);
 4     dir.normalize();
 5     degrees *= (-1.0*DEGTORAD64);
 6     double dCos=cos( degrees );
 7     double dSin=sin( degrees );
 8 
 9     matrix4 m;
10 
11     m(0,0) = (float)(( dir.X * dir.X) * ( 1.0f - dCos ) + dCos);
12     m(0,1) = (float)(( dir.X * dir.Y ) * ( 1.0f - dCos ) - (dir.Z * dSin));
13     m(0,2) = (float)(( dir.X * dir.Z ) * ( 1.0f - dCos ) + (dir.Y * dSin));
14 
15     m(1,0) = (float)(( dir.Y * dir.X) * ( 1.0f - dCos ) + (dir.Z * dSin));
16     m(1,1) = (float)(( dir.Y * dir.Y ) * ( 1.0f - dCos ) + dCos);
17     m(1,2) = (float)(( dir.Y * dir.Z ) * ( 1.0f - dCos ) - (dir.X* dSin));
18 
19     m(2,0) = (float)(( dir.Z * dir.X) * ( 1.0f - dCos ) - (dir.Y * dSin));
20     m(2,1) = (float)(( dir.Z * dir.Y ) * ( 1.0f - dCos ) + (dir.X* dSin));
21     m(2,2) = (float)(( dir.Z * dir.Z ) * ( 1.0f - dCos ) + dCos);
22 
23     m(0,3) = m(1,3) = m(2,3) = 0.0f;
24     m(3,0) = m(3,1) = m(3,2) = 0.0f;
25     m(3,3) = 1.0f;
26 
27     vector3df nc(n);
28     m.rotateVect(n,nc);
29 
30 }