matrix22实现如下:
1
2 ////////////////////////////////////////////////////////////
3 ///定义2*2矩阵模板类
4 ////////////////////////////////////////////////////////////
5 template<class T>
6 struct Matrix2
7 {
8 ////////////////////////////////////////////////////////
9 /// 矩阵默认构造函数
10 ////////////////////////////////////////////////////////
11 inline Matrix2()
12 {
13 memset(element,0,sizeof(T)*4);
14 }
15
16 ////////////////////////////////////////////////////////
17 /// 矩阵带参构造函数
18 ////////////////////////////////////////////////////////
19 inline Matrix2(T value)
20 {
21 Set(value,value,value,value);
22 }
23
24 ////////////////////////////////////////////////////////
25 /// 由数据元素构造矩阵
26 ////////////////////////////////////////////////////////
27 inline Matrix2(T a,T b,T c, T d)
28 {
29 Set(a,b,c,d);
30 }
31
32 ////////////////////////////////////////////////////////
33 /// 矩阵复制构造函数
34 ////////////////////////////////////////////////////////
35 inline Matrix2(const Matrix2<T>& mat)
36 {
37 *this = mat;
38 }
39
40 ////////////////////////////////////////////////////////
41 /// 矩阵赋值函数
42 ////////////////////////////////////////////////////////
43 inline Matrix2<T>& operator = (const Matrix2<T>& mat)
44 {
45 Set(mat.At(0,0),mat.At(0,1),mat.At(1,0),mat.At(1,1));
46 return *this;
47 }
48
49 ////////////////////////////////////////////////////////
50 /// 矩阵析构函数
51 ////////////////////////////////////////////////////////
52 inline ~Matrix2(){}
53
54 ////////////////////////////////////////////////////////
55 /// 获取矩阵指针
56 ////////////////////////////////////////////////////////
57 inline operator T *()
58 {
59 return element;
60 }
61
62 inline operator T *()const
63 {
64 return element;
65 }
66
67 ////////////////////////////////////////////////////////
68 /// 设置矩阵元素
69 ////////////////////////////////////////////////////////
70 inline void Set(T a, T b, T c, T d)
71 {
72 element[0][0] = a;
73 element[0][1] = b;
74 element[1][0] = c;
75 element[1][1] = d;
76 }
77
78
79
80 ////////////////////////////////////////////////////////
81 /// 矩阵运算符号重载
82 ////////////////////////////////////////////////////////
83 inline Matrix2<T> operator + (const Matrix2<T>& other)
84 {
85 return Matrix2<T>(At(0,0)+other.At(0,0),
86 At(0,1)+other.At(0,1),
87 At(1,0)+other.At(1,0),
88 At(1,1)+other.At(1,1));
89 }
90
91 ADDABLE(Matrix2<T>, Matrix2<T>)
92
93 inline Matrix2<T> operator - (const Matrix2<T>& other)
94 {
95 return Matrix2<T>(At(0,0)-other.At(0,0),
96 At(0,1)-other.At(0,1),
97 At(1,0)-other.At(1,0),
98 At(1,1)-other.At(1,1));
99 }
100
101 SUBTRACTABLE(Matrix2<T>, Matrix2<T>)
102
103 inline Matrix2<T> operator + (const T& value)
104 {
105 return Matrix2<T>(At(0,0)+value,
106 At(0,1)+value,
107 At(1,0)+value,
108 At(1,1)+value);
109 }
110
111 ADDABLE(Matrix2<T>, T)
112
113 inline Matrix2<T> operator - (const T& value)
114 {
115 return Matrix2<T>(At(0,0)-value,
116 At(0,1)-value,
117 At(1,0)-value,
118 At(1,1)-value);
119 }
120
121 SUBTRACTABLE(Matrix2<T>, T)
122
123 inline Matrix2<T> operator * (const T& value)
124 {
125 return Matrix2<T>(At(0,0)*value,At(0,1)*value,At(1,0)*value,At(1,1)*value);
126 }
127
128 MULTIPLIABLE(Matrix2<T>,T);
129
130 inline Matrix2<T> operator/(const T& value)
131 {
132 ASSERT(value !=0);
133 return Matrix2<T>(At(0,0)/value,At(0,1)/value,At(1,0)/value,At(1,1)/value);
134 }
135
136 DIVISIBLE(Matrix2<T>,T);
137
138 inline Matrix2<T> operator*(const Matrix2<T>& other)
139 {
140 T a1 = At(0,0);
141 T a2 = At(0,1);
142 T a3 = At(1,0);
143 T a4 = At(1,0);
144 T b1 = other.At(0,0);
145 T b2 = other.At(0,1);
146 T b3 = other.At(1,0);
147 T b4 = other.At(1,1);
148 return Matrix2<T>(a1*b1+a2*b3,a1*b2+a2*b4,a3*b1+a4*b3,a3*b2+a4*b4);
149 }
150
151 MULTIPLIABLE(Matrix2<T>,Matrix2<T>);
152
153 ////////////////////////////////////////////////////////
154 /// 矩阵和向量乘积
155 ////////////////////////////////////////////////////////
156 inline Vector2<T> operator*(const Vector2<T>& v)
157 {
158 return Vector2<T>(At(0,0)*v.x+At(0,0)*v.y,At(1,0)*v.x+At(1,1)*v.y);
159 }
160
161 ////////////////////////////////////////////////////////
162 /// 矩阵转置
163 ////////////////////////////////////////////////////////
164 inline Matrix2<T> Transpose()
165 {
166 return Matrix2<T>(At(0,0),At(1,0),At(0,1),At(1,1));
167 }
168
169 ////////////////////////////////////////////////////////
170 /// 行列式
171 ////////////////////////////////////////////////////////
172 inline T Determinant()
173 {
174 return element[0][0]*element[1][1]-element[0][1]*element[1][0];
175 }
176
177 ////////////////////////////////////////////////////////
178 /// 矩阵求逆
179 ////////////////////////////////////////////////////////
180 inline bool Inverse(Matrix2<T> &mat, float toler = 0.0001)
181 {
182 T det = Determinant();
183 if(fabs(det) < toler)
184 return false;
185
186 mat.Set(At(1,1)/det,-At(0,1)/det,-At(1,0)/det,At(0,0)/det);
187 return true;
188 }
189
190 ////////////////////////////////////////////////////////
191 /// 设置指定位置元素值
192 ////////////////////////////////////////////////////////
193 inline void Set(int i, int j, T value)
194 {
195 ASSERT(i > -1 && i < 2);
196 ASSERT(j > -1 && j < 2);
197 element[i][j] = value;
198 }
199
200 ////////////////////////////////////////////////////////
201 /// 获取指定位置元素值
202 ////////////////////////////////////////////////////////
203 inline T At(int i, int j)
204 {
205 ASSERT(i > -1 && i < 2);
206 ASSERT(j > -1 && j < 2);
207 return element[i][j];
208 }
209
210 ////////////////////////////////////////////////////////
211 /// 获取与设置指定行元素
212 ////////////////////////////////////////////////////////
213 inline Vector2<T> GetRow(int row)
214 {
215 ASSERT(row > -1 && row < 2);
216 return Vector2<T>(At(row,0),At(row,1));
217 }
218
219 inline void SetRow(int row,const Vector2<T>& m)
220 {
221 ASSERT(row > -1 && row < 2);
222 Set(row,0,m.x);
223 Set(row,1,m.y);
224 }
225
226 ////////////////////////////////////////////////////////
227 /// 获取与设置指定列元素
228 ////////////////////////////////////////////////////////
229 inline Vector2<T> GetCol(int col)
230 {
231 ASSERT(col > -1 && col < 2);
232 return Vector2<T>(At(0,col),At(1,col));
233 }
234
235 inline void SetCol(int col,const Vector2<T>& m)
236 {
237 ASSERT(col > -1 && col < 2);
238 Set(0,col,m.x);
239 Set(1,col,m.y);
240 }
241
242 ////////////////////////////////////////////////////////
243 /// 0矩阵
244 ////////////////////////////////////////////////////////
245 static Matrix2<T> Zero()
246 {
247 static Matrix2<T> mat(0,0,0,0);
248 return mat;
249 }
250
251 ////////////////////////////////////////////////////////
252 /// 单位矩阵
253 ////////////////////////////////////////////////////////
254 static Matrix2<T> Identity()
255 {
256 static Matrix2<T> mat(1,0,0,1);
257 return mat;
258 }
259
260 ////////////////////////////////////////////////////////
261 /// 矩阵数组
262 ////////////////////////////////////////////////////////
263 T element[2][2];
264 };
265
3*3矩阵实现如下：
1 ////////////////////////////////////////////////////////////
2 ///定义3*3矩阵模板类
3 ////////////////////////////////////////////////////////////
4 template<class T>
5 struct Matrix3
6 {
7 ////////////////////////////////////////////////////////
8 /// 矩阵默认构造函数
9 ////////////////////////////////////////////////////////
10 inline Matrix3()
11 {
12 memset(element,0,sizeof(T)*9);
13 }
14
15 ////////////////////////////////////////////////////////
16 /// 矩阵带参构造函数
17 ////////////////////////////////////////////////////////
18 inline Matrix3(T value)
19 {
20 Set(value,value,value,
21 value,value,value,
22 value,value,value);
23 }
24
25 ////////////////////////////////////////////////////////
26 /// 由数据元素构造矩阵
27 ////////////////////////////////////////////////////////
28 inline Matrix3(T a1,T a2,T a3,
29 T a4,T a5,T a6,
30 T a7,T a8,T a9)
31 {
32 Set(a1,a2,a3,a4,a5,a6,a7,a8,a9);
33 }
34
35 COPY_CLASS(Matrix3<T>)
36
37 ////////////////////////////////////////////////////////
38 /// 矩阵析构函数
39 ////////////////////////////////////////////////////////
40 inline ~Matrix3(){}
41
42 ////////////////////////////////////////////////////////
43 /// 获取矩阵指针
44 ////////////////////////////////////////////////////////
45 inline operator T *()
46 {
47 return element;
48 }
49
50 inline operator T *()const
51 {
52 return element;
53 }
54
55 ////////////////////////////////////////////////////////
56 /// 设置矩阵元素
57 ////////////////////////////////////////////////////////
58 inline void Set(T a1,T a2,T a3,
59 T a4,T a5,T a6,
60 T a7,T a8,T a9)
61 {
62 element[0][0] = a1;
63 element[0][1] = a2;
64 element[0][2] = a3;
65 element[1][0] = a4;
66 element[1][1] = a5;
67 element[1][2] = a6;
68 element[2][0] = a7;
69 element[2][1] = a8;
70 element[2][2] = a9;
71 }
72
73 ////////////////////////////////////////////////////////
74 /// 矩阵运算符号重载
75 ////////////////////////////////////////////////////////
76 inline Vector3<T> operator*(const Vector3<T>& v)
77 {
78 T a1 = At(0,0);
79 T a2 = At(0,1);
80 T a3 = At(0,2);
81 T a4 = At(1,0);
82 T a5 = At(1,1);
83 T a6 = At(1,2);
84 T a7 = At(2,0);
85 T a8 = At(2,1);
86 T a9 = At(2,2);
87 T b1 = v.x;
88 T b2 = v.y;
89 T b3 = v.z;
90 return Vector3<T>(a1*b1+a2*b2+a3*b3,b1*a4+a5*b2+a6*b3,a7*b1+a8*b2+a9*b3);
91 }
92
93 inline Matrix3<T> operator + (const Matrix3<T>& other)
94 {
95 return Matrix3<T>(At(0,0)+other.At(0,0),At(0,1)+other.At(0,1),At(0,2)+other.At(0,2),
96 At(1,0)+other.At(1,0),At(1,1)+other.At(1,1),At(1,2)+other.At(1,2),
97 At(2,0)+other.At(2,0),At(2,1)+other.At(2,1),At(2,2)+other.At(2,2));
98 }
99
100 ADDABLE(Matrix3<T>, Matrix3<T>)
101
102 inline Matrix3<T> operator - (const Matrix3<T>& other)
103 {
104 return Matrix3<T>(At(0,0)-other.At(0,0),At(0,1)-other.At(0,1),At(0,2)-other.At(0,2),
105 At(1,0)-other.At(1,0),At(1,1)-other.At(1,1),At(1,2)-other.At(1,2),
106 At(2,0)-other.At(2,0),At(2,1)-other.At(2,1),At(2,2)-other.At(2,2));
107 }
108
109 SUBTRACTABLE(Matrix3<T>, Matrix3<T>)
110
111 inline Matrix3<T> operator + (const T& value)
112 {
113 return Matrix3<T>(At(0,0)+value,At(0,1)+value,At(0,2)+value,
114 At(1,0)+value,At(1,1)+value,At(1,2)+value,
115 At(2,0)+value,At(2,1)+value,At(2,2)+value);
116 }
117
118 ADDABLE(Matrix3<T>, T)
119
120 inline Matrix3<T> operator - (const T& value)
121 {
122 return Matrix3<T>(At(0,0)-value,At(0,1)-value,At(0,2)-value,
123 At(1,0)-value,At(1,1)-value,At(1,2)-value,
124 At(2,0)-value,At(2,1)-value,At(2,2)-value);
125 }
126
127 SUBTRACTABLE(Matrix3<T>, T)
128
129 inline Matrix3<T> operator * (const T& value)
130 {
131 return Matrix3<T>(At(0,0)*value,At(0,1)*value,At(0,2)*value,
132 At(1,0)*value,At(1,1)*value,At(1,2)*value,
133 At(2,0)*value,At(2,1)*value,At(2,2)*value);
134 }
135
136 MULTIPLIABLE(Matrix3<T>,T);
137
138 inline Matrix3<T> operator/(const T& value)
139 {
140 ASSERT(value !=0);
141 return Matrix3<T>(At(0,0)/value,At(0,1)/value,At(0,2)/value,
142 At(1,0)/value,At(1,1)/value,At(1,2)/value,
143 At(2,0)/value,At(2,1)/value,At(2,2)/value);
144 }
145
146 DIVISIBLE(Matrix3<T>,T);
147
148 inline Matrix3<T> operator*(const Matrix3<T>& other)
149 {
150 T a1 = At(0,0);
151 T a2 = At(0,1);
152 T a3 = At(0,2);
153 T a4 = At(1,0);
154 T a5 = At(1,1);
155 T a6 = At(1,2);
156 T a7 = At(2,0);
157 T a8 = At(2,1);
158 T a9 = At(2,2);
159
160 T b1 = other.At(0,0);
161 T b2 = other.At(0,1);
162 T b3 = other.At(1,2);
163 T b4 = other.At(1,0);
164 T b5 = other.At(1,1);
165 T b6 = other.At(1,2);
166 T b7 = other.At(2,0);
167 T b8 = other.At(2,1);
168 T b9 = other.At(2,2);
169 return Matrix3<T>(a1*b1+a2*b4+a3*b7,a1*b2+a2*b5+a3*b8,a1*b3+a2*b6+a3*b9,
170 b1*a4+a5*b4+a6*b7,a4*b2+a5*b5+a6*b8,a4*b3+a5*b6+a6*b9,
171 a7*b1+a8*b4+a9*b7,a7*b2+a8*b5+a9*b8,a7*b3+a8*b6+a9*b9);
172 }
173
174 MULTIPLIABLE(Matrix3<T>,Matrix3<T>);
175
176 inline bool operator==(const Matrix3<T>& other)
177 {
178 for(int i = 0; i < 3; i ++)
179 for(int j = 0; j < 3; j++)
180 {
181 if(At(i,j) != other.At(i,j))
182 return false;
183 }
184 return true;
185 }
186
187 NOT_EQUAL(Matrix3<T>)
188
189 ////////////////////////////////////////////////////////
190 /// 矩阵转置
191 ////////////////////////////////////////////////////////
192 inline Matrix3<T> Transpose()
193 {
194 return Matrix3<T>(At(0,0),At(1,0),At(2,0),
195 At(0,1),At(1,1),At(1,0),
196 At(0,2),At(1,2),At(2,2));
197 }
198
199 ////////////////////////////////////////////////////////
200 /// 行列式
201 ////////////////////////////////////////////////////////
202 inline T Determinant()
203 {
204 T a1 = At(0,0);
205 T a2 = At(0,1);
206 T a3 = At(0,2);
207 T a4 = At(1,0);
208 T a5 = At(1,1);
209 T a6 = At(1,2);
210 T a7 = At(2,0);
211 T a8 = At(2,1);
212 T a9 = At(2,2);
213 return a1*a5*a9-a1*a6*a8-a4*a2*a9+a4*a3*a8+a7*a2*a6-a7*a3*a5;
214 }
215
216 ////////////////////////////////////////////////////////
217 /// 矩阵求逆
218 ////////////////////////////////////////////////////////
219 inline bool Inverse(Matrix3<T> &mat, float toler = 0.0001)
220 {
221 T det = Determinant();
222 if(fabs(det) < toler)
223 return false;
224 T b1 = At(0,0);
225 T b2 = At(0,1);
226 T b3 = At(0,2);
227 T b4 = At(1,0);
228 T b5 = At(1,1);
229 T b6 = At(1,2);
230 T b7 = At(2,0);
231 T b8 = At(2,1);
232 T b9 = At(2,2);
233 T a1,a2,a3,a4,a5,a6,a7,a8,a9;
234 a1 = (b5*b9-b6*b8)/det;
235 a2 = -(b9*b2-b3*b8)/det;
236 a3 = (b2*b6-b5*b3)/det;
237 a4 = -(b9*b4-b6*b7)/det;
238 a5 = (b1*b9-b3*b7)/det;
239 a6 = -(b1*b6-b3*b4)/det;
240 a7 = -(-b4*b8+b5*b7)/det;
241 a8 = -(b1*b8-b2*b7)/det;
242 a9 = (b1*b5-b2*b4)/det;
243 mat.Set(a1,a2,a3,a4,a5,a6,a7,a8,a9);
244 return true;
245 }
246
247 ////////////////////////////////////////////////////////
248 /// 设置指定位置元素值
249 ////////////////////////////////////////////////////////
250 inline void Set(int i, int j, T value)
251 {
252 ASSERT(i > -1 && i < 3);
253 ASSERT(j > -1 && j < 3);
254 element[i][j] = value;
255 }
256
257 ////////////////////////////////////////////////////////
258 /// 获取指定位置元素值
259 ////////////////////////////////////////////////////////
260 inline T At(int i, int j)
261 {
262 ASSERT(i > -1 && i < 3);
263 ASSERT(j > -1 && j < 3);
264 return element[i][j];
265 }
266
267 ////////////////////////////////////////////////////////
268 /// 获取与设置指定行元素
269 ////////////////////////////////////////////////////////
270 inline Vector3<T> GetRow(int row)
271 {
272 ASSERT(row > -1 && row < 3);
273 return Vector3<T>(At(row,0),At(row,1),At(row,2));
274 }
275
276 inline void SetRow(int row,const Vector3<T>& m)
277 {
278 ASSERT(row > -1 && row < 3);
279 Set(row,0,m.x);
280 Set(row,1,m.y);
281 Set(row,2,m.z);
282 }
283
284 ////////////////////////////////////////////////////////
285 /// 获取与设置指定列元素
286 ////////////////////////////////////////////////////////
287 inline Vector3<T> GetCol(int col)
288 {
289 ASSERT(col > -1 && col < 3);
290 return Vector3<T>(At(0,col),At(1,col),At(2,col));
291 }
292
293 inline void SetCol(int col,const Vector3<T>& m)
294 {
295 ASSERT(col > -1 && col < 3);
296 Set(0,col,m.x);
297 Set(1,col,m.y);
298 Set(2,col,m.z);
299 }
300
301 ////////////////////////////////////////////////////////
302 /// 从欧拉角到矩阵(单位弧度)
303 ////////////////////////////////////////////////////////
304 inline void SetRotation(T yaw, T pitch, T roll)
305 {
306 T sr = sinf(roll);
307 T sp = sinf(pitch);
308 T sy = sinf(yaw);
309 T cr = cosf(roll);
310 T cp = cosf(pitch);
311 T cy = cosf(yaw);
312 element[0][0] = cp * cy;
313 element[0][1] = cp * sy;
314 element[0][2] = sp;
315 element[1][0] = sr * sp * cy - cr * sy;
316 element[1][1] = sr * sp * sy + cr * cy;
317 element[1][2] = - sr * cp;
318 element[2][0] = -(cr * sp * cy + sr * sy);
319 element[2][1] = cy * sr - cr * sp * sy;
320 element[2][2] = cr * cp;
321 }
322
323 ////////////////////////////////////////////////////////
324 /// x轴旋转矩阵(单位弧度)
325 ////////////////////////////////////////////////////////
326 inline void SetRotationX(T a)
327 {
328 element[0][0] = 1.0;
329 element[0][1] = 0.0;
330 element[0][2] = 0.0;
331 element[1][0] = 0.0;
332 element[1][1] = cosf(a);
333 element[1][2] = sinf(a);
334 element[2][0] = 0.0;
335 element[2][1] =-element[1][2];
336 element[2][2] = element[1][1];
337 }
338
339 ////////////////////////////////////////////////////////
340 /// y轴旋转矩阵(单位弧度)
341 ////////////////////////////////////////////////////////
342 void SetRotationY(T a)
343 {
344 element[0][0] = cosf(a);
345 element[0][1] = 0.0;
346 element[0][2] =-sinf(a);
347 element[1][0] = 0.0;
348 element[1][1] = 1.0;
349 element[1][2] = 0.0;
350 element[2][0] =-element[0][2];
351 element[2][1] = 0.0;
352 element[2][2] = element[0][0];
353 }
354
355 ////////////////////////////////////////////////////////
356 /// y轴旋转矩阵(单位弧度)
357 ////////////////////////////////////////////////////////
358 void SetRotationZ(T a)
359 {
360 element[0][0] = cosf(a);
361 element[0][1] = sinf(a);
362 element[0][2] = 0.0;
363 element[1][0] =-element[0][1];
364 element[1][1] = element[0][0];
365 element[1][2] = 0.0;
366 element[2][0] = 0.0;
367 element[2][1] = 0.0;
368 element[2][2] = 1.0;
369 }
370
371 ////////////////////////////////////////////////////////
372 /// 0矩阵
373 ////////////////////////////////////////////////////////
374 static Matrix3<T> Zero()
375 {
376 static Matrix3<T> mat(0,0,0,0,0,0,0,0,0);
377 return mat;
378 }
379
380 ////////////////////////////////////////////////////////
381 /// 单位矩阵
382 ////////////////////////////////////////////////////////
383 static Matrix3<T> Identity()
384 {
385 static Matrix3<T> mat(1,0,0,0,1,0,0,0,1);
386 return mat;
387 }
388
389 ////////////////////////////////////////////////////////
390 /// 矩阵数组
391 ////////////////////////////////////////////////////////
392 T element[3][3];
393 };
394
1 ////////////////////////////////////////////////////////////
2 ///! 定义4*4矩阵
3 ////////////////////////////////////////////////////////////
4 template<class T>
5 struct Matrix4
6 {
7 ////////////////////////////////////////////////////////
8 /// 构造4*4矩阵
9 ////////////////////////////////////////////////////////
10 inline Matrix4()
11 {
12 *this = Identity();
13 }
14
15 ////////////////////////////////////////////////////////
16 /// 构造4*4矩阵
17 ////////////////////////////////////////////////////////
18 inline Matrix4(T value)
19 {
20 *this = Identity()*value;
21 }
22
23 ////////////////////////////////////////////////////////
24 /// 构造4*4矩阵
25 ////////////////////////////////////////////////////////
26 inline Matrix4(T a1, T a2, T a3, T a4,
27 T a5, T a6, T a7, T a8,
28 T a9, T a10,T a11,T a12,
29 T a13,T a14,T a15,T a16)
30 {
31 Set(a1,a2,a3,a4,a5,a6,a7,a8,a9,a10,a11,a12,a13,a14,a15,a16);
32 }
33
34 COPY_CLASS(Matrix4<T>)
35
36 ////////////////////////////////////////////////////////
37 /// 析构4*4矩阵
38 ////////////////////////////////////////////////////////
39 inline ~Matrix4(){}
40
41 ////////////////////////////////////////////////////////
42 /// 四元数到矩阵
43 ////////////////////////////////////////////////////////
44 void QuatToMatrix(const Quaternion<T>& quat);
45
46 ////////////////////////////////////////////////////////
47 /// 获取矩阵数据指针
48 ////////////////////////////////////////////////////////
49 inline operator T **(){return element;}
50
51 ////////////////////////////////////////////////////////
52 /// 设置矩阵值
53 ////////////////////////////////////////////////////////
54 inline void Set(T a1, T a2, T a3, T a4,
55 T a5, T a6, T a7, T a8,
56 T a9, T a10,T a11,T a12,
57 T a13,T a14,T a15,T a16)
58 {
59 element[0][0] = a1;
60 element[0][1] = a2;
61 element[0][2] = a3;
62 element[0][3] = a4;
63
64 element[1][0] = a5;
65 element[1][1] = a6;
66 element[1][2] = a7;
67 element[1][3] = a8;
68
69 element[2][0] = a9;
70 element[2][1] = a10;
71 element[2][2] = a11;
72 element[3][3] = a12;
73
74 element[3][0] = a13;
75 element[3][1] = a14;
76 element[3][2] = a15;
77 element[3][3] = a16;
78 }
79
80 inline void Set(T* ptr)
81 {
82 element[0][0] = *ptr;
83 element[0][1] = *(ptr+1);
84 element[0][2] = *(ptr+2);
85 element[0][3] = *(ptr+3);
86
87 element[1][0] = *(ptr+4);
88 element[1][1] = *(ptr+5);
89 element[1][2] = *(ptr+6);
90 element[1][3] = *(ptr+7);
91
92 element[2][0] = *(ptr+8);
93 element[2][1] = *(ptr+9);
94 element[2][2] = *(ptr+10);
95 element[3][3] = *(ptr+11);
96
97 element[3][0] = *(ptr+12);
98 element[3][1] = *(ptr+13);
99 element[3][2] = *(ptr+14);
100 element[3][3] = *(ptr+15);
101 }
102
103 ////////////////////////////////////////////////////////
104 /// 获取矩阵元素
105 ////////////////////////////////////////////////////////
106 inline void GetData(float data[16])
107 {
108 data[0] = element[0][0];
109 data[1] = element[0][1];
110 data[2] = element[0][2];
111 data[3] = element[0][3];
112 data[4] = element[1][0];
113 data[5] = element[1][1];
114 data[6] = element[1][2];
115 data[7] = element[1][3];
116 data[8] = element[2][0];
117 data[9] = element[2][1];
118 data[10] = element[2][2];
119 data[11] = element[2][3];
120 data[12] = element[3][0];
121 data[13] = element[3][1];
122 data[14] = element[3][2];
123 data[15] = element[3][3];
124 }
125
126 ////////////////////////////////////////////////////////
127 /// 鐭╅樀杩愮畻绗﹀彿閲嶈浇
128 ////////////////////////////////////////////////////////
129 inline Vector4<T> operator*(const Vector4<T>& v)
130 {
131 T a1 = At(0,0);
132 T a2 = At(0,1);
133 T a3 = At(0,2);
134 T a4 = At(0,3);
135
136 T a5 = At(1,0);
137 T a6 = At(1,1);
138 T a7 = At(1,2);
139 T a8 = At(1,3);
140
141 T a9 = At(2,0);
142 T a10= At(2,1);
143 T a11= At(2,2);
144 T a12= At(2,3);
145
146 T a13 = At(2,0);
147 T a14 = At(2,1);
148 T a15 = At(1,2);
149 T a16 = At(2,3);
150
151 T b1 = v.x;
152 T b2 = v.y;
153 T b3 = v.z;
154 T b4 = v.w;
155 return Vector4<T>(a1*b1+a2*b2+a3*b3+a4*b4,
156 a5*b1+a6*b2+a7*b3+a8*b4,
157 a9*b1+a10*b2+a11*b3+a12*b4,
158 a13*b1+a14*b2+a15*b3+a16*b4);
159 }
160
161 inline Matrix4<T> operator + (const Matrix4<T>& other)
162 {
163 return Matrix4<T>(At(0,0)+other.At(0,0),At(0,1)+other.At(0,1),At(0,2)+other.At(0,2),At(0,3)+other.At(0,3),
164 At(1,0)+other.At(1,0),At(1,1)+other.At(1,1),At(1,2)+other.At(1,2),At(1,3)+other.At(1,3),
165 At(2,0)+other.At(2,0),At(2,1)+other.At(2,1),At(2,2)+other.At(2,2),At(2,3)+other.At(2,3),
166 At(3,0)+other.At(3,0),At(3,1)+other.At(3,1),At(3,2)+other.At(3,2),At(3,3)+other.At(3,3));
167 }
168
169 ADDABLE(Matrix4<T>, Matrix4<T>)
170
171 inline Matrix4<T> operator - (const Matrix4<T>& other)
172 {
173 return Matrix4<T>(At(0,0)-other.At(0,0),At(0,1)-other.At(0,1),At(0,2)-other.At(0,2),At(0,3)-other.At(0,3),
174 At(1,0)-other.At(1,0),At(1,1)-other.At(1,1),At(1,2)-other.At(1,2),At(1,3)-other.At(1,3),
175 At(2,0)-other.At(2,0),At(2,1)-other.At(2,1),At(2,2)-other.At(2,2),At(2,3)-other.At(2,3),
176 At(3,0)-other.At(3,0),At(3,1)-other.At(3,1),At(3,2)-other.At(3,2),At(3,3)-other.At(3,3));
177 }
178
179 SUBTRACTABLE(Matrix4<T>, Matrix4<T>)
180
181 inline Matrix4<T> operator + (const T& value)
182 {
183 return Matrix4<T>(At(0,0)+value,At(0,1)+value,At(0,2)+value,
184 At(1,0)+value,At(1,1)+value,At(1,2)+value,
185 At(2,0)+value,At(2,1)+value,At(2,2)+value);
186 }
187
188 ADDABLE(Matrix4<T>, T)
189
190 inline Matrix4<T> operator - (const T& value)
191 {
192 return Matrix4<T>(At(0,0)-value,At(0,1)-value,At(0,2)-value,
193 At(1,0)-value,At(1,1)-value,At(1,2)-value,
194 At(2,0)-value,At(2,1)-value,At(2,2)-value);
195 }
196
197 SUBTRACTABLE(Matrix4<T>, T)
198
199 inline Matrix4<T> operator * (const T& value)
200 {
201 return Matrix4<T>(At(0,0)*value,At(0,1)*value,At(0,2)*value,At(0,3)*value,
202 At(1,0)*value,At(1,1)*value,At(1,2)*value,At(1,3)*value,
203 At(2,0)*value,At(2,1)*value,At(2,2)*value,At(2,3)*value,
204 At(3,0)*value,At(3,1)*value,At(3,2)*value,At(3,3)*value);
205 }
206
207 MULTIPLIABLE(Matrix4<T>,T);
208
209 inline Matrix4<T> operator/(const T& value)
210 {
211 ASSERT(value !=0);
212 T v = 1/value;
213 return (*this)*v;
214 }
215
216 DIVISIBLE(Matrix4<T>,T);
217
218 inline Matrix4<T> operator*(const Matrix4<T>& o)
219 {
220 T a1 = At(0,0);
221 T a2 = At(0,1);
222 T a3 = At(0,2);
223 T a4 = At(0,3);
224
225 T a5 = At(1,0);
226 T a6 = At(1,1);
227 T a7 = At(1,2);
228 T a8 = At(1,3);
229
230 T a9 = At(2,0);
231 T a10= At(2,1);
232 T a11= At(2,2);
233 T a12= At(2,3);
234
235 T a13 = At(3,0);
236 T a14 = At(3,1);
237 T a15 = At(3,2);
238 T a16 = At(3,3);
239
240 Matrix4<T> other(o);
241 T b1 = other.At(0,0);
242 T b2 = other.At(0,1);
243 T b3 = other.At(0,2);
244 T b4 = other.At(0,3);
245 T b5 = other.At(1,0);
246 T b6 = other.At(1,1);
247 T b7 = other.At(1,2);
248 T b8 = other.At(1,3);
249 T b9 = other.At(2,0);
250 T b10= other.At(2,1);
251 T b11= other.At(2,2);
252 T b12= other.At(2,3);
253 T b13= other.At(3,0);
254 T b14= other.At(3,1);
255 T b15= other.At(3,2);
256 T b16= other.At(3,3);
257 return Matrix4<T>(a1*b1+a2*b5+a3*b9+a4*b13,
258 a1*b2+a2*b6+a3*b10+a4*b14,
259 a1*b3+a2*b7+a3*b11+a4*b15,
260 a1*b4+a2*b8+a3*b12+a4*b16,
261 a5*b1+a6*b5+a7*b9+a8*b13,
262 a5*b2+a6*b6+a7*b10+a8*b14,
263 a5*b3+a6*b7+a7*b11+a8*b15,
264 a5*b4+a6*b8+a7*b12+a8*b16,
265 a9*b1+a10*b5+a11*b9+a12*b13,
266 a9*b2+a10*b6+a11*b10+a12*b14,
267 a9*b3+a10*b7+a11*b11+a12*b15,
268 a9*b4+a10*b8+a11*b12+a12*b16,
269 a13*b1+a14*b5+a15*b9+a16*b13,
270 a13*b2+a14*b6+a15*b10+a16*b14,
271 a13*b3+a14*b7+a15*b11+a16*b15,
272 a13*b4+a14*b8+a15*b12+a16*b16);
273 }
274
275 MULTIPLIABLE(Matrix4<T>,Matrix4<T>);
276
277 inline bool operator==(const Matrix4<T>& other)
278 {
279 for(int i = 0; i < 4; i ++)
280 for(int j = 0; j < 4; j++)
281 {
282 if(At(i,j) != other.At(i,j))
283 return false;
284 }
285 return true;
286 }
287
288 NOT_EQUAL(Matrix4<T>)
289
290 ////////////////////////////////////////////////////////
291 /// 矩阵转置
292 ////////////////////////////////////////////////////////
293 inline Matrix4<T> Transpose()
294 {
295 return Matrix4<T>(At(0,0),At(1,0),At(2,0),At(3,0),
296 At(0,1),At(1,1),At(2,1),At(3,1),
297 At(0,2),At(1,2),At(2,2),At(3,2),
298 At(0,3),At(1,3),At(2,3),At(3,3));
299 }
300
301 ////////////////////////////////////////////////////////
302 /// 行列式求值
303 ////////////////////////////////////////////////////////
304 inline T Determinant()
305 {
306 T a1 = At(0,0);
307 T a2 = At(0,1);
308 T a3 = At(0,2);
309 T a4 = At(1,0);
310 T a5 = At(1,1);
311 T a6 = At(1,2);
312 T a7 = At(2,0);
313 T a8 = At(2,1);
314 T a9 = At(2,2);
315 T a10 = element[2][1];
316 T a11 = element[2][2];
317 T a12 = element[2][3];
318 T a13 = element[3][0];
319 T a14 = element[3][1];
320 T a15 = element[3][2];
321 T a16 = element[3][3];
322 return a1*a6*a11*a16-a1*a6*a12*a15-a1*a10*a7*a16+a1*a10*a8*a15+a1*a14*a7*a12-a1*a14*a8*a11-a5*a2*a11*a16+a5*a2*a12*a15+a5*a10*a3*a16-a5*a10*a4*a15-a5*a14*a3*a12+a5*a14*a4*a11+a9*a2*a7*a16-a9*a2*a8*a15-a9*a6*a3*a16+a9*a6*a4*a15+a9*a14*a3*a8-a9*a14*a4*a7-a13*a2*a7*a12+a13*a2*a8*a11+a13*a6*a3*a12-a13*a6*a4*a11-a13*a10*a3*a8+a13*a10*a4*a7;
323 }
324
325 ////////////////////////////////////////////////////////
326 /// 求取逆矩阵
327 ////////////////////////////////////////////////////////
328 inline bool Inverse(Matrix4<T> &mat, float toler = 0.0001)
329 {
330 T det = Determinant();
331 if(fabs(det) < toler)
332 return false;
333 T a1 = element[0][0];
334 T a2 = element[0][1];
335 T a3 = element[0][2];
336 T a4 = element[0][3];
337 T a5 = element[1][0];
338 T a6 = element[1][1];
339 T a7 = element[1][2];
340 T a8 = element[1][3];
341 T a9 = element[2][0];
342 T a10 = element[2][1];
343 T a11 = element[2][2];
344 T a12 = element[2][3];
345 T a13 = element[3][0];
346 T a14 = element[3][1];
347 T a15 = element[3][2];
348 T a16 = element[3][3];
349 T b1,b2,b3,b4,b5,b6,b7,b8,b9,b10,b11,b12,b13,b14,b15,b16;
350 b1 = (a6*a11*a16-a6*a12*a15-a10*a7*a16+a10*a8*a15+a14*a7*a12-a14*a8*a11)/det;
351 b2 = -(a2*a11*a16-a2*a12*a15-a10*a3*a16+a10*a4*a15+a14*a3*a12-a14*a4*a11)/det;
352 b3 = (a2*a7*a16-a2*a8*a15-a6*a3*a16+a6*a4*a15+a14*a3*a8-a14*a4*a7)/det;
353 b4 = -(a2*a7*a12-a2*a8*a11-a6*a3*a12+a6*a4*a11+a10*a3*a8-a10*a4*a7)/det;
354 b5 = -(a5*a11*a16-a5*a12*a15-a9*a7*a16+a9*a8*a15+a13*a7*a12-a13*a8*a11)/det;
355 b6 = (a1*a11*a16-a1*a12*a15-a9*a3*a16+a9*a4*a15+a13*a3*a12-a13*a4*a11)/det;
356 b7 = -(a1*a7*a16-a1*a8*a15-a5*a3*a16+a5*a4*a15+a13*a3*a8-a13*a4*a7)/det;
357 b8 = (a1*a7*a12-a1*a8*a11-a5*a3*a12+a5*a4*a11+a9*a3*a8-a9*a4*a7)/det;
358 b9= (a5*a10*a16-a5*a12*a14-a9*a6*a16+a9*a8*a14+a13*a6*a12-a13*a8*a10)/det;
359 b10 = -(a1*a10*a16-a1*a12*a14-a9*a2*a16+a9*a4*a14+a13*a2*a12-a13*a4*a10)/det;
360 b11= (a1*a6*a16-a1*a8*a14-a5*a2*a16+a5*a4*a14+a13*a2*a8-a13*a4*a6)/det;
361 b12= -(a1*a6*a12-a1*a8*a10-a5*a2*a12+a5*a4*a10+a9*a2*a8-a9*a4*a6)/det;
362 b13 = -(a5*a10*a15-a5*a11*a14-a9*a6*a15+a9*a7*a14+a13*a6*a11-a13*a7*a10)/det;
363 b14 = (a1*a10*a15-a1*a11*a14-a9*a2*a15+a9*a3*a14+a13*a2*a11-a13*a3*a10)/det;
364 b15= -(a1*a6*a15-a1*a7*a14-a5*a2*a15+a5*a3*a14+a13*a2*a7-a13*a3*a6)/det;
365 b16 = (a1*a6*a11-a1*a7*a10-a5*a2*a11+a5*a3*a10+a9*a2*a7-a9*a3*a6)/det;
366 mat.Set(b1,b2,b3,b4,b5,b6,b7,b8,b9,b10,b11,b12,b13,b14,b15,b16);
367 return true;
368 }
369
370 ////////////////////////////////////////////////////////
371 /// 设置给定位置元素值
372 ////////////////////////////////////////////////////////
373 inline void Set(int i, int j, T value)
374 {
375 ASSERT(i > -1 && i < 4);
376 ASSERT(j > -1 && j < 4);
377 element[i][j] = value;
378 }
379
380 ////////////////////////////////////////////////////////
381 /// 获取给定位置元素值
382 ////////////////////////////////////////////////////////
383 inline T At(int i, int j)
384 {
385 ASSERT(i > -1 && i < 4);
386 ASSERT(j > -1 && j < 4);
387 return element[i][j];
388 }
389
390 ////////////////////////////////////////////////////////
391 /// 获取给定行元素值
392 ////////////////////////////////////////////////////////
393 inline Vector4<T> GetRow(int row)
394 {
395 ASSERT(row > -1 && row < 4);
396 return Vector4<T>(At(row,0),At(row,1),At(row,2),At(row,3));
397 }
398
399 inline void SetRow(int row,const Vector4<T>& m)
400 {
401 ASSERT(row > -1 && row < 3);
402 Set(row,0,m.x);
403 Set(row,1,m.y);
404 Set(row,2,m.z);
405 Set(row,3,m.w);
406 }
407
408 ////////////////////////////////////////////////////////
409 /// 获取给定列元素值
410 ////////////////////////////////////////////////////////
411 inline Vector4<T> GetCol(int col)
412 {
413 ASSERT(col > -1 && col < 4);
414 return Vector4<T>(At(0,col),At(1,col),At(2,col),At(3,col));
415 }
416
417 inline void SetCol(int col,const Vector4<T>& m)
418 {
419 ASSERT(col > -1 && col < 3);
420 Set(0,col,m.x);
421 Set(1,col,m.y);
422 Set(2,col,m.z);
423 Set(4,col,m.w);
424 }
425
426 ////////////////////////////////////////////////////////
427 /// 矩阵缩放
428 ////////////////////////////////////////////////////////
429 inline void SetScale(const Vector3<T>& v)
430 {
431 element[0][0] *= v.x;
432 element[1][1] *= v.y;
433 element[2][2] *= v.z;
434 }
435
436 inline void SetScale(const T& scale)
437 {
438 element[3][3]*= scale;
439 }
440
441 ////////////////////////////////////////////////////////
442 /// 设置3*3矩阵
443 ////////////////////////////////////////////////////////
444 template<class M>
445 inline void Set3x3(const Matrix3<M>& m)
446 {
447 element[0][0] = m.element[0][0];
448 element[0][1] = m.element[0][1];
449 element[0][2] = m.element[0][2];
450 element[1][0] = m.element[1][0];
451 element[1][1] = m.element[1][1];
452 element[1][2] = m.element[1][2];
453 element[2][0] = m.element[2][0];
454 element[2][1] = m.element[2][1];
455 element[2][2] = m.element[2][2];
456 }
457
458 ////////////////////////////////////////////////////////
459 /// 获取缩放矩阵
460 ////////////////////////////////////////////////////////
461 inline static Matrix4<T> GetScale(const Vector3<T>& v)
462 {
463 static Matrix4<T> r;
464 r.element[0][0] = v.x; r.element[0][1] = 0.0; r.element[0][2] = 0.0; r.element[0][3] = 0.0;
465 r.element[1][0] = 0.0; r.element[1][1] = v.y; r.element[1][2] = 0.0; r.element[1][3] = 0.0;
466 r.element[2][0] = 0.0; r.element[2][1] = 0.0; r.element[2][2] = v.z; r.element[2][3] = 0.0;
467 r.element[3][0] = 0.0; r.element[3][1] = 0.0; r.element[3][2] = 0.0; r.element[3][3] = 1.0;
468 return r;
469 }
470
471 ////////////////////////////////////////////////////////
472 /// 设置投影矩阵
473 ////////////////////////////////////////////////////////
474 inline Matrix4<T>& SetProject(const Vector3<T>& v)
475 {
476 element[0][3] = v.x;
477 element[1][3] = v.y;
478 element[2][3] = v.z;
479 return *this;
480 }
481
482 inline Vector3<T> GetProject() const
483 {
484 return Vector3<T>(element[0][3],element[1][3],element[2][3]);
485 }
486
487 ////////////////////////////////////////////////////////
488 /// 获取平移矩阵
489 ////////////////////////////////////////////////////////
490 static inline Matrix4<T> GetTranslate(const Vector3<T>& v)
491 {
492 static Matrix4<T> r;
493 r.element[0][0] = 1;
494 r.element[0][1] = 0.0;
495 r.element[0][2] = 0.0;
496 r.element[0][3] = 0.0;
497 r.element[1][0] = 0.0;
498 r.element[1][1] = 1;
499 r.element[1][2] = 0.0;
500 r.element[1][3] = 0.0;
501 r.element[2][0] = 0.0;
502 r.element[2][1] = 0.0;
503 r.element[2][2] = 1;
504 r.element[2][3] = 0.0;
505 r.element[3][0] = v.x;
506 r.element[3][1] = v.y;
507 r.element[3][2] = v.z;
508 r.element[3][3] = 1.0;
509 return r;
510 }
511
512 inline Vector3<T> GetOffset() const
513 {
514 return Vector3<T>(element[3][0],element[3][1],element[3][2]);
515 }
516
517 ////////////////////////////////////////////////////////
518 /// 获取旋转矩阵
519 ////////////////////////////////////////////////////////
520 static inline Matrix4<T> GetRotation(const Vector3<T>& rotation)
521 {
522 T cr = cosf(rotation.x); //= Math::CosTable( rotation.x );
523 T sr = sinf(rotation.x); //= Math::SinTable( rotation.x );
524 T cp = cosf(rotation.y); //= Math::CosTable( rotation.y );
525 T sp = sinf(rotation.y); //= Math::SinTable( rotation.y );
526 T cy = cosf(rotation.z); //= Math::CosTable( rotation.z );
527 T sy = sinf(rotation.z); //= Math::SinTable( rotation.z );
528 static Matrix4<T> r;
529 r.element[0][0] = cp*cy;
530 r.element[0][1] = cp*sy;
531 r.element[0][2] = -sp;
532 r.element[0][3] = 0.0;
533 r.element[1][0] = 0.0;
534 r.element[1][1] = 1;
535 r.element[1][2] = 0.0;
536 r.element[1][3] = 0.0;
537 r.element[2][0] = 0.0;
538 r.element[2][1] = 0.0;
539 r.element[2][2] = 1;
540 r.element[2][3] = 0.0;
541 r.element[3][0] = rotation.x;
542 r.element[3][1] = rotation.y;
543 r.element[3][2] = rotation.z;
544 r.element[3][3] = 1.0;
545 T srsp = sr*sp;
546 T crsp = cr*sp;
547 r.element[0][3] = srsp*cy-cr*sy;
548 r.element[0][4] = srsp*sy+cr*cy;
549 r.element[0][5] = sr*cp;
550 r.element[2][0] = crsp*cy+sr*sy;
551 r.element[2][1] = crsp*sy-sr*cy;
552 r.element[2][3] = cr*cp ;
553 return r;
554 }
555
556 ////////////////////////////////////////////////////////
557 /// 获取投影矩阵
558 ////////////////////////////////////////////////////////
559 inline static Matrix4<T> GetProjectMatrix(const Vector3<T>& v)
560 {
561 Matrix4<T> m;
562 m.element[0][0] = 1.0; m.element[0][1] = 0.0; m.element[0][2] = 0.0; m.element[0][3] = v.x;
563 m.element[1][0] = 0.0; m.element[1][1] = 1.0; m.element[1][2] = 0.0; m.element[1][3] = v.y;
564 m.element[2][0] = 0.0; m.element[2][1] = 0.0; m.element[2][2] = 1.0; m.element[2][3] = v.z;
565 m.element[3][0] = 0.0; m.element[3][1] = 0.0; m.element[3][2] = 0.0; m.element[3][3] = 1.0;
566 return m;
567 }
568
569 ////////////////////////////////////////////////////////
570 /// 获取平移矩阵
571 ////////////////////////////////////////////////////////
572 inline static Matrix4<T> GetPtojectOffset(const Vector3<T>& v)
573 {
574 Matrix4<T> m;
575 m.element[0][0] = 1.0; m.element[0][1] = 0.0; m.element[0][2] = 0.0; m.element[0][3] = 0.0;
576 m.element[1][0] = 0.0; m.element[1][1] = 1.0; m.element[1][2] = 0.0; m.element[1][3] = 0.0;
577 m.element[2][0] = 0.0; m.element[2][1] = 0.0; m.element[2][2] = 1.0; m.element[2][3] = 0.0;
578 m.element[3][0] = v.x; m.element[3][1] = v.y; m.element[3][2] = v.z; m.element[3][3] = 1.0;
579 return m;
580 }
581
582 ////////////////////////////////////////////////////////
583 /// 获取绕x轴旋转矩阵(弧度)
584 ////////////////////////////////////////////////////////
585 inline static Matrix4<T> GetRotatedX(float angle)
586 {
587 Matrix4<T> m;
588 m.element[0][0] = 1.0; m.element[0][1] = 0.0; m.element[0][2] = 0.0; m.element[0][3] = 0.0;
589 m.element[1][0] = 0.0; m.element[1][1] = cos(angle); m.element[1][2] = -sin(angle); m.element[1][3] = 0.0;
590 m.element[2][0] = 0.0; m.element[2][1] = sin(angle); m.element[2][2] = cos(angle); m.element[2][3] = 0.0;
591 m.element[3][0] = 0.0; m.element[3][1] = 0.0; m.element[3][2] = 0.0; m.element[3][3] = 1.0;
592 return m;
593 }
594
595 ////////////////////////////////////////////////////////
596 /// 获取绕x轴旋转矩阵(弧度)
597 ////////////////////////////////////////////////////////
598 inline static Matrix4<T> GetRotatedY(float angle)
599 {
600 Matrix4<T> m;
601 m.element[0][0] = cos(angle);
602 m.element[0][1] = 0.0;
603 m.element[0][2] = -sin(angle);
604 m.element[0][3] = 0.0;
605 m.element[1][0] = 0.0;
606 m.element[1][1] = 1.0;
607 m.element[1][2] = 0.0;
608 m.element[1][3] = 0.0;
609 m.element[2][0] = sin(angle);
610 m.element[2][1] = 0.0;
611 m.element[2][2] = cos(angle);
612 m.element[2][3] = 0.0;
613 m.element[3][0] = 0.0;
614 m.element[3][1] = 0.0;
615 m.element[3][2] = 0.0;
616 m.element[3][3] = 1.0;
617 return m;
618 }
619
620 ////////////////////////////////////////////////////////
621 /// 获取绕x轴旋转矩阵(弧度)
622 ////////////////////////////////////////////////////////
623 inline static Matrix4<T> GetRotatedZ(float angle)
624 {
625 Matrix4<T> m;
626 m.element[0][0] = cos(angle);
627 m.element[0][1] = sin(angle);
628 m.element[0][2] = 0.0;
629 m.element[0][3] = 0.0;
630 m.element[1][0] = -sin(angle);
631 m.element[1][1] = cos(angle);
632 m.element[1][2] = 0.0;
633 m.element[1][3] = 0.0;
634 m.element[2][0] = sin(angle);
635 m.element[2][1] = 0.0;
636 m.element[2][2] = 0.0;
637 m.element[2][3] = 0.0;
638 m.element[3][0] = 0.0;
639 m.element[3][1] = 0.0;
640 m.element[3][2] = 0.0;
641 m.element[3][3] = 1.0;
642 return m;
643 }
644
645 ////////////////////////////////////////////////////////
646 /// 获取3*3矩阵
647 ////////////////////////////////////////////////////////
648 inline Matrix4<T> As3X3()
649 {
650 Matrix4<T> t = *this;
651 t[0][3] = 0;
652 t[1][3] = 0;
653 t[2][3] = 0;
654 t[3][3] = 1;
655 t[3][0] = 0;
656 t[3][1] = 0;
657 t[3][2] = 0;
658 return t;
659 }
660
661 inline Matrix3<T> Get3X3()
662 {
663 return Matrix3<T>(element[0][0],element[0][1],element[0][2],
664 element[1][0],element[1][1],element[1][2],
665 element[2][0],element[2][1],element[2][2]);
666 }
667
668 ////////////////////////////////////////////////////////
669 /// 获取点对给定矩阵的平移
670 ////////////////////////////////////////////////////////
671 void Translate(const float* pos, const Matrix4<T>& mat)
672 {
673 element[0][0] = mat.element[0][0];
674 element[0][1] = mat.element[1][0];
675 element[0][2] = mat.element[2][0];
676 element[0][3] = 0;
677 element[1][0] = mat.element[0][1];
678 element[1][1] = mat.element[1][1];
679 element[1][2] = mat.element[2][1];
680 element[1][3] = 0;
681 element[2][0] = mat.element[0][2];
682 element[2][1] = mat.element[1][2];
683 element[2][2] = mat.element[2][2];
684 element[2][3] = 0;
685 element[3][0] = mat.element[0][3];
686 element[3][1] = mat.element[1][3];
687 element[3][2] = mat.element[2][3];
688 element[3][3] = 1;
689 }
690
691 Vector3<T> GetRotation()
692 {
693 const Matrix4<T> &mat = *this;
694 float y = -asin(mat.At(0,2));
695 const float c = cos(y);
696 y *= RADTODEG32;
697
698 float rotx, roty, x, z;
699 if (fabs(c)>1e-5)
700 {
701 const T invc = (T)(1.0/c);
702 rotx = mat.At(2,2) * invc;
703 roty = mat.At(1,2) * invc;
704 x = atan2( roty, rotx) * RADTODEG32;
705 rotx = mat.At(0,0) * invc;
706 roty = mat.At(0,1) * invc;
707 z = atan2(roty, rotx) * RADTODEG32;
708 }
709 else
710 {
711 x = 0.0;
712 rotx = mat(1,1);
713 roty = -mat(1,0);
714 z = atan2(roty, rotx) * RADTODEG64;
715 }
716
717 if (x < 0.0)
718 x += 360.0;
719 if (y < 0.0)
720 y += 360.0;
721 if (z < 0.0)
722 z += 360.0;
723
724 return Vector3<T>((T)x,(T)y,(T)z);
725 }
726
727 ////////////////////////////////////////////////////////
728 /// 3D图形函数
729 ////////////////////////////////////////////////////////
730 static Matrix4<T> Perspective(T fovy, T ratio, T znear, T zfar);
731 static Matrix4<T> Frustum(T left, T right, T bottom, T top, T near, T far);
732 static Matrix4<T> Ortho(T left, T right, T bottom, T top, T near, T far);
733 static Matrix4<T> Ortho2D(T left, T right, T bottom, T top);
734 static Matrix4<T> LookAt(const Vector3<T>& eye, const Vector3<T>& look, const Vector3<T>& up);
735
736 ////////////////////////////////////////////////////////
737 /// 获取0矩阵
738 ////////////////////////////////////////////////////////
739 static Matrix4<T> Zero()
740 {
741 static Matrix4<T> mat(0,0,0,0,
742 0,0,0,0,
743 0,0,0,0,
744 0,0,0,0);
745 return mat;
746 }
747
748 ////////////////////////////////////////////////////////
749 /// 获取单位化矩阵
750 ////////////////////////////////////////////////////////
751 static Matrix4<T> Identity()
752 {
753 static Matrix4<T> mat(1,0,0,0,
754 0,1,0,0,
755 0,0,1,0,
756 0,0,0,1);
757 return mat;
758 }
759
760 ////////////////////////////////////////////////////////
761 /// 矩阵数据元素
762 ////////////////////////////////////////////////////////
763 T element[4][4];
764 };
765
如果有什么问题的话 请联系我喔