Simbody
3.5
|
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_