00001 #ifndef SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_
00002 #define SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00041 namespace SimTK {
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060 template <int M, class E1, int S1, class E2, int S2> inline
00061 typename CNT<typename CNT<E1>::THerm>::template Result<E2>::Mul
00062 dot(const Vec<M,E1,S1>& r, const Vec<M,E2,S2>& v) {
00063 typename CNT<typename CNT<E1>::THerm>::template Result<E2>::Mul sum(dot(reinterpret_cast<const Vec<M-1,E1,S1>&>(r), reinterpret_cast<const Vec<M-1,E2,S2>&>(v)) + CNT<E1>::transpose(r[M-1])*v[M-1]);
00064 return sum;
00065 }
00066 template <class E1, int S1, class E2, int S2> inline
00067 typename CNT<typename CNT<E1>::THerm>::template Result<E2>::Mul
00068 dot(const Vec<1,E1,S1>& r, const Vec<1,E2,S2>& v) {
00069 typename CNT<typename CNT<E1>::THerm>::template Result<E2>::Mul sum(CNT<E1>::transpose(r[0])*v[0]);
00070 return sum;
00071 }
00072
00073
00074 template <int N, class E1, int S1, class E2, int S2> inline
00075 typename CNT<E1>::template Result<E2>::Mul
00076 operator*(const Row<N,E1,S1>& r, const Vec<N,E2,S2>& v) {
00077 typename CNT<E1>::template Result<E2>::Mul sum(reinterpret_cast<const Row<N-1,E1,S1>&>(r)*reinterpret_cast<const Vec<N-1,E2,S2>&>(v) + r[N-1]*v[N-1]);
00078 return sum;
00079 }
00080 template <class E1, int S1, class E2, int S2> inline
00081 typename CNT<E1>::template Result<E2>::Mul
00082 operator*(const Row<1,E1,S1>& r, const Vec<1,E2,S2>& v) {
00083 typename CNT<E1>::template Result<E2>::Mul sum(r[0]*v[0]);
00084 return sum;
00085 }
00086
00087
00088 template <int N, class E1, int S1, class E2, int S2> inline
00089 typename CNT<E1>::template Result<E2>::Mul
00090 dot(const Row<N,E1,S1>& r, const Vec<N,E2,S2>& v) {
00091 return dot(r.positionalTranspose(),v);
00092 }
00093 template <int M, class E1, int S1, class E2, int S2> inline
00094 typename CNT<E1>::template Result<E2>::Mul
00095 dot(const Vec<M,E1,S1>& v, const Row<M,E2,S2>& r) {
00096 return dot(v,r.positionalTranspose());
00097 }
00098 template <int N, class E1, int S1, class E2, int S2> inline
00099 typename CNT<E1>::template Result<E2>::Mul
00100 dot(const Row<N,E1,S1>& r, const Row<N,E2,S2>& s) {
00101 return dot(r.positionalTranspose(),s.positionalTranspose());
00102 }
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121 template <int M, class E1, int S1, class E2, int S2> inline
00122 Mat<M,M, typename CNT<E1>::template Result<typename CNT<E2>::THerm>::Mul>
00123 outer(const Vec<M,E1,S1>& v, const Vec<M,E2,S2>& w) {
00124 Mat<M,M, typename CNT<E1>::template Result<typename CNT<E2>::THerm>::Mul> m;
00125 for (int i=0; i<M; ++i)
00126 m[i] = v[i] * ~w;
00127 return m;
00128 }
00129
00130
00131 template <int M, class E1, int S1, class E2, int S2> inline
00132 typename Vec<M,E1,S1>::template Result<Row<M,E2,S2> >::Mul
00133 operator*(const Vec<M,E1,S1>& v, const Row<M,E2,S2>& r) {
00134 return Vec<M,E1,S1>::template Result<Row<M,E2,S2> >::MulOp::perform(v,r);
00135 }
00136
00137
00138
00139 template <int M, class E1, int S1, class E2, int S2> inline
00140 Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
00141 outer(const Vec<M,E1,S1>& v, const Row<M,E2,S2>& r) {
00142 return outer(v,r.positionalTranspose());
00143 }
00144 template <int M, class E1, int S1, class E2, int S2> inline
00145 Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
00146 outer(const Row<M,E1,S1>& r, const Vec<M,E2,S2>& v) {
00147 return outer(r.positionalTranspose(),v);
00148 }
00149 template <int M, class E1, int S1, class E2, int S2> inline
00150 Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
00151 outer(const Row<M,E1,S1>& r, const Row<M,E2,S2>& s) {
00152 return outer(r.positionalTranspose(),s.positionalTranspose());
00153 }
00154
00155
00156
00157
00158 template <int M, int N, class ME, int CS, int RS, class E, int S> inline
00159 typename Mat<M,N,ME,CS,RS>::template Result<Vec<N,E,S> >::Mul
00160 operator*(const Mat<M,N,ME,CS,RS>& m,const Vec<N,E,S>& v) {
00161 typename Mat<M,N,ME,CS,RS>::template Result<Vec<N,E,S> >::Mul result;
00162 for (int i=0; i<M; ++i)
00163 result[i] = m[i]*v;
00164 return result;
00165 }
00166
00167
00168 template <int M, class E, int S, int N, class ME, int CS, int RS> inline
00169 typename Row<M,E,S>::template Result<Mat<M,N,ME,CS,RS> >::Mul
00170 operator*(const Row<M,E,S>& r, const Mat<M,N,ME,CS,RS>& m) {
00171 typename Row<M,E,S>::template Result<Mat<M,N,ME,CS,RS> >::Mul result;
00172 for (int i=0; i<N; ++i)
00173 result[i] = r*m(i);
00174 return result;
00175 }
00176
00177
00178
00179
00180 template <int N, class ME, int RS, class E, int S> inline
00181 typename SymMat<N,ME,RS>::template Result<Vec<N,E,S> >::Mul
00182 operator*(const SymMat<N,ME,RS>& m,const Vec<N,E,S>& v) {
00183 typename SymMat<N,ME,RS>::template Result<Vec<N,E,S> >::Mul result;
00184 for (int i=0; i<N; ++i) {
00185 result[i] = m.getDiag()[i]*v[i];
00186 for (int j=0; j<i; ++j)
00187 result[i] += m.getEltLower(i,j)*v[j];
00188 for (int j=i+1; j<N; ++j)
00189 result[i] += m.getEltUpper(i,j)*v[j];
00190 }
00191 return result;
00192 }
00193
00194
00195
00196
00197 template <class ME, int RS, class E, int S> inline
00198 typename SymMat<1,ME,RS>::template Result<Vec<1,E,S> >::Mul
00199 operator*(const SymMat<1,ME,RS>& m,const Vec<1,E,S>& v) {
00200 typename SymMat<1,ME,RS>::template Result<Vec<1,E,S> >::Mul result;
00201 result[0] = m.getDiag()[0]*v[0];
00202 return result;
00203 }
00204
00205
00206 template <class ME, int RS, class E, int S> inline
00207 typename SymMat<2,ME,RS>::template Result<Vec<2,E,S> >::Mul
00208 operator*(const SymMat<2,ME,RS>& m,const Vec<2,E,S>& v) {
00209 typename SymMat<2,ME,RS>::template Result<Vec<2,E,S> >::Mul result;
00210 result[0] = m.getDiag()[0] *v[0] + m.getEltUpper(0,1)*v[1];
00211 result[1] = m.getEltLower(1,0)*v[0] + m.getDiag()[1] *v[1];
00212 return result;
00213 }
00214
00215
00216 template <class ME, int RS, class E, int S> inline
00217 typename SymMat<3,ME,RS>::template Result<Vec<3,E,S> >::Mul
00218 operator*(const SymMat<3,ME,RS>& m,const Vec<3,E,S>& v) {
00219 typename SymMat<3,ME,RS>::template Result<Vec<3,E,S> >::Mul result;
00220 result[0] = m.getDiag()[0] *v[0] + m.getEltUpper(0,1)*v[1] + m.getEltUpper(0,2)*v[2];
00221 result[1] = m.getEltLower(1,0)*v[0] + m.getDiag()[1] *v[1] + m.getEltUpper(1,2)*v[2];
00222 result[2] = m.getEltLower(2,0)*v[0] + m.getEltLower(2,1)*v[1] + m.getDiag()[2] *v[2];
00223 return result;
00224 }
00225
00226
00227 template <int M, class E, int S, class ME, int RS> inline
00228 typename Row<M,E,S>::template Result<SymMat<M,ME,RS> >::Mul
00229 operator*(const Row<M,E,S>& r, const SymMat<M,ME,RS>& m) {
00230 typename Row<M,E,S>::template Result<SymMat<M,ME,RS> >::Mul result;
00231 for (int j=0; j<M; ++j) {
00232 result[j] = r[j]*m.getDiag()[j];
00233 for (int i=0; i<j; ++i)
00234 result[j] += r[i]*m.getEltUpper(i,j);
00235 for (int i=j+1; i<M; ++i)
00236 result[j] += r[i]*m.getEltLower(i,j);
00237 }
00238 return result;
00239 }
00240
00241
00242
00243
00244 template <class E, int S, class ME, int RS> inline
00245 typename Row<1,E,S>::template Result<SymMat<1,ME,RS> >::Mul
00246 operator*(const Row<1,E,S>& r, const SymMat<1,ME,RS>& m) {
00247 typename Row<1,E,S>::template Result<SymMat<1,ME,RS> >::Mul result;
00248 result[0] = r[0]*m.getDiag()[0];
00249 return result;
00250 }
00251
00252
00253 template <class E, int S, class ME, int RS> inline
00254 typename Row<2,E,S>::template Result<SymMat<2,ME,RS> >::Mul
00255 operator*(const Row<2,E,S>& r, const SymMat<2,ME,RS>& m) {
00256 typename Row<2,E,S>::template Result<SymMat<2,ME,RS> >::Mul result;
00257 result[0] = r[0]*m.getDiag()[0] + r[1]*m.getEltLower(1,0);
00258 result[1] = r[0]*m.getEltUpper(0,1) + r[1]*m.getDiag()[1];
00259 return result;
00260 }
00261
00262
00263 template <class E, int S, class ME, int RS> inline
00264 typename Row<3,E,S>::template Result<SymMat<3,ME,RS> >::Mul
00265 operator*(const Row<3,E,S>& r, const SymMat<3,ME,RS>& m) {
00266 typename Row<3,E,S>::template Result<SymMat<3,ME,RS> >::Mul result;
00267 result[0] = r[0]*m.getDiag()[0] + r[1]*m.getEltLower(1,0) + r[2]*m.getEltLower(2,0);
00268 result[1] = r[0]*m.getEltUpper(0,1) + r[1]*m.getDiag()[1] + r[2]*m.getEltLower(2,1);
00269 result[2] = r[0]*m.getEltUpper(0,2) + r[1]*m.getEltUpper(1,2) + r[2]*m.getDiag()[2];
00270 return result;
00271 }
00272
00273
00274
00275
00276
00277
00278
00279 template <int M, class E1, int S1, int N, class E2, int S2> inline
00280 typename Vec<M,E1,S1>::template Result<Row<N,E2,S2> >::MulNon
00281 operator*(const Vec<M,E1,S1>& v, const Row<N,E2,S2>& r) {
00282 return Vec<M,E1,S1>::template Result<Row<N,E2,S2> >::MulOpNonConforming::perform(v,r);
00283 }
00284
00285 template <int M, class E1, int S1, int MM, int NN, class E2, int CS2, int RS2> inline
00286 typename Vec<M,E1,S1>::template Result<Mat<MM,NN,E2,CS2,RS2> >::MulNon
00287 operator*(const Vec<M,E1,S1>& v, const Mat<MM,NN,E2,CS2,RS2>& m) {
00288 return Vec<M,E1,S1>::template Result<Mat<MM,NN,E2,CS2,RS2> >
00289 ::MulOpNonConforming::perform(v,m);
00290 }
00291
00292 template <int M, class E1, int S1, int MM, class E2, int RS2> inline
00293 typename Vec<M,E1,S1>::template Result<SymMat<MM,E2,RS2> >::MulNon
00294 operator*(const Vec<M,E1,S1>& v, const SymMat<MM,E2,RS2>& m) {
00295 return Vec<M,E1,S1>::template Result<SymMat<MM,E2,RS2> >
00296 ::MulOpNonConforming::perform(v,m);
00297 }
00298
00299 template <int M, class E1, int S1, int MM, class E2, int S2> inline
00300 typename Vec<M,E1,S1>::template Result<Vec<MM,E2,S2> >::MulNon
00301 operator*(const Vec<M,E1,S1>& v1, const Vec<MM,E2,S2>& v2) {
00302 return Vec<M,E1,S1>::template Result<Vec<MM,E2,S2> >
00303 ::MulOpNonConforming::perform(v1,v2);
00304 }
00305
00306
00307
00308
00309
00310 template <int M, class E, int S, int MM, int NN, class ME, int CS, int RS> inline
00311 typename Row<M,E,S>::template Result<Mat<MM,NN,ME,CS,RS> >::MulNon
00312 operator*(const Row<M,E,S>& r, const Mat<MM,NN,ME,CS,RS>& m) {
00313 return Row<M,E,S>::template Result<Mat<MM,NN,ME,CS,RS> >
00314 ::MulOpNonConforming::perform(r,m);
00315 }
00316
00317 template <int N, class E1, int S1, int M, class E2, int S2> inline
00318 typename Row<N,E1,S1>::template Result<Vec<M,E2,S2> >::MulNon
00319 operator*(const Row<N,E1,S1>& r, const Vec<M,E2,S2>& v) {
00320 return Row<N,E1,S1>::template Result<Vec<M,E2,S2> >
00321 ::MulOpNonConforming::perform(r,v);
00322 }
00323
00324 template <int N1, class E1, int S1, int N2, class E2, int S2> inline
00325 typename Row<N1,E1,S1>::template Result<Row<N2,E2,S2> >::MulNon
00326 operator*(const Row<N1,E1,S1>& r1, const Row<N2,E2,S2>& r2) {
00327 return Row<N1,E1,S1>::template Result<Row<N2,E2,S2> >
00328 ::MulOpNonConforming::perform(r1,r2);
00329 }
00330
00331
00332
00333
00334 template <int M, int N, class ME, int CS, int RS, int MM, class E, int S> inline
00335 typename Mat<M,N,ME,CS,RS>::template Result<Vec<MM,E,S> >::MulNon
00336 operator*(const Mat<M,N,ME,CS,RS>& m,const Vec<MM,E,S>& v) {
00337 return Mat<M,N,ME,CS,RS>::template Result<Vec<MM,E,S> >
00338 ::MulOpNonConforming::perform(m,v);
00339 }
00340
00341 template <int M, int N, class ME, int CS, int RS, int NN, class E, int S> inline
00342 typename Mat<M,N,ME,CS,RS>::template Result<Row<NN,E,S> >::MulNon
00343 operator*(const Mat<M,N,ME,CS,RS>& m,const Row<NN,E,S>& r) {
00344 return Mat<M,N,ME,CS,RS>::template Result<Row<NN,E,S> >
00345 ::MulOpNonConforming::perform(m,r);
00346 }
00347
00348
00349 template <int M, int N, class ME, int CS, int RS, int Dim, class E, int S> inline
00350 typename Mat<M,N,ME,CS,RS>::template Result<SymMat<Dim,E,S> >::MulNon
00351 operator*(const Mat<M,N,ME,CS,RS>& m,const SymMat<Dim,E,S>& sy) {
00352 return Mat<M,N,ME,CS,RS>::template Result<SymMat<Dim,E,S> >
00353 ::MulOpNonConforming::perform(m,sy);
00354 }
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387 template <class E1, int S1, class E2, int S2> inline
00388 Vec<3,typename CNT<E1>::template Result<E2>::Mul>
00389 cross(const Vec<3,E1,S1>& a, const Vec<3,E2,S2>& b) {
00390 return Vec<3,typename CNT<E1>::template Result<E2>::Mul>
00391 (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]);
00392 }
00393 template <class E1, int S1, class E2, int S2> inline
00394 Vec<3,typename CNT<E1>::template Result<E2>::Mul>
00395 operator%(const Vec<3,E1,S1>& a, const Vec<3,E2,S2>& b) {return cross(a,b);}
00396
00397
00398 template <class E1, int S1, class E2, int S2> inline
00399 Row<3,typename CNT<E1>::template Result<E2>::Mul>
00400 cross(const Vec<3,E1,S1>& a, const Row<3,E2,S2>& b) {
00401 return Row<3,typename CNT<E1>::template Result<E2>::Mul>
00402 (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]);
00403 }
00404 template <class E1, int S1, class E2, int S2> inline
00405 Row<3,typename CNT<E1>::template Result<E2>::Mul>
00406 operator%(const Vec<3,E1,S1>& a, const Row<3,E2,S2>& b) {return cross(a,b);}
00407
00408
00409 template <class E1, int S1, class E2, int S2> inline
00410 Row<3,typename CNT<E1>::template Result<E2>::Mul>
00411 cross(const Row<3,E1,S1>& a, const Vec<3,E2,S2>& b) {
00412 return Row<3,typename CNT<E1>::template Result<E2>::Mul>
00413 (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]);
00414 }
00415 template <class E1, int S1, class E2, int S2> inline
00416 Row<3,typename CNT<E1>::template Result<E2>::Mul>
00417 operator%(const Row<3,E1,S1>& a, const Vec<3,E2,S2>& b) {return cross(a,b);}
00418
00419
00420 template <class E1, int S1, class E2, int S2> inline
00421 Row<3,typename CNT<E1>::template Result<E2>::Mul>
00422 cross(const Row<3,E1,S1>& a, const Row<3,E2,S2>& b) {
00423 return Row<3,typename CNT<E1>::template Result<E2>::Mul>
00424 (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]);
00425 }
00426 template <class E1, int S1, class E2, int S2> inline
00427 Row<3,typename CNT<E1>::template Result<E2>::Mul>
00428 operator%(const Row<3,E1,S1>& a, const Row<3,E2,S2>& b) {return cross(a,b);}
00429
00430
00431 template <class E1, int S1, int N, class E2, int CS, int RS> inline
00432 Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
00433 cross(const Vec<3,E1,S1>& v, const Mat<3,N,E2,CS,RS>& m) {
00434 Mat<3,N,typename CNT<E1>::template Result<E2>::Mul> result;
00435 for (int j=0; j < N; ++j)
00436 result(j) = v % m(j);
00437 return result;
00438 }
00439 template <class E1, int S1, int N, class E2, int CS, int RS> inline
00440 Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
00441 operator%(const Vec<3,E1,S1>& v, const Mat<3,N,E2,CS,RS>& m) {return cross(v,m);}
00442
00443
00444 template <class E1, int S1, int N, class E2, int CS, int RS> inline
00445 Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
00446 cross(const Row<3,E1,S1>& r, const Mat<3,N,E2,CS,RS>& m) {
00447 return cross(r.positionalTranspose(), m);
00448 }
00449 template <class E1, int S1, int N, class E2, int CS, int RS> inline
00450 Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
00451 operator%(const Row<3,E1,S1>& r, const Mat<3,N,E2,CS,RS>& m) {return cross(r,m);}
00452
00453
00454
00455 template <int M, class EM, int CS, int RS, class EV, int S> inline
00456 Mat<M,3,typename CNT<EM>::template Result<EV>::Mul>
00457 cross(const Mat<M,3,EM,CS,RS>& m, const Vec<3,EV,S>& v) {
00458 Mat<M,3,typename CNT<EM>::template Result<EV>::Mul> result;
00459 for (int i=0; i < M; ++i)
00460 result[i] = m[i] % v;
00461 return result;
00462 }
00463 template <int M, class EM, int CS, int RS, class EV, int S> inline
00464 Mat<M,3,typename CNT<EM>::template Result<EV>::Mul>
00465 operator%(const Mat<M,3,EM,CS,RS>& m, const Vec<3,EV,S>& v) {return cross(m,v);}
00466
00467
00468 template <int M, class EM, int CS, int RS, class ER, int S> inline
00469 Mat<M,3,typename CNT<EM>::template Result<ER>::Mul>
00470 cross(const Mat<M,3,EM,CS,RS>& m, const Row<3,ER,S>& r) {
00471 return cross(m,r.positionalTranspose());
00472 }
00473 template <int M, class EM, int CS, int RS, class ER, int S> inline
00474 Mat<M,3,typename CNT<EM>::template Result<ER>::Mul>
00475 operator%(const Mat<M,3,EM,CS,RS>& m, const Row<3,ER,S>& r) {return cross(m,r);}
00476
00477
00478
00479
00480
00481
00482
00483 template <class E1, int S1, class E2, int S2> inline
00484 typename CNT<E1>::template Result<E2>::Mul
00485 cross(const Vec<2,E1,S1>& a, const Vec<2,E2,S2>& b) {
00486 return a[0]*b[1]-a[1]*b[0];
00487 }
00488 template <class E1, int S1, class E2, int S2> inline
00489 typename CNT<E1>::template Result<E2>::Mul
00490 operator%(const Vec<2,E1,S1>& a, const Vec<2,E2,S2>& b) {return cross(a,b);}
00491
00492 template <class E1, int S1, class E2, int S2> inline
00493 typename CNT<E1>::template Result<E2>::Mul
00494 cross(const Row<2,E1,S1>& a, const Vec<2,E2,S2>& b) {
00495 return a[0]*b[1]-a[1]*b[0];
00496 }
00497 template <class E1, int S1, class E2, int S2> inline
00498 typename CNT<E1>::template Result<E2>::Mul
00499 operator%(const Row<2,E1,S1>& a, const Vec<2,E2,S2>& b) {return cross(a,b);}
00500
00501 template <class E1, int S1, class E2, int S2> inline
00502 typename CNT<E1>::template Result<E2>::Mul
00503 cross(const Vec<2,E1,S1>& a, const Row<2,E2,S2>& b) {
00504 return a[0]*b[1]-a[1]*b[0];
00505 }
00506 template <class E1, int S1, class E2, int S2> inline
00507 typename CNT<E1>::template Result<E2>::Mul
00508 operator%(const Vec<2,E1,S1>& a, const Row<2,E2,S2>& b) {return cross(a,b);}
00509
00510 template <class E1, int S1, class E2, int S2> inline
00511 typename CNT<E1>::template Result<E2>::Mul
00512 cross(const Row<2,E1,S1>& a, const Row<2,E2,S2>& b) {
00513 return a[0]*b[1]-a[1]*b[0];
00514 }
00515 template <class E1, int S1, class E2, int S2> inline
00516 typename CNT<E1>::template Result<E2>::Mul
00517 operator%(const Row<2,E1,S1>& a, const Row<2,E2,S2>& b) {return cross(a,b);}
00518
00519
00520
00524 template <class E, int S> inline
00525 Mat<3,3,E>
00526 crossMat(const Vec<3,E,S>& v) {
00527 return Mat<3,3,E>(Row<3,E>( E(0), -v[2], v[1]),
00528 Row<3,E>( v[2], E(0), -v[0]),
00529 Row<3,E>(-v[1], v[0], E(0)));
00530 }
00533 template <class E, int S> inline
00534 Mat<3,3,E>
00535 crossMat(const Vec<3,negator<E>,S>& v) {
00536
00537
00538 return Mat<3,3,E>(Row<3,E>( E(0), -v[2], E(v[1])),
00539 Row<3,E>( E(v[2]), E(0), -v[0]),
00540 Row<3,E>(-v[1], E(v[0]), E(0)));
00541 }
00542
00544 template <class E, int S> inline
00545 Mat<3,3,E> crossMat(const Row<3,E,S>& r) {return crossMat(r.positionalTranspose());}
00547 template <class E, int S> inline
00548 Mat<3,3,E> crossMat(const Row<3,negator<E>,S>& r) {return crossMat(r.positionalTranspose());}
00549
00553 template <class E, int S> inline
00554 Row<2,E> crossMat(const Vec<2,E,S>& v) {
00555 return Row<2,E>(-v[1], v[0]);
00556 }
00558 template <class E, int S> inline
00559 Row<2,E> crossMat(const Vec<2,negator<E>,S>& v) {
00560 return Row<2,E>(-v[1], E(v[0]));
00561 }
00562
00564 template <class E, int S> inline
00565 Row<2,E> crossMat(const Row<2,E,S>& r) {return crossMat(r.positionalTranspose());}
00567 template <class E, int S> inline
00568 Row<2,E> crossMat(const Row<2,negator<E>,S>& r) {return crossMat(r.positionalTranspose());}
00569
00570
00571
00589 template <class E, int S> inline
00590 SymMat<3,E>
00591 crossMatSq(const Vec<3,E,S>& v) {
00592 const E xx = square(v[0]);
00593 const E yy = square(v[1]);
00594 const E zz = square(v[2]);
00595 const E nx = -v[0];
00596 const E ny = -v[1];
00597 return SymMat<3,E>( yy+zz,
00598 nx*v[1], xx+zz,
00599 nx*v[2], ny*v[2], xx+yy );
00600 }
00601
00602
00603 template <class E, int S> inline
00604 SymMat<3,E>
00605 crossMatSq(const Vec<3,negator<E>,S>& v) {
00606 const E xx = square(v[0]);
00607 const E yy = square(v[1]);
00608 const E zz = square(v[2]);
00609 const E y = v[1];
00610 const E z = v[2];
00611
00612
00613 return SymMat<3,E>( yy+zz,
00614 -v[0]*y, xx+zz,
00615 -v[0]*z, -v[1]*z, xx+yy );
00616 }
00617
00618 template <class E, int S> inline
00619 SymMat<3,E> crossMatSq(const Row<3,E,S>& r) {return crossMatSq(r.positionalTranspose());}
00620 template <class E, int S> inline
00621 SymMat<3,E> crossMatSq(const Row<3,negator<E>,S>& r) {return crossMatSq(r.positionalTranspose());}
00622
00623
00624
00625
00627 template <class E, int CS, int RS> inline
00628 const E& det(const Mat<1,1,E,CS,RS>& m) {
00629 return m(0,0);
00630 }
00631
00633 template <class E, int CS, int RS> inline
00634 E det(const Mat<2,2,E,CS,RS>& m) {
00635 return E(m(0,0)*m(1,1) - m(0,1)*m(1,0));
00636 }
00637
00639 template <class E, int CS, int RS> inline
00640 E det(const Mat<3,3,E,CS,RS>& m) {
00641 return E( m(0,0)*(m(1,1)*m(2,2)-m(1,2)*m(2,1))
00642 - m(0,1)*(m(1,0)*m(2,2)-m(1,2)*m(2,0))
00643 + m(0,2)*(m(1,0)*m(2,1)-m(1,1)*m(2,0)));
00644 }
00645
00659 template <int M, class E, int CS, int RS> inline
00660 E det(const Mat<M,M,E,CS,RS>& m) {
00661 typename CNT<E>::StdNumber sign(1);
00662 E result(0);
00663
00664 const Mat<M-1,M,E,CS,RS>& m2 = m.template getSubMat<M-1,M>(1,0);
00665 for (int j=0; j < M; ++j) {
00666
00667 result += sign*m(0,j)*det(m2.dropCol(j));
00668 sign = -sign;
00669 }
00670 return result;
00671 }
00672
00673
00674
00676 template <class E, int CS, int RS> inline
00677 typename Mat<1,1,E,CS,RS>::TInvert lapackInverse(const Mat<1,1,E,CS,RS>& m) {
00678 typedef typename Mat<1,1,E,CS,RS>::TInvert MInv;
00679 return MInv( E(typename CNT<E>::StdNumber(1)/m(0,0)) );
00680 }
00681
00692 template <int M, class E, int CS, int RS> inline
00693 typename Mat<M,M,E,CS,RS>::TInvert lapackInverse(const Mat<M,M,E,CS,RS>& m) {
00694
00695
00696
00697
00698 typename Mat<M,M,E,CS,RS>::TInvert inv = m;
00699
00700
00701
00702
00703
00704 typedef typename CNT<E>::StdNumber Raw;
00705 Raw* rawData = reinterpret_cast<Raw*>(&inv(0,0));
00706 int ipiv[M], info;
00707
00708
00709
00710 Lapack::getrf<Raw>(M,M,rawData,M,&ipiv[0],info);
00711 SimTK_ASSERT1(info>=0, "Argument %d to Lapack getrf routine was bad", -info);
00712 SimTK_ERRCHK1_ALWAYS(info==0, "lapackInverse(Mat<>)",
00713 "Matrix is singular so can't be inverted (Lapack getrf info=%d).", info);
00714
00715
00716
00717
00718
00719
00720
00721
00722 Raw work[M];
00723 Lapack::getri<Raw>(M,rawData,M,&ipiv[0],&work[0],M,info);
00724 SimTK_ASSERT1(info>=0, "Argument %d to Lapack getri routine was bad", -info);
00725 SimTK_ERRCHK1_ALWAYS(info==0, "lapackInverse(Mat<>)",
00726 "Matrix is singular so can't be inverted (Lapack getri info=%d).", info);
00727 return inv;
00728 }
00729
00730
00732 template <class E, int CS, int RS> inline
00733 typename Mat<1,1,E,CS,RS>::TInvert inverse(const Mat<1,1,E,CS,RS>& m) {
00734 typedef typename Mat<1,1,E,CS,RS>::TInvert MInv;
00735 return MInv( E(typename CNT<E>::StdNumber(1)/m(0,0)) );
00736 }
00737
00739 template <class E, int CS, int RS> inline
00740 typename Mat<2,2,E,CS,RS>::TInvert inverse(const Mat<2,2,E,CS,RS>& m) {
00741 const E d ( det(m) );
00742 const typename CNT<E>::TInvert ood( typename CNT<E>::StdNumber(1)/d );
00743 return typename Mat<2,2,E,CS,RS>::TInvert( E( ood*m(1,1)), E(-ood*m(0,1)),
00744 E(-ood*m(1,0)), E( ood*m(0,0)));
00745 }
00746
00751 template <class E, int CS, int RS> inline
00752 typename Mat<3,3,E,CS,RS>::TInvert inverse(const Mat<3,3,E,CS,RS>& m) {
00753
00754
00755
00756 const E d00(m(1,1)*m(2,2)-m(1,2)*m(2,1)),
00757 d01(m(1,0)*m(2,2)-m(1,2)*m(2,0)),
00758 d02(m(1,0)*m(2,1)-m(1,1)*m(2,0));
00759
00760
00761 const E d (m(0,0)*d00 - m(0,1)*d01 + m(0,2)*d02);
00762 const typename CNT<E>::TInvert
00763 ood(typename CNT<E>::StdNumber(1)/d);
00764
00765
00766
00767 const E d10(m(0,1)*m(2,2)-m(0,2)*m(2,1)),
00768 d11(m(0,0)*m(2,2)-m(0,2)*m(2,0)),
00769 d12(m(0,0)*m(2,1)-m(0,1)*m(2,0)),
00770 d20(m(0,1)*m(1,2)-m(0,2)*m(1,1)),
00771 d21(m(0,0)*m(1,2)-m(0,2)*m(1,0)),
00772 d22(m(0,0)*m(1,1)-m(0,1)*m(1,0));
00773
00774 return typename Mat<3,3,E,CS,RS>::TInvert
00775 ( E( ood*d00), E(-ood*d10), E( ood*d20),
00776 E(-ood*d01), E( ood*d11), E(-ood*d21),
00777 E( ood*d02), E(-ood*d12), E( ood*d22) );
00778 }
00779
00782 template <int M, class E, int CS, int RS> inline
00783 typename Mat<M,M,E,CS,RS>::TInvert inverse(const Mat<M,M,E,CS,RS>& m) {
00784 return lapackInverse(m);
00785 }
00786
00787
00788
00789 template <int M, int N, class ELT, int CS, int RS> inline
00790 typename Mat<M,N,ELT,CS,RS>::TInvert
00791 Mat<M,N,ELT,CS,RS>::invert() const {
00792 return SimTK::inverse(*this);
00793 }
00794
00795 }
00796
00797
00798 #endif //SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_