1 // This code is in the public domain -- castanyo@yahoo.es
2
3 #ifndef NV_MATH_MATRIX_H
4 #define NV_MATH_MATRIX_H
5
6 #include <nvmath/nvmath.h>
7 #include <nvmath/Vector.h>
8
9 namespace nv
10 {
11
12 // @@ Use scalar defined in Vector.h, but should use a template instead.
13
14 /// 4x4 transformation matrix.
15 /// -# Matrices are stored in memory in column major order.
16 /// -# Points are to be though of as column vectors.
17 /// -# Transformation of a point p by a matrix M is: p' = M * p
18 class NVMATH_CLASS Matrix
19 {
20 public:
21 typedef Matrix const & Arg;
22
23 Matrix();
24 Matrix(zero_t);
25 Matrix(identity_t);
26 Matrix(const Matrix & m);
27
28 scalar data(uint idx) const;
29 scalar & data(uint idx);
30 scalar get(uint row, uint col) const;
31 scalar operator()(uint row, uint col) const;
32 scalar & operator()(uint row, uint col);
33 const scalar * ptr() const;
34
35 Vector4 row(uint i) const;
36 Vector4 column(uint i) const;
37
38 void scale(scalar s);
39 void scale(Vector3::Arg s);
40 void translate(Vector3::Arg t);
41 void rotate(scalar theta, scalar v0, scalar v1, scalar v2);
42 scalar determinant() const;
43
44 void apply(Matrix::Arg m);
45
46 private:
47 scalar m_data[16];
48 };
49
50
Matrix()51 inline Matrix::Matrix()
52 {
53 }
54
Matrix(zero_t)55 inline Matrix::Matrix(zero_t)
56 {
57 for(int i = 0; i < 16; i++) {
58 m_data[i] = 0.0f;
59 }
60 }
61
Matrix(identity_t)62 inline Matrix::Matrix(identity_t)
63 {
64 for(int i = 0; i < 4; i++) {
65 for(int j = 0; j < 4; j++) {
66 m_data[4*j+i] = (i == j) ? 1.0f : 0.0f;
67 }
68 }
69 }
70
Matrix(const Matrix & m)71 inline Matrix::Matrix(const Matrix & m)
72 {
73 for(int i = 0; i < 16; i++) {
74 m_data[i] = m.m_data[i];
75 }
76 }
77
78
79 // Accessors
data(uint idx)80 inline scalar Matrix::data(uint idx) const
81 {
82 nvDebugCheck(idx < 16);
83 return m_data[idx];
84 }
data(uint idx)85 inline scalar & Matrix::data(uint idx)
86 {
87 nvDebugCheck(idx < 16);
88 return m_data[idx];
89 }
get(uint row,uint col)90 inline scalar Matrix::get(uint row, uint col) const
91 {
92 nvDebugCheck(row < 4 && col < 4);
93 return m_data[col * 4 + row];
94 }
operator()95 inline scalar Matrix::operator()(uint row, uint col) const
96 {
97 nvDebugCheck(row < 4 && col < 4);
98 return m_data[col * 4 + row];
99 }
operator()100 inline scalar & Matrix::operator()(uint row, uint col)
101 {
102 nvDebugCheck(row < 4 && col < 4);
103 return m_data[col * 4 + row];
104 }
105
ptr()106 inline const scalar * Matrix::ptr() const
107 {
108 return m_data;
109 }
110
row(uint i)111 inline Vector4 Matrix::row(uint i) const
112 {
113 nvDebugCheck(i < 4);
114 return Vector4(get(i, 0), get(i, 1), get(i, 2), get(i, 3));
115 }
116
column(uint i)117 inline Vector4 Matrix::column(uint i) const
118 {
119 nvDebugCheck(i < 4);
120 return Vector4(get(0, i), get(1, i), get(2, i), get(3, i));
121 }
122
123 /// Apply scale.
scale(scalar s)124 inline void Matrix::scale(scalar s)
125 {
126 m_data[0] *= s; m_data[1] *= s; m_data[2] *= s; m_data[3] *= s;
127 m_data[4] *= s; m_data[5] *= s; m_data[6] *= s; m_data[7] *= s;
128 m_data[8] *= s; m_data[9] *= s; m_data[10] *= s; m_data[11] *= s;
129 m_data[12] *= s; m_data[13] *= s; m_data[14] *= s; m_data[15] *= s;
130 }
131
132 /// Apply scale.
scale(Vector3::Arg s)133 inline void Matrix::scale(Vector3::Arg s)
134 {
135 m_data[0] *= s.x(); m_data[1] *= s.x(); m_data[2] *= s.x(); m_data[3] *= s.x();
136 m_data[4] *= s.y(); m_data[5] *= s.y(); m_data[6] *= s.y(); m_data[7] *= s.y();
137 m_data[8] *= s.z(); m_data[9] *= s.z(); m_data[10] *= s.z(); m_data[11] *= s.z();
138 }
139
140 /// Apply translation.
translate(Vector3::Arg t)141 inline void Matrix::translate(Vector3::Arg t)
142 {
143 m_data[12] = m_data[0] * t.x() + m_data[4] * t.y() + m_data[8] * t.z() + m_data[12];
144 m_data[13] = m_data[1] * t.x() + m_data[5] * t.y() + m_data[9] * t.z() + m_data[13];
145 m_data[14] = m_data[2] * t.x() + m_data[6] * t.y() + m_data[10] * t.z() + m_data[14];
146 m_data[15] = m_data[3] * t.x() + m_data[7] * t.y() + m_data[11] * t.z() + m_data[15];
147 }
148
149 Matrix rotation(scalar theta, scalar v0, scalar v1, scalar v2);
150
151 /// Apply rotation.
rotate(scalar theta,scalar v0,scalar v1,scalar v2)152 inline void Matrix::rotate(scalar theta, scalar v0, scalar v1, scalar v2)
153 {
154 Matrix R(rotation(theta, v0, v1, v2));
155 apply(R);
156 }
157
158 /// Apply transform.
apply(Matrix::Arg m)159 inline void Matrix::apply(Matrix::Arg m)
160 {
161 nvDebugCheck(this != &m);
162
163 for(int i = 0; i < 4; i++) {
164 const scalar ai0 = get(i,0), ai1 = get(i,1), ai2 = get(i,2), ai3 = get(i,3);
165 m_data[0 + i] = ai0 * m(0,0) + ai1 * m(1,0) + ai2 * m(2,0) + ai3 * m(3,0);
166 m_data[4 + i] = ai0 * m(0,1) + ai1 * m(1,1) + ai2 * m(2,1) + ai3 * m(3,1);
167 m_data[8 + i] = ai0 * m(0,2) + ai1 * m(1,2) + ai2 * m(2,2) + ai3 * m(3,2);
168 m_data[12+ i] = ai0 * m(0,3) + ai1 * m(1,3) + ai2 * m(2,3) + ai3 * m(3,3);
169 }
170 }
171
172 /// Get scale matrix.
scale(Vector3::Arg s)173 inline Matrix scale(Vector3::Arg s)
174 {
175 Matrix m(identity);
176 m(0,0) = s.x();
177 m(1,1) = s.y();
178 m(2,2) = s.z();
179 return m;
180 }
181
182 /// Get scale matrix.
scale(scalar s)183 inline Matrix scale(scalar s)
184 {
185 Matrix m(identity);
186 m(0,0) = m(1,1) = m(2,2) = s;
187 return m;
188 }
189
190 /// Get translation matrix.
translation(Vector3::Arg t)191 inline Matrix translation(Vector3::Arg t)
192 {
193 Matrix m(identity);
194 m(0,3) = t.x();
195 m(1,3) = t.y();
196 m(2,3) = t.z();
197 return m;
198 }
199
200 /// Get rotation matrix.
rotation(scalar theta,scalar v0,scalar v1,scalar v2)201 inline Matrix rotation(scalar theta, scalar v0, scalar v1, scalar v2)
202 {
203 scalar cost = cosf(theta);
204 scalar sint = sinf(theta);
205
206 Matrix m(identity);
207
208 if( 1 == v0 && 0 == v1 && 0 == v2 ) {
209 m(1,1) = cost; m(2,1) = -sint;
210 m(1,2) = sint; m(2,2) = cost;
211 }
212 else if( 0 == v0 && 1 == v1 && 0 == v2 ) {
213 m(0,0) = cost; m(2,0) = sint;
214 m(1,2) = -sint; m(2,2) = cost;
215 }
216 else if( 0 == v0 && 0 == v1 && 1 == v2 ) {
217 m(0,0) = cost; m(1,0) = -sint;
218 m(0,1) = sint; m(1,1) = cost;
219 }
220 else {
221 scalar a2, b2, c2;
222 a2 = v0 * v0;
223 b2 = v1 * v1;
224 c2 = v2 * v2;
225
226 scalar iscale = 1.0f / sqrtf(a2 + b2 + c2);
227 v0 *= iscale;
228 v1 *= iscale;
229 v2 *= iscale;
230
231 scalar abm, acm, bcm;
232 scalar mcos, asin, bsin, csin;
233 mcos = 1.0f - cost;
234 abm = v0 * v1 * mcos;
235 acm = v0 * v2 * mcos;
236 bcm = v1 * v2 * mcos;
237 asin = v0 * sint;
238 bsin = v1 * sint;
239 csin = v2 * sint;
240 m(0,0) = a2 * mcos + cost;
241 m(1,0) = abm - csin;
242 m(2,0) = acm + bsin;
243 m(3,0) = abm + csin;
244 m(1,1) = b2 * mcos + cost;
245 m(2,1) = bcm - asin;
246 m(3,1) = acm - bsin;
247 m(1,2) = bcm + asin;
248 m(2,2) = c2 * mcos + cost;
249 }
250 return m;
251 }
252
253 //Matrix rotation(scalar yaw, scalar pitch, scalar roll);
254 //Matrix skew(scalar angle, Vector3::Arg v1, Vector3::Arg v2);
255
256 /// Get frustum matrix.
frustum(scalar xmin,scalar xmax,scalar ymin,scalar ymax,scalar zNear,scalar zFar)257 inline Matrix frustum(scalar xmin, scalar xmax, scalar ymin, scalar ymax, scalar zNear, scalar zFar)
258 {
259 Matrix m(zero);
260
261 scalar doubleznear = 2.0f * zNear;
262 scalar one_deltax = 1.0f / (xmax - xmin);
263 scalar one_deltay = 1.0f / (ymax - ymin);
264 scalar one_deltaz = 1.0f / (zFar - zNear);
265
266 m(0,0) = doubleznear * one_deltax;
267 m(1,1) = doubleznear * one_deltay;
268 m(0,2) = (xmax + xmin) * one_deltax;
269 m(1,2) = (ymax + ymin) * one_deltay;
270 m(2,2) = -(zFar + zNear) * one_deltaz;
271 m(3,2) = -1.0f;
272 m(2,3) = -(zFar * doubleznear) * one_deltaz;
273
274 return m;
275 }
276
277 /// Get infinite frustum matrix.
frustum(scalar xmin,scalar xmax,scalar ymin,scalar ymax,scalar zNear)278 inline Matrix frustum(scalar xmin, scalar xmax, scalar ymin, scalar ymax, scalar zNear)
279 {
280 Matrix m(zero);
281
282 scalar doubleznear = 2.0f * zNear;
283 scalar one_deltax = 1.0f / (xmax - xmin);
284 scalar one_deltay = 1.0f / (ymax - ymin);
285 scalar nudge = 1.0; // 0.999;
286
287 m(0,0) = doubleznear * one_deltax;
288 m(1,1) = doubleznear * one_deltay;
289 m(0,2) = (xmax + xmin) * one_deltax;
290 m(1,2) = (ymax + ymin) * one_deltay;
291 m(2,2) = -1.0f * nudge;
292 m(3,2) = -1.0f;
293 m(2,3) = -doubleznear * nudge;
294
295 return m;
296 }
297
298 /// Get perspective matrix.
perspective(scalar fovy,scalar aspect,scalar zNear,scalar zFar)299 inline Matrix perspective(scalar fovy, scalar aspect, scalar zNear, scalar zFar)
300 {
301 scalar xmax = zNear * tan(fovy / 2);
302 scalar xmin = -xmax;
303
304 scalar ymax = xmax / aspect;
305 scalar ymin = -ymax;
306
307 return frustum(xmin, xmax, ymin, ymax, zNear, zFar);
308 }
309
310 /// Get infinite perspective matrix.
perspective(scalar fovy,scalar aspect,scalar zNear)311 inline Matrix perspective(scalar fovy, scalar aspect, scalar zNear)
312 {
313 scalar x = zNear * tan(fovy / 2);
314 scalar y = x / aspect;
315 return frustum( -x, x, -y, y, zNear );
316 }
317
318 /// Get matrix determinant.
determinant()319 inline scalar Matrix::determinant() const
320 {
321 return
322 m_data[3] * m_data[6] * m_data[ 9] * m_data[12] - m_data[2] * m_data[7] * m_data[ 9] * m_data[12] - m_data[3] * m_data[5] * m_data[10] * m_data[12] + m_data[1] * m_data[7] * m_data[10] * m_data[12] +
323 m_data[2] * m_data[5] * m_data[11] * m_data[12] - m_data[1] * m_data[6] * m_data[11] * m_data[12] - m_data[3] * m_data[6] * m_data[ 8] * m_data[13] + m_data[2] * m_data[7] * m_data[ 8] * m_data[13] +
324 m_data[3] * m_data[4] * m_data[10] * m_data[13] - m_data[0] * m_data[7] * m_data[10] * m_data[13] - m_data[2] * m_data[4] * m_data[11] * m_data[13] + m_data[0] * m_data[6] * m_data[11] * m_data[13] +
325 m_data[3] * m_data[5] * m_data[ 8] * m_data[14] - m_data[1] * m_data[7] * m_data[ 8] * m_data[14] - m_data[3] * m_data[4] * m_data[ 9] * m_data[14] + m_data[0] * m_data[7] * m_data[ 9] * m_data[14] +
326 m_data[1] * m_data[4] * m_data[11] * m_data[14] - m_data[0] * m_data[5] * m_data[11] * m_data[14] - m_data[2] * m_data[5] * m_data[ 8] * m_data[15] + m_data[1] * m_data[6] * m_data[ 8] * m_data[15] +
327 m_data[2] * m_data[4] * m_data[ 9] * m_data[15] - m_data[0] * m_data[6] * m_data[ 9] * m_data[15] - m_data[1] * m_data[4] * m_data[10] * m_data[15] + m_data[0] * m_data[5] * m_data[10] * m_data[15];
328 }
329
transpose(Matrix::Arg m)330 inline Matrix transpose(Matrix::Arg m)
331 {
332 Matrix r;
333 for (int i = 0; i < 4; i++)
334 {
335 for (int j = 0; j < 4; j++)
336 {
337 r(i, j) = m(j, i);
338 }
339 }
340 return r;
341 }
342
inverse(Matrix::Arg m)343 inline Matrix inverse(Matrix::Arg m)
344 {
345 Matrix r;
346 r.data( 0) = m.data(6)*m.data(11)*m.data(13) - m.data(7)*m.data(10)*m.data(13) + m.data(7)*m.data(9)*m.data(14) - m.data(5)*m.data(11)*m.data(14) - m.data(6)*m.data(9)*m.data(15) + m.data(5)*m.data(10)*m.data(15);
347 r.data( 1) = m.data(3)*m.data(10)*m.data(13) - m.data(2)*m.data(11)*m.data(13) - m.data(3)*m.data(9)*m.data(14) + m.data(1)*m.data(11)*m.data(14) + m.data(2)*m.data(9)*m.data(15) - m.data(1)*m.data(10)*m.data(15);
348 r.data( 2) = m.data(2)*m.data( 7)*m.data(13) - m.data(3)*m.data( 6)*m.data(13) + m.data(3)*m.data(5)*m.data(14) - m.data(1)*m.data( 7)*m.data(14) - m.data(2)*m.data(5)*m.data(15) + m.data(1)*m.data( 6)*m.data(15);
349 r.data( 3) = m.data(3)*m.data( 6)*m.data( 9) - m.data(2)*m.data( 7)*m.data( 9) - m.data(3)*m.data(5)*m.data(10) + m.data(1)*m.data( 7)*m.data(10) + m.data(2)*m.data(5)*m.data(11) - m.data(1)*m.data( 6)*m.data(11);
350 r.data( 4) = m.data(7)*m.data(10)*m.data(12) - m.data(6)*m.data(11)*m.data(12) - m.data(7)*m.data(8)*m.data(14) + m.data(4)*m.data(11)*m.data(14) + m.data(6)*m.data(8)*m.data(15) - m.data(4)*m.data(10)*m.data(15);
351 r.data( 5) = m.data(2)*m.data(11)*m.data(12) - m.data(3)*m.data(10)*m.data(12) + m.data(3)*m.data(8)*m.data(14) - m.data(0)*m.data(11)*m.data(14) - m.data(2)*m.data(8)*m.data(15) + m.data(0)*m.data(10)*m.data(15);
352 r.data( 6) = m.data(3)*m.data( 6)*m.data(12) - m.data(2)*m.data( 7)*m.data(12) - m.data(3)*m.data(4)*m.data(14) + m.data(0)*m.data( 7)*m.data(14) + m.data(2)*m.data(4)*m.data(15) - m.data(0)*m.data( 6)*m.data(15);
353 r.data( 7) = m.data(2)*m.data( 7)*m.data( 8) - m.data(3)*m.data( 6)*m.data( 8) + m.data(3)*m.data(4)*m.data(10) - m.data(0)*m.data( 7)*m.data(10) - m.data(2)*m.data(4)*m.data(11) + m.data(0)*m.data( 6)*m.data(11);
354 r.data( 8) = m.data(5)*m.data(11)*m.data(12) - m.data(7)*m.data( 9)*m.data(12) + m.data(7)*m.data(8)*m.data(13) - m.data(4)*m.data(11)*m.data(13) - m.data(5)*m.data(8)*m.data(15) + m.data(4)*m.data( 9)*m.data(15);
355 r.data( 9) = m.data(3)*m.data( 9)*m.data(12) - m.data(1)*m.data(11)*m.data(12) - m.data(3)*m.data(8)*m.data(13) + m.data(0)*m.data(11)*m.data(13) + m.data(1)*m.data(8)*m.data(15) - m.data(0)*m.data( 9)*m.data(15);
356 r.data(10) = m.data(1)*m.data( 7)*m.data(12) - m.data(3)*m.data( 5)*m.data(12) + m.data(3)*m.data(4)*m.data(13) - m.data(0)*m.data( 7)*m.data(13) - m.data(1)*m.data(4)*m.data(15) + m.data(0)*m.data( 5)*m.data(15);
357 r.data(11) = m.data(3)*m.data( 5)*m.data( 8) - m.data(1)*m.data( 7)*m.data( 8) - m.data(3)*m.data(4)*m.data( 9) + m.data(0)*m.data( 7)*m.data( 9) + m.data(1)*m.data(4)*m.data(11) - m.data(0)*m.data( 5)*m.data(11);
358 r.data(12) = m.data(6)*m.data( 9)*m.data(12) - m.data(5)*m.data(10)*m.data(12) - m.data(6)*m.data(8)*m.data(13) + m.data(4)*m.data(10)*m.data(13) + m.data(5)*m.data(8)*m.data(14) - m.data(4)*m.data( 9)*m.data(14);
359 r.data(13) = m.data(1)*m.data(10)*m.data(12) - m.data(2)*m.data( 9)*m.data(12) + m.data(2)*m.data(8)*m.data(13) - m.data(0)*m.data(10)*m.data(13) - m.data(1)*m.data(8)*m.data(14) + m.data(0)*m.data( 9)*m.data(14);
360 r.data(14) = m.data(2)*m.data( 5)*m.data(12) - m.data(1)*m.data( 6)*m.data(12) - m.data(2)*m.data(4)*m.data(13) + m.data(0)*m.data( 6)*m.data(13) + m.data(1)*m.data(4)*m.data(14) - m.data(0)*m.data( 5)*m.data(14);
361 r.data(15) = m.data(1)*m.data( 6)*m.data( 8) - m.data(2)*m.data( 5)*m.data( 8) + m.data(2)*m.data(4)*m.data( 9) - m.data(0)*m.data( 6)*m.data( 9) - m.data(1)*m.data(4)*m.data(10) + m.data(0)*m.data( 5)*m.data(10);
362 r.scale(1.0f / m.determinant());
363 return r;
364 }
365
isometryInverse(Matrix::Arg m)366 inline Matrix isometryInverse(Matrix::Arg m)
367 {
368 Matrix r(identity);
369
370 // transposed 3x3 upper left matrix
371 for (int i = 0; i < 3; i++)
372 {
373 for (int j = 0; j < 3; j++)
374 {
375 r(i, j) = m(j, i);
376 }
377 }
378
379 // translate by the negative offsets
380 r.translate(-Vector3(m.data(12), m.data(13), m.data(14)));
381
382 return r;
383 }
384
385 //Matrix affineInverse(Matrix::Arg m);
386
387 /// Transform the given 3d point with the given matrix.
transformPoint(Matrix::Arg m,Vector3::Arg p)388 inline Vector3 transformPoint(Matrix::Arg m, Vector3::Arg p)
389 {
390 return Vector3(
391 p.x() * m(0,0) + p.y() * m(0,1) + p.z() * m(0,2) + m(0,3),
392 p.x() * m(1,0) + p.y() * m(1,1) + p.z() * m(1,2) + m(1,3),
393 p.x() * m(2,0) + p.y() * m(2,1) + p.z() * m(2,2) + m(2,3));
394 }
395
396 /// Transform the given 3d vector with the given matrix.
transformVector(Matrix::Arg m,Vector3::Arg p)397 inline Vector3 transformVector(Matrix::Arg m, Vector3::Arg p)
398 {
399 return Vector3(
400 p.x() * m(0,0) + p.y() * m(0,1) + p.z() * m(0,2),
401 p.x() * m(1,0) + p.y() * m(1,1) + p.z() * m(1,2),
402 p.x() * m(2,0) + p.y() * m(2,1) + p.z() * m(2,2));
403 }
404
405 /// Transform the given 4d vector with the given matrix.
transform(Matrix::Arg m,Vector4::Arg p)406 inline Vector4 transform(Matrix::Arg m, Vector4::Arg p)
407 {
408 return Vector4(
409 p.x() * m(0,0) + p.y() * m(0,1) + p.z() * m(0,2) + p.w() * m(0,3),
410 p.x() * m(1,0) + p.y() * m(1,1) + p.z() * m(1,2) + p.w() * m(1,3),
411 p.x() * m(2,0) + p.y() * m(2,1) + p.z() * m(2,2) + p.w() * m(2,3),
412 p.x() * m(3,0) + p.y() * m(3,1) + p.z() * m(3,2) + p.w() * m(3,3));
413 }
414
mul(Matrix::Arg a,Matrix::Arg b)415 inline Matrix mul(Matrix::Arg a, Matrix::Arg b)
416 {
417 // @@ Is this the right order? mul(a, b) = b * a
418 Matrix m = a;
419 m.apply(b);
420 return m;
421 }
422
423 } // nv namespace
424
425
426
427
428 #if 0
429 /** @name Special matrices. */
430 //@{
431 /** Generate a translation matrix. */
432 void TranslationMatrix(const Vec3 & v) {
433 data[0] = 1; data[1] = 0; data[2] = 0; data[3] = 0;
434 data[4] = 0; data[5] = 1; data[6] = 0; data[7] = 0;
435 data[8] = 0; data[9] = 0; data[10] = 1; data[11] = 0;
436 data[12] = v.x; data[13] = v.y; data[14] = v.z; data[15] = 1;
437 }
438
439 /** Rotate theta degrees around v. */
440 void RotationMatrix( scalar theta, scalar v0, scalar v1, scalar v2 ) {
441 scalar cost = cos(theta);
442 scalar sint = sin(theta);
443
444 if( 1 == v0 && 0 == v1 && 0 == v2 ) {
445 data[0] = 1.0f; data[1] = 0.0f; data[2] = 0.0f; data[3] = 0.0f;
446 data[4] = 0.0f; data[5] = cost; data[6] = -sint;data[7] = 0.0f;
447 data[8] = 0.0f; data[9] = sint; data[10] = cost;data[11] = 0.0f;
448 data[12] = 0.0f;data[13] = 0.0f;data[14] = 0.0f;data[15] = 1.0f;
449 }
450 else if( 0 == v0 && 1 == v1 && 0 == v2 ) {
451 data[0] = cost; data[1] = 0.0f; data[2] = sint; data[3] = 0.0f;
452 data[4] = 0.0f; data[5] = 1.0f; data[6] = 0.0f; data[7] = 0.0f;
453 data[8] = -sint;data[9] = 0.0f;data[10] = cost; data[11] = 0.0f;
454 data[12] = 0.0f;data[13] = 0.0f;data[14] = 0.0f;data[15] = 1.0f;
455 }
456 else if( 0 == v0 && 0 == v1 && 1 == v2 ) {
457 data[0] = cost; data[1] = -sint;data[2] = 0.0f; data[3] = 0.0f;
458 data[4] = sint; data[5] = cost; data[6] = 0.0f; data[7] = 0.0f;
459 data[8] = 0.0f; data[9] = 0.0f; data[10] = 1.0f;data[11] = 0.0f;
460 data[12] = 0.0f;data[13] = 0.0f;data[14] = 0.0f;data[15] = 1.0f;
461 }
462 else {
463 //we need scale a,b,c to unit length.
464 scalar a2, b2, c2;
465 a2 = v0 * v0;
466 b2 = v1 * v1;
467 c2 = v2 * v2;
468
469 scalar iscale = 1.0f / sqrtf(a2 + b2 + c2);
470 v0 *= iscale;
471 v1 *= iscale;
472 v2 *= iscale;
473
474 scalar abm, acm, bcm;
475 scalar mcos, asin, bsin, csin;
476 mcos = 1.0f - cost;
477 abm = v0 * v1 * mcos;
478 acm = v0 * v2 * mcos;
479 bcm = v1 * v2 * mcos;
480 asin = v0 * sint;
481 bsin = v1 * sint;
482 csin = v2 * sint;
483 data[0] = a2 * mcos + cost;
484 data[1] = abm - csin;
485 data[2] = acm + bsin;
486 data[3] = abm + csin;
487 data[4] = 0.0f;
488 data[5] = b2 * mcos + cost;
489 data[6] = bcm - asin;
490 data[7] = acm - bsin;
491 data[8] = 0.0f;
492 data[9] = bcm + asin;
493 data[10] = c2 * mcos + cost;
494 data[11] = 0.0f;
495 data[12] = 0.0f;
496 data[13] = 0.0f;
497 data[14] = 0.0f;
498 data[15] = 1.0f;
499 }
500 }
501
502 /*
503 void SkewMatrix(scalar angle, const Vec3 & v1, const Vec3 & v2) {
504 v1.Normalize();
505 v2.Normalize();
506
507 Vec3 v3;
508 v3.Cross(v1, v2);
509 v3.Normalize();
510
511 // Get skew factor.
512 scalar costheta = Vec3DotProduct(v1, v2);
513 scalar sintheta = Real.Sqrt(1 - costheta * costheta);
514 scalar skew = tan(Trig.DegreesToRadians(angle) + acos(sintheta)) * sintheta - costheta;
515
516 // Build orthonormal matrix.
517 v1 = FXVector3.Cross(v3, v2);
518 v1.Normalize();
519
520 Matrix R = Matrix::Identity;
521 R[0, 0] = v3.X;�// Not sure this is in the correct order...
522 R[1, 0] = v3.Y;
523 R[2, 0] = v3.Z;
524 R[0, 1] = v1.X;
525 R[1, 1] = v1.Y;
526 R[2, 1] = v1.Z;
527 R[0, 2] = v2.X;
528 R[1, 2] = v2.Y;
529 R[2, 2] = v2.Z;
530
531 // Build skew matrix.
532 Matrix S = Matrix::Identity;
533 S[2, 1] = -skew;
534
535 // Return skew transform.
536 return R * S * R.Transpose; // Not sure this is in the correct order...
537 }
538 */
539
540 /**
541 * Generate rotation matrix for the euler angles. This is the same as computing
542 * 3 rotation matrices and multiplying them together in our custom order.
543 *
544 * @todo Have to recompute this code for our new convention.
545 **/
546 void RotationMatrix( scalar yaw, scalar pitch, scalar roll ) {
547 scalar sy = sin(yaw+ToRadian(90));
548 scalar cy = cos(yaw+ToRadian(90));
549 scalar sp = sin(pitch-ToRadian(90));
550 scalar cp = cos(pitch-ToRadian(90));
551 scalar sr = sin(roll);
552 scalar cr = cos(roll);
553
554 data[0] = cr*cy + sr*sp*sy;
555 data[1] = cp*sy;
556 data[2] = -sr*cy + cr*sp*sy;
557 data[3] = 0;
558
559 data[4] = -cr*sy + sr*sp*cy;
560 data[5] = cp*cy;
561 data[6] = sr*sy + cr*sp*cy;
562 data[7] = 0;
563
564 data[8] = sr*cp;
565 data[9] = -sp;
566 data[10] = cr*cp;
567 data[11] = 0;
568
569 data[12] = 0;
570 data[13] = 0;
571 data[14] = 0;
572 data[15] = 1;
573 }
574
575 /** Create a frustum matrix with the far plane at the infinity. */
576 void Frustum( scalar xmin, scalar xmax, scalar ymin, scalar ymax, scalar zNear, scalar zFar ) {
577 scalar one_deltax, one_deltay, one_deltaz, doubleznear;
578
579 doubleznear = 2.0f * zNear;
580 one_deltax = 1.0f / (xmax - xmin);
581 one_deltay = 1.0f / (ymax - ymin);
582 one_deltaz = 1.0f / (zFar - zNear);
583
584 data[0] = (scalar)(doubleznear * one_deltax);
585 data[1] = 0.0f;
586 data[2] = 0.0f;
587 data[3] = 0.0f;
588 data[4] = 0.0f;
589 data[5] = (scalar)(doubleznear * one_deltay);
590 data[6] = 0.f;
591 data[7] = 0.f;
592 data[8] = (scalar)((xmax + xmin) * one_deltax);
593 data[9] = (scalar)((ymax + ymin) * one_deltay);
594 data[10] = (scalar)(-(zFar + zNear) * one_deltaz);
595 data[11] = -1.f;
596 data[12] = 0.f;
597 data[13] = 0.f;
598 data[14] = (scalar)(-(zFar * doubleznear) * one_deltaz);
599 data[15] = 0.f;
600 }
601
602 /** Create a frustum matrix with the far plane at the infinity. */
603 void FrustumInf( scalar xmin, scalar xmax, scalar ymin, scalar ymax, scalar zNear ) {
604 scalar one_deltax, one_deltay, doubleznear, nudge;
605
606 doubleznear = 2.0f * zNear;
607 one_deltax = 1.0f / (xmax - xmin);
608 one_deltay = 1.0f / (ymax - ymin);
609 nudge = 1.0; // 0.999;
610
611 data[0] = doubleznear * one_deltax;
612 data[1] = 0.0f;
613 data[2] = 0.0f;
614 data[3] = 0.0f;
615
616 data[4] = 0.0f;
617 data[5] = doubleznear * one_deltay;
618 data[6] = 0.f;
619 data[7] = 0.f;
620
621 data[8] = (xmax + xmin) * one_deltax;
622 data[9] = (ymax + ymin) * one_deltay;
623 data[10] = -1.0f * nudge;
624 data[11] = -1.0f;
625
626 data[12] = 0.f;
627 data[13] = 0.f;
628 data[14] = -doubleznear * nudge;
629 data[15] = 0.f;
630 }
631
632 /** Create an inverse frustum matrix with the far plane at the infinity. */
633 void FrustumInfInv( scalar left, scalar right, scalar bottom, scalar top, scalar zNear ) {
634 // this matrix is wrong (not tested scalarly) I think it should be transposed.
635 data[0] = (right - left) / (2 * zNear);
636 data[1] = 0;
637 data[2] = 0;
638 data[3] = (right + left) / (2 * zNear);
639 data[4] = 0;
640 data[5] = (top - bottom) / (2 * zNear);
641 data[6] = 0;
642 data[7] = (top + bottom) / (2 * zNear);
643 data[8] = 0;
644 data[9] = 0;
645 data[10] = 0;
646 data[11] = -1;
647 data[12] = 0;
648 data[13] = 0;
649 data[14] = -1 / (2 * zNear);
650 data[15] = 1 / (2 * zNear);
651 }
652
653 /** Create an homogeneous projection matrix. */
654 void Perspective( scalar fov, scalar aspect, scalar zNear, scalar zFar ) {
655 scalar xmin, xmax, ymin, ymax;
656
657 xmax = zNear * tan( fov/2 );
658 xmin = -xmax;
659
660 ymax = xmax / aspect;
661 ymin = -ymax;
662
663 Frustum(xmin, xmax, ymin, ymax, zNear, zFar);
664 }
665
666 /** Create a projection matrix with the far plane at the infinity. */
667 void PerspectiveInf( scalar fov, scalar aspect, scalar zNear ) {
668 scalar x = zNear * tan( fov/2 );
669 scalar y = x / aspect;
670 FrustumInf( -x, x, -y, y, zNear );
671 }
672
673 /** Create an inverse projection matrix with far plane at the infinity. */
674 void PerspectiveInfInv( scalar fov, scalar aspect, scalar zNear ) {
675 scalar x = zNear * tan( fov/2 );
676 scalar y = x / aspect;
677 FrustumInfInv( -x, x, -y, y, zNear );
678 }
679
680 /** Build bone matrix from quatertion and offset. */
681 void BoneMatrix(const Quat & q, const Vec3 & offset) {
682 scalar x2, y2, z2, xx, xy, xz, yy, yz, zz, wx, wy, wz;
683
684 // calculate coefficients
685 x2 = q.x + q.x;
686 y2 = q.y + q.y;
687 z2 = q.z + q.z;
688
689 xx = q.x * x2; xy = q.x * y2; xz = q.x * z2;
690 yy = q.y * y2; yz = q.y * z2; zz = q.z * z2;
691 wx = q.w * x2; wy = q.w * y2; wz = q.w * z2;
692
693 data[0] = 1.0f - (yy + zz);
694 data[1] = xy - wz;
695 data[2] = xz + wy;
696 data[3] = 0.0f;
697
698 data[4] = xy + wz;
699 data[5] = 1.0f - (xx + zz);
700 data[6] = yz - wx;
701 data[7] = 0.0f;
702
703 data[8] = xz - wy;
704 data[9] = yz + wx;
705 data[10] = 1.0f - (xx + yy);
706 data[11] = 0.0f;
707
708 data[12] = offset.x;
709 data[13] = offset.y;
710 data[14] = offset.z;
711 data[15] = 1.0f;
712 }
713
714 //@}
715
716
717 /** @name Transformations: */
718 //@{
719
720 /** Apply a general scale. */
721 void Scale( scalar x, scalar y, scalar z ) {
722 data[0] *= x; data[4] *= y; data[8] *= z;
723 data[1] *= x; data[5] *= y; data[9] *= z;
724 data[2] *= x; data[6] *= y; data[10] *= z;
725 data[3] *= x; data[7] *= y; data[11] *= z;
726 }
727
728 /** Apply a rotation of theta degrees around the axis v*/
729 void Rotate( scalar theta, const Vec3 & v ) {
730 Matrix b;
731 b.RotationMatrix( theta, v[0], v[1], v[2] );
732 Multiply4x3( b );
733 }
734
735 /** Apply a rotation of theta degrees around the axis v*/
736 void Rotate( scalar theta, scalar v0, scalar v1, scalar v2 ) {
737 Matrix b;
738 b.RotationMatrix( theta, v0, v1, v2 );
739 Multiply4x3( b );
740 }
741
742 /**
743 * Translate the matrix by t. This is the same as multiplying by a
744 * translation matrix with the given offset.
745 * this = T * this
746 */
747 void Translate( const Vec3 &t ) {
748 data[12] = data[0] * t.x + data[4] * t.y + data[8] * t.z + data[12];
749 data[13] = data[1] * t.x + data[5] * t.y + data[9] * t.z + data[13];
750 data[14] = data[2] * t.x + data[6] * t.y + data[10] * t.z + data[14];
751 data[15] = data[3] * t.x + data[7] * t.y + data[11] * t.z + data[15];
752 }
753
754 /**
755 * Translate the matrix by x, y, z. This is the same as multiplying by a
756 * translation matrix with the given offsets.
757 */
758 void Translate( scalar x, scalar y, scalar z ) {
759 data[12] = data[0] * x + data[4] * y + data[8] * z + data[12];
760 data[13] = data[1] * x + data[5] * y + data[9] * z + data[13];
761 data[14] = data[2] * x + data[6] * y + data[10] * z + data[14];
762 data[15] = data[3] * x + data[7] * y + data[11] * z + data[15];
763 }
764
765 /** Compute the transposed matrix. */
766 void Transpose() {
767 piSwap(data[1], data[4]);
768 piSwap(data[2], data[8]);
769 piSwap(data[6], data[9]);
770 piSwap(data[3], data[12]);
771 piSwap(data[7], data[13]);
772 piSwap(data[11], data[14]);
773 }
774
775 /** Compute the inverse of a rigid-body/isometry/orthonormal matrix. */
776 void IsometryInverse() {
777 // transposed 3x3 upper left matrix
778 piSwap(data[1], data[4]);
779 piSwap(data[2], data[8]);
780 piSwap(data[6], data[9]);
781
782 // translate by the negative offsets
783 Vec3 v(-data[12], -data[13], -data[14]);
784 data[12] = data[13] = data[14] = 0;
785 Translate(v);
786 }
787
788 /** Compute the inverse of the affine portion of this matrix. */
789 void AffineInverse() {
790 data[12] = data[13] = data[14] = 0;
791 Transpose();
792 }
793 //@}
794
795 /** @name Matrix operations: */
796 //@{
797
798 /** Return the determinant of this matrix. */
799 scalar Determinant() const {
800 return data[0] * data[5] * data[10] * data[15] +
801 data[1] * data[6] * data[11] * data[12] +
802 data[2] * data[7] * data[ 8] * data[13] +
803 data[3] * data[4] * data[ 9] * data[14] -
804 data[3] * data[6] * data[ 9] * data[12] -
805 data[2] * data[5] * data[ 8] * data[15] -
806 data[1] * data[4] * data[11] * data[14] -
807 data[0] * data[7] * data[10] * data[12];
808 }
809
810
811 /** Standard matrix product: this *= B. */
812 void Multiply4x4( const Matrix & restrict B ) {
813 Multiply4x4(*this, B);
814 }
815
816 /** Standard matrix product: this = A * B. this != B*/
817 void Multiply4x4( const Matrix & A, const Matrix & restrict B ) {
818 piDebugCheck(this != &B);
819
820 for(int i = 0; i < 4; i++) {
821 const scalar ai0 = A(i,0), ai1 = A(i,1), ai2 = A(i,2), ai3 = A(i,3);
822 GetElem(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0) + ai3 * B(3,0);
823 GetElem(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1) + ai3 * B(3,1);
824 GetElem(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2) + ai3 * B(3,2);
825 GetElem(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3 * B(3,3);
826 }
827
828 /* Unrolled but does not allow this == A
829 data[0] = A.data[0] * B.data[0] + A.data[4] * B.data[1] + A.data[8] * B.data[2] + A.data[12] * B.data[3];
830 data[1] = A.data[1] * B.data[0] + A.data[5] * B.data[1] + A.data[9] * B.data[2] + A.data[13] * B.data[3];
831 data[2] = A.data[2] * B.data[0] + A.data[6] * B.data[1] + A.data[10] * B.data[2] + A.data[14] * B.data[3];
832 data[3] = A.data[3] * B.data[0] + A.data[7] * B.data[1] + A.data[11] * B.data[2] + A.data[15] * B.data[3];
833 data[4] = A.data[0] * B.data[4] + A.data[4] * B.data[5] + A.data[8] * B.data[6] + A.data[12] * B.data[7];
834 data[5] = A.data[1] * B.data[4] + A.data[5] * B.data[5] + A.data[9] * B.data[6] + A.data[13] * B.data[7];
835 data[6] = A.data[2] * B.data[4] + A.data[6] * B.data[5] + A.data[10] * B.data[6] + A.data[14] * B.data[7];
836 data[7] = A.data[3] * B.data[4] + A.data[7] * B.data[5] + A.data[11] * B.data[6] + A.data[15] * B.data[7];
837 data[8] = A.data[0] * B.data[8] + A.data[4] * B.data[9] + A.data[8] * B.data[10] + A.data[12] * B.data[11];
838 data[9] = A.data[1] * B.data[8] + A.data[5] * B.data[9] + A.data[9] * B.data[10] + A.data[13] * B.data[11];
839 data[10]= A.data[2] * B.data[8] + A.data[6] * B.data[9] + A.data[10] * B.data[10] + A.data[14] * B.data[11];
840 data[11]= A.data[3] * B.data[8] + A.data[7] * B.data[9] + A.data[11] * B.data[10] + A.data[15] * B.data[11];
841 data[12]= A.data[0] * B.data[12] + A.data[4] * B.data[13] + A.data[8] * B.data[14] + A.data[12] * B.data[15];
842 data[13]= A.data[1] * B.data[12] + A.data[5] * B.data[13] + A.data[9] * B.data[14] + A.data[13] * B.data[15];
843 data[14]= A.data[2] * B.data[12] + A.data[6] * B.data[13] + A.data[10] * B.data[14] + A.data[14] * B.data[15];
844 data[15]= A.data[3] * B.data[12] + A.data[7] * B.data[13] + A.data[11] * B.data[14] + A.data[15] * B.data[15];
845 */
846 }
847
848 /** Standard matrix product: this *= B. */
849 void Multiply4x3( const Matrix & restrict B ) {
850 Multiply4x3(*this, B);
851 }
852
853 /** Standard product of matrices, where the last row is [0 0 0 1]. */
854 void Multiply4x3( const Matrix & A, const Matrix & restrict B ) {
855 piDebugCheck(this != &B);
856
857 for(int i = 0; i < 3; i++) {
858 const scalar ai0 = A(i,0), ai1 = A(i,1), ai2 = A(i,2), ai3 = A(i,3);
859 GetElem(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0) + ai3 * B(3,0);
860 GetElem(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1) + ai3 * B(3,1);
861 GetElem(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2) + ai3 * B(3,2);
862 GetElem(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3 * B(3,3);
863 }
864 data[3] = 0.0f; data[7] = 0.0f; data[11] = 0.0f; data[15] = 1.0f;
865
866 /* Unrolled but does not allow this == A
867 data[0] = a.data[0] * b.data[0] + a.data[4] * b.data[1] + a.data[8] * b.data[2] + a.data[12] * b.data[3];
868 data[1] = a.data[1] * b.data[0] + a.data[5] * b.data[1] + a.data[9] * b.data[2] + a.data[13] * b.data[3];
869 data[2] = a.data[2] * b.data[0] + a.data[6] * b.data[1] + a.data[10] * b.data[2] + a.data[14] * b.data[3];
870 data[3] = 0.0f;
871 data[4] = a.data[0] * b.data[4] + a.data[4] * b.data[5] + a.data[8] * b.data[6] + a.data[12] * b.data[7];
872 data[5] = a.data[1] * b.data[4] + a.data[5] * b.data[5] + a.data[9] * b.data[6] + a.data[13] * b.data[7];
873 data[6] = a.data[2] * b.data[4] + a.data[6] * b.data[5] + a.data[10] * b.data[6] + a.data[14] * b.data[7];
874 data[7] = 0.0f;
875 data[8] = a.data[0] * b.data[8] + a.data[4] * b.data[9] + a.data[8] * b.data[10] + a.data[12] * b.data[11];
876 data[9] = a.data[1] * b.data[8] + a.data[5] * b.data[9] + a.data[9] * b.data[10] + a.data[13] * b.data[11];
877 data[10]= a.data[2] * b.data[8] + a.data[6] * b.data[9] + a.data[10] * b.data[10] + a.data[14] * b.data[11];
878 data[11]= 0.0f;
879 data[12]= a.data[0] * b.data[12] + a.data[4] * b.data[13] + a.data[8] * b.data[14] + a.data[12] * b.data[15];
880 data[13]= a.data[1] * b.data[12] + a.data[5] * b.data[13] + a.data[9] * b.data[14] + a.data[13] * b.data[15];
881 data[14]= a.data[2] * b.data[12] + a.data[6] * b.data[13] + a.data[10] * b.data[14] + a.data[14] * b.data[15];
882 data[15]= 1.0f;
883 */
884 }
885 //@}
886
887
888 /** @name Vector operations: */
889 //@{
890
891 /** Transform 3d vector (w=0). */
892 void TransformVec3(const Vec3 & restrict orig, Vec3 * restrict dest) const {
893 piDebugCheck(&orig != dest);
894 dest->x = orig.x * data[0] + orig.y * data[4] + orig.z * data[8];
895 dest->y = orig.x * data[1] + orig.y * data[5] + orig.z * data[9];
896 dest->z = orig.x * data[2] + orig.y * data[6] + orig.z * data[10];
897 }
898 /** Transform 3d vector by the transpose (w=0). */
899 void TransformVec3T(const Vec3 & restrict orig, Vec3 * restrict dest) const {
900 piDebugCheck(&orig != dest);
901 dest->x = orig.x * data[0] + orig.y * data[1] + orig.z * data[2];
902 dest->y = orig.x * data[4] + orig.y * data[5] + orig.z * data[6];
903 dest->z = orig.x * data[8] + orig.y * data[9] + orig.z * data[10];
904 }
905
906 /** Transform a 3d homogeneous vector, where the fourth coordinate is assumed to be 1. */
907 void TransformPoint(const Vec3 & restrict orig, Vec3 * restrict dest) const {
908 piDebugCheck(&orig != dest);
909 dest->x = orig.x * data[0] + orig.y * data[4] + orig.z * data[8] + data[12];
910 dest->y = orig.x * data[1] + orig.y * data[5] + orig.z * data[9] + data[13];
911 dest->z = orig.x * data[2] + orig.y * data[6] + orig.z * data[10] + data[14];
912 }
913
914 /** Transform a point, normalize it, and return w. */
915 scalar TransformPointAndNormalize(const Vec3 & restrict orig, Vec3 * restrict dest) const {
916 piDebugCheck(&orig != dest);
917 scalar w;
918 dest->x = orig.x * data[0] + orig.y * data[4] + orig.z * data[8] + data[12];
919 dest->y = orig.x * data[1] + orig.y * data[5] + orig.z * data[9] + data[13];
920 dest->z = orig.x * data[2] + orig.y * data[6] + orig.z * data[10] + data[14];
921 w = 1 / (orig.x * data[3] + orig.y * data[7] + orig.z * data[11] + data[15]);
922 *dest *= w;
923 return w;
924 }
925
926 /** Transform a point and return w. */
927 scalar TransformPointReturnW(const Vec3 & restrict orig, Vec3 * restrict dest) const {
928 piDebugCheck(&orig != dest);
929 dest->x = orig.x * data[0] + orig.y * data[4] + orig.z * data[8] + data[12];
930 dest->y = orig.x * data[1] + orig.y * data[5] + orig.z * data[9] + data[13];
931 dest->z = orig.x * data[2] + orig.y * data[6] + orig.z * data[10] + data[14];
932 return orig.x * data[3] + orig.y * data[7] + orig.z * data[11] + data[15];
933 }
934
935 /** Transform a normalized 3d point by a 4d matrix and return the resulting 4d vector. */
936 void TransformVec4(const Vec3 & orig, Vec4 * dest) const {
937 dest->x = orig.x * data[0] + orig.y * data[4] + orig.z * data[8] + data[12];
938 dest->y = orig.x * data[1] + orig.y * data[5] + orig.z * data[9] + data[13];
939 dest->z = orig.x * data[2] + orig.y * data[6] + orig.z * data[10] + data[14];
940 dest->w = orig.x * data[3] + orig.y * data[7] + orig.z * data[11] + data[15];
941 }
942 //@}
943
944 /** @name Matrix analysis. */
945 //@{
946
947 /** Get the ZYZ euler angles from the matrix. Assumes the matrix is orthonormal. */
948 void GetEulerAnglesZYZ(scalar * s, scalar * t, scalar * r) const {
949 if( GetElem(2,2) < 1.0f ) {
950 if( GetElem(2,2) > -1.0f ) {
951 // cs*ct*cr-ss*sr -ss*ct*cr-cs*sr st*cr
952 // cs*ct*sr+ss*cr -ss*ct*sr+cs*cr st*sr
953 // -cs*st ss*st ct
954 *s = atan2(GetElem(1,2), -GetElem(0,2));
955 *t = acos(GetElem(2,2));
956 *r = atan2(GetElem(2,1), GetElem(2,0));
957 }
958 else {
959 // -c(s-r) s(s-r) 0
960 // s(s-r) c(s-r) 0
961 // 0 0 -1
962 *s = atan2(GetElem(0, 1), -GetElem(0, 0)); // = s-r
963 *t = PI;
964 *r = 0;
965 }
966 }
967 else {
968 // c(s+r) -s(s+r) 0
969 // s(s+r) c(s+r) 0
970 // 0 0 1
971 *s = atan2(GetElem(0, 1), GetElem(0, 0)); // = s+r
972 *t = 0;
973 *r = 0;
974 }
975 }
976
977 //@}
978
979 MATHLIB_API friend PiStream & operator<< ( PiStream & s, Matrix & m );
980
981 /** Print to debug output. */
982 void Print() const {
983 piDebug( "[ %5.2f %5.2f %5.2f %5.2f ]\n", data[0], data[4], data[8], data[12] );
984 piDebug( "[ %5.2f %5.2f %5.2f %5.2f ]\n", data[1], data[5], data[9], data[13] );
985 piDebug( "[ %5.2f %5.2f %5.2f %5.2f ]\n", data[2], data[6], data[10], data[14] );
986 piDebug( "[ %5.2f %5.2f %5.2f %5.2f ]\n", data[3], data[7], data[11], data[15] );
987 }
988
989
990 public:
991
992 scalar data[16];
993
994 };
995 #endif
996
997
998
999
1000 #endif // NV_MATH_MATRIX_H
1001