4 /// Expression templates for poor programmers ... :)
15 enum { nCols = 4, nRows = 4, nEnts = 16 };
18 /// Default constructor. Does not initialize at all.
19 /// If you need them zero initialized, use SGMatrix::zeros()
22 /// Initialize with nans in the debug build, that will guarantee to have
23 /// a fast uninitialized default constructor in the release but shows up
24 /// uninitialized values in the debug build very fast ...
26 for (unsigned i = 0; i < nEnts; ++i)
27 _data.flat[i] = SGLimits<T>::quiet_NaN();
30 /// Constructor. Initialize by the content of a plain array,
31 /// make sure it has at least 16 elements
32 explicit SGMatrix(const T* data)
33 { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] = data[i]; }
35 /// Constructor, build up a SGMatrix from given elements
36 SGMatrix(T m00, T m01, T m02, T m03,
37 T m10, T m11, T m12, T m13,
38 T m20, T m21, T m22, T m23,
39 T m30, T m31, T m32, T m33)
41 _data.flat[0] = m00; _data.flat[1] = m10;
42 _data.flat[2] = m20; _data.flat[3] = m30;
43 _data.flat[4] = m01; _data.flat[5] = m11;
44 _data.flat[6] = m21; _data.flat[7] = m31;
45 _data.flat[8] = m02; _data.flat[9] = m12;
46 _data.flat[10] = m22; _data.flat[11] = m32;
47 _data.flat[12] = m03; _data.flat[13] = m13;
48 _data.flat[14] = m23; _data.flat[15] = m33;
51 /// Constructor, build up a SGMatrix from a translation
52 SGMatrix(const SGVec3<T>& trans)
55 /// Constructor, build up a SGMatrix from a rotation and a translation
56 SGMatrix(const SGQuat<T>& quat, const SGVec3<T>& trans)
58 /// Constructor, build up a SGMatrix from a rotation and a translation
59 SGMatrix(const SGQuat<T>& quat)
62 /// Copy constructor for a transposed negated matrix
63 SGMatrix(const TransNegRef<T>& tm)
66 /// Set from a tranlation
67 void set(const SGVec3<T>& trans)
69 _data.flat[0] = 1; _data.flat[4] = 0;
70 _data.flat[8] = 0; _data.flat[12] = -trans(0);
71 _data.flat[1] = 0; _data.flat[5] = 1;
72 _data.flat[9] = 0; _data.flat[13] = -trans(1);
73 _data.flat[2] = 0; _data.flat[6] = 0;
74 _data.flat[10] = 1; _data.flat[14] = -trans(2);
75 _data.flat[3] = 0; _data.flat[7] = 0;
76 _data.flat[11] = 0; _data.flat[15] = 1;
79 /// Set from a scale/rotation and tranlation
80 void set(const SGQuat<T>& quat, const SGVec3<T>& trans)
82 T w = quat.w(); T x = quat.x(); T y = quat.y(); T z = quat.z();
83 T xx = x*x; T yy = y*y; T zz = z*z;
84 T wx = w*x; T wy = w*y; T wz = w*z;
85 T xy = x*y; T xz = x*z; T yz = y*z;
86 _data.flat[0] = 1-2*(yy+zz); _data.flat[1] = 2*(xy-wz);
87 _data.flat[2] = 2*(xz+wy); _data.flat[3] = 0;
88 _data.flat[4] = 2*(xy+wz); _data.flat[5] = 1-2*(xx+zz);
89 _data.flat[6] = 2*(yz-wx); _data.flat[7] = 0;
90 _data.flat[8] = 2*(xz-wy); _data.flat[9] = 2*(yz+wx);
91 _data.flat[10] = 1-2*(xx+yy); _data.flat[11] = 0;
92 // Well, this one is ugly here, as that xform method on the current
93 // object needs the above data to be already set ...
94 SGVec3<T> t = xformVec(trans);
95 _data.flat[12] = -t(0); _data.flat[13] = -t(1);
96 _data.flat[14] = -t(2); _data.flat[15] = 1;
98 /// Set from a scale/rotation and tranlation
99 void set(const SGQuat<T>& quat)
101 T w = quat.w(); T x = quat.x(); T y = quat.y(); T z = quat.z();
102 T xx = x*x; T yy = y*y; T zz = z*z;
103 T wx = w*x; T wy = w*y; T wz = w*z;
104 T xy = x*y; T xz = x*z; T yz = y*z;
105 _data.flat[0] = 1-2*(yy+zz); _data.flat[1] = 2*(xy-wz);
106 _data.flat[2] = 2*(xz+wy); _data.flat[3] = 0;
107 _data.flat[4] = 2*(xy+wz); _data.flat[5] = 1-2*(xx+zz);
108 _data.flat[6] = 2*(yz-wx); _data.flat[7] = 0;
109 _data.flat[8] = 2*(xz-wy); _data.flat[9] = 2*(yz+wx);
110 _data.flat[10] = 1-2*(xx+yy); _data.flat[11] = 0;
111 _data.flat[12] = 0; _data.flat[13] = 0;
112 _data.flat[14] = 0; _data.flat[15] = 1;
115 /// set from a transposed negated matrix
116 void set(const TransNegRef<T>& tm)
118 const SGMatrix& m = tm.m;
119 _data.flat[0] = m(0,0);
120 _data.flat[1] = m(0,1);
121 _data.flat[2] = m(0,2);
122 _data.flat[3] = m(3,0);
124 _data.flat[4] = m(1,0);
125 _data.flat[5] = m(1,1);
126 _data.flat[6] = m(1,2);
127 _data.flat[7] = m(3,1);
129 _data.flat[8] = m(2,0);
130 _data.flat[9] = m(2,1);
131 _data.flat[10] = m(2,2);
132 _data.flat[11] = m(3,2);
134 // Well, this one is ugly here, as that xform method on the current
135 // object needs the above data to be already set ...
136 SGVec3<T> t = xformVec(SGVec3<T>(m(0,3), m(1,3), m(2,3)));
137 _data.flat[12] = -t(0);
138 _data.flat[13] = -t(1);
139 _data.flat[14] = -t(2);
140 _data.flat[15] = m(3,3);
143 /// Access by index, the index is unchecked
144 const T& operator()(unsigned i, unsigned j) const
145 { return _data.flat[i + 4*j]; }
146 /// Access by index, the index is unchecked
147 T& operator()(unsigned i, unsigned j)
148 { return _data.flat[i + 4*j]; }
150 /// Access raw data by index, the index is unchecked
151 const T& operator[](unsigned i) const
152 { return _data.flat[i]; }
153 /// Access by index, the index is unchecked
154 T& operator[](unsigned i)
155 { return _data.flat[i]; }
157 /// Get the data pointer
158 const T* data(void) const
159 { return _data.flat; }
160 /// Get the data pointer
162 { return _data.flat; }
164 /// Readonly interface function to ssg's sgMat4/sgdMat4
165 const T (&sg(void) const)[4][4]
166 { return _data.carray; }
167 /// Interface function to ssg's sgMat4/sgdMat4
169 { return _data.carray; }
172 SGMatrix& operator+=(const SGMatrix& m)
173 { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] += m._data.flat[i]; return *this; }
174 /// Inplace subtraction
175 SGMatrix& operator-=(const SGMatrix& m)
176 { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] -= m._data.flat[i]; return *this; }
177 /// Inplace scalar multiplication
179 SGMatrix& operator*=(S s)
180 { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] *= s; return *this; }
181 /// Inplace scalar multiplication by 1/s
183 SGMatrix& operator/=(S s)
184 { return operator*=(1/T(s)); }
185 /// Inplace matrix multiplication, post multiply
186 SGMatrix& operator*=(const SGMatrix<T>& m2);
188 SGVec3<T> xformPt(const SGVec3<T>& pt) const
191 tpt(0) = (*this)(0,3);
192 tpt(1) = (*this)(1,3);
193 tpt(2) = (*this)(2,3);
194 for (unsigned i = 0; i < SGMatrix<T>::nCols-1; ++i) {
196 tpt(0) += tmp*(*this)(0,i);
197 tpt(1) += tmp*(*this)(1,i);
198 tpt(2) += tmp*(*this)(2,i);
202 SGVec3<T> xformVec(const SGVec3<T>& v) const
206 tv(0) = tmp*(*this)(0,0);
207 tv(1) = tmp*(*this)(1,0);
208 tv(2) = tmp*(*this)(2,0);
209 for (unsigned i = 1; i < SGMatrix<T>::nCols-1; ++i) {
211 tv(0) += tmp*(*this)(0,i);
212 tv(1) += tmp*(*this)(1,i);
213 tv(2) += tmp*(*this)(2,i);
218 /// Return an all zero matrix
219 static SGMatrix zeros(void)
220 { return SGMatrix(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); }
222 /// Return a unit matrix
223 static SGMatrix unit(void)
224 { return SGMatrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); }
227 /// Required to make that alias safe.
233 /// The actual data, the matrix is stored in column major order,
234 /// that matches the storage format of OpenGL
238 /// Class to distinguish between a matrix and the matrix with a transposed
239 /// rotational part and a negated translational part
242 TransNegRef(const SGMatrix<T>& _m) : m(_m) {}
243 const SGMatrix<T>& m;
246 /// Unary +, do nothing ...
250 operator+(const SGMatrix<T>& m)
253 /// Unary -, do nearly nothing
257 operator-(const SGMatrix<T>& m)
260 for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
269 operator+(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
272 for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
273 ret[i] = m1[i] + m2[i];
281 operator-(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
284 for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
285 ret[i] = m1[i] - m2[i];
289 /// Scalar multiplication
290 template<typename S, typename T>
293 operator*(S s, const SGMatrix<T>& m)
296 for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
301 /// Scalar multiplication
302 template<typename S, typename T>
305 operator*(const SGMatrix<T>& m, S s)
308 for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
313 /// Vector multiplication
317 operator*(const SGMatrix<T>& m, const SGVec4<T>& v)
325 for (unsigned i = 1; i < SGMatrix<T>::nCols; ++i) {
335 /// Vector multiplication
339 operator*(const TransNegRef<T>& tm, const SGVec4<T>& v)
341 const SGMatrix<T>& m = tm.m;
345 mv(0) = v2(0) = -tmp*m(0,3);
346 mv(1) = v2(1) = -tmp*m(1,3);
347 mv(2) = v2(2) = -tmp*m(2,3);
349 for (unsigned i = 0; i < SGMatrix<T>::nCols - 1; ++i) {
350 T tmp = v(i) + v2(i);
359 /// Matrix multiplication
363 operator*(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
366 for (unsigned j = 0; j < SGMatrix<T>::nCols; ++j) {
368 m(0,j) = tmp*m1(0,0);
369 m(1,j) = tmp*m1(1,0);
370 m(2,j) = tmp*m1(2,0);
371 m(3,j) = tmp*m1(3,0);
372 for (unsigned i = 1; i < SGMatrix<T>::nCols; ++i) {
374 m(0,j) += tmp*m1(0,i);
375 m(1,j) += tmp*m1(1,i);
376 m(2,j) += tmp*m1(2,i);
377 m(3,j) += tmp*m1(3,i);
383 /// Inplace matrix multiplication, post multiply
387 SGMatrix<T>::operator*=(const SGMatrix<T>& m2)
388 { (*this) = operator*(*this, m2); return *this; }
390 /// Return a reference to the matrix typed to make sure we use the transposed
395 transNeg(const SGMatrix<T>& m)
396 { return TransNegRef<T>(m); }
398 /// Compute the inverse if the matrix src. Store the result in dst.
399 /// Return if the matrix is nonsingular. If it is singular dst contains
404 invert(SGMatrix<T>& dst, const SGMatrix<T>& src)
406 // Do a LU decomposition with row pivoting and solve into dst
407 SGMatrix<T> tmp = src;
408 dst = SGMatrix<T>::unit();
410 for (unsigned i = 0; i < 4; ++i) {
414 // Find the row with the maximum value in the i-th colum
415 for (unsigned j = i + 1; j < 4; ++j) {
416 if (fabs(tmp(j, i)) > fabs(val)) {
424 for (unsigned j = 0; j < 4; ++j) {
426 t = dst(i,j); dst(i,j) = dst(ind,j); dst(ind,j) = t;
427 t = tmp(i,j); tmp(i,j) = tmp(ind,j); tmp(ind,j) = t;
431 // Check for singularity
432 if (fabs(val) <= SGLimits<T>::min())
436 for (unsigned j = 0; j < 4; ++j) {
441 for (unsigned j = 0; j < 4; ++j) {
446 for (unsigned k = 0; k < 4; ++k) {
447 tmp(j,k) -= tmp(i,k) * val;
448 dst(j,k) -= dst(i,k) * val;
455 /// The 1-norm of the matrix, this is the largest column sum of
456 /// the absolute values of A.
460 norm1(const SGMatrix<T>& m)
463 for (unsigned i = 0; i < SGMatrix<T>::nRows; ++i) {
464 T sum = fabs(m(i, 0)) + fabs(m(i, 1)) + fabs(m(i, 2)) + fabs(m(i, 3));
471 /// The inf-norm of the matrix, this is the largest row sum of
472 /// the absolute values of A.
476 normInf(const SGMatrix<T>& m)
479 for (unsigned i = 0; i < SGMatrix<T>::nCols; ++i) {
480 T sum = fabs(m(0, i)) + fabs(m(1, i)) + fabs(m(2, i)) + fabs(m(3, i));
487 /// Return true if exactly the same
491 operator==(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
493 for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
499 /// Return true if not exactly the same
503 operator!=(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
504 { return ! (m1 == m2); }
506 /// Return true if equal to the relative tolerance tol
510 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2, T rtol, T atol)
511 { return norm1(m1 - m2) < rtol*(norm1(m1) + norm1(m2)) + atol; }
513 /// Return true if equal to the relative tolerance tol
517 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2, T rtol)
518 { return norm1(m1 - m2) < rtol*(norm1(m1) + norm1(m2)); }
520 /// Return true if about equal to roundoff of the underlying type
524 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
526 T tol = 100*SGLimits<T>::epsilon();
527 return equivalent(m1, m2, tol, tol);
534 isNaN(const SGMatrix<T>& m)
536 for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i) {
537 if (SGMisc<T>::isNaN(m[i]))
544 /// Output to an ostream
545 template<typename char_type, typename traits_type, typename T>
547 std::basic_ostream<char_type, traits_type>&
548 operator<<(std::basic_ostream<char_type, traits_type>& s, const SGMatrix<T>& m)
550 s << "[ " << m(0,0) << ", " << m(0,1) << ", " << m(0,2) << ", " << m(0,3) << "\n";
551 s << " " << m(1,0) << ", " << m(1,1) << ", " << m(1,2) << ", " << m(1,3) << "\n";
552 s << " " << m(2,0) << ", " << m(2,1) << ", " << m(2,2) << ", " << m(2,3) << "\n";
553 s << " " << m(3,0) << ", " << m(3,1) << ", " << m(3,2) << ", " << m(3,3) << " ]";
557 /// Two classes doing actually the same on different types
558 typedef SGMatrix<float> SGMatrixf;
559 typedef SGMatrix<double> SGMatrixd;
563 toMatrixf(const SGMatrixd& m)
565 return SGMatrixf((float)m(0,0), (float)m(0,1), (float)m(0,2), (float)m(0,3),
566 (float)m(1,0), (float)m(1,1), (float)m(1,2), (float)m(1,3),
567 (float)m(3,0), (float)m(2,1), (float)m(2,2), (float)m(2,3),
568 (float)m(4,0), (float)m(4,1), (float)m(4,2), (float)m(4,3));
573 toMatrixd(const SGMatrixf& m)
575 return SGMatrixd(m(0,0), m(0,1), m(0,2), m(0,3),
576 m(1,0), m(1,1), m(1,2), m(1,3),
577 m(3,0), m(2,1), m(2,2), m(2,3),
578 m(4,0), m(4,1), m(4,2), m(4,3));