//=========================================================================== // GoTools Core - SINTEF Geometry Tools Core library, version 2.0.1 // // Copyright (C) 2000-2007, 2010 SINTEF ICT, Applied Mathematics, Norway. // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License // as published by the Free Software Foundation version 2 of the License. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program; if not, write to the Free Software // Foundation, Inc., // 59 Temple Place - Suite 330, // Boston, MA 02111-1307, USA. // // Contact information: E-mail: tor.dokken@sintef.no // SINTEF ICT, Department of Applied Mathematics, // P.O. Box 124 Blindern, // 0314 Oslo, Norway. // // Other licenses are also available for this software, notably licenses // for: // - Building commercial software. // - Building software whose source code you wish to keep private. //=========================================================================== 00015 #ifndef _MATRIXXD_H 00016 #define _MATRIXXD_H 00017 00018 #include "GoTools/utils/Array.h" 00019 #include <boost/static_assert.hpp> 00020 #include <algorithm> 00021 #include <iostream> 00022 #include <cmath> 00023 00024 00025 namespace Go 00026 { 00027 00032 template <typename T, int Dim> 00033 class MatrixXD 00034 { 00035 public: 00037 MatrixXD() {} 00038 00040 ~MatrixXD() {} 00041 00043 T& operator()(int row, int col) 00044 { 00045 return m_[row][col]; 00046 } 00047 00049 T** get() {return m_;} 00050 00052 const T& operator()(int row, int col) const 00053 { 00054 return m_[row][col]; 00055 } 00056 00058 void zero() 00059 { 00060 for (int i = 0; i < Dim; ++i) { 00061 for (int j = 0; j < Dim; ++j) { 00062 m_[i][j] = 0; 00063 } 00064 } 00065 } 00066 00068 void identity() 00069 { 00070 zero(); 00071 for (int i = 0; i < Dim; ++i) { 00072 m_[i][i] = 1; 00073 } 00074 } 00075 00077 void transpose() 00078 { 00079 for (int i = 0; i < Dim; ++i) { 00080 for (int j = 0; j < Dim; ++j) { 00081 std::swap(m_[i][j], m_[j][i]); 00082 } 00083 } 00084 } 00085 00090 void setToRotation(T angle, T x, T y, T z); 00091 00096 void setToRotation(const Vector3D& p, const Vector3D& q); 00097 00099 MatrixXD operator * (const MatrixXD& other) const 00100 { 00101 MatrixXD res; 00102 T inner; 00103 for (int i = 0; i < Dim; ++i) { 00104 for (int j = 0; j < Dim; ++j) { 00105 inner = 0; 00106 for (int k = 0; k < Dim; ++k) { 00107 inner += m_[i][k] * other.m_[k][j]; 00108 } 00109 res.m_[i][j] = inner; 00110 } 00111 } 00112 return res; 00113 } 00114 00116 MatrixXD& operator *= (const MatrixXD& other) 00117 { 00118 (*this) = (*this) * other; 00119 return *this; 00120 } 00121 00123 MatrixXD operator * (T scalar) const 00124 { 00125 MatrixXD dup(*this); 00126 dup *= scalar; 00127 return dup; 00128 } 00129 00131 MatrixXD& operator *= (T scalar) 00132 { 00133 for (int i = 0; i < Dim; ++i) { 00134 for (int j = 0; j < Dim; ++j) { 00135 m_[i][j] *= scalar; 00136 } 00137 } 00138 return *this; 00139 } 00140 00142 MatrixXD operator + (const MatrixXD& other) const 00143 { 00144 MatrixXD dup(*this); 00145 dup += other; 00146 return dup; 00147 } 00148 00150 MatrixXD& operator += (const MatrixXD& other) 00151 { 00152 for (int i = 0; i < Dim; ++i) { 00153 for (int j = 0; j < Dim; ++j) { 00154 m_[i][j] += other.m_[i][j]; 00155 } 00156 } 00157 return *this; 00158 } 00159 00161 MatrixXD operator - () const 00162 { 00163 MatrixXD dup(*this); 00164 for (int i = 0; i < Dim; ++i) { 00165 for (int j = 0; j < Dim; ++j) { 00166 dup.m_[i][j] = -m_[i][j]; 00167 } 00168 } 00169 return dup; 00170 } 00171 00173 template <class VectorType> 00174 VectorType operator * (const VectorType& vec) const 00175 { 00176 VectorType res(vec); // Make a copy so that it gets the right size. 00177 T inner; 00178 for (int i = 0; i < Dim; ++i) { 00179 inner = 0; 00180 for (int k = 0; k < Dim; ++k) { 00181 inner += m_[i][k] * vec[k]; 00182 } 00183 res[i] = inner; 00184 } 00185 return res; 00186 } 00187 00189 template <class FromConstIteratorType, class ToIteratorType> 00190 void mult (const FromConstIteratorType from, 00191 const ToIteratorType to) const 00192 { 00193 T inner; 00194 for (int i = 0; i < Dim; ++i) { 00195 inner = 0; 00196 for (int k = 0; k < Dim; ++k) { 00197 inner += m_[i][k] * from[k]; 00198 } 00199 to[i] = inner; 00200 } 00201 } 00202 00204 T det() const; 00205 00207 T trace() const 00208 { 00209 T tr(0.0); 00210 for (int i = 0; i < Dim ; ++i) { 00211 for (int j = 0; j < Dim ; ++j) { 00212 tr +=m_[i][j]; 00213 } 00214 } 00215 return tr; 00216 } 00217 00219 T frobeniusNorm() const 00220 { 00221 T fn(0.0); 00222 for (int i = 0; i < Dim ; ++i) { 00223 for (int j = 0; j < Dim ; ++j) { 00224 fn +=m_[i][j]*m_[i][j]; 00225 } 00226 } 00227 fn = std::sqrt(fn); 00228 return fn; 00229 } 00230 00232 MatrixXD<T, Dim-1> submatrix(int r, int c) const 00233 { 00234 MatrixXD<T, Dim-1> subm; 00235 for (int j = 0; j < Dim; ++j) { 00236 if (j == r) continue; 00237 int joff = 0; 00238 if (j > r) joff = -1; 00239 for (int k = 0; k < Dim; ++k) { 00240 if (k == c) continue; 00241 int koff = 0; 00242 if (k > c) koff = -1; 00243 subm(j + joff, k + koff) = m_[j][k]; 00244 } 00245 } 00246 return subm; 00247 } 00248 00249 private: 00250 T m_[Dim][Dim]; 00251 }; 00252 00253 00254 template <typename T, int Dim> 00255 inline void MatrixXD<T,Dim>::setToRotation(T angle, T x, T y, T z) 00256 { 00257 BOOST_STATIC_ASSERT(Dim == 3 || Dim == 4); 00258 THROW("This code should never be entered!"); 00259 } 00260 00261 template <> 00262 inline void MatrixXD<double,3>::setToRotation(double angle, double x, double y, double z) 00263 { 00264 Array<double, 3> u(x, y, z); 00265 u.normalize(); 00266 MatrixXD<double,3> S; 00267 S(0,0) = S(1,1) = S(2,2) = 0.0; 00268 S(0,1) = -u[2]; 00269 S(1,0) = u[2]; 00270 S(0,2) = u[1]; 00271 S(2,0) = -u[1]; 00272 S(1,2) = -u[0]; 00273 S(2,1) = u[0]; 00274 MatrixXD<double,3> uut; 00275 uut(0,0) = u[0]*u[0]; 00276 uut(0,1) = u[0]*u[1]; 00277 uut(0,2) = u[0]*u[2]; 00278 uut(1,0) = u[1]*u[0]; 00279 uut(1,1) = u[1]*u[1]; 00280 uut(1,2) = u[1]*u[2]; 00281 uut(2,0) = u[2]*u[0]; 00282 uut(2,1) = u[2]*u[1]; 00283 uut(2,2) = u[2]*u[2]; 00284 // Now, make this matrix into 00285 // uut + cos(angle)*(I-uut) + sin(angle)*S; 00286 double cosang = std::cos(angle); 00287 double sinang = std::sin(angle); 00288 uut *= (double(1.0) - cosang); 00289 S *= sinang; 00290 identity(); 00291 (*this) *= cosang; 00292 (*this) += uut; 00293 (*this) += S; 00294 } 00295 00296 00297 template <> 00298 inline void MatrixXD<double,4>::setToRotation(double angle, double x, double y, double z) 00299 { 00300 identity(); 00301 MatrixXD<double,3> r; 00302 r.setToRotation(angle, x, y, z); 00303 m_[0][0] = r(0,0); 00304 m_[0][1] = r(0,1); 00305 m_[0][2] = r(0,2); 00306 m_[1][0] = r(1,0); 00307 m_[1][1] = r(1,1); 00308 m_[1][2] = r(1,2); 00309 m_[2][0] = r(2,0); 00310 m_[2][1] = r(2,1); 00311 m_[2][2] = r(2,2); 00312 } 00313 00314 template <typename T, int Dim> 00315 inline void MatrixXD<T,Dim>::setToRotation(const Vector3D& p, 00316 const Vector3D& q) 00317 { 00318 BOOST_STATIC_ASSERT(Dim == 3); 00319 THROW("This code should never be entered!"); 00320 } 00321 00322 template <> 00323 inline void MatrixXD<double, 3>::setToRotation(const Vector3D& p, 00324 const Vector3D& q) 00325 { 00326 Vector3D v = p % q; 00327 00328 // double alpha = p.angle(q); 00329 // setToRotation(alpha, v[0], v[1], v[2]); 00330 00331 MatrixXD<double,3> S; 00332 S(0,0) = S(1,1) = S(2,2) = 0.0; 00333 S(0,1) = -v[2]; 00334 S(1,0) = v[2]; 00335 S(0,2) = v[1]; 00336 S(2,0) = -v[1]; 00337 S(1,2) = -v[0]; 00338 S(2,1) = v[0]; 00339 MatrixXD<double,3> vvt; 00340 vvt(0,0) = v[0]*v[0]; 00341 vvt(0,1) = v[0]*v[1]; 00342 vvt(0,2) = v[0]*v[2]; 00343 vvt(1,0) = v[1]*v[0]; 00344 vvt(1,1) = v[1]*v[1]; 00345 vvt(1,2) = v[1]*v[2]; 00346 vvt(2,0) = v[2]*v[0]; 00347 vvt(2,1) = v[2]*v[1]; 00348 vvt(2,2) = v[2]*v[2]; 00349 // Now, make this matrix into 00350 // vvt + cos(angle)*(I+vvt) + S; 00351 double cosang = p*q; 00352 if (cosang+1.0 > -1.0e-13 && cosang+1.0 < 1.0e-13) 00353 ; 00354 else 00355 vvt *= 1.0/(1.0 + cosang); 00356 identity(); 00357 (*this) *= cosang; 00358 (*this) += vvt; 00359 (*this) += S; 00360 } 00361 00363 template <typename T, int Dim> 00364 class DetComp 00365 { 00366 public: 00367 T det(const T m[Dim][Dim]) 00368 { 00369 // @@ Slow implementation... 00370 // Developing along the first coordinate (rows). 00371 T result(0); 00372 for (int i = 0; i < Dim; ++i) { 00373 // Make the submatrix 00374 MatrixXD<T, Dim-1> subm; 00375 for (int j = 1; j < Dim; ++j) { 00376 for (int k = 0; k < Dim; ++k) { 00377 if (k == i) continue; 00378 int koff = 0; 00379 if (k > i) koff = -1; 00380 subm(j - 1, k + koff) = m[j][k]; 00381 } 00382 } 00383 // Add or subtract the sub determinant 00384 if (i/2 == (i+1)/2) { 00385 result += subm.det()*m[0][i]; 00386 } else { 00387 result -= subm.det()*m[0][i]; 00388 } 00389 } 00390 return result; 00391 } 00392 }; 00394 00397 // template<> removed by JAM 00399 template<typename T> 00400 class DetComp<T,1> 00401 { 00402 public: 00403 T det(const T m[1][1]) 00404 { 00405 return m[0][0]; 00406 } 00407 }; 00408 00410 template <typename T, int Dim> 00411 inline T MatrixXD<T,Dim>::det() const 00412 { 00413 DetComp<T,Dim> dc; 00414 return dc.det(m_); 00415 } 00416 00418 template <typename T, int Dim> 00419 inline std::istream& operator>> (std::istream& is, MatrixXD<T, Dim>& m) 00420 { 00421 for (int i = 0; i < Dim; ++i) { 00422 for (int j = 0; j < Dim; ++j) { 00423 is >> m(i,j); 00424 } 00425 } 00426 return is; 00427 } 00429 00431 template <typename T, int Dim> 00432 inline std::ostream& operator<< (std::ostream& os, const MatrixXD<T, Dim>& m) 00433 { 00434 for (int i = 0; i < Dim; ++i) { 00435 for (int j = 0; j < Dim; ++j) { 00436 os << m(i,j) << ' '; 00437 } 00438 os << '\n'; 00439 } 00440 return os; 00441 } 00442 00443 00444 } // namespace Go 00445 00446 00447 #endif // _MATRIXXD_H 00448
Generated on Tue Sep 21 15:44:17 2010 for GoTools Core by  doxygen 1.6.3