SmallMatrixMixed.h

Go to the documentation of this file.
00001 #ifndef SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_
00002 #define SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_
00003 
00004 /* -------------------------------------------------------------------------- *
00005  *                      SimTK Core: SimTK Simmatrix(tm)                       *
00006  * -------------------------------------------------------------------------- *
00007  * This is part of the SimTK Core biosimulation toolkit originating from      *
00008  * Simbios, the NIH National Center for Physics-Based Simulation of           *
00009  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
00010  * Medical Research, grant U54 GM072970. See https://simtk.org.               *
00011  *                                                                            *
00012  * Portions copyright (c) 2005-9 Stanford University and the Authors.         *
00013  * Authors: Michael Sherman                                                   *
00014  * Contributors:                                                              *
00015  *                                                                            *
00016  * Permission is hereby granted, free of charge, to any person obtaining a    *
00017  * copy of this software and associated documentation files (the "Software"), *
00018  * to deal in the Software without restriction, including without limitation  *
00019  * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
00020  * and/or sell copies of the Software, and to permit persons to whom the      *
00021  * Software is furnished to do so, subject to the following conditions:       *
00022  *                                                                            *
00023  * The above copyright notice and this permission notice shall be included in *
00024  * all copies or substantial portions of the Software.                        *
00025  *                                                                            *
00026  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
00027  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
00028  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
00029  * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
00030  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
00031  * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
00032  * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
00033  * -------------------------------------------------------------------------- */
00034 
00041 namespace SimTK {
00042 
00043     // DOT PRODUCT
00044 
00045 // Dot product and corresponding infix operator*(). Note that
00046 // the '*' operator is just a matrix multiply so is strictly 
00047 // row*column to produce a scalar (1x1) result.
00048 //
00049 // In keeping with Matlab precedent, however, the explicit dot()
00050 // function is defined on two column vectors
00051 // v and w such that dot(v,w)= ~v * w. That means we use the
00052 // Hermitian transpose of the elements of v, and the elements of
00053 // w unchanged. If v and/or w are rows, we first convert them
00054 // to vectors via *positional* transpose. So no matter what shape
00055 // these have on the way in it is always the Hermitian transpose
00056 // (or complex conjugate for scalars) of the first item times
00057 // the unchanged elements of the second item.
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 // dot product (row * conforming vec)
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 // Alternate acceptable forms for dot().
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     // OUTER PRODUCT
00105 
00106 // Outer product and corresponding infix operator*(). Note that
00107 // the '*' operator is just a matrix multiply so is strictly 
00108 // column_mX1 * row_1Xm to produce an mXm matrix result.
00109 //
00110 // Although Matlab doesn't provide outer(), to be consistent
00111 // we'll treat it as discussed for dot() above. That is, outer
00112 // is defined on two column vectors
00113 // v and w such that outer(v,w)= v * ~w. That means we use the
00114 // elements of v unchanged but use the Hermitian transpose of
00115 // the elements of w. If v and/or w are rows, we first convert them
00116 // to vectors via *positional* transpose. So no matter what shape
00117 // these have on the way in it is always the unchanged elements of
00118 // the first item times the Hermitian transpose (or complex
00119 // conjugate for scalars) of the elements of the second item.
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 // This is the general conforming case of Vec*Row (outer product)
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 // Alternate acceptable forms for outer().
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     // MAT*VEC, ROW*MAT (conforming)
00156 
00157 // vec = mat * vec (conforming)
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 // row = row * mat (conforming)
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     // SYMMAT*VEC, ROW*SYMMAT (conforming)
00178 
00179 // vec = sym * vec (conforming)
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 // Unroll loops for small cases.
00195 
00196 // 1 flop.
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 // 6 flops.
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 // 15 flops.
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 // row = row * sym (conforming)
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 // Unroll loops for small cases.
00242 
00243 // 1 flop.
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 // 6 flops.
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 // 15 flops.
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     // NONCONFORMING MULTIPLY
00274 
00275     // Nonconforming: Vec on left: v*r v*m v*sym v*v
00276     // Result has the shape of the "most composite" (deepest) argument.
00277 
00278 // elementwise multiply (vec * nonconforming row)
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 // elementwise multiply (vec * nonconforming mat)
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 // elementwise multiply (vec * nonconforming symmat)
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 // elementwise multiply (vec * nonconforming vec)
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     // Nonconforming: Row on left -- r*v r*r r*m r*sym
00307 
00308 
00309 // (row or mat) = row * mat (nonconforming)
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 // (row or vec) = row * vec (nonconforming)
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 // (row1 or row2) = row1 * row2(nonconforming)
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     // Nonconforming: Mat on left -- m*v m*r m*sym
00332 
00333 // (mat or vec) = mat * vec (nonconforming)
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 // (mat or row) = mat * row (nonconforming)
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 // (mat or sym) = mat * sym (nonconforming)
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     // CROSS PRODUCT
00357 
00358 // Cross product and corresponding operator%, but only for 2- and 3-Vecs
00359 // and Rows. It is OK to mix same-length Vecs & Rows; cross product is
00360 // defined elementwise and never transposes the individual elements.
00361 //
00362 // We also define crossMat(v) below which produces a 2x2 or 3x3
00363 // matrix M such that M*w = v % w for w the same length vector (or row) as v.
00364 // TODO: the 3d crossMat is skew symmetric but currently there is no
00365 // SkewMat class defined so we have to return a full 3x3.
00366 
00367 // For 3d cross product, we'll follow Matlab and make the return type a
00368 // Row if either or both arguments are Rows, although I'm not sure that's
00369 // a good idea! Note that the definition of crossMat means crossMat(r)*v
00370 // will yield a column, while r%v is a Row.
00371 
00372 // We define v % m where v is a 3-vector and m is a 3xN matrix.
00373 // This returns a matrix c of the same dimensions as m where
00374 // column c(i) = v % m(i), that is, each column of c is the cross
00375 // product of v and the corresponding column of m. This definition means that
00376 //      v % m == crossMat(v)*m
00377 // which seems like a good idea. (But note that v%m takes 9*N flops while
00378 // crossMat(v)*m takes 15*N flops.) If we have a row vector r instead,
00379 // we define r % m == v % m so again r%m==crossMat(r)*m. We also
00380 // define the cross product operator with an Mx3 matrix on the left,
00381 // defined so that c[i] = m[i]%v, that is, the rows of c are the
00382 // cross products of the corresonding rows of m and vector v. Again,
00383 // we allow v to be a row without any change to the results or return type.
00384 // This definition means m % v = m * crossMat(v), but again it is faster.
00385 
00386 // v = v % v
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 // r = v % r
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 // r = r % v
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 // r = r % r 
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 // m = v % m
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> // packed
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 // m = r % m
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> // packed
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 // m = m % v
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> // packed
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> // packed
00465 operator%(const Mat<M,3,EM,CS,RS>& m, const Vec<3,EV,S>& v) {return cross(m,v);}
00466 
00467 // m = m % r
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> // packed
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> // packed
00475 operator%(const Mat<M,3,EM,CS,RS>& m, const Row<3,ER,S>& r) {return cross(m,r);}
00476 
00477 
00478 
00479 // 2d cross product just returns a scalar. This is the z-value you
00480 // would get if the arguments were treated as 3-vectors with 0
00481 // z components.
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     // CROSS MAT
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     // Here the "-" operators are just recasts, but the casts
00537     // to type E have to perform floating point negations.
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     // CROSS MAT SQ
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 // Specialize above for negated types. Returned matrix loses negator.
00602 // The total number of flops here is the same as above: 11 flops.
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]; // requires a negation to convert to E
00610     const E z = v[2];
00611     // The negations in the arguments below are not floating point
00612     // operations because the element type is already negated.
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     // DETERMINANT
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     // We're always going to drop the first row.
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         // det() here is recursive but terminates at 3x3 above.
00667         result += sign*m(0,j)*det(m2.dropCol(j));
00668         sign = -sign;
00669     }
00670     return result;
00671 }
00672 
00673     // INVERSE
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     // Copy the source matrix, which has arbitrary row and column spacing,
00695     // into the type for its inverse, which must be dense, columnwise
00696     // storage. That means its column spacing will be M and row spacing
00697     // will be 1, as Lapack expects for a Full matrix.
00698     typename Mat<M,M,E,CS,RS>::TInvert inv = m; // dense, columnwise storage
00699 
00700     // We'll perform the inversion ignoring negation and conjugation altogether, 
00701     // but the TInvert Mat type negates or conjugates the result. And because 
00702     // of the famous Sherman-Eastman theorem, we know that 
00703     // conj(inv(m))==inv(conj(m)), and of course -inv(m)==inv(-m).
00704     typedef typename CNT<E>::StdNumber Raw;   // Raw is E without negator or conjugate.
00705     Raw* rawData = reinterpret_cast<Raw*>(&inv(0,0));
00706     int ipiv[M], info;
00707 
00708     // This replaces "inv" with its LU facorization and pivot matrix P, such that
00709     // P*L*U = m (ignoring negation and conjugation).
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     // The workspace size must be at least M. For larger matrices, Lapack wants
00716     // to do factorization in blocks of size NB in which case the workspace should
00717     // be M*NB. However, we will assume that this matrix is small (in the sense
00718     // that it fits entirely in cache) so the minimum workspace size is fine and
00719     // we don't need an extra getri call to find the workspace size nor any heap
00720     // allocation.
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     // Calculate determinants for each 2x2 submatrix with first row removed.
00754     // (See the specialized 3x3 determinant routine above.) We're calculating
00755     // this explicitly here because we can re-use the intermediate terms.
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     // This is the 3x3 determinant and its inverse.
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     // The other six 2x2 determinants we can't re-use, but we can still
00766     // avoid some copying by calculating them explicitly here.
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 // Implement the Mat<>::invert() method using the above specialized 
00788 // inverse functions. This will only compile if M==N.
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 } //namespace SimTK
00796 
00797 
00798 #endif //SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_

Generated by  doxygen 1.6.2