simgear/math/SGMatrix.hxx

   1 // Copyright (C) 2006  Mathias Froehlich - Mathias.Froehlich@web.de
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Library General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2 of the License, or (at your option) any later version.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Library General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU General Public License
  14 // along with this program; if not, write to the Free Software
  15 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  16 //
  17
  18 #ifndef SGMatrix_H
  19 #define SGMatrix_H
  20
  21 /// Expression templates for poor programmers ... :)
  22 template<typename T>
  23 struct TransNegRef;
  24
  25 /// 3D Matrix Class
  26 template<typename T>
  27 class SGMatrix {
  28 public:
  29   enum { nCols = 4, nRows = 4, nEnts = 16 };
  30   typedef T value_type;
  31
  32   /// Default constructor. Does not initialize at all.
  33   /// If you need them zero initialized, use SGMatrix::zeros()
  34   SGMatrix(void)
  35   {
  36     /// Initialize with nans in the debug build, that will guarantee to have
  37     /// a fast uninitialized default constructor in the release but shows up
  38     /// uninitialized values in the debug build very fast ...
  39 #ifndef NDEBUG
  40     for (unsigned i = 0; i < nEnts; ++i)
  41       _data.flat[i] = SGLimits<T>::quiet_NaN();
  42 #endif
  43   }
  44   /// Constructor. Initialize by the content of a plain array,
  45   /// make sure it has at least 16 elements
  46   explicit SGMatrix(const T* data)
  47   { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] = data[i]; }
  48
  49   /// Constructor, build up a SGMatrix from given elements
  50   SGMatrix(T m00, T m01, T m02, T m03,
  51            T m10, T m11, T m12, T m13,
  52            T m20, T m21, T m22, T m23,
  53            T m30, T m31, T m32, T m33)
  54   {
  55     _data.flat[0] = m00; _data.flat[1] = m10;
  56     _data.flat[2] = m20; _data.flat[3] = m30;
  57     _data.flat[4] = m01; _data.flat[5] = m11;
  58     _data.flat[6] = m21; _data.flat[7] = m31;
  59     _data.flat[8] = m02; _data.flat[9] = m12;
  60     _data.flat[10] = m22; _data.flat[11] = m32;
  61     _data.flat[12] = m03; _data.flat[13] = m13;
  62     _data.flat[14] = m23; _data.flat[15] = m33;
  63   }
  64
  65   /// Constructor, build up a SGMatrix from a translation
  66   template<typename S>
  67   SGMatrix(const SGVec3<S>& trans)
  68   { set(trans); }
  69
  70   /// Constructor, build up a SGMatrix from a rotation and a translation
  71   template<typename S>
  72   SGMatrix(const SGQuat<S>& quat)
  73   { set(quat); }
  74
  75   /// Copy constructor for a transposed negated matrix
  76   SGMatrix(const TransNegRef<T>& tm)
  77   { set(tm); }
  78
  79   /// Set from a tranlation
  80   template<typename S>
  81   void set(const SGVec3<S>& trans)
  82   {
  83     _data.flat[0] = 1; _data.flat[4] = 0;
  84     _data.flat[8] = 0; _data.flat[12] = T(trans(0));
  85     _data.flat[1] = 0; _data.flat[5] = 1;
  86     _data.flat[9] = 0; _data.flat[13] = T(trans(1));
  87     _data.flat[2] = 0; _data.flat[6] = 0;
  88     _data.flat[10] = 1; _data.flat[14] = T(trans(2));
  89     _data.flat[3] = 0; _data.flat[7] = 0;
  90     _data.flat[11] = 0; _data.flat[15] = 1;
  91   }
  92
  93   /// Set from a scale/rotation and tranlation
  94   template<typename S>
  95   void set(const SGQuat<S>& quat)
  96   {
  97     T w = quat.w(); T x = quat.x(); T y = quat.y(); T z = quat.z();
  98     T xx = x*x; T yy = y*y; T zz = z*z;
  99     T wx = w*x; T wy = w*y; T wz = w*z;
 100     T xy = x*y; T xz = x*z; T yz = y*z;
 101     _data.flat[0] = 1-2*(yy+zz); _data.flat[1] = 2*(xy-wz);
 102     _data.flat[2] = 2*(xz+wy); _data.flat[3] = 0;
 103     _data.flat[4] = 2*(xy+wz); _data.flat[5] = 1-2*(xx+zz);
 104     _data.flat[6] = 2*(yz-wx); _data.flat[7] = 0;
 105     _data.flat[8] = 2*(xz-wy); _data.flat[9] = 2*(yz+wx);
 106     _data.flat[10] = 1-2*(xx+yy); _data.flat[11] = 0;
 107     _data.flat[12] = 0; _data.flat[13] = 0;
 108     _data.flat[14] = 0; _data.flat[15] = 1;
 109   }
 110
 111   /// set from a transposed negated matrix
 112   void set(const TransNegRef<T>& tm)
 113   {
 114     const SGMatrix& m = tm.m;
 115     _data.flat[0] = m(0,0);
 116     _data.flat[1] = m(0,1);
 117     _data.flat[2] = m(0,2);
 118     _data.flat[3] = m(3,0);
 119
 120     _data.flat[4] = m(1,0);
 121     _data.flat[5] = m(1,1);
 122     _data.flat[6] = m(1,2);
 123     _data.flat[7] = m(3,1);
 124
 125     _data.flat[8] = m(2,0);
 126     _data.flat[9] = m(2,1);
 127     _data.flat[10] = m(2,2);
 128     _data.flat[11] = m(3,2);
 129
 130     // Well, this one is ugly here, as that xform method on the current
 131     // object needs the above data to be already set ...
 132     SGVec3<T> t = xformVec(SGVec3<T>(m(0,3), m(1,3), m(2,3)));
 133     _data.flat[12] = -t(0);
 134     _data.flat[13] = -t(1);
 135     _data.flat[14] = -t(2);
 136     _data.flat[15] = m(3,3);
 137   }
 138
 139   /// Access by index, the index is unchecked
 140   const T& operator()(unsigned i, unsigned j) const
 141   { return _data.flat[i + 4*j]; }
 142   /// Access by index, the index is unchecked
 143   T& operator()(unsigned i, unsigned j)
 144   { return _data.flat[i + 4*j]; }
 145
 146   /// Access raw data by index, the index is unchecked
 147   const T& operator[](unsigned i) const
 148   { return _data.flat[i]; }
 149   /// Access by index, the index is unchecked
 150   T& operator[](unsigned i)
 151   { return _data.flat[i]; }
 152
 153   /// Get the data pointer
 154   const T* data(void) const
 155   { return _data.flat; }
 156   /// Get the data pointer
 157   T* data(void)
 158   { return _data.flat; }
 159
 160   /// Readonly interface function to ssg's sgMat4/sgdMat4
 161   const T (&sg(void) const)[4][4]
 162   { return _data.carray; }
 163   /// Interface function to ssg's sgMat4/sgdMat4
 164   T (&sg(void))[4][4]
 165   { return _data.carray; }
 166
 167   /// Inplace addition
 168   SGMatrix& operator+=(const SGMatrix& m)
 169   { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] += m._data.flat[i]; return *this; }
 170   /// Inplace subtraction
 171   SGMatrix& operator-=(const SGMatrix& m)
 172   { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] -= m._data.flat[i]; return *this; }
 173   /// Inplace scalar multiplication
 174   template<typename S>
 175   SGMatrix& operator*=(S s)
 176   { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] *= s; return *this; }
 177   /// Inplace scalar multiplication by 1/s
 178   template<typename S>
 179   SGMatrix& operator/=(S s)
 180   { return operator*=(1/T(s)); }
 181   /// Inplace matrix multiplication, post multiply
 182   SGMatrix& operator*=(const SGMatrix<T>& m2);
 183
 184   template<typename S>
 185   SGMatrix& preMultTranslate(const SGVec3<S>& t)
 186   {
 187     for (unsigned i = 0; i < 3; ++i) {
 188       T tmp = T(t(i));
 189       if (tmp == 0)
 190         continue;
 191       (*this)(i,0) += tmp*(*this)(3,0);
 192       (*this)(i,1) += tmp*(*this)(3,1);
 193       (*this)(i,2) += tmp*(*this)(3,2);
 194       (*this)(i,3) += tmp*(*this)(3,3);
 195     }
 196   }
 197   template<typename S>
 198   SGMatrix& postMultTranslate(const SGVec3<S>& t)
 199   {
 200     SGVec4<T> col3((*this)(0,3), (*this)(1,3), (*this)(2,3), (*this)(3,3));
 201     for (unsigned i = 0; i < SGMatrix<T>::nCols-1; ++i) {
 202       SGVec4<T> tmp((*this)(0,i), (*this)(1,i), (*this)(2,i), (*this)(3,i));
 203       col3 += T(t(i))*tmp;
 204     }
 205     (*this)(0,3) = col3(0); (*this)(1,3) = col3(1);
 206     (*this)(2,3) = col3(2); (*this)(3,3) = col3(3);
 207     return *this;
 208   }
 209
 210   SGMatrix& preMultRotate(const SGQuat<T>& r)
 211   {
 212     for (unsigned i = 0; i < SGMatrix<T>::nCols; ++i) {
 213       SGVec3<T> col((*this)(0,i), (*this)(1,i), (*this)(2,i));
 214       col = r.transform(col);
 215       (*this)(0,i) = col(0); (*this)(1,i) = col(1); (*this)(2,i) = col(2);
 216     }
 217     return *this;
 218   }
 219   SGMatrix& postMultRotate(const SGQuat<T>& r)
 220   {
 221     for (unsigned i = 0; i < SGMatrix<T>::nCols; ++i) {
 222       SGVec3<T> col((*this)(i,0), (*this)(i,1), (*this)(i,2));
 223       col = r.backTransform(col);
 224       (*this)(i,0) = col(0); (*this)(i,1) = col(1); (*this)(i,2) = col(2);
 225     }
 226     return *this;
 227   }
 228
 229   SGVec3<T> xformPt(const SGVec3<T>& pt) const
 230   {
 231     SGVec3<T> tpt;
 232     tpt(0) = (*this)(0,3);
 233     tpt(1) = (*this)(1,3);
 234     tpt(2) = (*this)(2,3);
 235     for (unsigned i = 0; i < SGMatrix<T>::nCols-1; ++i) {
 236       T tmp = pt(i);
 237       tpt(0) += tmp*(*this)(0,i);
 238       tpt(1) += tmp*(*this)(1,i);
 239       tpt(2) += tmp*(*this)(2,i);
 240     }
 241     return tpt;
 242   }
 243   SGVec3<T> xformVec(const SGVec3<T>& v) const
 244   {
 245     SGVec3<T> tv;
 246     T tmp = v(0);
 247     tv(0) = tmp*(*this)(0,0);
 248     tv(1) = tmp*(*this)(1,0);
 249     tv(2) = tmp*(*this)(2,0);
 250     for (unsigned i = 1; i < SGMatrix<T>::nCols-1; ++i) {
 251       T tmp = v(i);
 252       tv(0) += tmp*(*this)(0,i);
 253       tv(1) += tmp*(*this)(1,i);
 254       tv(2) += tmp*(*this)(2,i);
 255     }
 256     return tv;
 257   }
 258
 259   /// Return an all zero matrix
 260   static SGMatrix zeros(void)
 261   { return SGMatrix(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); }
 262
 263   /// Return a unit matrix
 264   static SGMatrix unit(void)
 265   { return SGMatrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); }
 266
 267 private:
 268   /// Required to make that alias safe.
 269   union Data {
 270     T flat[16];
 271     T carray[4][4];
 272   };
 273
 274   /// The actual data, the matrix is stored in column major order,
 275   /// that matches the storage format of OpenGL
 276   Data _data;
 277 };
 278
 279 /// Class to distinguish between a matrix and the matrix with a transposed
 280 /// rotational part and a negated translational part
 281 template<typename T>
 282 struct TransNegRef {
 283   TransNegRef(const SGMatrix<T>& _m) : m(_m) {}
 284   const SGMatrix<T>& m;
 285 };
 286
 287 /// Unary +, do nothing ...
 288 template<typename T>
 289 inline
 290 const SGMatrix<T>&
 291 operator+(const SGMatrix<T>& m)
 292 { return m; }
 293
 294 /// Unary -, do nearly nothing
 295 template<typename T>
 296 inline
 297 SGMatrix<T>
 298 operator-(const SGMatrix<T>& m)
 299 {
 300   SGMatrix<T> ret;
 301   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 302     ret[i] = -m[i];
 303   return ret;
 304 }
 305
 306 /// Binary +
 307 template<typename T>
 308 inline
 309 SGMatrix<T>
 310 operator+(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 311 {
 312   SGMatrix<T> ret;
 313   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 314     ret[i] = m1[i] + m2[i];
 315   return ret;
 316 }
 317
 318 /// Binary -
 319 template<typename T>
 320 inline
 321 SGMatrix<T>
 322 operator-(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 323 {
 324   SGMatrix<T> ret;
 325   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 326     ret[i] = m1[i] - m2[i];
 327   return ret;
 328 }
 329
 330 /// Scalar multiplication
 331 template<typename S, typename T>
 332 inline
 333 SGMatrix<T>
 334 operator*(S s, const SGMatrix<T>& m)
 335 {
 336   SGMatrix<T> ret;
 337   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 338     ret[i] = s*m[i];
 339   return ret;
 340 }
 341
 342 /// Scalar multiplication
 343 template<typename S, typename T>
 344 inline
 345 SGMatrix<T>
 346 operator*(const SGMatrix<T>& m, S s)
 347 {
 348   SGMatrix<T> ret;
 349   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 350     ret[i] = s*m[i];
 351   return ret;
 352 }
 353
 354 /// Vector multiplication
 355 template<typename T>
 356 inline
 357 SGVec4<T>
 358 operator*(const SGMatrix<T>& m, const SGVec4<T>& v)
 359 {
 360   SGVec4<T> mv;
 361   T tmp = v(0);
 362   mv(0) = tmp*m(0,0);
 363   mv(1) = tmp*m(1,0);
 364   mv(2) = tmp*m(2,0);
 365   mv(3) = tmp*m(3,0);
 366   for (unsigned i = 1; i < SGMatrix<T>::nCols; ++i) {
 367     T tmp = v(i);
 368     mv(0) += tmp*m(0,i);
 369     mv(1) += tmp*m(1,i);
 370     mv(2) += tmp*m(2,i);
 371     mv(3) += tmp*m(3,i);
 372   }
 373   return mv;
 374 }
 375
 376 /// Vector multiplication
 377 template<typename T>
 378 inline
 379 SGVec4<T>
 380 operator*(const TransNegRef<T>& tm, const SGVec4<T>& v)
 381 {
 382   const SGMatrix<T>& m = tm.m;
 383   SGVec4<T> mv;
 384   SGVec3<T> v2;
 385   T tmp = v(3);
 386   mv(0) = v2(0) = -tmp*m(0,3);
 387   mv(1) = v2(1) = -tmp*m(1,3);
 388   mv(2) = v2(2) = -tmp*m(2,3);
 389   mv(3) = tmp*m(3,3);
 390   for (unsigned i = 0; i < SGMatrix<T>::nCols - 1; ++i) {
 391     T tmp = v(i) + v2(i);
 392     mv(0) += tmp*m(i,0);
 393     mv(1) += tmp*m(i,1);
 394     mv(2) += tmp*m(i,2);
 395     mv(3) += tmp*m(3,i);
 396   }
 397   return mv;
 398 }
 399
 400 /// Matrix multiplication
 401 template<typename T>
 402 inline
 403 SGMatrix<T>
 404 operator*(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 405 {
 406   SGMatrix<T> m;
 407   for (unsigned j = 0; j < SGMatrix<T>::nCols; ++j) {
 408     T tmp = m2(0,j);
 409     m(0,j) = tmp*m1(0,0);
 410     m(1,j) = tmp*m1(1,0);
 411     m(2,j) = tmp*m1(2,0);
 412     m(3,j) = tmp*m1(3,0);
 413     for (unsigned i = 1; i < SGMatrix<T>::nCols; ++i) {
 414       T tmp = m2(i,j);
 415       m(0,j) += tmp*m1(0,i);
 416       m(1,j) += tmp*m1(1,i);
 417       m(2,j) += tmp*m1(2,i);
 418       m(3,j) += tmp*m1(3,i);
 419     }
 420   }
 421   return m;
 422 }
 423
 424 /// Inplace matrix multiplication, post multiply
 425 template<typename T>
 426 inline
 427 SGMatrix<T>&
 428 SGMatrix<T>::operator*=(const SGMatrix<T>& m2)
 429 { (*this) = operator*(*this, m2); return *this; }
 430
 431 /// Return a reference to the matrix typed to make sure we use the transposed
 432 /// negated matrix
 433 template<typename T>
 434 inline
 435 TransNegRef<T>
 436 transNeg(const SGMatrix<T>& m)
 437 { return TransNegRef<T>(m); }
 438
 439 /// Compute the inverse if the matrix src. Store the result in dst.
 440 /// Return if the matrix is nonsingular. If it is singular dst contains
 441 /// undefined values
 442 template<typename T>
 443 inline
 444 bool
 445 invert(SGMatrix<T>& dst, const SGMatrix<T>& src)
 446 {
 447   // Do a LU decomposition with row pivoting and solve into dst
 448   SGMatrix<T> tmp = src;
 449   dst = SGMatrix<T>::unit();
 450
 451   for (unsigned i = 0; i < 4; ++i) {
 452     T val = tmp(i,i);
 453     unsigned ind = i;
 454
 455     // Find the row with the maximum value in the i-th colum
 456     for (unsigned j = i + 1; j < 4; ++j) {
 457       if (fabs(tmp(j, i)) > fabs(val)) {
 458         ind = j;
 459         val = tmp(j, i);
 460       }
 461     }
 462
 463     // Do row pivoting
 464     if (ind != i) {
 465       for (unsigned j = 0; j < 4; ++j) {
 466         T t;
 467         t = dst(i,j); dst(i,j) = dst(ind,j); dst(ind,j) = t;
 468         t = tmp(i,j); tmp(i,j) = tmp(ind,j); tmp(ind,j) = t;
 469       }
 470     }
 471
 472     // Check for singularity
 473     if (fabs(val) <= SGLimits<T>::min())
 474       return false;
 475
 476     T ival = 1/val;
 477     for (unsigned j = 0; j < 4; ++j) {
 478       tmp(i,j) *= ival;
 479       dst(i,j) *= ival;
 480     }
 481
 482     for (unsigned j = 0; j < 4; ++j) {
 483       if (j == i)
 484         continue;
 485
 486       val = tmp(j,i);
 487       for (unsigned k = 0; k < 4; ++k) {
 488         tmp(j,k) -= tmp(i,k) * val;
 489         dst(j,k) -= dst(i,k) * val;
 490       }
 491     }
 492   }
 493   return true;
 494 }
 495
 496 /// The 1-norm of the matrix, this is the largest column sum of
 497 /// the absolute values of A.
 498 template<typename T>
 499 inline
 500 T
 501 norm1(const SGMatrix<T>& m)
 502 {
 503   T nrm = 0;
 504   for (unsigned i = 0; i < SGMatrix<T>::nRows; ++i) {
 505     T sum = fabs(m(i, 0)) + fabs(m(i, 1)) + fabs(m(i, 2)) + fabs(m(i, 3));
 506     if (nrm < sum)
 507       nrm = sum;
 508   }
 509   return nrm;
 510 }
 511
 512 /// The inf-norm of the matrix, this is the largest row sum of
 513 /// the absolute values of A.
 514 template<typename T>
 515 inline
 516 T
 517 normInf(const SGMatrix<T>& m)
 518 {
 519   T nrm = 0;
 520   for (unsigned i = 0; i < SGMatrix<T>::nCols; ++i) {
 521     T sum = fabs(m(0, i)) + fabs(m(1, i)) + fabs(m(2, i)) + fabs(m(3, i));
 522     if (nrm < sum)
 523       nrm = sum;
 524   }
 525   return nrm;
 526 }
 527
 528 /// Return true if exactly the same
 529 template<typename T>
 530 inline
 531 bool
 532 operator==(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 533 {
 534   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 535     if (m1[i] != m2[i])
 536       return false;
 537   return true;
 538 }
 539
 540 /// Return true if not exactly the same
 541 template<typename T>
 542 inline
 543 bool
 544 operator!=(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 545 { return ! (m1 == m2); }
 546
 547 /// Return true if equal to the relative tolerance tol
 548 template<typename T>
 549 inline
 550 bool
 551 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2, T rtol, T atol)
 552 { return norm1(m1 - m2) < rtol*(norm1(m1) + norm1(m2)) + atol; }
 553
 554 /// Return true if equal to the relative tolerance tol
 555 template<typename T>
 556 inline
 557 bool
 558 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2, T rtol)
 559 { return norm1(m1 - m2) < rtol*(norm1(m1) + norm1(m2)); }
 560
 561 /// Return true if about equal to roundoff of the underlying type
 562 template<typename T>
 563 inline
 564 bool
 565 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 566 {
 567   T tol = 100*SGLimits<T>::epsilon();
 568   return equivalent(m1, m2, tol, tol);
 569 }
 570
 571 #ifndef NDEBUG
 572 template<typename T>
 573 inline
 574 bool
 575 isNaN(const SGMatrix<T>& m)
 576 {
 577   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i) {
 578     if (SGMisc<T>::isNaN(m[i]))
 579       return true;
 580   }
 581   return false;
 582 }
 583 #endif
 584
 585 /// Output to an ostream
 586 template<typename char_type, typename traits_type, typename T>
 587 inline
 588 std::basic_ostream<char_type, traits_type>&
 589 operator<<(std::basic_ostream<char_type, traits_type>& s, const SGMatrix<T>& m)
 590 {
 591   s << "[ " << m(0,0) << ", " << m(0,1) << ", " << m(0,2) << ", " << m(0,3) << "\n";
 592   s << "  " << m(1,0) << ", " << m(1,1) << ", " << m(1,2) << ", " << m(1,3) << "\n";
 593   s << "  " << m(2,0) << ", " << m(2,1) << ", " << m(2,2) << ", " << m(2,3) << "\n";
 594   s << "  " << m(3,0) << ", " << m(3,1) << ", " << m(3,2) << ", " << m(3,3) << " ]";
 595   return s;
 596 }
 597
 598 inline
 599 SGMatrixf
 600 toMatrixf(const SGMatrixd& m)
 601 {
 602   return SGMatrixf((float)m(0,0), (float)m(0,1), (float)m(0,2), (float)m(0,3),
 603                    (float)m(1,0), (float)m(1,1), (float)m(1,2), (float)m(1,3),
 604                    (float)m(2,0), (float)m(2,1), (float)m(2,2), (float)m(2,3),
 605                    (float)m(3,0), (float)m(3,1), (float)m(3,2), (float)m(3,3));
 606 }
 607
 608 inline
 609 SGMatrixd
 610 toMatrixd(const SGMatrixf& m)
 611 {
 612   return SGMatrixd(m(0,0), m(0,1), m(0,2), m(0,3),
 613                    m(1,0), m(1,1), m(1,2), m(1,3),
 614                    m(2,0), m(2,1), m(2,2), m(2,3),
 615                    m(3,0), m(3,1), m(3,2), m(3,3));
 616 }
 617
 618 #endif