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     return *this;
 197   }
 198   template<typename S>
 199   SGMatrix& postMultTranslate(const SGVec3<S>& t)
 200   {
 201     SGVec4<T> col3((*this)(0,3), (*this)(1,3), (*this)(2,3), (*this)(3,3));
 202     for (unsigned i = 0; i < SGMatrix<T>::nCols-1; ++i) {
 203       SGVec4<T> tmp((*this)(0,i), (*this)(1,i), (*this)(2,i), (*this)(3,i));
 204       col3 += T(t(i))*tmp;
 205     }
 206     (*this)(0,3) = col3(0); (*this)(1,3) = col3(1);
 207     (*this)(2,3) = col3(2); (*this)(3,3) = col3(3);
 208     return *this;
 209   }
 210
 211   SGMatrix& preMultRotate(const SGQuat<T>& r)
 212   {
 213     for (unsigned i = 0; i < SGMatrix<T>::nCols; ++i) {
 214       SGVec3<T> col((*this)(0,i), (*this)(1,i), (*this)(2,i));
 215       col = r.transform(col);
 216       (*this)(0,i) = col(0); (*this)(1,i) = col(1); (*this)(2,i) = col(2);
 217     }
 218     return *this;
 219   }
 220   SGMatrix& postMultRotate(const SGQuat<T>& r)
 221   {
 222     for (unsigned i = 0; i < SGMatrix<T>::nCols; ++i) {
 223       SGVec3<T> col((*this)(i,0), (*this)(i,1), (*this)(i,2));
 224       col = r.backTransform(col);
 225       (*this)(i,0) = col(0); (*this)(i,1) = col(1); (*this)(i,2) = col(2);
 226     }
 227     return *this;
 228   }
 229
 230   SGVec3<T> xformPt(const SGVec3<T>& pt) const
 231   {
 232     SGVec3<T> tpt;
 233     tpt(0) = (*this)(0,3);
 234     tpt(1) = (*this)(1,3);
 235     tpt(2) = (*this)(2,3);
 236     for (unsigned i = 0; i < SGMatrix<T>::nCols-1; ++i) {
 237       T tmp = pt(i);
 238       tpt(0) += tmp*(*this)(0,i);
 239       tpt(1) += tmp*(*this)(1,i);
 240       tpt(2) += tmp*(*this)(2,i);
 241     }
 242     return tpt;
 243   }
 244   SGVec3<T> xformVec(const SGVec3<T>& v) const
 245   {
 246     SGVec3<T> tv;
 247     T tmp = v(0);
 248     tv(0) = tmp*(*this)(0,0);
 249     tv(1) = tmp*(*this)(1,0);
 250     tv(2) = tmp*(*this)(2,0);
 251     for (unsigned i = 1; i < SGMatrix<T>::nCols-1; ++i) {
 252       T tmp = v(i);
 253       tv(0) += tmp*(*this)(0,i);
 254       tv(1) += tmp*(*this)(1,i);
 255       tv(2) += tmp*(*this)(2,i);
 256     }
 257     return tv;
 258   }
 259
 260   /// Return an all zero matrix
 261   static SGMatrix zeros(void)
 262   { return SGMatrix(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); }
 263
 264   /// Return a unit matrix
 265   static SGMatrix unit(void)
 266   { return SGMatrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); }
 267
 268 private:
 269   /// Required to make that alias safe.
 270   union Data {
 271     T flat[16];
 272     T carray[4][4];
 273   };
 274
 275   /// The actual data, the matrix is stored in column major order,
 276   /// that matches the storage format of OpenGL
 277   Data _data;
 278 };
 279
 280 /// Class to distinguish between a matrix and the matrix with a transposed
 281 /// rotational part and a negated translational part
 282 template<typename T>
 283 struct TransNegRef {
 284   TransNegRef(const SGMatrix<T>& _m) : m(_m) {}
 285   const SGMatrix<T>& m;
 286 };
 287
 288 /// Unary +, do nothing ...
 289 template<typename T>
 290 inline
 291 const SGMatrix<T>&
 292 operator+(const SGMatrix<T>& m)
 293 { return m; }
 294
 295 /// Unary -, do nearly nothing
 296 template<typename T>
 297 inline
 298 SGMatrix<T>
 299 operator-(const SGMatrix<T>& m)
 300 {
 301   SGMatrix<T> ret;
 302   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 303     ret[i] = -m[i];
 304   return ret;
 305 }
 306
 307 /// Binary +
 308 template<typename T>
 309 inline
 310 SGMatrix<T>
 311 operator+(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 312 {
 313   SGMatrix<T> ret;
 314   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 315     ret[i] = m1[i] + m2[i];
 316   return ret;
 317 }
 318
 319 /// Binary -
 320 template<typename T>
 321 inline
 322 SGMatrix<T>
 323 operator-(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 324 {
 325   SGMatrix<T> ret;
 326   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 327     ret[i] = m1[i] - m2[i];
 328   return ret;
 329 }
 330
 331 /// Scalar multiplication
 332 template<typename S, typename T>
 333 inline
 334 SGMatrix<T>
 335 operator*(S s, const SGMatrix<T>& m)
 336 {
 337   SGMatrix<T> ret;
 338   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 339     ret[i] = s*m[i];
 340   return ret;
 341 }
 342
 343 /// Scalar multiplication
 344 template<typename S, typename T>
 345 inline
 346 SGMatrix<T>
 347 operator*(const SGMatrix<T>& m, S s)
 348 {
 349   SGMatrix<T> ret;
 350   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 351     ret[i] = s*m[i];
 352   return ret;
 353 }
 354
 355 /// Vector multiplication
 356 template<typename T>
 357 inline
 358 SGVec4<T>
 359 operator*(const SGMatrix<T>& m, const SGVec4<T>& v)
 360 {
 361   SGVec4<T> mv;
 362   T tmp = v(0);
 363   mv(0) = tmp*m(0,0);
 364   mv(1) = tmp*m(1,0);
 365   mv(2) = tmp*m(2,0);
 366   mv(3) = tmp*m(3,0);
 367   for (unsigned i = 1; i < SGMatrix<T>::nCols; ++i) {
 368     T tmp = v(i);
 369     mv(0) += tmp*m(0,i);
 370     mv(1) += tmp*m(1,i);
 371     mv(2) += tmp*m(2,i);
 372     mv(3) += tmp*m(3,i);
 373   }
 374   return mv;
 375 }
 376
 377 /// Vector multiplication
 378 template<typename T>
 379 inline
 380 SGVec4<T>
 381 operator*(const TransNegRef<T>& tm, const SGVec4<T>& v)
 382 {
 383   const SGMatrix<T>& m = tm.m;
 384   SGVec4<T> mv;
 385   SGVec3<T> v2;
 386   T tmp = v(3);
 387   mv(0) = v2(0) = -tmp*m(0,3);
 388   mv(1) = v2(1) = -tmp*m(1,3);
 389   mv(2) = v2(2) = -tmp*m(2,3);
 390   mv(3) = tmp*m(3,3);
 391   for (unsigned i = 0; i < SGMatrix<T>::nCols - 1; ++i) {
 392     T tmp = v(i) + v2(i);
 393     mv(0) += tmp*m(i,0);
 394     mv(1) += tmp*m(i,1);
 395     mv(2) += tmp*m(i,2);
 396     mv(3) += tmp*m(3,i);
 397   }
 398   return mv;
 399 }
 400
 401 /// Matrix multiplication
 402 template<typename T>
 403 inline
 404 SGMatrix<T>
 405 operator*(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 406 {
 407   SGMatrix<T> m;
 408   for (unsigned j = 0; j < SGMatrix<T>::nCols; ++j) {
 409     T tmp = m2(0,j);
 410     m(0,j) = tmp*m1(0,0);
 411     m(1,j) = tmp*m1(1,0);
 412     m(2,j) = tmp*m1(2,0);
 413     m(3,j) = tmp*m1(3,0);
 414     for (unsigned i = 1; i < SGMatrix<T>::nCols; ++i) {
 415       T tmp = m2(i,j);
 416       m(0,j) += tmp*m1(0,i);
 417       m(1,j) += tmp*m1(1,i);
 418       m(2,j) += tmp*m1(2,i);
 419       m(3,j) += tmp*m1(3,i);
 420     }
 421   }
 422   return m;
 423 }
 424
 425 /// Inplace matrix multiplication, post multiply
 426 template<typename T>
 427 inline
 428 SGMatrix<T>&
 429 SGMatrix<T>::operator*=(const SGMatrix<T>& m2)
 430 { (*this) = operator*(*this, m2); return *this; }
 431
 432 /// Return a reference to the matrix typed to make sure we use the transposed
 433 /// negated matrix
 434 template<typename T>
 435 inline
 436 TransNegRef<T>
 437 transNeg(const SGMatrix<T>& m)
 438 { return TransNegRef<T>(m); }
 439
 440 /// Compute the inverse if the matrix src. Store the result in dst.
 441 /// Return if the matrix is nonsingular. If it is singular dst contains
 442 /// undefined values
 443 template<typename T>
 444 inline
 445 bool
 446 invert(SGMatrix<T>& dst, const SGMatrix<T>& src)
 447 {
 448   // Do a LU decomposition with row pivoting and solve into dst
 449   SGMatrix<T> tmp = src;
 450   dst = SGMatrix<T>::unit();
 451
 452   for (unsigned i = 0; i < 4; ++i) {
 453     T val = tmp(i,i);
 454     unsigned ind = i;
 455
 456     // Find the row with the maximum value in the i-th colum
 457     for (unsigned j = i + 1; j < 4; ++j) {
 458       if (fabs(tmp(j, i)) > fabs(val)) {
 459         ind = j;
 460         val = tmp(j, i);
 461       }
 462     }
 463
 464     // Do row pivoting
 465     if (ind != i) {
 466       for (unsigned j = 0; j < 4; ++j) {
 467         T t;
 468         t = dst(i,j); dst(i,j) = dst(ind,j); dst(ind,j) = t;
 469         t = tmp(i,j); tmp(i,j) = tmp(ind,j); tmp(ind,j) = t;
 470       }
 471     }
 472
 473     // Check for singularity
 474     if (fabs(val) <= SGLimits<T>::min())
 475       return false;
 476
 477     T ival = 1/val;
 478     for (unsigned j = 0; j < 4; ++j) {
 479       tmp(i,j) *= ival;
 480       dst(i,j) *= ival;
 481     }
 482
 483     for (unsigned j = 0; j < 4; ++j) {
 484       if (j == i)
 485         continue;
 486
 487       val = tmp(j,i);
 488       for (unsigned k = 0; k < 4; ++k) {
 489         tmp(j,k) -= tmp(i,k) * val;
 490         dst(j,k) -= dst(i,k) * val;
 491       }
 492     }
 493   }
 494   return true;
 495 }
 496
 497 /// The 1-norm of the matrix, this is the largest column sum of
 498 /// the absolute values of A.
 499 template<typename T>
 500 inline
 501 T
 502 norm1(const SGMatrix<T>& m)
 503 {
 504   T nrm = 0;
 505   for (unsigned i = 0; i < SGMatrix<T>::nRows; ++i) {
 506     T sum = fabs(m(i, 0)) + fabs(m(i, 1)) + fabs(m(i, 2)) + fabs(m(i, 3));
 507     if (nrm < sum)
 508       nrm = sum;
 509   }
 510   return nrm;
 511 }
 512
 513 /// The inf-norm of the matrix, this is the largest row sum of
 514 /// the absolute values of A.
 515 template<typename T>
 516 inline
 517 T
 518 normInf(const SGMatrix<T>& m)
 519 {
 520   T nrm = 0;
 521   for (unsigned i = 0; i < SGMatrix<T>::nCols; ++i) {
 522     T sum = fabs(m(0, i)) + fabs(m(1, i)) + fabs(m(2, i)) + fabs(m(3, i));
 523     if (nrm < sum)
 524       nrm = sum;
 525   }
 526   return nrm;
 527 }
 528
 529 /// Return true if exactly the same
 530 template<typename T>
 531 inline
 532 bool
 533 operator==(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 534 {
 535   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 536     if (m1[i] != m2[i])
 537       return false;
 538   return true;
 539 }
 540
 541 /// Return true if not exactly the same
 542 template<typename T>
 543 inline
 544 bool
 545 operator!=(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 546 { return ! (m1 == m2); }
 547
 548 /// Return true if equal to the relative tolerance tol
 549 template<typename T>
 550 inline
 551 bool
 552 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2, T rtol, T atol)
 553 { return norm1(m1 - m2) < rtol*(norm1(m1) + norm1(m2)) + atol; }
 554
 555 /// Return true if equal to the relative tolerance tol
 556 template<typename T>
 557 inline
 558 bool
 559 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2, T rtol)
 560 { return norm1(m1 - m2) < rtol*(norm1(m1) + norm1(m2)); }
 561
 562 /// Return true if about equal to roundoff of the underlying type
 563 template<typename T>
 564 inline
 565 bool
 566 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 567 {
 568   T tol = 100*SGLimits<T>::epsilon();
 569   return equivalent(m1, m2, tol, tol);
 570 }
 571
 572 #ifndef NDEBUG
 573 template<typename T>
 574 inline
 575 bool
 576 isNaN(const SGMatrix<T>& m)
 577 {
 578   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i) {
 579     if (SGMisc<T>::isNaN(m[i]))
 580       return true;
 581   }
 582   return false;
 583 }
 584 #endif
 585
 586 /// Output to an ostream
 587 template<typename char_type, typename traits_type, typename T>
 588 inline
 589 std::basic_ostream<char_type, traits_type>&
 590 operator<<(std::basic_ostream<char_type, traits_type>& s, const SGMatrix<T>& m)
 591 {
 592   s << "[ " << m(0,0) << ", " << m(0,1) << ", " << m(0,2) << ", " << m(0,3) << "\n";
 593   s << "  " << m(1,0) << ", " << m(1,1) << ", " << m(1,2) << ", " << m(1,3) << "\n";
 594   s << "  " << m(2,0) << ", " << m(2,1) << ", " << m(2,2) << ", " << m(2,3) << "\n";
 595   s << "  " << m(3,0) << ", " << m(3,1) << ", " << m(3,2) << ", " << m(3,3) << " ]";
 596   return s;
 597 }
 598
 599 inline
 600 SGMatrixf
 601 toMatrixf(const SGMatrixd& m)
 602 {
 603   return SGMatrixf((float)m(0,0), (float)m(0,1), (float)m(0,2), (float)m(0,3),
 604                    (float)m(1,0), (float)m(1,1), (float)m(1,2), (float)m(1,3),
 605                    (float)m(2,0), (float)m(2,1), (float)m(2,2), (float)m(2,3),
 606                    (float)m(3,0), (float)m(3,1), (float)m(3,2), (float)m(3,3));
 607 }
 608
 609 inline
 610 SGMatrixd
 611 toMatrixd(const SGMatrixf& m)
 612 {
 613   return SGMatrixd(m(0,0), m(0,1), m(0,2), m(0,3),
 614                    m(1,0), m(1,1), m(1,2), m(1,3),
 615                    m(2,0), m(2,1), m(2,2), m(2,3),
 616                    m(3,0), m(3,1), m(3,2), m(3,3));
 617 }
 618
 619 #endif