标签：
Matrix4x4
1 // 重置矩阵 2 void MatrixIdentity(float m[4][4]) 3 { 4 m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0; 5 m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = 0; 6 m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = 0; 7 m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1; 8 } 9 10 // 缩放变换 11 void MatrixScale(float m[4][4], const float s[3]) 12 { 13 m[0][0] = s[0]; 14 m[1][1] = s[1]; 15 m[2][2] = s[2]; 16 } 17 18 // 平移变换 19 void MatrixTranslation(float m[4][4], const float t[3]) 20 { 21 m[3][0] = t[0]; 22 m[3][1] = t[1]; 23 m[3][2] = t[2]; 24 } 25 26 // 旋转变换 27 void MatrixRotation(float m[4][4], float a, int x, int y, int z) 28 { 29 if (x) 30 { 31 m[1][1] = cos(a); 32 m[1][2] = sin(a); 33 m[2][1] = -sin(a); 34 m[2][2] = cos(a); 35 } 36 if (y) 37 { 38 m[0][0] = cos(a); 39 m[0][2] = -sin(a); 40 m[2][0] = sin(a); 41 m[2][2] = cos(a); 42 } 43 if (z) 44 { 45 m[0][0] = cos(a); 46 m[0][1] = sin(a); 47 m[1][0] = -sin(a); 48 m[1][1] = cos(a); 49 } 50 } 51 52 // 矩阵相乘 53 void MatrixConcat(const float in1[4][4], const float in2[4][4], float out[4][4]) 54 { 55 out[0][0] = in1[0][0]*in2[0][0] + in1[0][1]*in2[1][0] + in1[0][2]*in2[2][0] + in1[0][3]*in2[3][0]; 56 out[0][1] = in1[0][0]*in2[0][1] + in1[0][1]*in2[1][1] + in1[0][2]*in2[2][1] + in1[0][3]*in2[3][1]; 57 out[0][2] = in1[0][0]*in2[0][2] + in1[0][1]*in2[1][2] + in1[0][2]*in2[2][2] + in1[0][3]*in2[3][2]; 58 out[0][3] = in1[0][0]*in2[0][3] + in1[0][1]*in2[1][3] + in1[0][2]*in2[2][3] + in1[0][3]*in2[3][3]; 59 60 out[1][0] = in1[1][0]*in2[0][0] + in1[1][1]*in2[1][0] + in1[1][2]*in2[2][0] + in1[1][3]*in2[3][0]; 61 out[1][1] = in1[1][0]*in2[0][1] + in1[1][1]*in2[1][1] + in1[1][2]*in2[2][1] + in1[1][3]*in2[3][1]; 62 out[1][2] = in1[1][0]*in2[0][2] + in1[1][1]*in2[1][2] + in1[1][2]*in2[2][2] + in1[1][3]*in2[3][2]; 63 out[1][3] = in1[1][0]*in2[0][3] + in1[1][1]*in2[1][3] + in1[1][2]*in2[2][3] + in1[1][3]*in2[3][3]; 64 65 out[2][0] = in1[2][0]*in2[0][0] + in1[2][1]*in2[1][0] + in1[2][2]*in2[2][0] + in1[2][3]*in2[3][0]; 66 out[2][1] = in1[2][0]*in2[0][1] + in1[2][1]*in2[1][1] + in1[2][2]*in2[2][1] + in1[2][3]*in2[3][1]; 67 out[2][2] = in1[2][0]*in2[0][2] + in1[2][1]*in2[1][2] + in1[2][2]*in2[2][2] + in1[2][3]*in2[3][2]; 68 out[2][3] = in1[2][0]*in2[0][3] + in1[2][1]*in2[1][3] + in1[2][2]*in2[2][3] + in1[2][3]*in2[3][3]; 69 70 out[3][0] = in1[3][0]*in2[0][0] + in1[3][1]*in2[1][0] + in1[3][2]*in2[2][0] + in1[3][3]*in2[3][0]; 71 out[3][1] = in1[3][0]*in2[0][1] + in1[3][1]*in2[1][1] + in1[3][2]*in2[2][1] + in1[3][3]*in2[3][1]; 72 out[3][2] = in1[3][0]*in2[0][2] + in1[3][1]*in2[1][2] + in1[3][2]*in2[2][2] + in1[3][3]*in2[3][2]; 73 out[3][3] = in1[3][0]*in2[0][3] + in1[3][1]*in2[1][2] + in1[3][2]*in2[2][3] + in1[3][3]*in2[3][3]; 74 } 75 76 // 应用变换 77 void VectorTransform(const float in[3], const float m[4][4], float out[3]) 78 { 79 out[0] = in[0]*m[0][0] + in[1]*m[1][0] + in[2]*m[2][0] + 1*m[3][0]; 80 out[1] = in[0]*m[0][1] + in[1]*m[1][1] + in[2]*m[2][1] + 1*m[3][1]; 81 out[2] = in[0]*m[0][2] + in[1]*m[1][2] + in[2]*m[2][2] + 1*m[3][2]; 82 }
欧拉角转四元数、四元数转欧拉角
1 // 欧拉角转四元数 2 void AngleQuat(const float in[3], float out[4]) 3 { 4 out[3] = cos(in[0]/2.0)*cos(in[1]/2.0)*cos(in[2]/2.0) + sin(in[0]/2.0)*sin(in[1]/2.0)*sin(in[2]/2.0); 5 out[0] = sin(in[0]/2.0)*cos(in[1]/2.0)*cos(in[2]/2.0) - cos(in[0]/2.0)*sin(in[1]/2.0)*sin(in[2]/2.0); 6 out[1] = cos(in[0]/2.0)*sin(in[1]/2.0)*cos(in[2]/2.0) + sin(in[0]/2.0)*cos(in[1]/2.0)*sin(in[2]/2.0); 7 out[2] = cos(in[0]/2.0)*cos(in[1]/2.0)*sin(in[2]/2.0) - sin(in[0]/2.0)*sin(in[1]/2.0)*cos(in[2]/2.0); 8 } 9 10 // 四元数转欧拉角 11 void QuatAngle(const float in[4], float out[3]) 12 { 13 out[0] = atan2(2*(in[3]*in[0]+in[1]*in[2]), 1-2*(in[0]*in[0]+in[1]*in[1])); 14 out[1] = atan(2*(in[3]*in[1]-in[2]*in[0])); 15 out[2] = atan2(2*(in[3]*in[2]+in[0]*in[1]), 1-2*(in[1]*in[1]+in[2]*in[2])); 16 }
角度转弧度、弧度转角度
1 // 角度转弧度 2 void DegreeRadian(const float in[3], float out[3]) 3 { 4 out[0] = in[0] * (M_PI / 180.0); 5 out[1] = in[1] * (M_PI / 180.0); 6 out[2] = in[2] * (M_PI / 180.0); 7 } 8 9 // 弧度转角度 10 void RadianDegree(const float in[3], float out[3]) 11 { 12 out[0] = in[0] * (180.0 / M_PI); 13 out[1] = in[1] * (180.0 / M_PI); 14 out[2] = in[2] * (180.0 / M_PI); 15 }
四元数转矩阵、矩阵转四元数
1 // 四元数转矩阵 2 void QuatMatrix(const float in[4], float out[4][4]) 3 { 4 float xx, xy, xz, xw; 5 float yy, yz, yw; 6 float zz, zw; 7 8 xx = in[0]*in[0]; 9 xy = in[0]*in[1]; 10 xz = in[0]*in[2]; 11 xw = in[0]*in[3]; 12 yy = in[1]*in[1]; 13 yz = in[1]*in[2]; 14 yw = in[1]*in[3]; 15 zz = in[2]*in[2]; 16 zw = in[2]*in[3]; 17 18 out[0][0] = 1-2*(yy+zz); 19 out[0][1] = 2*(xy-zw); 20 out[0][2] = 2*(xz+yw); 21 out[0][3] = 0; 22 out[1][0] = 2*(xy+zw); 23 out[1][1] = 1-2*(xx+zz); 24 out[1][2] = 2*(yz-xw); 25 out[1][3] = 0; 26 out[2][0] = 2*(xz-yw); 27 out[2][1] = 2*(yz+xw); 28 out[2][2] = 1-2*(xx+yy); 29 out[2][3] = 0; 30 out[3][0] = 0; 31 out[3][1] = 0; 32 out[3][2] = 0; 33 out[3][3] = 1; 34 } 35 36 // 矩阵转四元数 37 void MatrixQuat(const float in[4][4], float out[4]) 38 { 39 float s; 40 41 if (in[0][0]>in[1][1] && in[0][0]>in[2][2]) 42 { 43 s = 2*sqrt(1.0+in[0][0]-in[1][1]-in[2][2]); 44 out[0] = 0.25*s; 45 out[1] = (in[1][0]+in[0][1])/s; 46 out[2] = (in[0][2]+in[2][0])/s; 47 out[3] = (in[2][1]-in[1][2])/s; 48 } 49 else if (in[1][1]>in[2][2]) 50 { 51 s = 2*sqrt(1.0+in[1][1]-in[0][0]-in[2][2]); 52 out[0] = (in[1][0]+in[0][1])/s; 53 out[1] = 0.25*s; 54 out[2] = (in[2][1]+in[1][2])/s; 55 out[3] = (in[0][2]-in[2][0])/s; 56 } 57 else 58 { 59 s = 2*sqrt(1.0+in[2][2]-in[0][0]-in[1][1]); 60 out[0] = (in[0][2]+in[2][0])/s; 61 out[1] = (in[2][1]+in[1][2])/s; 62 out[2] = 0.25*s; 63 out[3] = (in[1][0]-in[0][1])/s; 64 } 65 }
欧拉角转矩阵、矩阵转欧拉角
1 // 欧拉角转矩阵 2 void AngleMatrix(const float in[3], float out[4][4]) 3 { 4 float cx, sx, cy, sy, cz, sz; 5 float yx, yy; 6 7 cx = cos(in[0]); 8 sx = sin(in[0]); 9 cy = cos(in[1]); 10 sy = sin(in[1]); 11 cz = cos(in[2]); 12 sz = sin(in[2]); 13 14 yx = sy*cx; 15 yy = sy*sx; 16 17 out[0][0] = cy*cz; 18 out[0][1] = -cy*sz; 19 out[0][2] = sy; 20 out[0][3] = 0; 21 out[1][0] = yy*cz+cx*sz; 22 out[1][1] = -yy*sz+cx*cz; 23 out[1][2] = -sx; 24 out[1][3] = 0; 25 out[2][0] = -yx*cz+sx*sz; 26 out[2][1] = yx*sz+sx*cz; 27 out[2][2] = cx*cy; 28 out[2][3] = 0; 29 out[3][0] = 0; 30 out[3][1] = 0; 31 out[3][2] = 0; 32 out[3][3] = 1; 33 } 34 35 // 矩阵转欧拉角 36 void MatrixAngle(const float in[4][4], float out[3]) 37 { 38 float c; 39 float tx, ty; 40 41 out[1] = asin(in[0][2]); 42 c = cos(out[1]); 43 if (fabs(c)>0.005) 44 { 45 tx = in[2][2]/c; 46 ty = -in[1][2]/c; 47 out[0] = atan2(ty, tx); 48 tx = in[0][0]/c; 49 ty = -in[0][1]/c; 50 out[2] = atan2(ty, tx); 51 } 52 else 53 { 54 out[0] = 0; 55 tx = in[1][1]; 56 ty = in[1][0]; 57 out[2] = atan2(ty, tx); 58 } 59 }
标签：
原文地址：http://www.cnblogs.com/crsky/p/5485577.html
