Simbody  3.5
Mat.h
Go to the documentation of this file.
00001 #ifndef SimTK_SIMMATRIX_SMALLMATRIX_MAT_H_
00002 #define SimTK_SIMMATRIX_SMALLMATRIX_MAT_H_
00003 
00004 /* -------------------------------------------------------------------------- *
00005  *                       Simbody(tm): SimTKcommon                             *
00006  * -------------------------------------------------------------------------- *
00007  * This is part of the SimTK 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/home/simbody.  *
00011  *                                                                            *
00012  * Portions copyright (c) 2005-12 Stanford University and the Authors.        *
00013  * Authors: Michael Sherman                                                   *
00014  * Contributors:                                                              *
00015  *                                                                            *
00016  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
00017  * not use this file except in compliance with the License. You may obtain a  *
00018  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
00019  *                                                                            *
00020  * Unless required by applicable law or agreed to in writing, software        *
00021  * distributed under the License is distributed on an "AS IS" BASIS,          *
00022  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
00023  * See the License for the specific language governing permissions and        *
00024  * limitations under the License.                                             *
00025  * -------------------------------------------------------------------------- */
00026 
00031 #include "SimTKcommon/internal/common.h"
00032 
00033 namespace SimTK {
00034 
00097 template <int M, int N, class ELT, int CS, int RS> class Mat {
00098     typedef ELT                                 E;
00099     typedef typename CNT<E>::TNeg               ENeg;
00100     typedef typename CNT<E>::TWithoutNegator    EWithoutNegator;
00101     typedef typename CNT<E>::TReal              EReal;
00102     typedef typename CNT<E>::TImag              EImag;
00103     typedef typename CNT<E>::TComplex           EComplex;
00104     typedef typename CNT<E>::THerm              EHerm;
00105     typedef typename CNT<E>::TPosTrans          EPosTrans;
00106     typedef typename CNT<E>::TSqHermT           ESqHermT;
00107     typedef typename CNT<E>::TSqTHerm           ESqTHerm;
00108 
00109     typedef typename CNT<E>::TSqrt              ESqrt;
00110     typedef typename CNT<E>::TAbs               EAbs;
00111     typedef typename CNT<E>::TStandard          EStandard;
00112     typedef typename CNT<E>::TInvert            EInvert;
00113     typedef typename CNT<E>::TNormalize         ENormalize;
00114 
00115     typedef typename CNT<E>::Scalar             EScalar;
00116     typedef typename CNT<E>::ULessScalar        EULessScalar;
00117     typedef typename CNT<E>::Number             ENumber;
00118     typedef typename CNT<E>::StdNumber          EStdNumber;
00119     typedef typename CNT<E>::Precision          EPrecision;
00120     typedef typename CNT<E>::ScalarNormSq       EScalarNormSq;
00121 
00122 public:
00124     enum {
00125         NRows               = M,
00126         NCols               = N,
00127         MinDim              = N < M ? N : M,
00128         MaxDim              = N > M ? N : M,
00129         RowSpacing          = RS,
00130         ColSpacing          = CS,
00131         NPackedElements     = M * N,
00132         NActualElements     = (N-1)*CS + (M-1)*RS + 1,
00133         NActualScalars      = CNT<E>::NActualScalars * NActualElements,
00134         ImagOffset          = NTraits<ENumber>::ImagOffset,
00135         RealStrideFactor    = 1, // composite types don't change size when
00136                                  // cast from complex to real or imaginary
00137         ArgDepth            = ((int)CNT<E>::ArgDepth < (int)MAX_RESOLVED_DEPTH 
00138                                 ? CNT<E>::ArgDepth + 1 
00139                                 : MAX_RESOLVED_DEPTH),
00140         IsScalar            = 0,
00141         IsULessScalar       = 0,
00142         IsNumber            = 0,
00143         IsStdNumber         = 0,
00144         IsPrecision         = 0,
00145         SignInterpretation  = CNT<E>::SignInterpretation
00146     };
00147 
00148     typedef Mat<M,N,E,CS,RS>                T;
00149     typedef Mat<M,N,ENeg,CS,RS>             TNeg;
00150     typedef Mat<M,N,EWithoutNegator,CS,RS>  TWithoutNegator;
00151 
00152     typedef Mat<M,N,EReal,CS*CNT<E>::RealStrideFactor,RS*CNT<E>::RealStrideFactor>
00153                                             TReal;
00154     typedef Mat<M,N,EImag,CS*CNT<E>::RealStrideFactor,RS*CNT<E>::RealStrideFactor>
00155                                             TImag;
00156     typedef Mat<M,N,EComplex,CS,RS>         TComplex;
00157     typedef Mat<N,M,EHerm,RS,CS>            THerm;
00158     typedef Mat<N,M,E,RS,CS>                TPosTrans;
00159     typedef E                               TElement;
00160     typedef Row<N,E,CS>                     TRow;    
00161     typedef Vec<M,E,RS>                     TCol;
00162     typedef Vec<MinDim,E,RS+CS>             TDiag;
00163 
00164     // These are the results of calculations, so are returned in new, packed
00165     // memory. Be sure to refer to element types here which are also packed.
00166     typedef Mat<M,N,ESqrt,M,1>              TSqrt;      // Note strides are packed
00167     typedef Mat<M,N,EAbs,M,1>               TAbs;       // Note strides are packed
00168     typedef Mat<M,N,EStandard,M,1>          TStandard;
00169     typedef Mat<N,M,EInvert,N,1>            TInvert;    // like THerm but packed
00170     typedef Mat<M,N,ENormalize,M,1>         TNormalize;
00171 
00172     typedef SymMat<N,ESqHermT>              TSqHermT;   // ~Mat*Mat
00173     typedef SymMat<M,ESqTHerm>              TSqTHerm;   // Mat*~Mat
00174 
00175     // Here the elements are copied unchanged but the result matrix
00176     // is an ordinary packed, column order matrix.
00177     typedef Mat<M,N,E,M,1>                  TPacked;
00178     typedef Mat<M-1,N,E,M-1,1>              TDropRow;
00179     typedef Mat<M,N-1,E,M,1>                TDropCol;
00180     typedef Mat<M-1,N-1,E,M-1,1>            TDropRowCol;
00181     typedef Mat<M+1,N,E,M+1,1>              TAppendRow;
00182     typedef Mat<M,N+1,E,M,1>                TAppendCol;
00183     typedef Mat<M+1,N+1,E,M+1,1>            TAppendRowCol;
00184 
00185     typedef EScalar                         Scalar;
00186     typedef EULessScalar                    ULessScalar;
00187     typedef ENumber                         Number;
00188     typedef EStdNumber                      StdNumber;
00189     typedef EPrecision                      Precision;
00190     typedef EScalarNormSq                   ScalarNormSq;
00191 
00192     typedef THerm                           TransposeType; // TODO
00193 
00195     static int size() { return M*N; }
00198     static int nrow() { return M; }
00201     static int ncol() { return N; }
00202 
00208     ScalarNormSq scalarNormSqr() const { 
00209         ScalarNormSq sum(0);
00210         for(int j=0;j<N;++j) sum += CNT<TCol>::scalarNormSqr((*this)(j));
00211         return sum;
00212     }
00213 
00217     TSqrt sqrt() const { 
00218         TSqrt msqrt;
00219         for(int j=0;j<N;++j) msqrt(j) = (*this)(j).sqrt();
00220         return msqrt;
00221     }
00222 
00226     TAbs abs() const { 
00227         TAbs mabs;
00228         for(int j=0;j<N;++j) mabs(j) = (*this)(j).abs();
00229         return mabs;
00230     }
00231 
00232     TStandard standardize() const {
00233         TStandard mstd;
00234         for(int j=0;j<N;++j) mstd(j) = (*this)(j).standardize();
00235         return mstd;
00236     }
00237 
00238     // This gives the resulting matrix type when (m(i,j) op P) is applied to each element.
00239     // It is an MxN vector with default column and row spacing, and element types which
00240     // are the regular composite result of E op P. Typically P is a scalar type but
00241     // it doesn't have to be.
00242     template <class P> struct EltResult { 
00243         typedef Mat<M,N, typename CNT<E>::template Result<P>::Mul, M, 1> Mul;
00244         typedef Mat<M,N, typename CNT<E>::template Result<P>::Dvd, M, 1> Dvd;
00245         typedef Mat<M,N, typename CNT<E>::template Result<P>::Add, M, 1> Add;
00246         typedef Mat<M,N, typename CNT<E>::template Result<P>::Sub, M, 1> Sub;
00247     };
00248 
00249     // This is the composite result for m op P where P is some kind of appropriately shaped
00250     // non-scalar type.
00251     template <class P> struct Result { 
00252         typedef MulCNTs<M,N,ArgDepth,Mat,ColSpacing,RowSpacing,
00253             CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
00254             P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOp;
00255         typedef typename MulOp::Type Mul;
00256 
00257         typedef MulCNTsNonConforming<M,N,ArgDepth,Mat,ColSpacing,RowSpacing,
00258             CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
00259             P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOpNonConforming;
00260         typedef typename MulOpNonConforming::Type MulNon;
00261 
00262         typedef DvdCNTs<M,N,ArgDepth,Mat,ColSpacing,RowSpacing,
00263             CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
00264             P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> DvdOp;
00265         typedef typename DvdOp::Type Dvd;
00266 
00267         typedef AddCNTs<M,N,ArgDepth,Mat,ColSpacing,RowSpacing,
00268             CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
00269             P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> AddOp;
00270         typedef typename AddOp::Type Add;
00271 
00272         typedef SubCNTs<M,N,ArgDepth,Mat,ColSpacing,RowSpacing,
00273             CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
00274             P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> SubOp;
00275         typedef typename SubOp::Type Sub;
00276     };
00277 
00278     // Shape-preserving element substitution (always packed)
00279     template <class P> struct Substitute {
00280         typedef Mat<M,N,P> Type;
00281     };
00282 
00285     Mat(){ 
00286     #ifndef NDEBUG
00287         setToNaN();
00288     #endif
00289     }
00290 
00291     // It's important not to use the default copy constructor or copy
00292     // assignment because the compiler doesn't understand that we may
00293     // have noncontiguous storage and will try to copy the whole array.
00294 
00297     Mat(const Mat& src) {
00298         for (int j=0; j<N; ++j)
00299             (*this)(j) = src(j);
00300     }
00304     Mat& operator=(const Mat& src) {    
00305         for (int j=0; j<N; ++j)
00306            (*this)(j) = src(j); // no harm if src and 'this' are the same
00307         return *this;
00308     }
00309 
00314     explicit Mat(const SymMat<M, ELT>& src) {
00315         updDiag() = src.diag();
00316         for (int j = 0; j < M; ++j)
00317             for (int i = j+1; i < M; ++i) {
00318                 (*this)(i, j) = src.getEltLower(i, j);
00319                 (*this)(j, i) = src.getEltUpper(j, i);
00320             }
00321     }
00322 
00327     template <int CSS, int RSS> 
00328     Mat(const Mat<M,N,E,CSS,RSS>& src) {
00329         for (int j=0; j<N; ++j)
00330             (*this)(j) = src(j);
00331     }
00332 
00338     template <int CSS, int RSS> 
00339     Mat(const Mat<M,N,ENeg,CSS,RSS>& src) {
00340         for (int j=0; j<N; ++j)
00341             (*this)(j) = src(j);
00342     }
00343 
00351     template <class EE, int CSS, int RSS> 
00352     explicit Mat(const Mat<M,N,EE,CSS,RSS>& mm)
00353       { for (int j=0;j<N;++j) (*this)(j) = mm(j);}
00354 
00358     explicit Mat(const E& e)
00359       { for (int j=0;j<N;++j) (*this)(j) = E(0); diag()=e; }
00360 
00365     explicit Mat(const ENeg& e)
00366       { for (int j=0;j<N;++j) (*this)(j) = E(0); diag()=e; }
00367 
00374     explicit Mat(int i) 
00375       { new (this) Mat(E(Precision(i))); }
00376 
00377     // A bevy of constructors from individual exact-match elements IN ROW ORDER.
00378     Mat(const E& e0,const E& e1)
00379       {assert(M*N==2);d[rIx(0)]=e0;d[rIx(1)]=e1;}
00380     Mat(const E& e0,const E& e1,const E& e2)
00381       {assert(M*N==3);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;}
00382     Mat(const E& e0,const E& e1,const E& e2,const E& e3)
00383       {assert(M*N==4);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;}
00384     Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4)
00385       {assert(M*N==5);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;}
00386     Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
00387         const E& e5)
00388       {assert(M*N==6);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
00389        d[rIx(5)]=e5;}
00390     Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
00391         const E& e5,const E& e6)
00392       {assert(M*N==7);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
00393        d[rIx(5)]=e5;d[rIx(6)]=e6;}
00394     Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
00395         const E& e5,const E& e6,const E& e7)
00396       {assert(M*N==8);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
00397        d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;}
00398     Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
00399         const E& e5,const E& e6,const E& e7,const E& e8)
00400       {assert(M*N==9);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
00401        d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;}
00402     Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
00403         const E& e5,const E& e6,const E& e7,const E& e8,const E& e9)
00404       {assert(M*N==10);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
00405        d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;}
00406     Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
00407         const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
00408         const E& e10)
00409       {assert(M*N==11);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
00410        d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;}
00411     Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
00412         const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
00413         const E& e10, const E& e11)
00414       {assert(M*N==12);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
00415        d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;
00416        d[rIx(11)]=e11;}
00417     Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
00418         const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
00419         const E& e10, const E& e11, const E& e12)
00420       {assert(M*N==13);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
00421        d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;
00422        d[rIx(11)]=e11;d[rIx(12)]=e12;}
00423     Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
00424         const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
00425         const E& e10, const E& e11, const E& e12, const E& e13)
00426       {assert(M*N==14);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
00427        d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;
00428        d[rIx(11)]=e11;d[rIx(12)]=e12;d[rIx(13)]=e13;}
00429     Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
00430         const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
00431         const E& e10, const E& e11, const E& e12, const E& e13, const E& e14)
00432       {assert(M*N==15);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
00433        d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;
00434        d[rIx(11)]=e11;d[rIx(12)]=e12;d[rIx(13)]=e13;d[rIx(14)]=e14;}
00435     Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
00436         const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
00437         const E& e10, const E& e11, const E& e12, const E& e13, const E& e14, 
00438         const E& e15)
00439       {assert(M*N==16);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
00440        d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;
00441        d[rIx(11)]=e11;d[rIx(12)]=e12;d[rIx(13)]=e13;d[rIx(14)]=e14;d[rIx(15)]=e15;}
00442 
00443     // Construction from 1-6 *exact match* Rows
00444     explicit Mat(const TRow& r0)
00445       { assert(M==1); (*this)[0]=r0; }
00446     Mat(const TRow& r0,const TRow& r1)
00447       { assert(M==2);(*this)[0]=r0;(*this)[1]=r1; }
00448     Mat(const TRow& r0,const TRow& r1,const TRow& r2)
00449       { assert(M==3);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2; }
00450     Mat(const TRow& r0,const TRow& r1,const TRow& r2,
00451         const TRow& r3)
00452       { assert(M==4);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;(*this)[3]=r3; }
00453     Mat(const TRow& r0,const TRow& r1,const TRow& r2,
00454         const TRow& r3,const TRow& r4)
00455       { assert(M==5);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;
00456         (*this)[3]=r3;(*this)[4]=r4; }
00457     Mat(const TRow& r0,const TRow& r1,const TRow& r2,
00458         const TRow& r3,const TRow& r4,const TRow& r5)
00459       { assert(M==6);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;
00460         (*this)[3]=r3;(*this)[4]=r4;(*this)[5]=r5; }
00461 
00462     // Construction from 1-6 *compatible* Rows
00463     template <class EE, int SS> explicit Mat(const Row<N,EE,SS>& r0)
00464       { assert(M==1); (*this)[0]=r0; }
00465     template <class EE, int SS> Mat(const Row<N,EE,SS>& r0,const Row<N,EE,SS>& r1)
00466       { assert(M==2);(*this)[0]=r0;(*this)[1]=r1; }
00467     template <class EE, int SS> 
00468     Mat(const Row<N,EE,SS>& r0,const Row<N,EE,SS>& r1,const Row<N,EE,SS>& r2)
00469       { assert(M==3);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2; }
00470     template <class EE, int SS> 
00471     Mat(const Row<N,EE,SS>& r0,const Row<N,EE,SS>& r1,const Row<N,EE,SS>& r2,
00472         const Row<N,EE,SS>& r3)
00473       { assert(M==4);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;(*this)[3]=r3; }
00474     template <class EE, int SS> 
00475     Mat(const Row<N,EE,SS>& r0,const Row<N,EE,SS>& r1,const Row<N,EE,SS>& r2,
00476         const Row<N,EE,SS>& r3,const Row<N,EE,SS>& r4)
00477       { assert(M==5);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;
00478         (*this)[3]=r3;(*this)[4]=r4; }
00479     template <class EE, int SS> 
00480     Mat(const Row<N,EE,SS>& r0,const Row<N,EE,SS>& r1,const Row<N,EE,SS>& r2,
00481         const Row<N,EE,SS>& r3,const Row<N,EE,SS>& r4,const Row<N,EE,SS>& r5)
00482       { assert(M==6);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;
00483         (*this)[3]=r3;(*this)[4]=r4;(*this)[5]=r5; }
00484 
00485 
00486     // Construction from 1-6 *exact match* Vecs
00487     explicit Mat(const TCol& r0)
00488       { assert(N==1); (*this)(0)=r0; }
00489     Mat(const TCol& r0,const TCol& r1)
00490       { assert(N==2);(*this)(0)=r0;(*this)(1)=r1; }
00491     Mat(const TCol& r0,const TCol& r1,const TCol& r2)
00492       { assert(N==3);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2; }
00493     Mat(const TCol& r0,const TCol& r1,const TCol& r2,
00494         const TCol& r3)
00495       { assert(N==4);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;(*this)(3)=r3; }
00496     Mat(const TCol& r0,const TCol& r1,const TCol& r2,
00497         const TCol& r3,const TCol& r4)
00498       { assert(N==5);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;
00499         (*this)(3)=r3;(*this)(4)=r4; }
00500     Mat(const TCol& r0,const TCol& r1,const TCol& r2,
00501         const TCol& r3,const TCol& r4,const TCol& r5)
00502       { assert(N==6);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;
00503         (*this)(3)=r3;(*this)(4)=r4;(*this)(5)=r5; }
00504 
00505     // Construction from 1-6 *compatible* Vecs
00506     template <class EE, int SS> explicit Mat(const Vec<M,EE,SS>& r0)
00507       { assert(N==1); (*this)(0)=r0; }
00508     template <class EE, int SS> Mat(const Vec<M,EE,SS>& r0,const Vec<M,EE,SS>& r1)
00509       { assert(N==2);(*this)(0)=r0;(*this)(1)=r1; }
00510     template <class EE, int SS> 
00511     Mat(const Vec<M,EE,SS>& r0,const Vec<M,EE,SS>& r1,const Vec<M,EE,SS>& r2)
00512       { assert(N==3);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2; }
00513     template <class EE, int SS> 
00514     Mat(const Vec<M,EE,SS>& r0,const Vec<M,EE,SS>& r1,const Vec<M,EE,SS>& r2,
00515         const Vec<M,EE,SS>& r3)
00516       { assert(N==4);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;(*this)(3)=r3; }
00517     template <class EE, int SS> 
00518     Mat(const Vec<M,EE,SS>& r0,const Vec<M,EE,SS>& r1,const Vec<M,EE,SS>& r2,
00519         const Vec<M,EE,SS>& r3,const Vec<M,EE,SS>& r4)
00520       { assert(N==5);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;
00521         (*this)(3)=r3;(*this)(4)=r4; }
00522     template <class EE, int SS> 
00523     Mat(const Vec<M,EE,SS>& r0,const Vec<M,EE,SS>& r1,const Vec<M,EE,SS>& r2,
00524         const Vec<M,EE,SS>& r3,const Vec<M,EE,SS>& r4,const Vec<M,EE,SS>& r5)
00525       { assert(N==6);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;
00526         (*this)(3)=r3;(*this)(4)=r4;(*this)(5)=r5; }
00527 
00528     // Construction from a pointer to anything assumes we're pointing
00529     // at a packed element list of the right length, in row order.
00530     template <class EE> explicit Mat(const EE* p)
00531       { assert(p); for(int i=0;i<M;++i) (*this)[i]=&p[i*N]; }
00532 
00533     // Assignment works similarly to copy -- if the lengths match,
00534     // go element-by-element. Otherwise, zero and then copy to each 
00535     // diagonal element.
00536     template <class EE, int CSS, int RSS> Mat& operator=(const Mat<M,N,EE,CSS,RSS>& mm) {
00537         for (int j=0; j<N; ++j) (*this)(j) = mm(j);
00538         return *this;
00539     }
00540 
00541     template <class EE> Mat& operator=(const EE* p) {
00542         assert(p); for(int i=0;i<M;++i) (*this)[i]=&p[i*N];
00543         return *this;
00544     }
00545 
00546     // Assignment ops
00547     template <class EE, int CSS, int RSS> Mat& 
00548     operator+=(const Mat<M,N,EE,CSS,RSS>& mm) {
00549         for (int j=0; j<N; ++j) (*this)(j) += mm(j);
00550         return *this;
00551     }
00552     template <class EE, int CSS, int RSS> Mat&
00553     operator+=(const Mat<M,N,negator<EE>,CSS,RSS>& mm) {
00554         for (int j=0; j<N; ++j) (*this)(j) -= -(mm(j));
00555         return *this;
00556     }
00557 
00558     template <class EE, int CSS, int RSS> Mat&
00559     operator-=(const Mat<M,N,EE,CSS,RSS>& mm) {
00560         for (int j=0; j<N; ++j) (*this)(j) -= mm(j);
00561         return *this;
00562     }
00563     template <class EE, int CSS, int RSS> Mat&
00564     operator-=(const Mat<M,N,negator<EE>,CSS,RSS>& mm) {
00565         for (int j=0; j<N; ++j) (*this)(j) += -(mm(j));
00566         return *this;
00567     }
00568 
00569     // In place matrix multiply can only be done when the RHS matrix is square of dimension
00570     // N (i.e., number of columns), and the elements are also *= compatible.
00571     template <class EE, int CSS, int RSS> Mat&
00572     operator*=(const Mat<N,N,EE,CSS,RSS>& mm) {
00573         const Mat t(*this);
00574         for (int j=0; j<N; ++j)
00575             for (int i=0; i<M; ++i)
00576                 (*this)(i,j) = t[i] * mm(j);
00577         return *this;
00578     }
00579 
00580     // Conforming binary ops with 'this' on left, producing new packed result.
00581     // Cases: m=m+-m, m=m+-sy, m=m*m, m=m*sy, v=m*v
00582 
00583     // m= this + m
00584     template <class E2, int CS2, int RS2> 
00585     typename Result<Mat<M,N,E2,CS2,RS2> >::Add
00586     conformingAdd(const Mat<M,N,E2,CS2,RS2>& r) const {
00587         typename Result<Mat<M,N,E2,CS2,RS2> >::Add result;
00588         for (int j=0;j<N;++j) result(j) = (*this)(j) + r(j);
00589         return result;
00590     }
00591     // m= this - m
00592     template <class E2, int CS2, int RS2> 
00593     typename Result<Mat<M,N,E2,CS2,RS2> >::Sub
00594     conformingSubtract(const Mat<M,N,E2,CS2,RS2>& r) const {
00595         typename Result<Mat<M,N,E2,CS2,RS2> >::Sub result;
00596         for (int j=0;j<N;++j) result(j) = (*this)(j) - r(j);
00597         return result;
00598     }
00599     // m= m - this
00600     template <class E2, int CS2, int RS2> 
00601     typename Mat<M,N,E2,CS2,RS2>::template Result<Mat>::Sub
00602     conformingSubtractFromLeft(const Mat<M,N,E2,CS2,RS2>& l) const {
00603         return l.conformingSubtract(*this);
00604     }
00605 
00606     // m= this .* m
00607     template <class E2, int CS2, int RS2> 
00608     typename EltResult<E2>::Mul
00609     elementwiseMultiply(const Mat<M,N,E2,CS2,RS2>& r) const {
00610         typename EltResult<E2>::Mul result;
00611         for (int j=0;j<N;++j) 
00612             result(j) = (*this)(j).elementwiseMultiply(r(j));
00613         return result;
00614     }
00615 
00616     // m= this ./ m
00617     template <class E2, int CS2, int RS2> 
00618     typename EltResult<E2>::Dvd
00619     elementwiseDivide(const Mat<M,N,E2,CS2,RS2>& r) const {
00620         typename EltResult<E2>::Dvd result;
00621         for (int j=0;j<N;++j) 
00622             result(j) = (*this)(j).elementwiseDivide(r(j));
00623         return result;
00624     }
00625 
00626     // We always punt to the SymMat since it knows better what to do.
00627     // m = this + sym
00628     template <class E2, int RS2> 
00629     typename Result<SymMat<M,E2,RS2> >::Add
00630     conformingAdd(const SymMat<M,E2,RS2>& sy) const {
00631         assert(M==N);
00632         return sy.conformingAdd(*this);
00633     }
00634     // m= this - sym
00635     template <class E2, int RS2> 
00636     typename Result<SymMat<M,E2,RS2> >::Sub
00637     conformingSubtract(const SymMat<M,E2,RS2>& sy) const {
00638         assert(M==N);
00639         return sy.conformingSubtractFromLeft(*this);
00640     }
00641     // m= sym - this
00642     template <class E2, int RS2> 
00643     typename SymMat<M,E2,RS2>::template Result<Mat>::Sub
00644     conformingSubtractFromLeft(const SymMat<M,E2,RS2>& sy) const {
00645         assert(M==N);
00646         return sy.conformingSubtract(*this);
00647     }
00648 
00649     // m= this * m
00650     template <int N2, class E2, int CS2, int RS2>
00651     typename Result<Mat<N,N2,E2,CS2,RS2> >::Mul
00652     conformingMultiply(const Mat<N,N2,E2,CS2,RS2>& m) const {
00653         typename Result<Mat<N,N2,E2,CS2,RS2> >::Mul result;
00654         for (int j=0;j<N2;++j)
00655             for (int i=0;i<M;++i)
00656                 result(i,j) = (*this)[i].conformingMultiply(m(j)); // i.e., dot()
00657         return result;
00658     }
00659     // m= m * this
00660     template <int M2, class E2, int CS2, int RS2>
00661     typename Mat<M2,M,E2,CS2,RS2>::template Result<Mat>::Mul
00662     conformingMultiplyFromLeft(const Mat<M2,M,E2,CS2,RS2>& m) const {
00663         return m.conformingMultiply(*this);
00664     }
00665 
00666     // m= this / m = this * m.invert()
00667     template <int M2, class E2, int CS2, int RS2> 
00668     typename Result<Mat<M2,N,E2,CS2,RS2> >::Dvd
00669     conformingDivide(const Mat<M2,N,E2,CS2,RS2>& m) const {
00670         return conformingMultiply(m.invert());
00671     }
00672     // m= m / this = m * this.invert()
00673     template <int M2, class E2, int CS2, int RS2> 
00674     typename Mat<M2,N,E2,CS2,RS2>::template Result<Mat>::Dvd
00675     conformingDivideFromLeft(const Mat<M2,N,E2,CS2,RS2>& m) const {
00676         return m.conformingMultiply((*this).invert());
00677     }
00678     
00679     const TRow& operator[](int i) const { return row(i); }
00680     TRow&       operator[](int i)       { return row(i); }    
00681     const TCol& operator()(int j) const { return col(j); }
00682     TCol&       operator()(int j)       { return col(j); }
00683     
00684     const E& operator()(int i,int j) const { return elt(i,j); }
00685     E&       operator()(int i,int j)       { return elt(i,j); }
00686 
00687     // This is the scalar Frobenius norm.
00688     ScalarNormSq normSqr() const { return scalarNormSqr(); }
00689     typename CNT<ScalarNormSq>::TSqrt 
00690         norm() const { return CNT<ScalarNormSq>::sqrt(scalarNormSqr()); }
00691 
00692     // There is no conventional meaning for normalize() applied to a matrix. We
00693     // choose to define it as follows:
00694     // If the elements of this Mat are scalars, the result is what you get by
00695     // dividing each element by the Frobenius norm() calculated above. If the elements are
00696     // *not* scalars, then the elements are *separately* normalized. That means
00697     // you will get a different answer from Mat<2,2,Mat33>::normalize() than you
00698     // would from a Mat<6,6>::normalize() containing the same scalars.
00699     //
00700     // Normalize returns a matrix of the same dimension but in new, packed storage
00701     // and with a return type that does not include negator<> even if the original
00702     // Mat<> does, because we can eliminate the negation here almost for free.
00703     // But we can't standardize (change conjugate to complex) for free, so we'll retain
00704     // conjugates if there are any.
00705     TNormalize normalize() const {
00706         if (CNT<E>::IsScalar) {
00707             return castAwayNegatorIfAny() / (SignInterpretation*norm());
00708         } else {
00709             TNormalize elementwiseNormalized;
00710             // punt to the column Vec to deal with the elements
00711             for (int j=0; j<N; ++j) 
00712                 elementwiseNormalized(j) = (*this)(j).normalize();
00713             return elementwiseNormalized;
00714         }
00715     }
00716 
00717     // Default inversion. Assume full rank if square, otherwise return
00718     // pseudoinverse. (Mostly TODO)
00719     TInvert invert() const;
00720 
00721     const Mat&   operator+() const { return *this; }
00722     const TNeg&  operator-() const { return negate(); }
00723     TNeg&        operator-()       { return updNegate(); }
00724     const THerm& operator~() const { return transpose(); }
00725     THerm&       operator~()       { return updTranspose(); }
00726 
00727     const TNeg&  negate() const { return *reinterpret_cast<const TNeg*>(this); }
00728     TNeg&        updNegate()    { return *reinterpret_cast<TNeg*>(this); }
00729 
00730     const THerm& transpose()    const { return *reinterpret_cast<const THerm*>(this); }
00731     THerm&       updTranspose()       { return *reinterpret_cast<THerm*>(this); }
00732 
00733     const TPosTrans& positionalTranspose() const
00734         { return *reinterpret_cast<const TPosTrans*>(this); }
00735     TPosTrans&       updPositionalTranspose()
00736         { return *reinterpret_cast<TPosTrans*>(this); }
00737 
00738     // If the underlying scalars are complex or conjugate, we can return a
00739     // reference to the real part just by recasting the data to a matrix of
00740     // the same dimensions but containing real elements, with the scalar
00741     // spacing doubled.
00742     const TReal& real() const { return *reinterpret_cast<const TReal*>(this); }
00743     TReal&       real()       { return *reinterpret_cast<      TReal*>(this); }
00744 
00745     // Getting the imaginary part is almost the same as real, but we have
00746     // to shift the starting address of the returned object by 1 real-size
00747     // ("precision") scalar so that the first element is the imaginary part
00748     // of the original first element.
00749     // TODO: should blow up or return a reference to a zero matrix if called
00750     // on a real object.
00751     // Had to contort these routines to get them through VC++ 7.net
00752     const TImag& imag()    const { 
00753         const int offs = ImagOffset;
00754         const Precision* p = reinterpret_cast<const Precision*>(this);
00755         return *reinterpret_cast<const TImag*>(p+offs);
00756     }
00757     TImag& imag() { 
00758         const int offs = ImagOffset;
00759         Precision* p = reinterpret_cast<Precision*>(this);
00760         return *reinterpret_cast<TImag*>(p+offs);
00761     }
00762 
00763     const TWithoutNegator& castAwayNegatorIfAny() const {return *reinterpret_cast<const TWithoutNegator*>(this);}
00764     TWithoutNegator&       updCastAwayNegatorIfAny()    {return *reinterpret_cast<TWithoutNegator*>(this);}
00765 
00766     const TRow& row(int i) const { 
00767         SimTK_INDEXCHECK(i,M, "Mat::row[i]");
00768         return *reinterpret_cast<const TRow*>(&d[i*RS]); 
00769     }
00770     TRow& row(int i) { 
00771         SimTK_INDEXCHECK(i,M, "Mat::row[i]");
00772         return *reinterpret_cast<TRow*>(&d[i*RS]); 
00773     }
00774 
00775     const TCol& col(int j) const { 
00776         SimTK_INDEXCHECK(j,N, "Mat::col(j)");
00777         return *reinterpret_cast<const TCol*>(&d[j*CS]); 
00778     }
00779     TCol& col(int j) { 
00780         SimTK_INDEXCHECK(j,N, "Mat::col(j)");
00781         return *reinterpret_cast<TCol*>(&d[j*CS]); 
00782     }    
00783     
00784     const E& elt(int i, int j) const {
00785         SimTK_INDEXCHECK(i,M, "Mat::elt(i,j)");
00786         SimTK_INDEXCHECK(j,N, "Mat::elt(i,j)");
00787         return d[i*RS+j*CS]; 
00788     }
00789     E& elt(int i, int j) { 
00790         SimTK_INDEXCHECK(i,M, "Mat::elt(i,j)");
00791         SimTK_INDEXCHECK(j,N, "Mat::elt(i,j)");
00792         return d[i*RS+j*CS]; 
00793     }
00794 
00798     const TDiag& diag() const { return *reinterpret_cast<const TDiag*>(d); }
00802     TDiag&       updDiag()    { return *reinterpret_cast<TDiag*>(d); }
00805     TDiag&       diag()       { return *reinterpret_cast<TDiag*>(d); }
00806 
00807     EStandard trace() const {return diag().sum();}
00808 
00809     // These are elementwise binary operators, (this op ee) by default but (ee op this) if
00810     // 'FromLeft' appears in the name. The result is a packed Mat<M,N> but the element type
00811     // may change. These are mostly used to implement global operators.
00812     // We call these "scalar" operators but actually the "scalar" can be a composite type.
00813 
00814     //TODO: consider converting 'e' to Standard Numbers as precalculation and changing
00815     // return type appropriately.
00816     template <class EE> Mat<M,N, typename CNT<E>::template Result<EE>::Mul>
00817     scalarMultiply(const EE& e) const {
00818         Mat<M,N, typename CNT<E>::template Result<EE>::Mul> result;
00819         for (int j=0; j<N; ++j) result(j) = (*this)(j).scalarMultiply(e);
00820         return result;
00821     }
00822     template <class EE> Mat<M,N, typename CNT<EE>::template Result<E>::Mul>
00823     scalarMultiplyFromLeft(const EE& e) const {
00824         Mat<M,N, typename CNT<EE>::template Result<E>::Mul> result;
00825         for (int j=0; j<N; ++j) result(j) = (*this)(j).scalarMultiplyFromLeft(e);
00826         return result;
00827     }
00828 
00829     // TODO: should precalculate and store 1/e, while converting to Standard Numbers. Note
00830     // that return type should change appropriately.
00831     template <class EE> Mat<M,N, typename CNT<E>::template Result<EE>::Dvd>
00832     scalarDivide(const EE& e) const {
00833         Mat<M,N, typename CNT<E>::template Result<EE>::Dvd> result;
00834         for (int j=0; j<N; ++j) result(j) = (*this)(j).scalarDivide(e);
00835         return result;
00836     }
00837     template <class EE> Mat<M,N, typename CNT<EE>::template Result<E>::Dvd>
00838     scalarDivideFromLeft(const EE& e) const {
00839         Mat<M,N, typename CNT<EE>::template Result<E>::Dvd> result;
00840         for (int j=0; j<N; ++j) result(j) = (*this)(j).scalarDivideFromLeft(e);
00841         return result;
00842     }
00843 
00844     // Additive operators for scalars operate only on the diagonal.
00845     template <class EE> Mat<M,N, typename CNT<E>::template Result<EE>::Add>
00846     scalarAdd(const EE& e) const {
00847         Mat<M,N, typename CNT<E>::template Result<EE>::Add> result(*this);
00848         result.diag() += e;
00849         return result;
00850     }
00851     // Add is commutative, so no 'FromLeft'.
00852 
00853     template <class EE> Mat<M,N, typename CNT<E>::template Result<EE>::Sub>
00854     scalarSubtract(const EE& e) const {
00855         Mat<M,N, typename CNT<E>::template Result<EE>::Sub> result(*this);
00856         result.diag() -= e;
00857         return result;
00858     }
00859     // Should probably do something clever with negation here (s - m)
00860     template <class EE> Mat<M,N, typename CNT<EE>::template Result<E>::Sub>
00861     scalarSubtractFromLeft(const EE& e) const {
00862         Mat<M,N, typename CNT<EE>::template Result<E>::Sub> result(-(*this));
00863         result.diag() += e; // yes, add
00864         return result;
00865     }
00866 
00867     // Generic assignments for any element type not listed explicitly, including scalars.
00868     // These are done repeatedly for each element and only work if the operation can
00869     // be performed leaving the original element type.
00870     template <class EE> Mat& operator =(const EE& e) {return scalarEq(e);}
00871     template <class EE> Mat& operator+=(const EE& e) {return scalarPlusEq(e);}
00872     template <class EE> Mat& operator-=(const EE& e) {return scalarMinusEq(e);}
00873     template <class EE> Mat& operator*=(const EE& e) {return scalarTimesEq(e);}
00874     template <class EE> Mat& operator/=(const EE& e) {return scalarDivideEq(e);}
00875 
00876     // Generalized scalar assignment & computed assignment methods. These will work
00877     // for any assignment-compatible element, not just scalars.
00878     template <class EE> Mat& scalarEq(const EE& ee)
00879       { for(int j=0; j<N; ++j) (*this)(j).scalarEq(EE(0)); 
00880         diag().scalarEq(ee); 
00881         return *this; }
00882 
00883     template <class EE> Mat& scalarPlusEq(const EE& ee)
00884       { diag().scalarPlusEq(ee); return *this; }
00885 
00886     template <class EE> Mat& scalarMinusEq(const EE& ee)
00887       { diag().scalarMinusEq(ee); return *this; }
00888     // m = s - m; negate m, then add s
00889     template <class EE> Mat& scalarMinusEqFromLeft(const EE& ee)
00890       { scalarTimesEq(E(-1)); diag().scalarAdd(ee); return *this; }
00891 
00892     template <class EE> Mat& scalarTimesEq(const EE& ee)
00893       { for(int j=0; j<N; ++j) (*this)(j).scalarTimesEq(ee); return *this; }
00894     template <class EE> Mat& scalarTimesEqFromLeft(const EE& ee)
00895       { for(int j=0; j<N; ++j) (*this)(j).scalarTimesEqFromLeft(ee); return *this; } 
00896 
00897     template <class EE> Mat& scalarDivideEq(const EE& ee)
00898       { for(int j=0; j<N; ++j) (*this)(j).scalarDivideEq(ee); return *this; }
00899     template <class EE> Mat& scalarDivideEqFromLeft(const EE& ee)
00900       { for(int j=0; j<N; ++j) (*this)(j).scalarDivideEqFromLeft(ee); return *this; } 
00901 
00902     void setToNaN() {
00903         for (int j=0; j<N; ++j)
00904             (*this)(j).setToNaN();
00905     }
00906 
00907     void setToZero() {
00908         for (int j=0; j<N; ++j)
00909             (*this)(j).setToZero();
00910     }
00911 
00912     // Extract a sub-Mat with size known at compile time. These have to be
00913     // called with explicit template arguments, e.g. getSubMat<3,4>(i,j).
00914 
00915     template <int MM, int NN> struct SubMat {
00916         typedef Mat<MM,NN,ELT,CS,RS> Type;
00917     };
00918 
00919     template <int MM, int NN>
00920     const typename SubMat<MM,NN>::Type& getSubMat(int i, int j) const {
00921         assert(0 <= i && i + MM <= M);
00922         assert(0 <= j && j + NN <= N);
00923         return SubMat<MM,NN>::Type::getAs(&(*this)(i,j));
00924     }
00925     template <int MM, int NN>
00926     typename SubMat<MM,NN>::Type& updSubMat(int i, int j) {
00927         assert(0 <= i && i + MM <= M);
00928         assert(0 <= j && j + NN <= N);
00929         return SubMat<MM,NN>::Type::updAs(&(*this)(i,j));
00930     }
00931     template <int MM, int NN>
00932     void setSubMat(int i, int j, const typename SubMat<MM,NN>::Type& value) {
00933         assert(0 <= i && i + MM <= M);
00934         assert(0 <= j && j + NN <= N);
00935         SubMat<MM,NN>::Type::updAs(&(*this)(i,j)) = value;
00936     }
00937 
00940     TDropRow dropRow(int i) const {
00941         assert(0 <= i && i < M);
00942         TDropRow out;
00943         for (int r=0, nxt=0; r<M-1; ++r, ++nxt) {
00944             if (nxt==i) ++nxt;  // skip the loser
00945             out[r] = (*this)[nxt];
00946         }
00947         return out;
00948     }
00949 
00952     TDropCol dropCol(int j) const {
00953         assert(0 <= j && j < N);
00954         TDropCol out;
00955         for (int c=0, nxt=0; c<N-1; ++c, ++nxt) {
00956             if (nxt==j) ++nxt;  // skip the loser
00957             out(c) = (*this)(nxt);
00958         }
00959         return out;
00960     }
00961 
00965     TDropRowCol dropRowCol(int i, int j) const {
00966         assert(0 <= i && i < M);
00967         assert(0 <= j && j < N);
00968         TDropRowCol out;
00969         for (int c=0, nxtc=0; c<N-1; ++c, ++nxtc) { 
00970             if (nxtc==j) ++nxtc;
00971             for (int r=0, nxtr=0; r<M-1; ++r, ++nxtr) {
00972                 if (nxtr==i) ++nxtr;
00973                 out(r,c) = (*this)(nxtr,nxtc);
00974             }
00975         }
00976         return out;
00977     }
00978 
00982     template <class EE, int SS> 
00983     TAppendRow appendRow(const Row<N,EE,SS>& row) const {
00984         TAppendRow out;
00985         out.template updSubMat<M,N>(0,0) = (*this);
00986         out[M] = row;
00987         return out;
00988     }
00989 
00993     template <class EE, int SS> 
00994     TAppendCol appendCol(const Vec<M,EE,SS>& col) const {
00995         TAppendCol out;
00996         out.template updSubMat<M,N>(0,0) = (*this);
00997         out(N) = col;
00998         return out;
00999     }
01000 
01007     template <class ER, int SR, class EC, int SC> 
01008     TAppendRowCol appendRowCol(const Row<N+1,ER,SR>& row,
01009                                const Vec<M+1,EC,SC>& col) const 
01010     {
01011         TAppendRowCol out;
01012         out.template updSubMat<M,N>(0,0) = (*this);
01013         out[M].template updSubRow<N>(0) = 
01014             row.template getSubRow<N>(0); // ignore last element
01015         out(N) = col;
01016         return out;
01017     }
01018 
01024     template <class EE, int SS> 
01025     TAppendRow insertRow(int i, const Row<N,EE,SS>& row) const {
01026         assert(0 <= i && i <= M);
01027         if (i==M) return appendRow(row);
01028         TAppendRow out;
01029         for (int r=0, nxt=0; r<M; ++r, ++nxt) {
01030             if (nxt==i) out[nxt++] = row;
01031             out[nxt] = (*this)[r];
01032         }
01033         return out;
01034     }
01035 
01041     template <class EE, int SS> 
01042     TAppendCol insertCol(int j, const Vec<M,EE,SS>& col) const {
01043         assert(0 <= j && j <= N);
01044         if (j==N) return appendCol(col);
01045         TAppendCol out;
01046         for (int c=0, nxt=0; c<N; ++c, ++nxt) {
01047             if (nxt==j) out(nxt++) = col;
01048             out(nxt) = (*this)(c);
01049         }
01050         return out;
01051     }
01052 
01060     template <class ER, int SR, class EC, int SC>
01061     TAppendRowCol insertRowCol(int i, int j, const Row<N+1,ER,SR>& row,
01062                                              const Vec<M+1,EC,SC>& col) const {
01063         assert(0 <= i && i <= M);
01064         assert(0 <= j && j <= N);
01065         TAppendRowCol out;
01066         for (int c=0, nxtc=0; c<N; ++c, ++nxtc) { 
01067             if (nxtc==j) ++nxtc;   // leave room
01068             for (int r=0, nxtr=0; r<M; ++r, ++nxtr) {
01069                 if (nxtr==i) ++nxtr;
01070                 out(nxtr,nxtc) = (*this)(r,c);
01071             }
01072         }
01073         out[i] = row;
01074         out(j) = col; // overwrites row's j'th element
01075         return out;
01076     }
01077 
01078     // These assume we are given a pointer to d[0] of a Mat<M,N,E,CS,RS> like this one.
01079     static const Mat& getAs(const ELT* p)  {return *reinterpret_cast<const Mat*>(p);}
01080     static Mat&       updAs(ELT* p)        {return *reinterpret_cast<Mat*>(p);}
01081 
01082     // Note packed spacing
01083     static Mat<M,N,ELT,M,1> getNaN() { 
01084         Mat<M,N,ELT,M,1> m;
01085         m.setToNaN();
01086         return m;
01087     }
01088 
01090     bool isNaN() const {
01091         for (int j=0; j<N; ++j)
01092             if (this->col(j).isNaN())
01093                 return true;
01094         return false;
01095     }
01096 
01099     bool isInf() const {
01100         bool seenInf = false;
01101         for (int j=0; j<N; ++j) {
01102             if (!this->col(j).isFinite()) {
01103                 if (!this->col(j).isInf()) 
01104                     return false; // something bad was found
01105                 seenInf = true; 
01106             }
01107         }
01108         return seenInf;
01109     }
01110 
01112     bool isFinite() const {
01113         for (int j=0; j<N; ++j)
01114             if (!this->col(j).isFinite())
01115                 return false;
01116         return true;
01117     }
01118 
01121     static double getDefaultTolerance() {return MinDim*CNT<ELT>::getDefaultTolerance();}
01122 
01125     template <class E2, int CS2, int RS2>
01126     bool isNumericallyEqual(const Mat<M,N,E2,CS2,RS2>& m, double tol) const {
01127         for (int j=0; j < N; ++j)
01128             if (!(*this)(j).isNumericallyEqual(m(j), tol))
01129                 return false;
01130         return true;
01131     }
01132 
01136     template <class E2, int CS2, int RS2>
01137     bool isNumericallyEqual(const Mat<M,N,E2,CS2,RS2>& m) const {
01138         const double tol = std::max(getDefaultTolerance(),m.getDefaultTolerance());
01139         return isNumericallyEqual(m, tol);
01140     }
01141 
01147     bool isNumericallyEqual
01148        (const ELT& e,
01149         double     tol = getDefaultTolerance()) const 
01150     {
01151         for (int i=0; i<M; ++i)
01152             for (int j=0; j<N; ++j) {
01153                 if (i==j) {
01154                     if (!CNT<ELT>::isNumericallyEqual((*this)(i,i), e, tol))
01155                         return false;
01156                 } else {
01157                     // off-diagonals must be zero
01158                     if (!CNT<ELT>::isNumericallyEqual((*this)(i,j), ELT(0), tol))
01159                         return false;
01160                 }
01161             }
01162         return true;
01163     }
01164 
01172     bool isNumericallySymmetric(double tol = getDefaultTolerance()) const {
01173         if (M != N) return false; // handled at compile time
01174         for (int j=0; j<M; ++j)
01175             for (int i=j; i<M; ++i)
01176                 if (!CNT<ELT>::isNumericallyEqual(elt(j,i), CNT<ELT>::transpose(elt(i,j)), tol))
01177                     return false;
01178         return true;
01179     }
01180 
01187     bool isExactlySymmetric() const {
01188         if (M != N) return false; // handled at compile time
01189         for (int j=0; j<M; ++j)
01190             for (int i=j; i<M; ++i)
01191                 if (elt(j,i) != CNT<ELT>::transpose(elt(i,j)))
01192                     return false;
01193         return true;
01194     }
01195     
01197     TRow colSum() const {
01198         TRow temp;
01199         for (int j = 0; j < N; ++j)
01200             temp[j] = col(j).sum();
01201         return temp;
01202     }
01205     TRow sum() const {return colSum();}
01206 
01208     TCol rowSum() const {
01209         TCol temp;
01210         for (int i = 0; i < M; ++i)
01211             temp[i] = row(i).sum();
01212         return temp;
01213     }
01214 
01215     // Functions to be used for Scripting in MATLAB and languages that do not support operator overloading
01217     std::string toString() const {
01218         std::stringstream stream;
01219         stream <<  (*this) ;
01220         return stream.str(); 
01221     }
01223     const ELT& get(int i,int j) const { return elt(i,j); }
01225     void       set(int i,int j, const ELT& value)       { elt(i,j)=value; }
01226 
01227 private:
01228     E d[NActualElements];
01229 
01230     // This permits running through d as though it were stored
01231     // in row order with packed elements. Pass in the k'th value
01232     // of the row ordering and get back the index into d where
01233     // that element is stored.
01234     int rIx(int k) const {
01235         const int row = k / N;
01236         const int col = k % N; // that's modulus, not cross product!
01237         return row*RS + col*CS;
01238     }
01239 };
01240 
01242 // Global operators involving two matrices. //
01243 //   m+m, m-m, m*m, m==m, m!=m              //
01245 
01246 template <int M, int N, class EL, int CSL, int RSL, class ER, int CSR, int RSR> inline 
01247 typename Mat<M,N,EL,CSL,RSL>::template Result<Mat<M,N,ER,CSR,RSR> >::Add
01248 operator+(const Mat<M,N,EL,CSL,RSL>& l, const Mat<M,N,ER,CSR,RSR>& r) { 
01249     return Mat<M,N,EL,CSL,RSL>::template Result<Mat<M,N,ER,CSR,RSR> >
01250         ::AddOp::perform(l,r);
01251 }
01252 
01253 template <int M, int N, class EL, int CSL, int RSL, class ER, int CSR, int RSR> inline
01254 typename Mat<M,N,EL,CSL,RSL>::template Result<Mat<M,N,ER,CSR,RSR> >::Sub
01255 operator-(const Mat<M,N,EL,CSL,RSL>& l, const Mat<M,N,ER,CSR,RSR>& r) { 
01256     return Mat<M,N,EL,CSL,RSL>::template Result<Mat<M,N,ER,CSR,RSR> >
01257         ::SubOp::perform(l,r);
01258 }
01259 
01260 // Matrix multiply of an MxN by NxP to produce a packed MxP.
01261 template <int M, int N, class EL, int CSL, int RSL, int P, class ER, int CSR, int RSR> inline
01262 typename Mat<M,N,EL,CSL,RSL>::template Result<Mat<N,P,ER,CSR,RSR> >::Mul
01263 operator*(const Mat<M,N,EL,CSL,RSL>& l, const Mat<N,P,ER,CSR,RSR>& r) { 
01264     return Mat<M,N,EL,CSL,RSL>::template Result<Mat<N,P,ER,CSR,RSR> >
01265         ::MulOp::perform(l,r);
01266 }
01267 
01268 // Non-conforming matrix multiply of an MxN by MMxNN; will be a scalar multiply if one
01269 // has scalar elements and the other has composite elements.
01270 template <int M, int N, class EL, int CSL, int RSL, int MM, int NN, class ER, int CSR, int RSR> inline
01271 typename Mat<M,N,EL,CSL,RSL>::template Result<Mat<MM,NN,ER,CSR,RSR> >::MulNon
01272 operator*(const Mat<M,N,EL,CSL,RSL>& l, const Mat<MM,NN,ER,CSR,RSR>& r) { 
01273     return Mat<M,N,EL,CSL,RSL>::template Result<Mat<MM,NN,ER,CSR,RSR> >
01274                 ::MulOpNonConforming::perform(l,r);
01275 }
01276 
01277 template <int M, int N, class EL, int CSL, int RSL, class ER, int CSR, int RSR> inline
01278 bool operator==(const Mat<M,N,EL,CSL,RSL>& l, const Mat<M,N,ER,CSR,RSR>& r) { 
01279     for (int j=0; j<N; ++j)
01280         if (l(j) != r(j)) return false;
01281     return true;
01282 }
01283 template <int M, int N, class EL, int CSL, int RSL, class ER, int CSR, int RSR> inline
01284 bool operator!=(const Mat<M,N,EL,CSL,RSL>& l, const Mat<M,N,ER,CSR,RSR>& r) { 
01285     return !(l==r);
01286 }
01287 
01288 
01290 // Global operators involving a matrix and a scalar. //
01292 
01293 // SCALAR MULTIPLY
01294 
01295 // m = m*real, real*m 
01296 template <int M, int N, class E, int CS, int RS> inline
01297 typename Mat<M,N,E,CS,RS>::template Result<float>::Mul
01298 operator*(const Mat<M,N,E,CS,RS>& l, const float& r)
01299   { return Mat<M,N,E,CS,RS>::template Result<float>::MulOp::perform(l,r); }
01300 template <int M, int N, class E, int CS, int RS> inline
01301 typename Mat<M,N,E,CS,RS>::template Result<float>::Mul
01302 operator*(const float& l, const Mat<M,N,E,CS,RS>& r) {return r*l;}
01303 
01304 template <int M, int N, class E, int CS, int RS> inline
01305 typename Mat<M,N,E,CS,RS>::template Result<double>::Mul
01306 operator*(const Mat<M,N,E,CS,RS>& l, const double& r)
01307   { return Mat<M,N,E,CS,RS>::template Result<double>::MulOp::perform(l,r); }
01308 template <int M, int N, class E, int CS, int RS> inline
01309 typename Mat<M,N,E,CS,RS>::template Result<double>::Mul
01310 operator*(const double& l, const Mat<M,N,E,CS,RS>& r) {return r*l;}
01311 
01312 template <int M, int N, class E, int CS, int RS> inline
01313 typename Mat<M,N,E,CS,RS>::template Result<long double>::Mul
01314 operator*(const Mat<M,N,E,CS,RS>& l, const long double& r)
01315   { return Mat<M,N,E,CS,RS>::template Result<long double>::MulOp::perform(l,r); }
01316 template <int M, int N, class E, int CS, int RS> inline
01317 typename Mat<M,N,E,CS,RS>::template Result<long double>::Mul
01318 operator*(const long double& l, const Mat<M,N,E,CS,RS>& r) {return r*l;}
01319 
01320 // m = m*int, int*m -- just convert int to m's precision float
01321 template <int M, int N, class E, int CS, int RS> inline
01322 typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Mul
01323 operator*(const Mat<M,N,E,CS,RS>& l, int r) {return l * (typename CNT<E>::Precision)r;}
01324 template <int M, int N, class E, int CS, int RS> inline
01325 typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Mul
01326 operator*(int l, const Mat<M,N,E,CS,RS>& r) {return r * (typename CNT<E>::Precision)l;}
01327 
01328 // Complex, conjugate, and negator are all easy to templatize.
01329 
01330 // m = m*complex, complex*m
01331 template <int M, int N, class E, int CS, int RS, class R> inline
01332 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Mul
01333 operator*(const Mat<M,N,E,CS,RS>& l, const std::complex<R>& r)
01334   { return Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::MulOp::perform(l,r); }
01335 template <int M, int N, class E, int CS, int RS, class R> inline
01336 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Mul
01337 operator*(const std::complex<R>& l, const Mat<M,N,E,CS,RS>& r) {return r*l;}
01338 
01339 // m = m*conjugate, conjugate*m (convert conjugate->complex)
01340 template <int M, int N, class E, int CS, int RS, class R> inline
01341 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Mul
01342 operator*(const Mat<M,N,E,CS,RS>& l, const conjugate<R>& r) {return l*(std::complex<R>)r;}
01343 template <int M, int N, class E, int CS, int RS, class R> inline
01344 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Mul
01345 operator*(const conjugate<R>& l, const Mat<M,N,E,CS,RS>& r) {return r*(std::complex<R>)l;}
01346 
01347 // m = m*negator, negator*m: convert negator to standard number
01348 template <int M, int N, class E, int CS, int RS, class R> inline
01349 typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Mul
01350 operator*(const Mat<M,N,E,CS,RS>& l, const negator<R>& r) {return l * (typename negator<R>::StdNumber)(R)r;}
01351 template <int M, int N, class E, int CS, int RS, class R> inline
01352 typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Mul
01353 operator*(const negator<R>& l, const Mat<M,N,E,CS,RS>& r) {return r * (typename negator<R>::StdNumber)(R)l;}
01354 
01355 
01356 // SCALAR DIVIDE. This is a scalar operation when the scalar is on the right,
01357 // but when it is on the left it means scalar * pseudoInverse(mat), 
01358 // which is a matrix whose type is like the matrix's Hermitian transpose.
01359 // TODO: for now it is just going to call mat.invert() which will fail on
01360 // singular matrices.
01361 
01362 // m = m/real, real/m 
01363 template <int M, int N, class E, int CS, int RS> inline
01364 typename Mat<M,N,E,CS,RS>::template Result<float>::Dvd
01365 operator/(const Mat<M,N,E,CS,RS>& l, const float& r)
01366 {   return Mat<M,N,E,CS,RS>::template Result<float>::DvdOp::perform(l,r); }
01367 
01368 template <int M, int N, class E, int CS, int RS> inline
01369 typename CNT<float>::template Result<Mat<M,N,E,CS,RS> >::Dvd
01370 operator/(const float& l, const Mat<M,N,E,CS,RS>& r)
01371 {   return l * r.invert(); }
01372 
01373 template <int M, int N, class E, int CS, int RS> inline
01374 typename Mat<M,N,E,CS,RS>::template Result<double>::Dvd
01375 operator/(const Mat<M,N,E,CS,RS>& l, const double& r)
01376 {   return Mat<M,N,E,CS,RS>::template Result<double>::DvdOp::perform(l,r); }
01377 
01378 template <int M, int N, class E, int CS, int RS> inline
01379 typename CNT<double>::template Result<Mat<M,N,E,CS,RS> >::Dvd
01380 operator/(const double& l, const Mat<M,N,E,CS,RS>& r)
01381 {   return l * r.invert(); }
01382 
01383 template <int M, int N, class E, int CS, int RS> inline
01384 typename Mat<M,N,E,CS,RS>::template Result<long double>::Dvd
01385 operator/(const Mat<M,N,E,CS,RS>& l, const long double& r)
01386 {   return Mat<M,N,E,CS,RS>::template Result<long double>::DvdOp::perform(l,r); }
01387 
01388 template <int M, int N, class E, int CS, int RS> inline
01389 typename CNT<long double>::template Result<Mat<M,N,E,CS,RS> >::Dvd
01390 operator/(const long double& l, const Mat<M,N,E,CS,RS>& r)
01391 {   return l * r.invert(); }
01392 
01393 // m = m/int, int/m -- just convert int to m's precision float
01394 template <int M, int N, class E, int CS, int RS> inline
01395 typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Dvd
01396 operator/(const Mat<M,N,E,CS,RS>& l, int r) 
01397 {   return l / (typename CNT<E>::Precision)r; }
01398 
01399 template <int M, int N, class E, int CS, int RS> inline
01400 typename CNT<typename CNT<E>::Precision>::template Result<Mat<M,N,E,CS,RS> >::Dvd
01401 operator/(int l, const Mat<M,N,E,CS,RS>& r) 
01402 {   return (typename CNT<E>::Precision)l / r; }
01403 
01404 
01405 // Complex, conjugate, and negator are all easy to templatize.
01406 
01407 // m = m/complex, complex/m
01408 template <int M, int N, class E, int CS, int RS, class R> inline
01409 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Dvd
01410 operator/(const Mat<M,N,E,CS,RS>& l, const std::complex<R>& r)
01411   { return Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::DvdOp::perform(l,r); }
01412 template <int M, int N, class E, int CS, int RS, class R> inline
01413 typename CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::Dvd
01414 operator/(const std::complex<R>& l, const Mat<M,N,E,CS,RS>& r)
01415   { return CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::DvdOp::perform(l,r); }
01416 
01417 // m = m/conjugate, conjugate/m (convert conjugate->complex)
01418 template <int M, int N, class E, int CS, int RS, class R> inline
01419 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Dvd
01420 operator/(const Mat<M,N,E,CS,RS>& l, const conjugate<R>& r) {return l/(std::complex<R>)r;}
01421 template <int M, int N, class E, int CS, int RS, class R> inline
01422 typename CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::Dvd
01423 operator/(const conjugate<R>& l, const Mat<M,N,E,CS,RS>& r) {return (std::complex<R>)l/r;}
01424 
01425 // m = m/negator, negator/m: convert negator to a standard number
01426 template <int M, int N, class E, int CS, int RS, class R> inline
01427 typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Dvd
01428 operator/(const Mat<M,N,E,CS,RS>& l, const negator<R>& r) {return l/(typename negator<R>::StdNumber)(R)r;}
01429 template <int M, int N, class E, int CS, int RS, class R> inline
01430 typename CNT<R>::template Result<Mat<M,N,E,CS,RS> >::Dvd
01431 operator/(const negator<R>& l, const Mat<M,N,E,CS,RS>& r) {return (typename negator<R>::StdNumber)(R)l/r;}
01432 
01433 
01434 // Add and subtract are odd as scalar ops. They behave as though the
01435 // scalar stands for a conforming matrix whose diagonal elements are that,
01436 // scalar and then a normal matrix add or subtract is done.
01437 
01438 // SCALAR ADD
01439 
01440 // m = m+real, real+m 
01441 template <int M, int N, class E, int CS, int RS> inline
01442 typename Mat<M,N,E,CS,RS>::template Result<float>::Add
01443 operator+(const Mat<M,N,E,CS,RS>& l, const float& r)
01444   { return Mat<M,N,E,CS,RS>::template Result<float>::AddOp::perform(l,r); }
01445 template <int M, int N, class E, int CS, int RS> inline
01446 typename Mat<M,N,E,CS,RS>::template Result<float>::Add
01447 operator+(const float& l, const Mat<M,N,E,CS,RS>& r) {return r+l;}
01448 
01449 template <int M, int N, class E, int CS, int RS> inline
01450 typename Mat<M,N,E,CS,RS>::template Result<double>::Add
01451 operator+(const Mat<M,N,E,CS,RS>& l, const double& r)
01452   { return Mat<M,N,E,CS,RS>::template Result<double>::AddOp::perform(l,r); }
01453 template <int M, int N, class E, int CS, int RS> inline
01454 typename Mat<M,N,E,CS,RS>::template Result<double>::Add
01455 operator+(const double& l, const Mat<M,N,E,CS,RS>& r) {return r+l;}
01456 
01457 template <int M, int N, class E, int CS, int RS> inline
01458 typename Mat<M,N,E,CS,RS>::template Result<long double>::Add
01459 operator+(const Mat<M,N,E,CS,RS>& l, const long double& r)
01460   { return Mat<M,N,E,CS,RS>::template Result<long double>::AddOp::perform(l,r); }
01461 template <int M, int N, class E, int CS, int RS> inline
01462 typename Mat<M,N,E,CS,RS>::template Result<long double>::Add
01463 operator+(const long double& l, const Mat<M,N,E,CS,RS>& r) {return r+l;}
01464 
01465 // m = m+int, int+m -- just convert int to m's precision float
01466 template <int M, int N, class E, int CS, int RS> inline
01467 typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Add
01468 operator+(const Mat<M,N,E,CS,RS>& l, int r) {return l + (typename CNT<E>::Precision)r;}
01469 template <int M, int N, class E, int CS, int RS> inline
01470 typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Add
01471 operator+(int l, const Mat<M,N,E,CS,RS>& r) {return r + (typename CNT<E>::Precision)l;}
01472 
01473 // Complex, conjugate, and negator are all easy to templatize.
01474 
01475 // m = m+complex, complex+m
01476 template <int M, int N, class E, int CS, int RS, class R> inline
01477 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Add
01478 operator+(const Mat<M,N,E,CS,RS>& l, const std::complex<R>& r)
01479   { return Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::AddOp::perform(l,r); }
01480 template <int M, int N, class E, int CS, int RS, class R> inline
01481 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Add
01482 operator+(const std::complex<R>& l, const Mat<M,N,E,CS,RS>& r) {return r+l;}
01483 
01484 // m = m+conjugate, conjugate+m (convert conjugate->complex)
01485 template <int M, int N, class E, int CS, int RS, class R> inline
01486 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Add
01487 operator+(const Mat<M,N,E,CS,RS>& l, const conjugate<R>& r) {return l+(std::complex<R>)r;}
01488 template <int M, int N, class E, int CS, int RS, class R> inline
01489 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Add
01490 operator+(const conjugate<R>& l, const Mat<M,N,E,CS,RS>& r) {return r+(std::complex<R>)l;}
01491 
01492 // m = m+negator, negator+m: convert negator to standard number
01493 template <int M, int N, class E, int CS, int RS, class R> inline
01494 typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Add
01495 operator+(const Mat<M,N,E,CS,RS>& l, const negator<R>& r) {return l + (typename negator<R>::StdNumber)(R)r;}
01496 template <int M, int N, class E, int CS, int RS, class R> inline
01497 typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Add
01498 operator+(const negator<R>& l, const Mat<M,N,E,CS,RS>& r) {return r + (typename negator<R>::StdNumber)(R)l;}
01499 
01500 // SCALAR SUBTRACT -- careful, not commutative.
01501 
01502 // m = m-real, real-m 
01503 template <int M, int N, class E, int CS, int RS> inline
01504 typename Mat<M,N,E,CS,RS>::template Result<float>::Sub
01505 operator-(const Mat<M,N,E,CS,RS>& l, const float& r)
01506   { return Mat<M,N,E,CS,RS>::template Result<float>::SubOp::perform(l,r); }
01507 template <int M, int N, class E, int CS, int RS> inline
01508 typename CNT<float>::template Result<Mat<M,N,E,CS,RS> >::Sub
01509 operator-(const float& l, const Mat<M,N,E,CS,RS>& r)
01510   { return CNT<float>::template Result<Mat<M,N,E,CS,RS> >::SubOp::perform(l,r); }
01511 
01512 template <int M, int N, class E, int CS, int RS> inline
01513 typename Mat<M,N,E,CS,RS>::template Result<double>::Sub
01514 operator-(const Mat<M,N,E,CS,RS>& l, const double& r)
01515   { return Mat<M,N,E,CS,RS>::template Result<double>::SubOp::perform(l,r); }
01516 template <int M, int N, class E, int CS, int RS> inline
01517 typename CNT<double>::template Result<Mat<M,N,E,CS,RS> >::Sub
01518 operator-(const double& l, const Mat<M,N,E,CS,RS>& r)
01519   { return CNT<double>::template Result<Mat<M,N,E,CS,RS> >::SubOp::perform(l,r); }
01520 
01521 template <int M, int N, class E, int CS, int RS> inline
01522 typename Mat<M,N,E,CS,RS>::template Result<long double>::Sub
01523 operator-(const Mat<M,N,E,CS,RS>& l, const long double& r)
01524   { return Mat<M,N,E,CS,RS>::template Result<long double>::SubOp::perform(l,r); }
01525 template <int M, int N, class E, int CS, int RS> inline
01526 typename CNT<long double>::template Result<Mat<M,N,E,CS,RS> >::Sub
01527 operator-(const long double& l, const Mat<M,N,E,CS,RS>& r)
01528   { return CNT<long double>::template Result<Mat<M,N,E,CS,RS> >::SubOp::perform(l,r); }
01529 
01530 // m = m-int, int-m // just convert int to m's precision float
01531 template <int M, int N, class E, int CS, int RS> inline
01532 typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Sub
01533 operator-(const Mat<M,N,E,CS,RS>& l, int r) {return l - (typename CNT<E>::Precision)r;}
01534 template <int M, int N, class E, int CS, int RS> inline
01535 typename CNT<typename CNT<E>::Precision>::template Result<Mat<M,N,E,CS,RS> >::Sub
01536 operator-(int l, const Mat<M,N,E,CS,RS>& r) {return (typename CNT<E>::Precision)l - r;}
01537 
01538 
01539 // Complex, conjugate, and negator are all easy to templatize.
01540 
01541 // m = m-complex, complex-m
01542 template <int M, int N, class E, int CS, int RS, class R> inline
01543 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Sub
01544 operator-(const Mat<M,N,E,CS,RS>& l, const std::complex<R>& r)
01545   { return Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::SubOp::perform(l,r); }
01546 template <int M, int N, class E, int CS, int RS, class R> inline
01547 typename CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::Sub
01548 operator-(const std::complex<R>& l, const Mat<M,N,E,CS,RS>& r)
01549   { return CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::SubOp::perform(l,r); }
01550 
01551 // m = m-conjugate, conjugate-m (convert conjugate->complex)
01552 template <int M, int N, class E, int CS, int RS, class R> inline
01553 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Sub
01554 operator-(const Mat<M,N,E,CS,RS>& l, const conjugate<R>& r) {return l-(std::complex<R>)r;}
01555 template <int M, int N, class E, int CS, int RS, class R> inline
01556 typename CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::Sub
01557 operator-(const conjugate<R>& l, const Mat<M,N,E,CS,RS>& r) {return (std::complex<R>)l-r;}
01558 
01559 // m = m-negator, negator-m: convert negator to standard number
01560 template <int M, int N, class E, int CS, int RS, class R> inline
01561 typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Sub
01562 operator-(const Mat<M,N,E,CS,RS>& l, const negator<R>& r) {return l-(typename negator<R>::StdNumber)(R)r;}
01563 template <int M, int N, class E, int CS, int RS, class R> inline
01564 typename CNT<R>::template Result<Mat<M,N,E,CS,RS> >::Sub
01565 operator-(const negator<R>& l, const Mat<M,N,E,CS,RS>& r) {return (typename negator<R>::StdNumber)(R)l-r;}
01566 
01567 
01568 // Mat I/O
01569 template <int M, int N, class E, int CS, int RS, class CHAR, class TRAITS> inline
01570 std::basic_ostream<CHAR,TRAITS>&
01571 operator<<(std::basic_ostream<CHAR,TRAITS>& o, const Mat<M,N,E,CS,RS>& m) {
01572     for (int i=0;i<M;++i) {
01573         o << std::endl << "[";
01574         for (int j=0;j<N;++j)         
01575             o << (j>0?",":"") << m(i,j);
01576         o << "]";
01577     }
01578     if (M) o << std::endl;
01579     return o; 
01580 }
01581 
01582 template <int M, int N, class E, int CS, int RS, class CHAR, class TRAITS> inline
01583 std::basic_istream<CHAR,TRAITS>&
01584 operator>>(std::basic_istream<CHAR,TRAITS>& is, Mat<M,N,E,CS,RS>& m) {
01585     // TODO: not sure how to do Vec input yet
01586     assert(false);
01587     return is;
01588 }
01589 
01590 } //namespace SimTK
01591 
01592 
01593 #endif //SimTK_SIMMATRIX_SMALLMATRIX_MAT_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines