OpenVDB  0.104.0
Mat4.h
Go to the documentation of this file.
1 
2 //
3 // Copyright (c) 2012 DreamWorks Animation LLC
4 //
5 // All rights reserved. This software is distributed under the
6 // Mozilla Public License 2.0 ( http://www.mozilla.org/MPL/2.0/ )
7 //
8 // Redistributions of source code must retain the above copyright
9 // and license notice and the following restrictions and disclaimer.
10 //
11 // * Neither the name of DreamWorks Animation nor the names of
12 // its contributors may be used to endorse or promote products derived
13 // from this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
16 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
17 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
18 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
21 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
22 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
23 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 // IN NO EVENT SHALL THE COPYRIGHT HOLDERS' AND CONTRIBUTORS' AGGREGATE
27 // LIABILITY FOR ALL CLAIMS REGARDLESS OF THEIR BASIS EXCEED US$250.00.
28 //
30 
31 #ifndef OPENVDB_MATH_MAT4_H_HAS_BEEN_INCLUDED
32 #define OPENVDB_MATH_MAT4_H_HAS_BEEN_INCLUDED
33 
34 #include <openvdb/Exceptions.h>
35 #include <openvdb/Platform.h>
36 #include <iomanip>
37 #include <assert.h>
38 #include <math.h>
39 #include <algorithm>
40 #include "Math.h"
41 #include "Mat3.h"
42 #include "Vec3.h"
43 #include "Vec4.h"
44 
45 
46 namespace openvdb {
48 namespace OPENVDB_VERSION_NAME {
49 namespace math {
50 
51 template<typename T> class Vec4;
52 
53 
56 template<typename T>
57 class Mat4: public Mat<4, T>
58 {
59 public:
61  typedef T value_type;
62  typedef T ValueType;
63  typedef Mat<4, T> MyBase;
64 
66  Mat4() {}
67 
69 
75  template<typename Source>
76  Mat4(Source *a)
77  {
78  register int i;
79 
80  for (i = 0; i < 16; i++) {
81  MyBase::mm[i] = a[i];
82  }
83  }
84 
86 
92  template<typename Source>
93  Mat4(Source a, Source b, Source c, Source d,
94  Source e, Source f, Source g, Source h,
95  Source i, Source j, Source k, Source l,
96  Source m, Source n, Source o, Source p)
97  {
98  MyBase::mm[ 0] = a;
99  MyBase::mm[ 1] = b;
100  MyBase::mm[ 2] = c;
101  MyBase::mm[ 3] = d;
102 
103  MyBase::mm[ 4] = e;
104  MyBase::mm[ 5] = f;
105  MyBase::mm[ 6] = g;
106  MyBase::mm[ 7] = h;
107 
108  MyBase::mm[ 8] = i;
109  MyBase::mm[ 9] = j;
110  MyBase::mm[10] = k;
111  MyBase::mm[11] = l;
112 
113  MyBase::mm[12] = m;
114  MyBase::mm[13] = n;
115  MyBase::mm[14] = o;
116  MyBase::mm[15] = p;
117  }
118 
120  template<typename Source>
121  Mat4(const Vec4<Source> &v1, const Vec4<Source> &v2,
122  const Vec4<Source> &v3, const Vec4<Source> &v4)
123  {
124  setBasis(v1, v2, v3, v4);
125  }
126 
128  Mat4(const Mat<4, T> &m)
129  {
130  for (int i = 0; i < 4; ++i) {
131  for (int j = 0; j < 4; ++j) {
132  MyBase::mm[i*4 + j] = m[i][j];
133  }
134  }
135  }
136 
138  template<typename Source>
139  explicit Mat4(const Mat4<Source> &m)
140  {
141  const Source *src = m.asPointer();
142 
143  for (int i=0; i<16; ++i) {
144  MyBase::mm[i] = static_cast<T>(src[i]);
145  }
146  }
147 
149  static const Mat4<T>& identity() {
150  return sIdentity;
151  }
152 
154  static const Mat4<T>& zero() {
155  return sZero;
156  }
157 
159  void setRow(int i, const Vec4<T> &v)
160  {
161  // assert(i>=0 && i<4);
162  int i4 = i * 4;
163  MyBase::mm[i4+0] = v[0];
164  MyBase::mm[i4+1] = v[1];
165  MyBase::mm[i4+2] = v[2];
166  MyBase::mm[i4+3] = v[3];
167  }
168 
170  Vec4<T> row(int i) const
171  {
172  // assert(i>=0 && i<3);
173  return Vec4<T>((*this)(i,0), (*this)(i,1), (*this)(i,2), (*this)(i,3));
174  }
175 
177  void setCol(int j, const Vec4<T>& v)
178  {
179  // assert(j>=0 && j<4);
180  MyBase::mm[ 0+j] = v[0];
181  MyBase::mm[ 4+j] = v[1];
182  MyBase::mm[ 8+j] = v[2];
183  MyBase::mm[12+j] = v[3];
184  }
185 
187  Vec4<T> col(int j) const
188  {
189  // assert(j>=0 && j<4);
190  return Vec4<T>((*this)(0,j), (*this)(1,j), (*this)(2,j), (*this)(3,j));
191  }
192 
194 
195 
196  T* operator[](int i) { return &(MyBase::mm[i<<2]); }
197  const T* operator[](int i) const { return &(MyBase::mm[i<<2]); }
199 
201  T* asPointer() {return MyBase::mm;}
202  const T* asPointer() const {return MyBase::mm;}
203 
207  T& operator()(int i, int j)
208  {
209  // assert(i>=0 && i<4);
210  // assert(j>=0 && j<4);
211  return MyBase::mm[4*i+j];
212  }
213 
217  T operator()(int i, int j) const
218  {
219  // assert(i>=0 && i<4);
220  // assert(j>=0 && j<4);
221  return MyBase::mm[4*i+j];
222  }
223 
225  void setBasis(const Vec4<T> &v1, const Vec4<T> &v2,
226  const Vec4<T> &v3, const Vec4<T> &v4)
227  {
228  MyBase::mm[ 0] = v1[0];
229  MyBase::mm[ 1] = v1[1];
230  MyBase::mm[ 2] = v1[2];
231  MyBase::mm[ 3] = v1[3];
232 
233  MyBase::mm[ 4] = v2[0];
234  MyBase::mm[ 5] = v2[1];
235  MyBase::mm[ 6] = v2[2];
236  MyBase::mm[ 7] = v2[3];
237 
238  MyBase::mm[ 8] = v3[0];
239  MyBase::mm[ 9] = v3[1];
240  MyBase::mm[10] = v3[2];
241  MyBase::mm[11] = v3[3];
242 
243  MyBase::mm[12] = v4[0];
244  MyBase::mm[13] = v4[1];
245  MyBase::mm[14] = v4[2];
246  MyBase::mm[15] = v4[3];
247  }
248 
249 
250  // Set "this" matrix to zero
251  void setZero()
252  {
253  MyBase::mm[ 0] = 0;
254  MyBase::mm[ 1] = 0;
255  MyBase::mm[ 2] = 0;
256  MyBase::mm[ 3] = 0;
257  MyBase::mm[ 4] = 0;
258  MyBase::mm[ 5] = 0;
259  MyBase::mm[ 6] = 0;
260  MyBase::mm[ 7] = 0;
261  MyBase::mm[ 8] = 0;
262  MyBase::mm[ 9] = 0;
263  MyBase::mm[10] = 0;
264  MyBase::mm[11] = 0;
265  MyBase::mm[12] = 0;
266  MyBase::mm[13] = 0;
267  MyBase::mm[14] = 0;
268  MyBase::mm[15] = 0;
269  }
270 
272  void setIdentity()
273  {
274  MyBase::mm[ 0] = 1;
275  MyBase::mm[ 1] = 0;
276  MyBase::mm[ 2] = 0;
277  MyBase::mm[ 3] = 0;
278 
279  MyBase::mm[ 4] = 0;
280  MyBase::mm[ 5] = 1;
281  MyBase::mm[ 6] = 0;
282  MyBase::mm[ 7] = 0;
283 
284  MyBase::mm[ 8] = 0;
285  MyBase::mm[ 9] = 0;
286  MyBase::mm[10] = 1;
287  MyBase::mm[11] = 0;
288 
289  MyBase::mm[12] = 0;
290  MyBase::mm[13] = 0;
291  MyBase::mm[14] = 0;
292  MyBase::mm[15] = 1;
293  }
294 
295 
297  void setMat3(const Mat3<T> &m)
298  {
299  for (int i = 0; i < 3; i++)
300  for (int j=0; j < 3; j++)
301  MyBase::mm[i*4+j] = m[i][j];
302  }
303 
304  Mat3<T> getMat3() const
305  {
306  Mat3<T> m;
307 
308  for (int i = 0; i < 3; i++)
309  for (int j = 0; j < 3; j++)
310  m[i][j] = MyBase::mm[i*4+j];
311 
312  return m;
313  }
314 
316  Vec3<T> getTranslation() const
317  {
318  return Vec3<T>(MyBase::mm[12], MyBase::mm[13], MyBase::mm[14]);
319  }
320 
321  void setTranslation(const Vec3<T> &t)
322  {
323  MyBase::mm[12] = t[0];
324  MyBase::mm[13] = t[1];
325  MyBase::mm[14] = t[2];
326  }
327 
329  template<typename Source>
330  const Mat4& operator=(const Mat4<Source> &m)
331  {
332  const Source *src = m.asPointer();
333 
334  // don't suppress warnings when assigning from different numerical types
335  std::copy(src, (src + this->numElements()), MyBase::mm);
336  return *this;
337  }
338 
340  bool eq(const Mat4 &m, T eps=1.0e-8) const
341  {
342  for (int i = 0; i < 16; i++) {
343  if (!isApproxEqual(MyBase::mm[i], m.mm[i], eps))
344  return false;
345  }
346  return true;
347  }
348 
351  {
352  return Mat4<T>(
353  -MyBase::mm[ 0], -MyBase::mm[ 1], -MyBase::mm[ 2], -MyBase::mm[ 3],
354  -MyBase::mm[ 4], -MyBase::mm[ 5], -MyBase::mm[ 6], -MyBase::mm[ 7],
355  -MyBase::mm[ 8], -MyBase::mm[ 9], -MyBase::mm[10], -MyBase::mm[11],
356  -MyBase::mm[12], -MyBase::mm[13], -MyBase::mm[14], -MyBase::mm[15]
357  );
358  } // trivial
359 
361  template <typename S>
362  const Mat4<T>& operator*=(S scalar)
363  {
364  MyBase::mm[ 0] *= scalar;
365  MyBase::mm[ 1] *= scalar;
366  MyBase::mm[ 2] *= scalar;
367  MyBase::mm[ 3] *= scalar;
368 
369  MyBase::mm[ 4] *= scalar;
370  MyBase::mm[ 5] *= scalar;
371  MyBase::mm[ 6] *= scalar;
372  MyBase::mm[ 7] *= scalar;
373 
374  MyBase::mm[ 8] *= scalar;
375  MyBase::mm[ 9] *= scalar;
376  MyBase::mm[10] *= scalar;
377  MyBase::mm[11] *= scalar;
378 
379  MyBase::mm[12] *= scalar;
380  MyBase::mm[13] *= scalar;
381  MyBase::mm[14] *= scalar;
382  MyBase::mm[15] *= scalar;
383  return *this;
384  }
385 
387  template <typename S>
388  const Mat4<T> &operator+=(const Mat4<S> &m1)
389  {
390  const S* s = m1.asPointer();
391 
392  MyBase::mm[ 0] += s[ 0];
393  MyBase::mm[ 1] += s[ 1];
394  MyBase::mm[ 2] += s[ 2];
395  MyBase::mm[ 3] += s[ 3];
396 
397  MyBase::mm[ 4] += s[ 4];
398  MyBase::mm[ 5] += s[ 5];
399  MyBase::mm[ 6] += s[ 6];
400  MyBase::mm[ 7] += s[ 7];
401 
402  MyBase::mm[ 8] += s[ 8];
403  MyBase::mm[ 9] += s[ 9];
404  MyBase::mm[10] += s[10];
405  MyBase::mm[11] += s[11];
406 
407  MyBase::mm[12] += s[12];
408  MyBase::mm[13] += s[13];
409  MyBase::mm[14] += s[14];
410  MyBase::mm[15] += s[15];
411 
412  return *this;
413  }
414 
416  template <typename S>
417  const Mat4<T> &operator-=(const Mat4<S> &m1)
418  {
419  const S* s = m1.asPointer();
420 
421  MyBase::mm[ 0] -= s[ 0];
422  MyBase::mm[ 1] -= s[ 1];
423  MyBase::mm[ 2] -= s[ 2];
424  MyBase::mm[ 3] -= s[ 3];
425 
426  MyBase::mm[ 4] -= s[ 4];
427  MyBase::mm[ 5] -= s[ 5];
428  MyBase::mm[ 6] -= s[ 6];
429  MyBase::mm[ 7] -= s[ 7];
430 
431  MyBase::mm[ 8] -= s[ 8];
432  MyBase::mm[ 9] -= s[ 9];
433  MyBase::mm[10] -= s[10];
434  MyBase::mm[11] -= s[11];
435 
436  MyBase::mm[12] -= s[12];
437  MyBase::mm[13] -= s[13];
438  MyBase::mm[14] -= s[14];
439  MyBase::mm[15] -= s[15];
440 
441  return *this;
442  }
443 
445  template <typename S>
446  const Mat4<T> &operator*=(const Mat4<S> &m1)
447  {
448  Mat4<T> m0(*this);
449 
450  const T* s0 = m0.asPointer();
451  const S* s1 = m1.asPointer();
452 
453  for (int i = 0; i < 4; i++) {
454  int i4 = 4 * i;
455  MyBase::mm[i4+0] = static_cast<T>(s0[i4+0] * s1[ 0] +
456  s0[i4+1] * s1[ 4] +
457  s0[i4+2] * s1[ 8] +
458  s0[i4+3] * s1[12]);
459 
460  MyBase::mm[i4+1] = static_cast<T>(s0[i4+0] * s1[ 1] +
461  s0[i4+1] * s1[ 5] +
462  s0[i4+2] * s1[ 9] +
463  s0[i4+3] * s1[13]);
464 
465  MyBase::mm[i4+2] = static_cast<T>(s0[i4+0] * s1[ 2] +
466  s0[i4+1] * s1[ 6] +
467  s0[i4+2] * s1[10] +
468  s0[i4+3] * s1[14]);
469 
470  MyBase::mm[i4+3] = static_cast<T>(s0[i4+0] * s1[ 3] +
471  s0[i4+1] * s1[ 7] +
472  s0[i4+2] * s1[11] +
473  s0[i4+3] * s1[15]);
474  }
475  return *this;
476  }
477 
479  Mat4 transpose() const
480  {
481  return Mat4<T>(
482  MyBase::mm[ 0], MyBase::mm[ 4], MyBase::mm[ 8], MyBase::mm[12],
483  MyBase::mm[ 1], MyBase::mm[ 5], MyBase::mm[ 9], MyBase::mm[13],
484  MyBase::mm[ 2], MyBase::mm[ 6], MyBase::mm[10], MyBase::mm[14],
485  MyBase::mm[ 3], MyBase::mm[ 7], MyBase::mm[11], MyBase::mm[15]
486  );
487  }
488 
489 
492  Mat4 inverse(T tolerance = 0) const
493  {
494  //
495  // inv [ A | b ] = [ E | f ] A: 3x3, b: 3x1, c': 1x3 d: 1x1
496  // [ c' | d ] [ g' | h ]
497  //
498  // If A is invertible use
499  //
500  // E = A^-1 + p*h*r
501  // p = A^-1 * b
502  // f = -p * h
503  // g' = -h * c'
504  // h = 1 / (d - c'*p)
505  // r' = c'*A^-1
506  //
507  // Otherwise use gauss-jordan elimination
508  //
509 
510  //
511  // We create this alias to ourself so we can easily use own subscript
512  // operator.
513  const Mat4<T>& m(*this);
514 
515  T m0011 = m[0][0] * m[1][1];
516  T m0012 = m[0][0] * m[1][2];
517  T m0110 = m[0][1] * m[1][0];
518  T m0210 = m[0][2] * m[1][0];
519  T m0120 = m[0][1] * m[2][0];
520  T m0220 = m[0][2] * m[2][0];
521 
522  T detA = m0011 * m[2][2] - m0012 * m[2][1] - m0110 * m[2][2]
523  + m0210 * m[2][1] + m0120 * m[1][2] - m0220 * m[1][1];
524 
525  bool hasPerspective =
526  (!isExactlyEqual(m[0][3], T(0.0)) ||
527  !isExactlyEqual(m[1][3], T(0.0)) ||
528  !isExactlyEqual(m[2][3], T(0.0)) ||
529  !isExactlyEqual(m[3][3], T(1.0)));
530 
531  T det;
532  if (hasPerspective) {
533  det = m[0][3] * det3(m, 1,2,3, 0,2,1)
534  + m[1][3] * det3(m, 2,0,3, 0,2,1)
535  + m[2][3] * det3(m, 3,0,1, 0,2,1)
536  + m[3][3] * detA;
537  } else {
538  det = detA * m[3][3];
539  }
540 
541  Mat4<T> inv;
542  bool invertible;
543 
544  if (isApproxEqual(det,T(0.0),tolerance)) {
545  invertible = false;
546 
547  } else if (isApproxEqual(detA,T(0.0),T(1e-8))) {
548  // det is too small to rely on inversion by subblocks
549  invertible = m.invert(inv, tolerance);
550 
551  } else {
552  invertible = true;
553  detA = 1.0 / detA;
554 
555  //
556  // Calculate A^-1
557  //
558  inv[0][0] = detA * ( m[1][1] * m[2][2] - m[1][2] * m[2][1]);
559  inv[0][1] = detA * (-m[0][1] * m[2][2] + m[0][2] * m[2][1]);
560  inv[0][2] = detA * ( m[0][1] * m[1][2] - m[0][2] * m[1][1]);
561 
562  inv[1][0] = detA * (-m[1][0] * m[2][2] + m[1][2] * m[2][0]);
563  inv[1][1] = detA * ( m[0][0] * m[2][2] - m0220);
564  inv[1][2] = detA * ( m0210 - m0012);
565 
566  inv[2][0] = detA * ( m[1][0] * m[2][1] - m[1][1] * m[2][0]);
567  inv[2][1] = detA * ( m0120 - m[0][0] * m[2][1]);
568  inv[2][2] = detA * ( m0011 - m0110);
569 
570  if (hasPerspective) {
571  //
572  // Calculate r, p, and h
573  //
574  Vec3<T> r;
575  r[0] = m[3][0] * inv[0][0] + m[3][1] * inv[1][0]
576  + m[3][2] * inv[2][0];
577  r[1] = m[3][0] * inv[0][1] + m[3][1] * inv[1][1]
578  + m[3][2] * inv[2][1];
579  r[2] = m[3][0] * inv[0][2] + m[3][1] * inv[1][2]
580  + m[3][2] * inv[2][2];
581 
582  Vec3<T> p;
583  p[0] = inv[0][0] * m[0][3] + inv[0][1] * m[1][3]
584  + inv[0][2] * m[2][3];
585  p[1] = inv[1][0] * m[0][3] + inv[1][1] * m[1][3]
586  + inv[1][2] * m[2][3];
587  p[2] = inv[2][0] * m[0][3] + inv[2][1] * m[1][3]
588  + inv[2][2] * m[2][3];
589 
590  T h = m[3][3] - p.dot(Vec3<T>(m[3][0],m[3][1],m[3][2]));
591  if (isApproxEqual(h,T(0.0),tolerance)) {
592  invertible = false;
593 
594  } else {
595  h = 1.0 / h;
596 
597  //
598  // Calculate h, g, and f
599  //
600  inv[3][3] = h;
601  inv[3][0] = -h * r[0];
602  inv[3][1] = -h * r[1];
603  inv[3][2] = -h * r[2];
604 
605  inv[0][3] = -h * p[0];
606  inv[1][3] = -h * p[1];
607  inv[2][3] = -h * p[2];
608 
609  //
610  // Calculate E
611  //
612  p *= h;
613  inv[0][0] += p[0] * r[0];
614  inv[0][1] += p[0] * r[1];
615  inv[0][2] += p[0] * r[2];
616  inv[1][0] += p[1] * r[0];
617  inv[1][1] += p[1] * r[1];
618  inv[1][2] += p[1] * r[2];
619  inv[2][0] += p[2] * r[0];
620  inv[2][1] += p[2] * r[1];
621  inv[2][2] += p[2] * r[2];
622  }
623  } else {
624  // Equations are much simpler in the non-perspective case
625  inv[3][0] = - (m[3][0] * inv[0][0] + m[3][1] * inv[1][0]
626  + m[3][2] * inv[2][0]);
627  inv[3][1] = - (m[3][0] * inv[0][1] + m[3][1] * inv[1][1]
628  + m[3][2] * inv[2][1]);
629  inv[3][2] = - (m[3][0] * inv[0][2] + m[3][1] * inv[1][2]
630  + m[3][2] * inv[2][2]);
631  inv[0][3] = 0.0;
632  inv[1][3] = 0.0;
633  inv[2][3] = 0.0;
634  inv[3][3] = 1.0;
635  }
636  }
637 
638  if (!invertible) OPENVDB_THROW(ArithmeticError, "Inversion of singular 4x4 matrix");
639  return inv;
640  }
641 
642 
644  T det() const
645  {
646  const T *ap;
647  Mat3<T> submat;
648  T det;
649  T *sp;
650  int i, j, k, sign;
651 
652  det = 0;
653  sign = 1;
654  for (i = 0; i < 4; i++) {
655  ap = &MyBase::mm[ 0];
656  sp = submat.asPointer();
657  for (j = 0; j < 4; j++) {
658  for (k = 0; k < 4; k++) {
659  if ((k != i) && (j != 0)) {
660  *sp++ = *ap;
661  }
662  ap++;
663  }
664  }
665 
666  det += sign * MyBase::mm[i] * submat.det();
667  sign = -sign;
668  }
669 
670  return det;
671  }
672 
677  Mat4 snapBasis(Axis axis, const Vec3<T> &direction)
678  {return snapBasis(*this, axis, direction);}
679 
681  static Mat4 translation(const Vec3d& v)
682  {
683  return Mat4(
684  T(1), T(0), T(0), T(0),
685  T(0), T(1), T(0), T(0),
686  T(0), T(0), T(1), T(0),
687  T(v.x()), T(v.y()),T(v.z()), T(1));
688  }
689 
691  template <typename T0>
692  void setToTranslation(const Vec3<T0>& v)
693  {
694  MyBase::mm[ 0] = 1;
695  MyBase::mm[ 1] = 0;
696  MyBase::mm[ 2] = 0;
697  MyBase::mm[ 3] = 0;
698 
699  MyBase::mm[ 4] = 0;
700  MyBase::mm[ 5] = 1;
701  MyBase::mm[ 6] = 0;
702  MyBase::mm[ 7] = 0;
703 
704  MyBase::mm[ 8] = 0;
705  MyBase::mm[ 9] = 0;
706  MyBase::mm[10] = 1;
707  MyBase::mm[11] = 0;
708 
709  MyBase::mm[12] = v.x();
710  MyBase::mm[13] = v.y();
711  MyBase::mm[14] = v.z();
712  MyBase::mm[15] = 1;
713  }
714 
716  template <typename T0>
717  void preTranslate(const Vec3<T0>& tr)
718  {
719  Vec3<T> tmp(tr.x(), tr.y(), tr.z());
720  Mat4<T> Tr = Mat4<T>::translation(tmp);
721 
722  *this = Tr * (*this);
723 
724  }
725 
727  template <typename T0>
728  void postTranslate(const Vec3<T0>& tr)
729  {
730  Vec3<T> tmp(tr.x(), tr.y(), tr.z());
731  Mat4<T> Tr = Mat4<T>::translation(tmp);
732 
733  *this = (*this) * Tr;
734 
735  }
736 
737 
739  template <typename T0>
740  void setToScale(const Vec3<T0>& v)
741  {
742  this->setIdentity();
743  MyBase::mm[ 0] = v.x();
744  MyBase::mm[ 5] = v.y();
745  MyBase::mm[10] = v.z();
746  }
747 
748  // Left multiples by the specified scale matrix, i.e. Sc * (*this)
749  template <typename T0>
750  void preScale(const Vec3<T0>& v)
751  {
752  MyBase::mm[ 0] *= v.x();
753  MyBase::mm[ 1] *= v.x();
754  MyBase::mm[ 2] *= v.x();
755  MyBase::mm[ 3] *= v.x();
756 
757  MyBase::mm[ 4] *= v.y();
758  MyBase::mm[ 5] *= v.y();
759  MyBase::mm[ 6] *= v.y();
760  MyBase::mm[ 7] *= v.y();
761 
762  MyBase::mm[ 8] *= v.z();
763  MyBase::mm[ 9] *= v.z();
764  MyBase::mm[10] *= v.z();
765  MyBase::mm[11] *= v.z();
766  }
767 
768 
769 
770  // Right multiples by the specified scale matrix, i.e. (*this) * Sc
771  template <typename T0>
772  void postScale(const Vec3<T0>& v)
773  {
774 
775  MyBase::mm[ 0] *= v.x();
776  MyBase::mm[ 1] *= v.x();
777  MyBase::mm[ 2] *= v.x();
778 
779  MyBase::mm[ 4] *= v.y();
780  MyBase::mm[ 5] *= v.y();
781  MyBase::mm[ 6] *= v.y();
782 
783  MyBase::mm[ 8] *= v.z();
784  MyBase::mm[ 9] *= v.z();
785  MyBase::mm[10] *= v.z();
786 
787  MyBase::mm[12] *= v.x();
788  MyBase::mm[13] *= v.y();
789  MyBase::mm[14] *= v.z();
790 
791  }
792 
793 
797  void setToRotation(Axis axis, T angle) {*this = rotation<Mat4<T> >(axis, angle);}
798 
802  void setToRotation(const Vec3<T>& axis, T angle) {*this = rotation<Mat4<T> >(axis, angle);}
803 
806  void setToRotation(const Vec3<T>& v1, const Vec3<T>& v2) {*this = rotation<Mat4<T> >(v1, v2);}
807 
808 
812  void preRotate(Axis axis, T angle)
813  {
814  T c = static_cast<T>(cos(angle));
815  T s = -static_cast<T>(sin(angle)); // the "-" makes it clockwise
816 
817  switch (axis) {
818  case X_AXIS:
819  {
820  T a4, a5, a6, a7;
821 
822  a4 = c * MyBase::mm[ 4] - s * MyBase::mm[ 8];
823  a5 = c * MyBase::mm[ 5] - s * MyBase::mm[ 9];
824  a6 = c * MyBase::mm[ 6] - s * MyBase::mm[10];
825  a7 = c * MyBase::mm[ 7] - s * MyBase::mm[11];
826 
827 
828  MyBase::mm[ 8] = s * MyBase::mm[ 4] + c * MyBase::mm[ 8];
829  MyBase::mm[ 9] = s * MyBase::mm[ 5] + c * MyBase::mm[ 9];
830  MyBase::mm[10] = s * MyBase::mm[ 6] + c * MyBase::mm[10];
831  MyBase::mm[11] = s * MyBase::mm[ 7] + c * MyBase::mm[11];
832 
833  MyBase::mm[ 4] = a4;
834  MyBase::mm[ 5] = a5;
835  MyBase::mm[ 6] = a6;
836  MyBase::mm[ 7] = a7;
837  }
838  break;
839 
840  case Y_AXIS:
841  {
842  T a0, a1, a2, a3;
843 
844  a0 = c * MyBase::mm[ 0] + s * MyBase::mm[ 8];
845  a1 = c * MyBase::mm[ 1] + s * MyBase::mm[ 9];
846  a2 = c * MyBase::mm[ 2] + s * MyBase::mm[10];
847  a3 = c * MyBase::mm[ 3] + s * MyBase::mm[11];
848 
849  MyBase::mm[ 8] = -s * MyBase::mm[ 0] + c * MyBase::mm[ 8];
850  MyBase::mm[ 9] = -s * MyBase::mm[ 1] + c * MyBase::mm[ 9];
851  MyBase::mm[10] = -s * MyBase::mm[ 2] + c * MyBase::mm[10];
852  MyBase::mm[11] = -s * MyBase::mm[ 3] + c * MyBase::mm[11];
853 
854 
855  MyBase::mm[ 0] = a0;
856  MyBase::mm[ 1] = a1;
857  MyBase::mm[ 2] = a2;
858  MyBase::mm[ 3] = a3;
859  }
860  break;
861 
862  case Z_AXIS:
863  {
864  T a0, a1, a2, a3;
865 
866  a0 = c * MyBase::mm[ 0] - s * MyBase::mm[ 4];
867  a1 = c * MyBase::mm[ 1] - s * MyBase::mm[ 5];
868  a2 = c * MyBase::mm[ 2] - s * MyBase::mm[ 6];
869  a3 = c * MyBase::mm[ 3] - s * MyBase::mm[ 7];
870 
871  MyBase::mm[ 4] = s * MyBase::mm[ 0] + c * MyBase::mm[ 4];
872  MyBase::mm[ 5] = s * MyBase::mm[ 1] + c * MyBase::mm[ 5];
873  MyBase::mm[ 6] = s * MyBase::mm[ 2] + c * MyBase::mm[ 6];
874  MyBase::mm[ 7] = s * MyBase::mm[ 3] + c * MyBase::mm[ 7];
875 
876  MyBase::mm[ 0] = a0;
877  MyBase::mm[ 1] = a1;
878  MyBase::mm[ 2] = a2;
879  MyBase::mm[ 3] = a3;
880  }
881  break;
882 
883  default:
884  assert(axis==X_AXIS || axis==Y_AXIS || axis==Z_AXIS);
885  }
886  }
887 
888 
892  void postRotate(Axis axis, T angle)
893  {
894  T c = static_cast<T>(cos(angle));
895  T s = -static_cast<T>(sin(angle)); // the "-" makes it clockwise
896 
897 
898 
899  switch (axis) {
900  case X_AXIS:
901  {
902  T a2, a6, a10, a14;
903 
904  a2 = c * MyBase::mm[ 2] - s * MyBase::mm[ 1];
905  a6 = c * MyBase::mm[ 6] - s * MyBase::mm[ 5];
906  a10 = c * MyBase::mm[10] - s * MyBase::mm[ 9];
907  a14 = c * MyBase::mm[14] - s * MyBase::mm[13];
908 
909 
910  MyBase::mm[ 1] = c * MyBase::mm[ 1] + s * MyBase::mm[ 2];
911  MyBase::mm[ 5] = c * MyBase::mm[ 5] + s * MyBase::mm[ 6];
912  MyBase::mm[ 9] = c * MyBase::mm[ 9] + s * MyBase::mm[10];
913  MyBase::mm[13] = c * MyBase::mm[13] + s * MyBase::mm[14];
914 
915  MyBase::mm[ 2] = a2;
916  MyBase::mm[ 6] = a6;
917  MyBase::mm[10] = a10;
918  MyBase::mm[14] = a14;
919  }
920  break;
921 
922  case Y_AXIS:
923  {
924  T a2, a6, a10, a14;
925 
926  a2 = c * MyBase::mm[ 2] + s * MyBase::mm[ 0];
927  a6 = c * MyBase::mm[ 6] + s * MyBase::mm[ 4];
928  a10 = c * MyBase::mm[10] + s * MyBase::mm[ 8];
929  a14 = c * MyBase::mm[14] + s * MyBase::mm[12];
930 
931  MyBase::mm[ 0] = c * MyBase::mm[ 0] - s * MyBase::mm[ 2];
932  MyBase::mm[ 4] = c * MyBase::mm[ 4] - s * MyBase::mm[ 6];
933  MyBase::mm[ 8] = c * MyBase::mm[ 8] - s * MyBase::mm[10];
934  MyBase::mm[12] = c * MyBase::mm[12] - s * MyBase::mm[14];
935 
936  MyBase::mm[ 2] = a2;
937  MyBase::mm[ 6] = a6;
938  MyBase::mm[10] = a10;
939  MyBase::mm[14] = a14;
940  }
941  break;
942 
943  case Z_AXIS:
944  {
945  T a1, a5, a9, a13;
946 
947  a1 = c * MyBase::mm[ 1] - s * MyBase::mm[ 0];
948  a5 = c * MyBase::mm[ 5] - s * MyBase::mm[ 4];
949  a9 = c * MyBase::mm[ 9] - s * MyBase::mm[ 8];
950  a13 = c * MyBase::mm[13] - s * MyBase::mm[12];
951 
952  MyBase::mm[ 0] = c * MyBase::mm[ 0] + s * MyBase::mm[ 1];
953  MyBase::mm[ 4] = c * MyBase::mm[ 4] + s * MyBase::mm[ 5];
954  MyBase::mm[ 8] = c * MyBase::mm[ 8] + s * MyBase::mm[ 9];
955  MyBase::mm[12] = c * MyBase::mm[12] + s * MyBase::mm[13];
956 
957  MyBase::mm[ 1] = a1;
958  MyBase::mm[ 5] = a5;
959  MyBase::mm[ 9] = a9;
960  MyBase::mm[13] = a13;
961 
962  }
963  break;
964 
965  default:
966  assert(axis==X_AXIS || axis==Y_AXIS || axis==Z_AXIS);
967  }
968  }
969 
974  void setToShear(Axis axis0, Axis axis1, T shearby)
975  {
976  *this = shear<Mat4<T> >(axis0, axis1, shearby);
977  }
978 
979 
982  void preShear(Axis axis0, Axis axis1, T shear)
983  {
984  int index0 = static_cast<int>(axis0);
985  int index1 = static_cast<int>(axis1);
986 
987  // to row "index1" add a multiple of the index0 row
988  MyBase::mm[index1 * 4 + 0] += shear * MyBase::mm[index0 * 4 + 0];
989  MyBase::mm[index1 * 4 + 1] += shear * MyBase::mm[index0 * 4 + 1];
990  MyBase::mm[index1 * 4 + 2] += shear * MyBase::mm[index0 * 4 + 2];
991  MyBase::mm[index1 * 4 + 3] += shear * MyBase::mm[index0 * 4 + 3];
992  }
993 
994 
997  void postShear(Axis axis0, Axis axis1, T shear)
998  {
999  int index0 = static_cast<int>(axis0);
1000  int index1 = static_cast<int>(axis1);
1001 
1002  // to collumn "index0" add a multiple of the index1 row
1003  MyBase::mm[index0 + 0] += shear * MyBase::mm[index1 + 0];
1004  MyBase::mm[index0 + 4] += shear * MyBase::mm[index1 + 4];
1005  MyBase::mm[index0 + 8] += shear * MyBase::mm[index1 + 8];
1006  MyBase::mm[index0 + 12] += shear * MyBase::mm[index1 + 12];
1007 
1008  }
1009 
1011  template<typename T0>
1012  Vec4<T0> transform(const Vec4<T0> &v) const
1013  {
1014  return static_cast< Vec4<T0> >(v * *this);
1015  }
1016 
1018  template<typename T0>
1019  Vec3<T0> transform(const Vec3<T0> &v) const
1020  {
1021  return static_cast< Vec3<T0> >(v * *this);
1022  }
1023 
1025  template<typename T0>
1026  Vec4<T0> pretransform(const Vec4<T0> &v) const
1027  {
1028  return static_cast< Vec4<T0> >(*this * v);
1029  }
1030 
1032  template<typename T0>
1033  Vec3<T0> pretransform(const Vec3<T0> &v) const
1034  {
1035  return static_cast< Vec3<T0> >(*this * v);
1036  }
1037 
1039  template<typename T0>
1040  Vec3<T0> transformH(const Vec3<T0> &p) const
1041  {
1042  T0 w;
1043 
1044  // w = p * (*this).col(3);
1045  w = p[0] * MyBase::mm[ 3] + p[1] * MyBase::mm[ 7] + p[2] * MyBase::mm[11] + MyBase::mm[15];
1046 
1047  if ( !isExactlyEqual(w , 0.0) ) {
1048  return Vec3<T0>(static_cast<T0>((p[0] * MyBase::mm[ 0] + p[1] * MyBase::mm[ 4] +
1049  p[2] * MyBase::mm[ 8] + MyBase::mm[12]) / w),
1050  static_cast<T0>((p[0] * MyBase::mm[ 1] + p[1] * MyBase::mm[ 5] +
1051  p[2] * MyBase::mm[ 9] + MyBase::mm[13]) / w),
1052  static_cast<T0>((p[0] * MyBase::mm[ 2] + p[1] * MyBase::mm[ 6] +
1053  p[2] * MyBase::mm[10] + MyBase::mm[14]) / w));
1054  }
1055 
1056  return Vec3<T0>(0, 0, 0);
1057  }
1058 
1060  template<typename T0>
1061  Vec3<T0> pretransformH(const Vec3<T0> &p) const
1062  {
1063  T0 w;
1064 
1065  // w = p * (*this).col(3);
1066  w = p[0] * MyBase::mm[12] + p[1] * MyBase::mm[13] + p[2] * MyBase::mm[14] + MyBase::mm[15];
1067 
1068  if ( !isExactlyEqual(w , 0.0) ) {
1069  return Vec3<T0>(static_cast<T0>((p[0] * MyBase::mm[ 0] + p[1] * MyBase::mm[ 1] +
1070  p[2] * MyBase::mm[ 2] + MyBase::mm[ 3]) / w),
1071  static_cast<T0>((p[0] * MyBase::mm[ 4] + p[1] * MyBase::mm[ 5] +
1072  p[2] * MyBase::mm[ 6] + MyBase::mm[ 7]) / w),
1073  static_cast<T0>((p[0] * MyBase::mm[ 8] + p[1] * MyBase::mm[ 9] +
1074  p[2] * MyBase::mm[10] + MyBase::mm[11]) / w));
1075  }
1076 
1077  return Vec3<T0>(0, 0, 0);
1078  }
1079 
1081  template<typename T0>
1082  Vec3<T0> transform3x3(const Vec3<T0> &v) const
1083  {
1084  return Vec3<T0>(
1085  static_cast<T0>(v[0] * MyBase::mm[ 0] + v[1] * MyBase::mm[ 4] + v[2] * MyBase::mm[ 8]),
1086  static_cast<T0>(v[0] * MyBase::mm[ 1] + v[1] * MyBase::mm[ 5] + v[2] * MyBase::mm[ 9]),
1087  static_cast<T0>(v[0] * MyBase::mm[ 2] + v[1] * MyBase::mm[ 6] + v[2] * MyBase::mm[10]));
1088  }
1089 
1090 
1091 
1096 #ifdef ALLOW_CAST_TO_POINTER
1097 
1098 OPENVDB_DEPRECATED operator T*()
1099 #if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4))
1100  __attribute__ ((deprecated))
1101 #endif
1102  {return MyBase::mm;}
1103 OPENVDB_DEPRECATED operator const T*() const
1104 #if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4))
1105  __attribute__ ((deprecated))
1106 #endif
1107  {return MyBase::mm;}
1108 #endif
1109 
1110 
1111 
1114 OPENVDB_DEPRECATED void accumShear(Axis axis0, Axis axis1, T shear)
1115  {
1116  int index0 = static_cast<int>(axis0);
1117  int index1 = static_cast<int>(axis1);
1118 
1119  MyBase::mm[index1 * 4 + 0] += shear * MyBase::mm[index0 * 4 ];
1120  MyBase::mm[index1 * 4 + 1] += shear * MyBase::mm[index0 * 4 + 1];
1121  MyBase::mm[index1 * 4 + 2] += shear * MyBase::mm[index0 * 4 + 2];
1122  MyBase::mm[index1 * 4 + 3] += shear * MyBase::mm[index0 * 4 + 3];
1123  }
1124 
1125 
1127  template <typename T0>
1128 OPENVDB_DEPRECATED void accumScale(const Vec3<T0>& v)
1129  {
1130  preScale(v);
1131  }
1132 
1133 
1136  template <typename T0>
1137 OPENVDB_DEPRECATED void accumTranslation(const Vec3<T0>& v)
1138  {
1139  this->mm[12] += v.dot(Vec3<T>(this->mm[ 0], this->mm[ 4], this->mm[ 8]));
1140  this->mm[13] += v.dot(Vec3<T>(this->mm[ 1], this->mm[ 5], this->mm[ 9]));
1141  this->mm[14] += v.dot(Vec3<T>(this->mm[ 2], this->mm[ 6], this->mm[10]));
1142  this->mm[15] += v.dot(Vec3<T>(this->mm[ 3], this->mm[ 7], this->mm[11]));
1143  }
1144 
1145 
1149 OPENVDB_DEPRECATED void accumRotation(Axis axis, T angle)
1150  {
1151  T c = static_cast<T>(cos(angle));
1152  T s = static_cast<T>(sin(angle));
1153 
1154  T a10, a11, a12;
1155  T a00, a01, a02;
1156 
1157  switch (axis) {
1158  case X_AXIS:
1159  a10 = c*this->mm[ 4] + s*this->mm[ 8];
1160  a11 = c*this->mm[ 5] + s*this->mm[ 9];
1161  a12 = c*this->mm[ 6] + s*this->mm[10];
1162 
1163  this->mm[ 8] = -s*this->mm[ 4] + c*this->mm[ 8];
1164  this->mm[ 9] = -s*this->mm[ 5] + c*this->mm[ 9];
1165  this->mm[10] = -s*this->mm[ 6] + c*this->mm[10];
1166 
1167  this->mm[ 4] = a10;
1168  this->mm[ 5] = a11;
1169  this->mm[ 6] = a12;
1170 
1171  break;
1172 
1173  case Y_AXIS:
1174  a00 = c*this->mm[ 0] - s*this->mm[ 8];
1175  a01 = c*this->mm[ 1] - s*this->mm[ 9];
1176  a02 = c*this->mm[ 2] - s*this->mm[10];
1177 
1178  this->mm[ 8] = s*this->mm[ 0] + c*this->mm[ 8];
1179  this->mm[ 9] = s*this->mm[ 1] + c*this->mm[ 9];
1180  this->mm[10] = s*this->mm[ 2] + c*this->mm[10];
1181 
1182  this->mm[ 0] = a00;
1183  this->mm[ 1] = a01;
1184  this->mm[ 2] = a02;
1185 
1186  break;
1187 
1188  case Z_AXIS:
1189  a00 = c*this->mm[ 0] + s*this->mm[ 4];
1190  a01 = c*this->mm[ 1] + s*this->mm[ 5];
1191  a02 = c*this->mm[ 2] + s*this->mm[ 6];
1192 
1193  this->mm[ 4] = -s*this->mm[ 0] + c*this->mm[ 4];
1194  this->mm[ 5] = -s*this->mm[ 1] + c*this->mm[ 5];
1195  this->mm[ 6] = -s*this->mm[ 2] + c*this->mm[ 6];
1196 
1197  this->mm[ 0] = a00;
1198  this->mm[ 1] = a01;
1199  this->mm[ 2] = a02;
1200 
1201  break;
1202 
1203  default:
1204  assert(axis==X_AXIS || axis==Y_AXIS || axis==Z_AXIS);
1205  }
1206  }
1207 
1208 
1211  template <typename T0, typename T1>
1212 OPENVDB_DEPRECATED const Mat4& add(const Mat4<T0> &m1, const Mat4<T1> &m2)
1213  {
1214  const T0 *src1 = m1.asPointer();
1215  const T1 *src2 = m2.asPointer();
1216  this->mm[ 0] = src1[0] + src2[0];
1217  this->mm[ 1] = src1[1] + src2[1];
1218  this->mm[ 2] = src1[2] + src2[2];
1219  this->mm[ 3] = src1[3] + src2[3];
1220 
1221  this->mm[ 4] = src1[4] + src2[4];
1222  this->mm[ 5] = src1[5] + src2[5];
1223  this->mm[ 6] = src1[6] + src2[6];
1224  this->mm[ 7] = src1[7] + src2[7];
1225 
1226  this->mm[ 8] = src1[8] + src2[8];
1227  this->mm[ 9] = src1[9] + src2[9];
1228  this->mm[10] = src1[10] + src2[10];
1229  this->mm[11] = src1[11] + src2[11];
1230 
1231  this->mm[12] = src1[12] + src2[12];
1232  this->mm[13] = src1[13] + src2[13];
1233  this->mm[14] = src1[14] + src2[14];
1234  this->mm[15] = src1[15] + src2[15];
1235  return *this;
1236  }
1237 
1240  template <typename T0, typename T1>
1241 OPENVDB_DEPRECATED const Mat4& sub(const Mat4<T0> &m1, const Mat4<T1> &m2)
1242  {
1243  const T0 *src1 = m1.asPointer();
1244  const T1 *src2 = m2.asPointer();
1245  this->mm[ 0] = src1[0] - src2[0];
1246  this->mm[ 1] = src1[1] - src2[1];
1247  this->mm[ 2] = src1[2] - src2[2];
1248  this->mm[ 3] = src1[3] - src2[3];
1249 
1250  this->mm[ 4] = src1[4] - src2[4];
1251  this->mm[ 5] = src1[5] - src2[5];
1252  this->mm[ 6] = src1[6] - src2[6];
1253  this->mm[ 7] = src1[7] - src2[7];
1254 
1255  this->mm[ 8] = src1[8] - src2[8];
1256  this->mm[ 9] = src1[9] - src2[9];
1257  this->mm[10] = src1[10] - src2[10];
1258  this->mm[11] = src1[11] - src2[11];
1259 
1260  this->mm[12] = src1[12] - src2[12];
1261  this->mm[13] = src1[13] - src2[13];
1262  this->mm[14] = src1[14] - src2[14];
1263  this->mm[15] = src1[15] - src2[15];
1264  return *this;
1265  }
1266 
1268  template <typename T0, typename T1>
1269 OPENVDB_DEPRECATED const Mat4& scale(T0 scalar, const Mat4<T1> &m)
1270  {
1271  const T1 *src = m.asPointer();
1272  this->mm[ 0] = scalar * src[0];
1273  this->mm[ 1] = scalar * src[1];
1274  this->mm[ 2] = scalar * src[2];
1275  this->mm[ 3] = scalar * src[3];
1276 
1277  this->mm[ 4] = scalar * src[4];
1278  this->mm[ 5] = scalar * src[5];
1279  this->mm[ 6] = scalar * src[6];
1280  this->mm[ 7] = scalar * src[7];
1281 
1282  this->mm[ 8] = scalar * src[8];
1283  this->mm[ 9] = scalar * src[9];
1284  this->mm[10] = scalar * src[10];
1285  this->mm[11] = scalar * src[11];
1286 
1287  this->mm[12] = scalar * src[12];
1288  this->mm[13] = scalar * src[13];
1289  this->mm[14] = scalar * src[14];
1290  this->mm[15] = scalar * src[15];
1291  return *this;
1292  }
1293 
1296  template <typename T0, typename T1>
1297 OPENVDB_DEPRECATED const Mat4& mult(const Mat4<T0> &m1, const Mat4<T1> &m2)
1298  {
1299  register int i, i4;
1300  const T0 *src1 = m1.asPointer();
1301  const T1 *src2 = m2.asPointer();
1302 
1303  for (i = 0; i < 4; i++) {
1304  i4 = 4 * i;
1305  this->mm[i4+0] = static_cast<T>(src1[i4+0]*src2[4*0+0] +
1306  src1[i4+1]*src2[4*1+0] +
1307  src1[i4+2]*src2[4*2+0] +
1308  src1[i4+3]*src2[4*3+0]);
1309 
1310  this->mm[i4+1] = static_cast<T>(src1[i4+0]*src2[4*0+1] +
1311  src1[i4+1]*src2[4*1+1] +
1312  src1[i4+2]*src2[4*2+1] +
1313  src1[i4+3]*src2[4*3+1]);
1314 
1315  this->mm[i4+2] = static_cast<T>(src1[i4+0]*src2[4*0+2] +
1316  src1[i4+1]*src2[4*1+2] +
1317  src1[i4+2]*src2[4*2+2] +
1318  src1[i4+3]*src2[4*3+2]);
1319 
1320  this->mm[i4+3] = static_cast<T>(src1[i4+0]*src2[4*0+3] +
1321  src1[i4+1]*src2[4*1+3] +
1322  src1[i4+2]*src2[4*2+3] +
1323  src1[i4+3]*src2[4*3+3]);
1324  }
1325 
1326  return *this;
1327  }
1328 
1329 
1330 
1331 private:
1332  bool invert(Mat4<T> &inverse, T tolerance) const;
1333 
1334  T det2(const Mat4<T> &a, int i0, int i1, int j0, int j1) const {
1335  int i0row = i0 * 4;
1336  int i1row = i1 * 4;
1337  return a.mm[i0row+j0]*a.mm[i1row+j1] - a.mm[i0row+j1]*a.mm[i1row+j0];
1338  }
1339 
1340  T det3(const Mat4<T> &a, int i0, int i1, int i2,
1341  int j0, int j1, int j2) const {
1342  int i0row = i0 * 4;
1343  return a.mm[i0row+j0]*det2(a, i1,i2, j1,j2) +
1344  a.mm[i0row+j1]*det2(a, i1,i2, j2,j0) +
1345  a.mm[i0row+j2]*det2(a, i1,i2, j0,j1);
1346  }
1347 
1348  static const Mat4<T> sIdentity;
1349  static const Mat4<T> sZero;
1350 }; // class Mat4
1351 
1352 
1353 template <typename T>
1354 const Mat4<T> Mat4<T>::sIdentity = Mat4<T>(1, 0, 0, 0,
1355  0, 1, 0, 0,
1356  0, 0, 1, 0,
1357  0, 0, 0, 1);
1358 
1359 template <typename T>
1360 const Mat4<T> Mat4<T>::sZero = Mat4<T>(0, 0, 0, 0,
1361  0, 0, 0, 0,
1362  0, 0, 0, 0,
1363  0, 0, 0, 0);
1364 
1367 template <typename T0, typename T1>
1368 bool operator==(const Mat4<T0> &m0, const Mat4<T1> &m1)
1369 {
1370  const T0 *t0 = m0.asPointer();
1371  const T1 *t1 = m1.asPointer();
1372 
1373  for (int i=0; i<16; ++i) if (!isExactlyEqual(t0[i], t1[i])) return false;
1374  return true;
1375 }
1376 
1379 template <typename T0, typename T1>
1380 bool operator!=(const Mat4<T0> &m0, const Mat4<T1> &m1) { return !(m0 == m1); }
1381 
1384 template <typename S, typename T>
1386 {
1387  return m*scalar;
1388 }
1389 
1392 template <typename S, typename T>
1394 {
1396  result *= scalar;
1397  return result;
1398 }
1399 
1402 template<typename T, typename MT>
1405  const Vec4<T> &_v)
1406 {
1407  MT const *m = _m.asPointer();
1409  _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2] + _v[3]*m[3],
1410  _v[0]*m[4] + _v[1]*m[5] + _v[2]*m[6] + _v[3]*m[7],
1411  _v[0]*m[8] + _v[1]*m[9] + _v[2]*m[10] + _v[3]*m[11],
1412  _v[0]*m[12] + _v[1]*m[13] + _v[2]*m[14] + _v[3]*m[15]);
1413 }
1414 
1417 template<typename T, typename MT>
1419 operator*(const Vec4<T> &_v,
1420  const Mat4<MT> &_m)
1421 {
1422  MT const *m = _m.asPointer();
1424  _v[0]*m[0] + _v[1]*m[4] + _v[2]*m[8] + _v[3]*m[12],
1425  _v[0]*m[1] + _v[1]*m[5] + _v[2]*m[9] + _v[3]*m[13],
1426  _v[0]*m[2] + _v[1]*m[6] + _v[2]*m[10] + _v[3]*m[14],
1427  _v[0]*m[3] + _v[1]*m[7] + _v[2]*m[11] + _v[3]*m[15]);
1428 }
1429 
1433 template<typename T, typename MT>
1436  const Vec3<T> &_v)
1437 {
1438  MT const *m = _m.asPointer();
1440  _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2] + m[3],
1441  _v[0]*m[4] + _v[1]*m[5] + _v[2]*m[6] + m[7],
1442  _v[0]*m[8] + _v[1]*m[9] + _v[2]*m[10] + m[11]);
1443 }
1444 
1448 template<typename T, typename MT>
1450 operator*(const Vec3<T> &_v,
1451  const Mat4<MT> &_m)
1452 {
1453  MT const *m = _m.asPointer();
1455  _v[0]*m[0] + _v[1]*m[4] + _v[2]*m[8] + m[12],
1456  _v[0]*m[1] + _v[1]*m[5] + _v[2]*m[9] + m[13],
1457  _v[0]*m[2] + _v[1]*m[6] + _v[2]*m[10] + m[14]);
1458 }
1459 
1462 template <typename T0, typename T1>
1464 operator+(const Mat4<T0> &m0, const Mat4<T1> &m1)
1465 {
1467  result += m1;
1468  return result;
1469 }
1470 
1473 template <typename T0, typename T1>
1475 operator-(const Mat4<T0> &m0, const Mat4<T1> &m1)
1476 {
1478  result -= m1;
1479  return result;
1480 }
1481 
1485 template <typename T0, typename T1>
1487 operator*(const Mat4<T0> &m0, const Mat4<T1> &m1)
1488 {
1490  result *= m1;
1491  return result;
1492 }
1493 
1494 
1498 template<typename T0, typename T1>
1500 {
1501  return Vec3<T1>(
1502  static_cast<T1>(m[0][0]*n[0] + m[0][1]*n[1] + m[0][2]*n[2]),
1503  static_cast<T1>(m[1][0]*n[0] + m[1][1]*n[1] + m[1][2]*n[2]),
1504  static_cast<T1>(m[2][0]*n[0] + m[2][1]*n[1] + m[2][2]*n[2]));
1505 }
1506 
1507 
1509 template<typename T>
1510 bool Mat4<T>::invert(Mat4<T> &inverse, T tolerance) const
1511 {
1512  Mat4<T> temp(*this);
1513  inverse.setIdentity();
1514 
1515  // Forward elimination step
1516  double det = 1.0;
1517  for (int i = 0; i < 4; ++i) {
1518  int row = i;
1519  double max = fabs(temp[i][i]);
1520 
1521  for (int k = i+1; k < 4; ++k) {
1522  if (fabs(temp[k][i]) > max) {
1523  row = k;
1524  max = fabs(temp[k][i]);
1525  }
1526  }
1527 
1528  if (isExactlyEqual(max, 0.0)) return false;
1529 
1530  // must move pivot to row i
1531  if (row != i) {
1532  det = -det;
1533  for (int k = 0; k < 4; ++k) {
1534  std::swap(temp[row][k], temp[i][k]);
1535  std::swap(inverse[row][k], inverse[i][k]);
1536  }
1537  }
1538 
1539  double pivot = temp[i][i];
1540  det *= pivot;
1541 
1542  // scale row i
1543  for (int k = 0; k < 4; ++k) {
1544  temp[i][k] /= pivot;
1545  inverse[i][k] /= pivot;
1546  }
1547 
1548  // eliminate in rows below i
1549  for (int j = i+1; j < 4; ++j) {
1550  double t = temp[j][i];
1551  if (!isExactlyEqual(t, 0.0)) {
1552  // subtract scaled row i from row j
1553  for (int k = 0; k < 4; ++k) {
1554  temp[j][k] -= temp[i][k] * t;
1555  inverse[j][k] -= inverse[i][k] * t;
1556  }
1557  }
1558  }
1559  }
1560 
1561  // Backward elimination step
1562  for (int i = 3; i > 0; --i) {
1563  for (int j = 0; j < i; ++j) {
1564  double t = temp[j][i];
1565 
1566  if (!isExactlyEqual(t, 0.0)) {
1567  for (int k = 0; k < 4; ++k) {
1568  inverse[j][k] -= inverse[i][k]*t;
1569  }
1570  }
1571  }
1572  }
1573  return det*det >= tolerance*tolerance;
1574 }
1575 
1576 template <typename T>
1577 inline bool isAffine(const Mat4<T>& m) {
1578  return (m.col(3) == Vec4<T>(0, 0, 0, 1));
1579 }
1580 
1581 template <typename T>
1582 inline bool hasTranslation(const Mat4<T>& m) {
1583  return (m.row(3) != Vec4<T>(0, 0, 0, 1));
1584 }
1585 
1586 
1589 
1590 #if DWREAL_IS_DOUBLE == 1
1591 typedef Mat4d Mat4f;
1592 #else
1593 typedef Mat4s Mat4f;
1594 #endif // DWREAL_IS_DOUBLE
1595 
1596 } // namespace math
1597 
1598 
1600 {
1601  return math::Mat4s::identity();
1602 }
1604 {
1605  return math::Mat4d::identity();
1606 }
1607 
1608 } // namespace OPENVDB_VERSION_NAME
1609 } // namespace openvdb
1610 
1611 #endif // OPENVDB_UTIL_MAT4_H_HAS_BEEN_INCLUDED
1612 
1613 // Copyright (c) 2012 DreamWorks Animation LLC
1614 // All rights reserved. This software is distributed under the
1615 // Mozilla Public License 2.0 ( http://www.mozilla.org/MPL/2.0/ )