Simbody  3.4
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Mat.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMMATRIX_SMALLMATRIX_MAT_H_
2 #define SimTK_SIMMATRIX_SMALLMATRIX_MAT_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm): SimTKcommon *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2005-12 Stanford University and the Authors. *
13  * Authors: Michael Sherman *
14  * Contributors: *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
31 #include "SimTKcommon/internal/common.h"
32 
33 namespace SimTK {
34 
97 template <int M, int N, class ELT, int CS, int RS> class Mat {
98  typedef ELT E;
99  typedef typename CNT<E>::TNeg ENeg;
100  typedef typename CNT<E>::TWithoutNegator EWithoutNegator;
101  typedef typename CNT<E>::TReal EReal;
102  typedef typename CNT<E>::TImag EImag;
103  typedef typename CNT<E>::TComplex EComplex;
104  typedef typename CNT<E>::THerm EHerm;
105  typedef typename CNT<E>::TPosTrans EPosTrans;
106  typedef typename CNT<E>::TSqHermT ESqHermT;
107  typedef typename CNT<E>::TSqTHerm ESqTHerm;
108 
109  typedef typename CNT<E>::TSqrt ESqrt;
110  typedef typename CNT<E>::TAbs EAbs;
111  typedef typename CNT<E>::TStandard EStandard;
112  typedef typename CNT<E>::TInvert EInvert;
113  typedef typename CNT<E>::TNormalize ENormalize;
114 
115  typedef typename CNT<E>::Scalar EScalar;
116  typedef typename CNT<E>::ULessScalar EULessScalar;
117  typedef typename CNT<E>::Number ENumber;
118  typedef typename CNT<E>::StdNumber EStdNumber;
119  typedef typename CNT<E>::Precision EPrecision;
120  typedef typename CNT<E>::ScalarNormSq EScalarNormSq;
121 
122 public:
124  enum {
125  NRows = M,
126  NCols = N,
127  MinDim = N < M ? N : M,
128  MaxDim = N > M ? N : M,
129  RowSpacing = RS,
130  ColSpacing = CS,
131  NPackedElements = M * N,
132  NActualElements = (N-1)*CS + (M-1)*RS + 1,
133  NActualScalars = CNT<E>::NActualScalars * NActualElements,
134  ImagOffset = NTraits<ENumber>::ImagOffset,
135  RealStrideFactor = 1, // composite types don't change size when
136  // cast from complex to real or imaginary
137  ArgDepth = ((int)CNT<E>::ArgDepth < (int)MAX_RESOLVED_DEPTH
138  ? CNT<E>::ArgDepth + 1
139  : MAX_RESOLVED_DEPTH),
140  IsScalar = 0,
141  IsULessScalar = 0,
142  IsNumber = 0,
143  IsStdNumber = 0,
144  IsPrecision = 0,
145  SignInterpretation = CNT<E>::SignInterpretation
146  };
147 
148  typedef Mat<M,N,E,CS,RS> T;
149  typedef Mat<M,N,ENeg,CS,RS> TNeg;
150  typedef Mat<M,N,EWithoutNegator,CS,RS> TWithoutNegator;
151 
152  typedef Mat<M,N,EReal,CS*CNT<E>::RealStrideFactor,RS*CNT<E>::RealStrideFactor>
153  TReal;
154  typedef Mat<M,N,EImag,CS*CNT<E>::RealStrideFactor,RS*CNT<E>::RealStrideFactor>
155  TImag;
156  typedef Mat<M,N,EComplex,CS,RS> TComplex;
157  typedef Mat<N,M,EHerm,RS,CS> THerm;
158  typedef Mat<N,M,E,RS,CS> TPosTrans;
159  typedef E TElement;
160  typedef Row<N,E,CS> TRow;
161  typedef Vec<M,E,RS> TCol;
162  typedef Vec<MinDim,E,RS+CS> TDiag;
163 
164  // These are the results of calculations, so are returned in new, packed
165  // memory. Be sure to refer to element types here which are also packed.
166  typedef Mat<M,N,ESqrt,M,1> TSqrt; // Note strides are packed
167  typedef Mat<M,N,EAbs,M,1> TAbs; // Note strides are packed
168  typedef Mat<M,N,EStandard,M,1> TStandard;
169  typedef Mat<N,M,EInvert,N,1> TInvert; // like THerm but packed
170  typedef Mat<M,N,ENormalize,M,1> TNormalize;
171 
172  typedef SymMat<N,ESqHermT> TSqHermT; // ~Mat*Mat
173  typedef SymMat<M,ESqTHerm> TSqTHerm; // Mat*~Mat
174 
175  // Here the elements are copied unchanged but the result matrix
176  // is an ordinary packed, column order matrix.
177  typedef Mat<M,N,E,M,1> TPacked;
178  typedef Mat<M-1,N,E,M-1,1> TDropRow;
179  typedef Mat<M,N-1,E,M,1> TDropCol;
180  typedef Mat<M-1,N-1,E,M-1,1> TDropRowCol;
181  typedef Mat<M+1,N,E,M+1,1> TAppendRow;
182  typedef Mat<M,N+1,E,M,1> TAppendCol;
183  typedef Mat<M+1,N+1,E,M+1,1> TAppendRowCol;
184 
185  typedef EScalar Scalar;
186  typedef EULessScalar ULessScalar;
187  typedef ENumber Number;
188  typedef EStdNumber StdNumber;
189  typedef EPrecision Precision;
190  typedef EScalarNormSq ScalarNormSq;
191 
192  typedef THerm TransposeType; // TODO
193 
195  static int size() { return M*N; }
198  static int nrow() { return M; }
201  static int ncol() { return N; }
202 
208  ScalarNormSq scalarNormSqr() const {
209  ScalarNormSq sum(0);
210  for(int j=0;j<N;++j) sum += CNT<TCol>::scalarNormSqr((*this)(j));
211  return sum;
212  }
213 
217  TSqrt sqrt() const {
218  TSqrt msqrt;
219  for(int j=0;j<N;++j) msqrt(j) = (*this)(j).sqrt();
220  return msqrt;
221  }
222 
226  TAbs abs() const {
227  TAbs mabs;
228  for(int j=0;j<N;++j) mabs(j) = (*this)(j).abs();
229  return mabs;
230  }
231 
232  TStandard standardize() const {
233  TStandard mstd;
234  for(int j=0;j<N;++j) mstd(j) = (*this)(j).standardize();
235  return mstd;
236  }
237 
238  // This gives the resulting matrix type when (m(i,j) op P) is applied to each element.
239  // It is an MxN vector with default column and row spacing, and element types which
240  // are the regular composite result of E op P. Typically P is a scalar type but
241  // it doesn't have to be.
242  template <class P> struct EltResult {
243  typedef Mat<M,N, typename CNT<E>::template Result<P>::Mul, M, 1> Mul;
244  typedef Mat<M,N, typename CNT<E>::template Result<P>::Dvd, M, 1> Dvd;
245  typedef Mat<M,N, typename CNT<E>::template Result<P>::Add, M, 1> Add;
246  typedef Mat<M,N, typename CNT<E>::template Result<P>::Sub, M, 1> Sub;
247  };
248 
249  // This is the composite result for m op P where P is some kind of appropriately shaped
250  // non-scalar type.
251  template <class P> struct Result {
252  typedef MulCNTs<M,N,ArgDepth,Mat,ColSpacing,RowSpacing,
253  CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
254  P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOp;
255  typedef typename MulOp::Type Mul;
256 
257  typedef MulCNTsNonConforming<M,N,ArgDepth,Mat,ColSpacing,RowSpacing,
258  CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
259  P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOpNonConforming;
260  typedef typename MulOpNonConforming::Type MulNon;
261 
262  typedef DvdCNTs<M,N,ArgDepth,Mat,ColSpacing,RowSpacing,
263  CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
264  P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> DvdOp;
265  typedef typename DvdOp::Type Dvd;
266 
267  typedef AddCNTs<M,N,ArgDepth,Mat,ColSpacing,RowSpacing,
268  CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
269  P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> AddOp;
270  typedef typename AddOp::Type Add;
271 
272  typedef SubCNTs<M,N,ArgDepth,Mat,ColSpacing,RowSpacing,
273  CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
274  P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> SubOp;
275  typedef typename SubOp::Type Sub;
276  };
277 
278  // Shape-preserving element substitution (always packed)
279  template <class P> struct Substitute {
280  typedef Mat<M,N,P> Type;
281  };
282 
285  Mat(){
286  #ifndef NDEBUG
287  setToNaN();
288  #endif
289  }
290 
291  // It's important not to use the default copy constructor or copy
292  // assignment because the compiler doesn't understand that we may
293  // have noncontiguous storage and will try to copy the whole array.
294 
297  Mat(const Mat& src) {
298  for (int j=0; j<N; ++j)
299  (*this)(j) = src(j);
300  }
304  Mat& operator=(const Mat& src) {
305  for (int j=0; j<N; ++j)
306  (*this)(j) = src(j); // no harm if src and 'this' are the same
307  return *this;
308  }
309 
314  explicit Mat(const SymMat<M, ELT>& src) {
315  updDiag() = src.diag();
316  for (int j = 0; j < M; ++j)
317  for (int i = j+1; i < M; ++i) {
318  (*this)(i, j) = src.getEltLower(i, j);
319  (*this)(j, i) = src.getEltUpper(j, i);
320  }
321  }
322 
327  template <int CSS, int RSS>
328  Mat(const Mat<M,N,E,CSS,RSS>& src) {
329  for (int j=0; j<N; ++j)
330  (*this)(j) = src(j);
331  }
332 
338  template <int CSS, int RSS>
339  Mat(const Mat<M,N,ENeg,CSS,RSS>& src) {
340  for (int j=0; j<N; ++j)
341  (*this)(j) = src(j);
342  }
343 
351  template <class EE, int CSS, int RSS>
352  explicit Mat(const Mat<M,N,EE,CSS,RSS>& mm)
353  { for (int j=0;j<N;++j) (*this)(j) = mm(j);}
354 
358  explicit Mat(const E& e)
359  { for (int j=0;j<N;++j) (*this)(j) = E(0); diag()=e; }
360 
365  explicit Mat(const ENeg& e)
366  { for (int j=0;j<N;++j) (*this)(j) = E(0); diag()=e; }
367 
374  explicit Mat(int i)
375  { new (this) Mat(E(Precision(i))); }
376 
377  // A bevy of constructors from individual exact-match elements IN ROW ORDER.
378  Mat(const E& e0,const E& e1)
379  {assert(M*N==2);d[rIx(0)]=e0;d[rIx(1)]=e1;}
380  Mat(const E& e0,const E& e1,const E& e2)
381  {assert(M*N==3);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;}
382  Mat(const E& e0,const E& e1,const E& e2,const E& e3)
383  {assert(M*N==4);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;}
384  Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4)
385  {assert(M*N==5);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;}
386  Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
387  const E& e5)
388  {assert(M*N==6);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
389  d[rIx(5)]=e5;}
390  Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
391  const E& e5,const E& e6)
392  {assert(M*N==7);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
393  d[rIx(5)]=e5;d[rIx(6)]=e6;}
394  Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
395  const E& e5,const E& e6,const E& e7)
396  {assert(M*N==8);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
397  d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;}
398  Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
399  const E& e5,const E& e6,const E& e7,const E& e8)
400  {assert(M*N==9);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
401  d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;}
402  Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
403  const E& e5,const E& e6,const E& e7,const E& e8,const E& e9)
404  {assert(M*N==10);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
405  d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;}
406  Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
407  const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
408  const E& e10)
409  {assert(M*N==11);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
410  d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;}
411  Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
412  const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
413  const E& e10, const E& e11)
414  {assert(M*N==12);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
415  d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;
416  d[rIx(11)]=e11;}
417  Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
418  const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
419  const E& e10, const E& e11, const E& e12)
420  {assert(M*N==13);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
421  d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;
422  d[rIx(11)]=e11;d[rIx(12)]=e12;}
423  Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
424  const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
425  const E& e10, const E& e11, const E& e12, const E& e13)
426  {assert(M*N==14);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
427  d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;
428  d[rIx(11)]=e11;d[rIx(12)]=e12;d[rIx(13)]=e13;}
429  Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
430  const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
431  const E& e10, const E& e11, const E& e12, const E& e13, const E& e14)
432  {assert(M*N==15);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
433  d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;
434  d[rIx(11)]=e11;d[rIx(12)]=e12;d[rIx(13)]=e13;d[rIx(14)]=e14;}
435  Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
436  const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
437  const E& e10, const E& e11, const E& e12, const E& e13, const E& e14,
438  const E& e15)
439  {assert(M*N==16);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
440  d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;
441  d[rIx(11)]=e11;d[rIx(12)]=e12;d[rIx(13)]=e13;d[rIx(14)]=e14;d[rIx(15)]=e15;}
442 
443  // Construction from 1-6 *exact match* Rows
444  explicit Mat(const TRow& r0)
445  { assert(M==1); (*this)[0]=r0; }
446  Mat(const TRow& r0,const TRow& r1)
447  { assert(M==2);(*this)[0]=r0;(*this)[1]=r1; }
448  Mat(const TRow& r0,const TRow& r1,const TRow& r2)
449  { assert(M==3);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2; }
450  Mat(const TRow& r0,const TRow& r1,const TRow& r2,
451  const TRow& r3)
452  { assert(M==4);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;(*this)[3]=r3; }
453  Mat(const TRow& r0,const TRow& r1,const TRow& r2,
454  const TRow& r3,const TRow& r4)
455  { assert(M==5);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;
456  (*this)[3]=r3;(*this)[4]=r4; }
457  Mat(const TRow& r0,const TRow& r1,const TRow& r2,
458  const TRow& r3,const TRow& r4,const TRow& r5)
459  { assert(M==6);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;
460  (*this)[3]=r3;(*this)[4]=r4;(*this)[5]=r5; }
461 
462  // Construction from 1-6 *compatible* Rows
463  template <class EE, int SS> explicit Mat(const Row<N,EE,SS>& r0)
464  { assert(M==1); (*this)[0]=r0; }
465  template <class EE, int SS> Mat(const Row<N,EE,SS>& r0,const Row<N,EE,SS>& r1)
466  { assert(M==2);(*this)[0]=r0;(*this)[1]=r1; }
467  template <class EE, int SS>
468  Mat(const Row<N,EE,SS>& r0,const Row<N,EE,SS>& r1,const Row<N,EE,SS>& r2)
469  { assert(M==3);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2; }
470  template <class EE, int SS>
471  Mat(const Row<N,EE,SS>& r0,const Row<N,EE,SS>& r1,const Row<N,EE,SS>& r2,
472  const Row<N,EE,SS>& r3)
473  { assert(M==4);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;(*this)[3]=r3; }
474  template <class EE, int SS>
475  Mat(const Row<N,EE,SS>& r0,const Row<N,EE,SS>& r1,const Row<N,EE,SS>& r2,
476  const Row<N,EE,SS>& r3,const Row<N,EE,SS>& r4)
477  { assert(M==5);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;
478  (*this)[3]=r3;(*this)[4]=r4; }
479  template <class EE, int SS>
480  Mat(const Row<N,EE,SS>& r0,const Row<N,EE,SS>& r1,const Row<N,EE,SS>& r2,
481  const Row<N,EE,SS>& r3,const Row<N,EE,SS>& r4,const Row<N,EE,SS>& r5)
482  { assert(M==6);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;
483  (*this)[3]=r3;(*this)[4]=r4;(*this)[5]=r5; }
484 
485 
486  // Construction from 1-6 *exact match* Vecs
487  explicit Mat(const TCol& r0)
488  { assert(N==1); (*this)(0)=r0; }
489  Mat(const TCol& r0,const TCol& r1)
490  { assert(N==2);(*this)(0)=r0;(*this)(1)=r1; }
491  Mat(const TCol& r0,const TCol& r1,const TCol& r2)
492  { assert(N==3);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2; }
493  Mat(const TCol& r0,const TCol& r1,const TCol& r2,
494  const TCol& r3)
495  { assert(N==4);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;(*this)(3)=r3; }
496  Mat(const TCol& r0,const TCol& r1,const TCol& r2,
497  const TCol& r3,const TCol& r4)
498  { assert(N==5);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;
499  (*this)(3)=r3;(*this)(4)=r4; }
500  Mat(const TCol& r0,const TCol& r1,const TCol& r2,
501  const TCol& r3,const TCol& r4,const TCol& r5)
502  { assert(N==6);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;
503  (*this)(3)=r3;(*this)(4)=r4;(*this)(5)=r5; }
504 
505  // Construction from 1-6 *compatible* Vecs
506  template <class EE, int SS> explicit Mat(const Vec<M,EE,SS>& r0)
507  { assert(N==1); (*this)(0)=r0; }
508  template <class EE, int SS> Mat(const Vec<M,EE,SS>& r0,const Vec<M,EE,SS>& r1)
509  { assert(N==2);(*this)(0)=r0;(*this)(1)=r1; }
510  template <class EE, int SS>
511  Mat(const Vec<M,EE,SS>& r0,const Vec<M,EE,SS>& r1,const Vec<M,EE,SS>& r2)
512  { assert(N==3);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2; }
513  template <class EE, int SS>
514  Mat(const Vec<M,EE,SS>& r0,const Vec<M,EE,SS>& r1,const Vec<M,EE,SS>& r2,
515  const Vec<M,EE,SS>& r3)
516  { assert(N==4);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;(*this)(3)=r3; }
517  template <class EE, int SS>
518  Mat(const Vec<M,EE,SS>& r0,const Vec<M,EE,SS>& r1,const Vec<M,EE,SS>& r2,
519  const Vec<M,EE,SS>& r3,const Vec<M,EE,SS>& r4)
520  { assert(N==5);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;
521  (*this)(3)=r3;(*this)(4)=r4; }
522  template <class EE, int SS>
523  Mat(const Vec<M,EE,SS>& r0,const Vec<M,EE,SS>& r1,const Vec<M,EE,SS>& r2,
524  const Vec<M,EE,SS>& r3,const Vec<M,EE,SS>& r4,const Vec<M,EE,SS>& r5)
525  { assert(N==6);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;
526  (*this)(3)=r3;(*this)(4)=r4;(*this)(5)=r5; }
527 
528  // Construction from a pointer to anything assumes we're pointing
529  // at a packed element list of the right length, in row order.
530  template <class EE> explicit Mat(const EE* p)
531  { assert(p); for(int i=0;i<M;++i) (*this)[i]=&p[i*N]; }
532 
533  // Assignment works similarly to copy -- if the lengths match,
534  // go element-by-element. Otherwise, zero and then copy to each
535  // diagonal element.
536  template <class EE, int CSS, int RSS> Mat& operator=(const Mat<M,N,EE,CSS,RSS>& mm) {
537  for (int j=0; j<N; ++j) (*this)(j) = mm(j);
538  return *this;
539  }
540 
541  template <class EE> Mat& operator=(const EE* p) {
542  assert(p); for(int i=0;i<M;++i) (*this)[i]=&p[i*N];
543  return *this;
544  }
545 
546  // Assignment ops
547  template <class EE, int CSS, int RSS> Mat&
548  operator+=(const Mat<M,N,EE,CSS,RSS>& mm) {
549  for (int j=0; j<N; ++j) (*this)(j) += mm(j);
550  return *this;
551  }
552  template <class EE, int CSS, int RSS> Mat&
553  operator+=(const Mat<M,N,negator<EE>,CSS,RSS>& mm) {
554  for (int j=0; j<N; ++j) (*this)(j) -= -(mm(j));
555  return *this;
556  }
557 
558  template <class EE, int CSS, int RSS> Mat&
559  operator-=(const Mat<M,N,EE,CSS,RSS>& mm) {
560  for (int j=0; j<N; ++j) (*this)(j) -= mm(j);
561  return *this;
562  }
563  template <class EE, int CSS, int RSS> Mat&
564  operator-=(const Mat<M,N,negator<EE>,CSS,RSS>& mm) {
565  for (int j=0; j<N; ++j) (*this)(j) += -(mm(j));
566  return *this;
567  }
568 
569  // In place matrix multiply can only be done when the RHS matrix is square of dimension
570  // N (i.e., number of columns), and the elements are also *= compatible.
571  template <class EE, int CSS, int RSS> Mat&
572  operator*=(const Mat<N,N,EE,CSS,RSS>& mm) {
573  const Mat t(*this);
574  for (int j=0; j<N; ++j)
575  for (int i=0; i<M; ++i)
576  (*this)(i,j) = t[i] * mm(j);
577  return *this;
578  }
579 
580  // Conforming binary ops with 'this' on left, producing new packed result.
581  // Cases: m=m+-m, m=m+-sy, m=m*m, m=m*sy, v=m*v
582 
583  // m= this + m
584  template <class E2, int CS2, int RS2>
585  typename Result<Mat<M,N,E2,CS2,RS2> >::Add
586  conformingAdd(const Mat<M,N,E2,CS2,RS2>& r) const {
587  typename Result<Mat<M,N,E2,CS2,RS2> >::Add result;
588  for (int j=0;j<N;++j) result(j) = (*this)(j) + r(j);
589  return result;
590  }
591  // m= this - m
592  template <class E2, int CS2, int RS2>
593  typename Result<Mat<M,N,E2,CS2,RS2> >::Sub
594  conformingSubtract(const Mat<M,N,E2,CS2,RS2>& r) const {
595  typename Result<Mat<M,N,E2,CS2,RS2> >::Sub result;
596  for (int j=0;j<N;++j) result(j) = (*this)(j) - r(j);
597  return result;
598  }
599  // m= m - this
600  template <class E2, int CS2, int RS2>
601  typename Mat<M,N,E2,CS2,RS2>::template Result<Mat>::Sub
602  conformingSubtractFromLeft(const Mat<M,N,E2,CS2,RS2>& l) const {
603  return l.conformingSubtract(*this);
604  }
605 
606  // m= this .* m
607  template <class E2, int CS2, int RS2>
608  typename EltResult<E2>::Mul
609  elementwiseMultiply(const Mat<M,N,E2,CS2,RS2>& r) const {
610  typename EltResult<E2>::Mul result;
611  for (int j=0;j<N;++j)
612  result(j) = (*this)(j).elementwiseMultiply(r(j));
613  return result;
614  }
615 
616  // m= this ./ m
617  template <class E2, int CS2, int RS2>
618  typename EltResult<E2>::Dvd
619  elementwiseDivide(const Mat<M,N,E2,CS2,RS2>& r) const {
620  typename EltResult<E2>::Dvd result;
621  for (int j=0;j<N;++j)
622  result(j) = (*this)(j).elementwiseDivide(r(j));
623  return result;
624  }
625 
626  // We always punt to the SymMat since it knows better what to do.
627  // m = this + sym
628  template <class E2, int RS2>
629  typename Result<SymMat<M,E2,RS2> >::Add
630  conformingAdd(const SymMat<M,E2,RS2>& sy) const {
631  assert(M==N);
632  return sy.conformingAdd(*this);
633  }
634  // m= this - sym
635  template <class E2, int RS2>
636  typename Result<SymMat<M,E2,RS2> >::Sub
637  conformingSubtract(const SymMat<M,E2,RS2>& sy) const {
638  assert(M==N);
639  return sy.conformingSubtractFromLeft(*this);
640  }
641  // m= sym - this
642  template <class E2, int RS2>
643  typename SymMat<M,E2,RS2>::template Result<Mat>::Sub
644  conformingSubtractFromLeft(const SymMat<M,E2,RS2>& sy) const {
645  assert(M==N);
646  return sy.conformingSubtract(*this);
647  }
648 
649  // m= this * m
650  template <int N2, class E2, int CS2, int RS2>
651  typename Result<Mat<N,N2,E2,CS2,RS2> >::Mul
652  conformingMultiply(const Mat<N,N2,E2,CS2,RS2>& m) const {
653  typename Result<Mat<N,N2,E2,CS2,RS2> >::Mul result;
654  for (int j=0;j<N2;++j)
655  for (int i=0;i<M;++i)
656  result(i,j) = (*this)[i].conformingMultiply(m(j)); // i.e., dot()
657  return result;
658  }
659  // m= m * this
660  template <int M2, class E2, int CS2, int RS2>
661  typename Mat<M2,M,E2,CS2,RS2>::template Result<Mat>::Mul
662  conformingMultiplyFromLeft(const Mat<M2,M,E2,CS2,RS2>& m) const {
663  return m.conformingMultiply(*this);
664  }
665 
666  // m= this / m = this * m.invert()
667  template <int M2, class E2, int CS2, int RS2>
668  typename Result<Mat<M2,N,E2,CS2,RS2> >::Dvd
669  conformingDivide(const Mat<M2,N,E2,CS2,RS2>& m) const {
670  return conformingMultiply(m.invert());
671  }
672  // m= m / this = m * this.invert()
673  template <int M2, class E2, int CS2, int RS2>
674  typename Mat<M2,N,E2,CS2,RS2>::template Result<Mat>::Dvd
675  conformingDivideFromLeft(const Mat<M2,N,E2,CS2,RS2>& m) const {
676  return m.conformingMultiply((*this).invert());
677  }
678 
679  const TRow& operator[](int i) const { return row(i); }
680  TRow& operator[](int i) { return row(i); }
681  const TCol& operator()(int j) const { return col(j); }
682  TCol& operator()(int j) { return col(j); }
683 
684  const E& operator()(int i,int j) const { return elt(i,j); }
685  E& operator()(int i,int j) { return elt(i,j); }
686 
687  // This is the scalar Frobenius norm.
688  ScalarNormSq normSqr() const { return scalarNormSqr(); }
689  typename CNT<ScalarNormSq>::TSqrt
690  norm() const { return CNT<ScalarNormSq>::sqrt(scalarNormSqr()); }
691 
692  // There is no conventional meaning for normalize() applied to a matrix. We
693  // choose to define it as follows:
694  // If the elements of this Mat are scalars, the result is what you get by
695  // dividing each element by the Frobenius norm() calculated above. If the elements are
696  // *not* scalars, then the elements are *separately* normalized. That means
697  // you will get a different answer from Mat<2,2,Mat33>::normalize() than you
698  // would from a Mat<6,6>::normalize() containing the same scalars.
699  //
700  // Normalize returns a matrix of the same dimension but in new, packed storage
701  // and with a return type that does not include negator<> even if the original
702  // Mat<> does, because we can eliminate the negation here almost for free.
703  // But we can't standardize (change conjugate to complex) for free, so we'll retain
704  // conjugates if there are any.
705  TNormalize normalize() const {
706  if (CNT<E>::IsScalar) {
707  return castAwayNegatorIfAny() / (SignInterpretation*norm());
708  } else {
709  TNormalize elementwiseNormalized;
710  // punt to the column Vec to deal with the elements
711  for (int j=0; j<N; ++j)
712  elementwiseNormalized(j) = (*this)(j).normalize();
713  return elementwiseNormalized;
714  }
715  }
716 
717  // Default inversion. Assume full rank if square, otherwise return
718  // pseudoinverse. (Mostly TODO)
719  TInvert invert() const;
720 
721  const Mat& operator+() const { return *this; }
722  const TNeg& operator-() const { return negate(); }
723  TNeg& operator-() { return updNegate(); }
724  const THerm& operator~() const { return transpose(); }
725  THerm& operator~() { return updTranspose(); }
726 
727  const TNeg& negate() const { return *reinterpret_cast<const TNeg*>(this); }
728  TNeg& updNegate() { return *reinterpret_cast<TNeg*>(this); }
729 
730  const THerm& transpose() const { return *reinterpret_cast<const THerm*>(this); }
731  THerm& updTranspose() { return *reinterpret_cast<THerm*>(this); }
732 
733  const TPosTrans& positionalTranspose() const
734  { return *reinterpret_cast<const TPosTrans*>(this); }
735  TPosTrans& updPositionalTranspose()
736  { return *reinterpret_cast<TPosTrans*>(this); }
737 
738  // If the underlying scalars are complex or conjugate, we can return a
739  // reference to the real part just by recasting the data to a matrix of
740  // the same dimensions but containing real elements, with the scalar
741  // spacing doubled.
742  const TReal& real() const { return *reinterpret_cast<const TReal*>(this); }
743  TReal& real() { return *reinterpret_cast< TReal*>(this); }
744 
745  // Getting the imaginary part is almost the same as real, but we have
746  // to shift the starting address of the returned object by 1 real-size
747  // ("precision") scalar so that the first element is the imaginary part
748  // of the original first element.
749  // TODO: should blow up or return a reference to a zero matrix if called
750  // on a real object.
751  // Had to contort these routines to get them through VC++ 7.net
752  const TImag& imag() const {
753  const int offs = ImagOffset;
754  const Precision* p = reinterpret_cast<const Precision*>(this);
755  return *reinterpret_cast<const TImag*>(p+offs);
756  }
757  TImag& imag() {
758  const int offs = ImagOffset;
759  Precision* p = reinterpret_cast<Precision*>(this);
760  return *reinterpret_cast<TImag*>(p+offs);
761  }
762 
763  const TWithoutNegator& castAwayNegatorIfAny() const {return *reinterpret_cast<const TWithoutNegator*>(this);}
764  TWithoutNegator& updCastAwayNegatorIfAny() {return *reinterpret_cast<TWithoutNegator*>(this);}
765 
766  const TRow& row(int i) const {
767  SimTK_INDEXCHECK(i,M, "Mat::row[i]");
768  return *reinterpret_cast<const TRow*>(&d[i*RS]);
769  }
770  TRow& row(int i) {
771  SimTK_INDEXCHECK(i,M, "Mat::row[i]");
772  return *reinterpret_cast<TRow*>(&d[i*RS]);
773  }
774 
775  const TCol& col(int j) const {
776  SimTK_INDEXCHECK(j,N, "Mat::col(j)");
777  return *reinterpret_cast<const TCol*>(&d[j*CS]);
778  }
779  TCol& col(int j) {
780  SimTK_INDEXCHECK(j,N, "Mat::col(j)");
781  return *reinterpret_cast<TCol*>(&d[j*CS]);
782  }
783 
784  const E& elt(int i, int j) const {
785  SimTK_INDEXCHECK(i,M, "Mat::elt(i,j)");
786  SimTK_INDEXCHECK(j,N, "Mat::elt(i,j)");
787  return d[i*RS+j*CS];
788  }
789  E& elt(int i, int j) {
790  SimTK_INDEXCHECK(i,M, "Mat::elt(i,j)");
791  SimTK_INDEXCHECK(j,N, "Mat::elt(i,j)");
792  return d[i*RS+j*CS];
793  }
794 
798  const TDiag& diag() const { return *reinterpret_cast<const TDiag*>(d); }
802  TDiag& updDiag() { return *reinterpret_cast<TDiag*>(d); }
805  TDiag& diag() { return *reinterpret_cast<TDiag*>(d); }
806 
807  EStandard trace() const {return diag().sum();}
808 
809  // These are elementwise binary operators, (this op ee) by default but (ee op this) if
810  // 'FromLeft' appears in the name. The result is a packed Mat<M,N> but the element type
811  // may change. These are mostly used to implement global operators.
812  // We call these "scalar" operators but actually the "scalar" can be a composite type.
813 
814  //TODO: consider converting 'e' to Standard Numbers as precalculation and changing
815  // return type appropriately.
816  template <class EE> Mat<M,N, typename CNT<E>::template Result<EE>::Mul>
817  scalarMultiply(const EE& e) const {
818  Mat<M,N, typename CNT<E>::template Result<EE>::Mul> result;
819  for (int j=0; j<N; ++j) result(j) = (*this)(j).scalarMultiply(e);
820  return result;
821  }
822  template <class EE> Mat<M,N, typename CNT<EE>::template Result<E>::Mul>
823  scalarMultiplyFromLeft(const EE& e) const {
824  Mat<M,N, typename CNT<EE>::template Result<E>::Mul> result;
825  for (int j=0; j<N; ++j) result(j) = (*this)(j).scalarMultiplyFromLeft(e);
826  return result;
827  }
828 
829  // TODO: should precalculate and store 1/e, while converting to Standard Numbers. Note
830  // that return type should change appropriately.
831  template <class EE> Mat<M,N, typename CNT<E>::template Result<EE>::Dvd>
832  scalarDivide(const EE& e) const {
833  Mat<M,N, typename CNT<E>::template Result<EE>::Dvd> result;
834  for (int j=0; j<N; ++j) result(j) = (*this)(j).scalarDivide(e);
835  return result;
836  }
837  template <class EE> Mat<M,N, typename CNT<EE>::template Result<E>::Dvd>
838  scalarDivideFromLeft(const EE& e) const {
839  Mat<M,N, typename CNT<EE>::template Result<E>::Dvd> result;
840  for (int j=0; j<N; ++j) result(j) = (*this)(j).scalarDivideFromLeft(e);
841  return result;
842  }
843 
844  // Additive operators for scalars operate only on the diagonal.
845  template <class EE> Mat<M,N, typename CNT<E>::template Result<EE>::Add>
846  scalarAdd(const EE& e) const {
847  Mat<M,N, typename CNT<E>::template Result<EE>::Add> result(*this);
848  result.diag() += e;
849  return result;
850  }
851  // Add is commutative, so no 'FromLeft'.
852 
853  template <class EE> Mat<M,N, typename CNT<E>::template Result<EE>::Sub>
854  scalarSubtract(const EE& e) const {
855  Mat<M,N, typename CNT<E>::template Result<EE>::Sub> result(*this);
856  result.diag() -= e;
857  return result;
858  }
859  // Should probably do something clever with negation here (s - m)
860  template <class EE> Mat<M,N, typename CNT<EE>::template Result<E>::Sub>
861  scalarSubtractFromLeft(const EE& e) const {
862  Mat<M,N, typename CNT<EE>::template Result<E>::Sub> result(-(*this));
863  result.diag() += e; // yes, add
864  return result;
865  }
866 
867  // Generic assignments for any element type not listed explicitly, including scalars.
868  // These are done repeatedly for each element and only work if the operation can
869  // be performed leaving the original element type.
870  template <class EE> Mat& operator =(const EE& e) {return scalarEq(e);}
871  template <class EE> Mat& operator+=(const EE& e) {return scalarPlusEq(e);}
872  template <class EE> Mat& operator-=(const EE& e) {return scalarMinusEq(e);}
873  template <class EE> Mat& operator*=(const EE& e) {return scalarTimesEq(e);}
874  template <class EE> Mat& operator/=(const EE& e) {return scalarDivideEq(e);}
875 
876  // Generalized scalar assignment & computed assignment methods. These will work
877  // for any assignment-compatible element, not just scalars.
878  template <class EE> Mat& scalarEq(const EE& ee)
879  { for(int j=0; j<N; ++j) (*this)(j).scalarEq(EE(0));
880  diag().scalarEq(ee);
881  return *this; }
882 
883  template <class EE> Mat& scalarPlusEq(const EE& ee)
884  { diag().scalarPlusEq(ee); return *this; }
885 
886  template <class EE> Mat& scalarMinusEq(const EE& ee)
887  { diag().scalarMinusEq(ee); return *this; }
888  // m = s - m; negate m, then add s
889  template <class EE> Mat& scalarMinusEqFromLeft(const EE& ee)
890  { scalarTimesEq(E(-1)); diag().scalarAdd(ee); return *this; }
891 
892  template <class EE> Mat& scalarTimesEq(const EE& ee)
893  { for(int j=0; j<N; ++j) (*this)(j).scalarTimesEq(ee); return *this; }
894  template <class EE> Mat& scalarTimesEqFromLeft(const EE& ee)
895  { for(int j=0; j<N; ++j) (*this)(j).scalarTimesEqFromLeft(ee); return *this; }
896 
897  template <class EE> Mat& scalarDivideEq(const EE& ee)
898  { for(int j=0; j<N; ++j) (*this)(j).scalarDivideEq(ee); return *this; }
899  template <class EE> Mat& scalarDivideEqFromLeft(const EE& ee)
900  { for(int j=0; j<N; ++j) (*this)(j).scalarDivideEqFromLeft(ee); return *this; }
901 
902  void setToNaN() {
903  for (int j=0; j<N; ++j)
904  (*this)(j).setToNaN();
905  }
906 
907  void setToZero() {
908  for (int j=0; j<N; ++j)
909  (*this)(j).setToZero();
910  }
911 
912  // Extract a sub-Mat with size known at compile time. These have to be
913  // called with explicit template arguments, e.g. getSubMat<3,4>(i,j).
914 
915  template <int MM, int NN> struct SubMat {
916  typedef Mat<MM,NN,ELT,CS,RS> Type;
917  };
918 
919  template <int MM, int NN>
920  const typename SubMat<MM,NN>::Type& getSubMat(int i, int j) const {
921  assert(0 <= i && i + MM <= M);
922  assert(0 <= j && j + NN <= N);
923  return SubMat<MM,NN>::Type::getAs(&(*this)(i,j));
924  }
925  template <int MM, int NN>
926  typename SubMat<MM,NN>::Type& updSubMat(int i, int j) {
927  assert(0 <= i && i + MM <= M);
928  assert(0 <= j && j + NN <= N);
929  return SubMat<MM,NN>::Type::updAs(&(*this)(i,j));
930  }
931  template <int MM, int NN>
932  void setSubMat(int i, int j, const typename SubMat<MM,NN>::Type& value) {
933  assert(0 <= i && i + MM <= M);
934  assert(0 <= j && j + NN <= N);
935  SubMat<MM,NN>::Type::updAs(&(*this)(i,j)) = value;
936  }
937 
940  TDropRow dropRow(int i) const {
941  assert(0 <= i && i < M);
942  TDropRow out;
943  for (int r=0, nxt=0; r<M-1; ++r, ++nxt) {
944  if (nxt==i) ++nxt; // skip the loser
945  out[r] = (*this)[nxt];
946  }
947  return out;
948  }
949 
952  TDropCol dropCol(int j) const {
953  assert(0 <= j && j < N);
954  TDropCol out;
955  for (int c=0, nxt=0; c<N-1; ++c, ++nxt) {
956  if (nxt==j) ++nxt; // skip the loser
957  out(c) = (*this)(nxt);
958  }
959  return out;
960  }
961 
965  TDropRowCol dropRowCol(int i, int j) const {
966  assert(0 <= i && i < M);
967  assert(0 <= j && j < N);
968  TDropRowCol out;
969  for (int c=0, nxtc=0; c<N-1; ++c, ++nxtc) {
970  if (nxtc==j) ++nxtc;
971  for (int r=0, nxtr=0; r<M-1; ++r, ++nxtr) {
972  if (nxtr==i) ++nxtr;
973  out(r,c) = (*this)(nxtr,nxtc);
974  }
975  }
976  return out;
977  }
978 
982  template <class EE, int SS>
983  TAppendRow appendRow(const Row<N,EE,SS>& row) const {
984  TAppendRow out;
985  out.template updSubMat<M,N>(0,0) = (*this);
986  out[M] = row;
987  return out;
988  }
989 
993  template <class EE, int SS>
994  TAppendCol appendCol(const Vec<M,EE,SS>& col) const {
995  TAppendCol out;
996  out.template updSubMat<M,N>(0,0) = (*this);
997  out(N) = col;
998  return out;
999  }
1000 
1007  template <class ER, int SR, class EC, int SC>
1008  TAppendRowCol appendRowCol(const Row<N+1,ER,SR>& row,
1009  const Vec<M+1,EC,SC>& col) const
1010  {
1011  TAppendRowCol out;
1012  out.template updSubMat<M,N>(0,0) = (*this);
1013  out[M].template updSubRow<N>(0) =
1014  row.template getSubRow<N>(0); // ignore last element
1015  out(N) = col;
1016  return out;
1017  }
1018 
1024  template <class EE, int SS>
1025  TAppendRow insertRow(int i, const Row<N,EE,SS>& row) const {
1026  assert(0 <= i && i <= M);
1027  if (i==M) return appendRow(row);
1028  TAppendRow out;
1029  for (int r=0, nxt=0; r<M; ++r, ++nxt) {
1030  if (nxt==i) out[nxt++] = row;
1031  out[nxt] = (*this)[r];
1032  }
1033  return out;
1034  }
1035 
1041  template <class EE, int SS>
1042  TAppendCol insertCol(int j, const Vec<M,EE,SS>& col) const {
1043  assert(0 <= j && j <= N);
1044  if (j==N) return appendCol(col);
1045  TAppendCol out;
1046  for (int c=0, nxt=0; c<N; ++c, ++nxt) {
1047  if (nxt==j) out(nxt++) = col;
1048  out(nxt) = (*this)(c);
1049  }
1050  return out;
1051  }
1052 
1060  template <class ER, int SR, class EC, int SC>
1061  TAppendRowCol insertRowCol(int i, int j, const Row<N+1,ER,SR>& row,
1062  const Vec<M+1,EC,SC>& col) const {
1063  assert(0 <= i && i <= M);
1064  assert(0 <= j && j <= N);
1065  TAppendRowCol out;
1066  for (int c=0, nxtc=0; c<N; ++c, ++nxtc) {
1067  if (nxtc==j) ++nxtc; // leave room
1068  for (int r=0, nxtr=0; r<M; ++r, ++nxtr) {
1069  if (nxtr==i) ++nxtr;
1070  out(nxtr,nxtc) = (*this)(r,c);
1071  }
1072  }
1073  out[i] = row;
1074  out(j) = col; // overwrites row's j'th element
1075  return out;
1076  }
1077 
1078  // These assume we are given a pointer to d[0] of a Mat<M,N,E,CS,RS> like this one.
1079  static const Mat& getAs(const ELT* p) {return *reinterpret_cast<const Mat*>(p);}
1080  static Mat& updAs(ELT* p) {return *reinterpret_cast<Mat*>(p);}
1081 
1082  // Note packed spacing
1083  static Mat<M,N,ELT,M,1> getNaN() {
1084  Mat<M,N,ELT,M,1> m;
1085  m.setToNaN();
1086  return m;
1087  }
1088 
1090  bool isNaN() const {
1091  for (int j=0; j<N; ++j)
1092  if (this->col(j).isNaN())
1093  return true;
1094  return false;
1095  }
1096 
1099  bool isInf() const {
1100  bool seenInf = false;
1101  for (int j=0; j<N; ++j) {
1102  if (!this->col(j).isFinite()) {
1103  if (!this->col(j).isInf())
1104  return false; // something bad was found
1105  seenInf = true;
1106  }
1107  }
1108  return seenInf;
1109  }
1110 
1112  bool isFinite() const {
1113  for (int j=0; j<N; ++j)
1114  if (!this->col(j).isFinite())
1115  return false;
1116  return true;
1117  }
1118 
1121  static double getDefaultTolerance() {return MinDim*CNT<ELT>::getDefaultTolerance();}
1122 
1125  template <class E2, int CS2, int RS2>
1126  bool isNumericallyEqual(const Mat<M,N,E2,CS2,RS2>& m, double tol) const {
1127  for (int j=0; j < N; ++j)
1128  if (!(*this)(j).isNumericallyEqual(m(j), tol))
1129  return false;
1130  return true;
1131  }
1132 
1136  template <class E2, int CS2, int RS2>
1137  bool isNumericallyEqual(const Mat<M,N,E2,CS2,RS2>& m) const {
1138  const double tol = std::max(getDefaultTolerance(),m.getDefaultTolerance());
1139  return isNumericallyEqual(m, tol);
1140  }
1141 
1147  bool isNumericallyEqual
1148  (const ELT& e,
1149  double tol = getDefaultTolerance()) const
1150  {
1151  for (int i=0; i<M; ++i)
1152  for (int j=0; j<N; ++j) {
1153  if (i==j) {
1154  if (!CNT<ELT>::isNumericallyEqual((*this)(i,i), e, tol))
1155  return false;
1156  } else {
1157  // off-diagonals must be zero
1158  if (!CNT<ELT>::isNumericallyEqual((*this)(i,j), ELT(0), tol))
1159  return false;
1160  }
1161  }
1162  return true;
1163  }
1164 
1172  bool isNumericallySymmetric(double tol = getDefaultTolerance()) const {
1173  if (M != N) return false; // handled at compile time
1174  for (int j=0; j<M; ++j)
1175  for (int i=j; i<M; ++i)
1176  if (!CNT<ELT>::isNumericallyEqual(elt(j,i), CNT<ELT>::transpose(elt(i,j)), tol))
1177  return false;
1178  return true;
1179  }
1180 
1187  bool isExactlySymmetric() const {
1188  if (M != N) return false; // handled at compile time
1189  for (int j=0; j<M; ++j)
1190  for (int i=j; i<M; ++i)
1191  if (elt(j,i) != CNT<ELT>::transpose(elt(i,j)))
1192  return false;
1193  return true;
1194  }
1195 
1197  TRow colSum() const {
1198  TRow temp;
1199  for (int j = 0; j < N; ++j)
1200  temp[j] = col(j).sum();
1201  return temp;
1202  }
1205  TRow sum() const {return colSum();}
1206 
1208  TCol rowSum() const {
1209  TCol temp;
1210  for (int i = 0; i < M; ++i)
1211  temp[i] = row(i).sum();
1212  return temp;
1213  }
1214 
1215  // Functions to be used for Scripting in MATLAB and languages that do not support operator overloading
1217  std::string toString() const {
1218  std::stringstream stream;
1219  stream << (*this) ;
1220  return stream.str();
1221  }
1223  const ELT& get(int i,int j) const { return elt(i,j); }
1225  void set(int i,int j, const ELT& value) { elt(i,j)=value; }
1226 
1227 private:
1228  E d[NActualElements];
1229 
1230  // This permits running through d as though it were stored
1231  // in row order with packed elements. Pass in the k'th value
1232  // of the row ordering and get back the index into d where
1233  // that element is stored.
1234  int rIx(int k) const {
1235  const int row = k / N;
1236  const int col = k % N; // that's modulus, not cross product!
1237  return row*RS + col*CS;
1238  }
1239 };
1240 
1242 // Global operators involving two matrices. //
1243 // m+m, m-m, m*m, m==m, m!=m //
1245 
1246 template <int M, int N, class EL, int CSL, int RSL, class ER, int CSR, int RSR> inline
1247 typename Mat<M,N,EL,CSL,RSL>::template Result<Mat<M,N,ER,CSR,RSR> >::Add
1248 operator+(const Mat<M,N,EL,CSL,RSL>& l, const Mat<M,N,ER,CSR,RSR>& r) {
1249  return Mat<M,N,EL,CSL,RSL>::template Result<Mat<M,N,ER,CSR,RSR> >
1250  ::AddOp::perform(l,r);
1251 }
1252 
1253 template <int M, int N, class EL, int CSL, int RSL, class ER, int CSR, int RSR> inline
1254 typename Mat<M,N,EL,CSL,RSL>::template Result<Mat<M,N,ER,CSR,RSR> >::Sub
1255 operator-(const Mat<M,N,EL,CSL,RSL>& l, const Mat<M,N,ER,CSR,RSR>& r) {
1256  return Mat<M,N,EL,CSL,RSL>::template Result<Mat<M,N,ER,CSR,RSR> >
1257  ::SubOp::perform(l,r);
1258 }
1259 
1260 // Matrix multiply of an MxN by NxP to produce a packed MxP.
1261 template <int M, int N, class EL, int CSL, int RSL, int P, class ER, int CSR, int RSR> inline
1262 typename Mat<M,N,EL,CSL,RSL>::template Result<Mat<N,P,ER,CSR,RSR> >::Mul
1263 operator*(const Mat<M,N,EL,CSL,RSL>& l, const Mat<N,P,ER,CSR,RSR>& r) {
1264  return Mat<M,N,EL,CSL,RSL>::template Result<Mat<N,P,ER,CSR,RSR> >
1265  ::MulOp::perform(l,r);
1266 }
1267 
1268 // Non-conforming matrix multiply of an MxN by MMxNN; will be a scalar multiply if one
1269 // has scalar elements and the other has composite elements.
1270 template <int M, int N, class EL, int CSL, int RSL, int MM, int NN, class ER, int CSR, int RSR> inline
1271 typename Mat<M,N,EL,CSL,RSL>::template Result<Mat<MM,NN,ER,CSR,RSR> >::MulNon
1272 operator*(const Mat<M,N,EL,CSL,RSL>& l, const Mat<MM,NN,ER,CSR,RSR>& r) {
1273  return Mat<M,N,EL,CSL,RSL>::template Result<Mat<MM,NN,ER,CSR,RSR> >
1274  ::MulOpNonConforming::perform(l,r);
1275 }
1276 
1277 template <int M, int N, class EL, int CSL, int RSL, class ER, int CSR, int RSR> inline
1278 bool operator==(const Mat<M,N,EL,CSL,RSL>& l, const Mat<M,N,ER,CSR,RSR>& r) {
1279  for (int j=0; j<N; ++j)
1280  if (l(j) != r(j)) return false;
1281  return true;
1282 }
1283 template <int M, int N, class EL, int CSL, int RSL, class ER, int CSR, int RSR> inline
1284 bool operator!=(const Mat<M,N,EL,CSL,RSL>& l, const Mat<M,N,ER,CSR,RSR>& r) {
1285  return !(l==r);
1286 }
1287 
1288 
1290 // Global operators involving a matrix and a scalar. //
1292 
1293 // SCALAR MULTIPLY
1294 
1295 // m = m*real, real*m
1296 template <int M, int N, class E, int CS, int RS> inline
1297 typename Mat<M,N,E,CS,RS>::template Result<float>::Mul
1298 operator*(const Mat<M,N,E,CS,RS>& l, const float& r)
1299  { return Mat<M,N,E,CS,RS>::template Result<float>::MulOp::perform(l,r); }
1300 template <int M, int N, class E, int CS, int RS> inline
1301 typename Mat<M,N,E,CS,RS>::template Result<float>::Mul
1302 operator*(const float& l, const Mat<M,N,E,CS,RS>& r) {return r*l;}
1303 
1304 template <int M, int N, class E, int CS, int RS> inline
1305 typename Mat<M,N,E,CS,RS>::template Result<double>::Mul
1306 operator*(const Mat<M,N,E,CS,RS>& l, const double& r)
1307  { return Mat<M,N,E,CS,RS>::template Result<double>::MulOp::perform(l,r); }
1308 template <int M, int N, class E, int CS, int RS> inline
1309 typename Mat<M,N,E,CS,RS>::template Result<double>::Mul
1310 operator*(const double& l, const Mat<M,N,E,CS,RS>& r) {return r*l;}
1311 
1312 template <int M, int N, class E, int CS, int RS> inline
1313 typename Mat<M,N,E,CS,RS>::template Result<long double>::Mul
1314 operator*(const Mat<M,N,E,CS,RS>& l, const long double& r)
1315  { return Mat<M,N,E,CS,RS>::template Result<long double>::MulOp::perform(l,r); }
1316 template <int M, int N, class E, int CS, int RS> inline
1317 typename Mat<M,N,E,CS,RS>::template Result<long double>::Mul
1318 operator*(const long double& l, const Mat<M,N,E,CS,RS>& r) {return r*l;}
1319 
1320 // m = m*int, int*m -- just convert int to m's precision float
1321 template <int M, int N, class E, int CS, int RS> inline
1322 typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Mul
1323 operator*(const Mat<M,N,E,CS,RS>& l, int r) {return l * (typename CNT<E>::Precision)r;}
1324 template <int M, int N, class E, int CS, int RS> inline
1325 typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Mul
1326 operator*(int l, const Mat<M,N,E,CS,RS>& r) {return r * (typename CNT<E>::Precision)l;}
1327 
1328 // Complex, conjugate, and negator are all easy to templatize.
1329 
1330 // m = m*complex, complex*m
1331 template <int M, int N, class E, int CS, int RS, class R> inline
1332 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Mul
1333 operator*(const Mat<M,N,E,CS,RS>& l, const std::complex<R>& r)
1334  { return Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::MulOp::perform(l,r); }
1335 template <int M, int N, class E, int CS, int RS, class R> inline
1336 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Mul
1337 operator*(const std::complex<R>& l, const Mat<M,N,E,CS,RS>& r) {return r*l;}
1338 
1339 // m = m*conjugate, conjugate*m (convert conjugate->complex)
1340 template <int M, int N, class E, int CS, int RS, class R> inline
1341 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Mul
1342 operator*(const Mat<M,N,E,CS,RS>& l, const conjugate<R>& r) {return l*(std::complex<R>)r;}
1343 template <int M, int N, class E, int CS, int RS, class R> inline
1344 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Mul
1345 operator*(const conjugate<R>& l, const Mat<M,N,E,CS,RS>& r) {return r*(std::complex<R>)l;}
1346 
1347 // m = m*negator, negator*m: convert negator to standard number
1348 template <int M, int N, class E, int CS, int RS, class R> inline
1349 typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Mul
1350 operator*(const Mat<M,N,E,CS,RS>& l, const negator<R>& r) {return l * (typename negator<R>::StdNumber)(R)r;}
1351 template <int M, int N, class E, int CS, int RS, class R> inline
1352 typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Mul
1353 operator*(const negator<R>& l, const Mat<M,N,E,CS,RS>& r) {return r * (typename negator<R>::StdNumber)(R)l;}
1354 
1355 
1356 // SCALAR DIVIDE. This is a scalar operation when the scalar is on the right,
1357 // but when it is on the left it means scalar * pseudoInverse(mat),
1358 // which is a matrix whose type is like the matrix's Hermitian transpose.
1359 // TODO: for now it is just going to call mat.invert() which will fail on
1360 // singular matrices.
1361 
1362 // m = m/real, real/m
1363 template <int M, int N, class E, int CS, int RS> inline
1364 typename Mat<M,N,E,CS,RS>::template Result<float>::Dvd
1365 operator/(const Mat<M,N,E,CS,RS>& l, const float& r)
1366 { return Mat<M,N,E,CS,RS>::template Result<float>::DvdOp::perform(l,r); }
1367 
1368 template <int M, int N, class E, int CS, int RS> inline
1369 typename CNT<float>::template Result<Mat<M,N,E,CS,RS> >::Dvd
1370 operator/(const float& l, const Mat<M,N,E,CS,RS>& r)
1371 { return l * r.invert(); }
1372 
1373 template <int M, int N, class E, int CS, int RS> inline
1374 typename Mat<M,N,E,CS,RS>::template Result<double>::Dvd
1375 operator/(const Mat<M,N,E,CS,RS>& l, const double& r)
1376 { return Mat<M,N,E,CS,RS>::template Result<double>::DvdOp::perform(l,r); }
1377 
1378 template <int M, int N, class E, int CS, int RS> inline
1379 typename CNT<double>::template Result<Mat<M,N,E,CS,RS> >::Dvd
1380 operator/(const double& l, const Mat<M,N,E,CS,RS>& r)
1381 { return l * r.invert(); }
1382 
1383 template <int M, int N, class E, int CS, int RS> inline
1384 typename Mat<M,N,E,CS,RS>::template Result<long double>::Dvd
1385 operator/(const Mat<M,N,E,CS,RS>& l, const long double& r)
1386 { return Mat<M,N,E,CS,RS>::template Result<long double>::DvdOp::perform(l,r); }
1387 
1388 template <int M, int N, class E, int CS, int RS> inline
1389 typename CNT<long double>::template Result<Mat<M,N,E,CS,RS> >::Dvd
1390 operator/(const long double& l, const Mat<M,N,E,CS,RS>& r)
1391 { return l * r.invert(); }
1392 
1393 // m = m/int, int/m -- just convert int to m's precision float
1394 template <int M, int N, class E, int CS, int RS> inline
1395 typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Dvd
1396 operator/(const Mat<M,N,E,CS,RS>& l, int r)
1397 { return l / (typename CNT<E>::Precision)r; }
1398 
1399 template <int M, int N, class E, int CS, int RS> inline
1400 typename CNT<typename CNT<E>::Precision>::template Result<Mat<M,N,E,CS,RS> >::Dvd
1401 operator/(int l, const Mat<M,N,E,CS,RS>& r)
1402 { return (typename CNT<E>::Precision)l / r; }
1403 
1404 
1405 // Complex, conjugate, and negator are all easy to templatize.
1406 
1407 // m = m/complex, complex/m
1408 template <int M, int N, class E, int CS, int RS, class R> inline
1409 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Dvd
1410 operator/(const Mat<M,N,E,CS,RS>& l, const std::complex<R>& r)
1411  { return Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::DvdOp::perform(l,r); }
1412 template <int M, int N, class E, int CS, int RS, class R> inline
1413 typename CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::Dvd
1414 operator/(const std::complex<R>& l, const Mat<M,N,E,CS,RS>& r)
1415  { return CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::DvdOp::perform(l,r); }
1416 
1417 // m = m/conjugate, conjugate/m (convert conjugate->complex)
1418 template <int M, int N, class E, int CS, int RS, class R> inline
1419 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Dvd
1420 operator/(const Mat<M,N,E,CS,RS>& l, const conjugate<R>& r) {return l/(std::complex<R>)r;}
1421 template <int M, int N, class E, int CS, int RS, class R> inline
1422 typename CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::Dvd
1423 operator/(const conjugate<R>& l, const Mat<M,N,E,CS,RS>& r) {return (std::complex<R>)l/r;}
1424 
1425 // m = m/negator, negator/m: convert negator to a standard number
1426 template <int M, int N, class E, int CS, int RS, class R> inline
1427 typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Dvd
1428 operator/(const Mat<M,N,E,CS,RS>& l, const negator<R>& r) {return l/(typename negator<R>::StdNumber)(R)r;}
1429 template <int M, int N, class E, int CS, int RS, class R> inline
1430 typename CNT<R>::template Result<Mat<M,N,E,CS,RS> >::Dvd
1431 operator/(const negator<R>& l, const Mat<M,N,E,CS,RS>& r) {return (typename negator<R>::StdNumber)(R)l/r;}
1432 
1433 
1434 // Add and subtract are odd as scalar ops. They behave as though the
1435 // scalar stands for a conforming matrix whose diagonal elements are that,
1436 // scalar and then a normal matrix add or subtract is done.
1437 
1438 // SCALAR ADD
1439 
1440 // m = m+real, real+m
1441 template <int M, int N, class E, int CS, int RS> inline
1442 typename Mat<M,N,E,CS,RS>::template Result<float>::Add
1443 operator+(const Mat<M,N,E,CS,RS>& l, const float& r)
1444  { return Mat<M,N,E,CS,RS>::template Result<float>::AddOp::perform(l,r); }
1445 template <int M, int N, class E, int CS, int RS> inline
1446 typename Mat<M,N,E,CS,RS>::template Result<float>::Add
1447 operator+(const float& l, const Mat<M,N,E,CS,RS>& r) {return r+l;}
1448 
1449 template <int M, int N, class E, int CS, int RS> inline
1450 typename Mat<M,N,E,CS,RS>::template Result<double>::Add
1451 operator+(const Mat<M,N,E,CS,RS>& l, const double& r)
1452  { return Mat<M,N,E,CS,RS>::template Result<double>::AddOp::perform(l,r); }
1453 template <int M, int N, class E, int CS, int RS> inline
1454 typename Mat<M,N,E,CS,RS>::template Result<double>::Add
1455 operator+(const double& l, const Mat<M,N,E,CS,RS>& r) {return r+l;}
1456 
1457 template <int M, int N, class E, int CS, int RS> inline
1458 typename Mat<M,N,E,CS,RS>::template Result<long double>::Add
1459 operator+(const Mat<M,N,E,CS,RS>& l, const long double& r)
1460  { return Mat<M,N,E,CS,RS>::template Result<long double>::AddOp::perform(l,r); }
1461 template <int M, int N, class E, int CS, int RS> inline
1462 typename Mat<M,N,E,CS,RS>::template Result<long double>::Add
1463 operator+(const long double& l, const Mat<M,N,E,CS,RS>& r) {return r+l;}
1464 
1465 // m = m+int, int+m -- just convert int to m's precision float
1466 template <int M, int N, class E, int CS, int RS> inline
1467 typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Add
1468 operator+(const Mat<M,N,E,CS,RS>& l, int r) {return l + (typename CNT<E>::Precision)r;}
1469 template <int M, int N, class E, int CS, int RS> inline
1470 typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Add
1471 operator+(int l, const Mat<M,N,E,CS,RS>& r) {return r + (typename CNT<E>::Precision)l;}
1472 
1473 // Complex, conjugate, and negator are all easy to templatize.
1474 
1475 // m = m+complex, complex+m
1476 template <int M, int N, class E, int CS, int RS, class R> inline
1477 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Add
1478 operator+(const Mat<M,N,E,CS,RS>& l, const std::complex<R>& r)
1479  { return Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::AddOp::perform(l,r); }
1480 template <int M, int N, class E, int CS, int RS, class R> inline
1481 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Add
1482 operator+(const std::complex<R>& l, const Mat<M,N,E,CS,RS>& r) {return r+l;}
1483 
1484 // m = m+conjugate, conjugate+m (convert conjugate->complex)
1485 template <int M, int N, class E, int CS, int RS, class R> inline
1486 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Add
1487 operator+(const Mat<M,N,E,CS,RS>& l, const conjugate<R>& r) {return l+(std::complex<R>)r;}
1488 template <int M, int N, class E, int CS, int RS, class R> inline
1489 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Add
1490 operator+(const conjugate<R>& l, const Mat<M,N,E,CS,RS>& r) {return r+(std::complex<R>)l;}
1491 
1492 // m = m+negator, negator+m: convert negator to standard number
1493 template <int M, int N, class E, int CS, int RS, class R> inline
1494 typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Add
1495 operator+(const Mat<M,N,E,CS,RS>& l, const negator<R>& r) {return l + (typename negator<R>::StdNumber)(R)r;}
1496 template <int M, int N, class E, int CS, int RS, class R> inline
1497 typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Add
1498 operator+(const negator<R>& l, const Mat<M,N,E,CS,RS>& r) {return r + (typename negator<R>::StdNumber)(R)l;}
1499 
1500 // SCALAR SUBTRACT -- careful, not commutative.
1501 
1502 // m = m-real, real-m
1503 template <int M, int N, class E, int CS, int RS> inline
1504 typename Mat<M,N,E,CS,RS>::template Result<float>::Sub
1505 operator-(const Mat<M,N,E,CS,RS>& l, const float& r)
1506  { return Mat<M,N,E,CS,RS>::template Result<float>::SubOp::perform(l,r); }
1507 template <int M, int N, class E, int CS, int RS> inline
1508 typename CNT<float>::template Result<Mat<M,N,E,CS,RS> >::Sub
1509 operator-(const float& l, const Mat<M,N,E,CS,RS>& r)
1510  { return CNT<float>::template Result<Mat<M,N,E,CS,RS> >::SubOp::perform(l,r); }
1511 
1512 template <int M, int N, class E, int CS, int RS> inline
1513 typename Mat<M,N,E,CS,RS>::template Result<double>::Sub
1514 operator-(const Mat<M,N,E,CS,RS>& l, const double& r)
1515  { return Mat<M,N,E,CS,RS>::template Result<double>::SubOp::perform(l,r); }
1516 template <int M, int N, class E, int CS, int RS> inline
1517 typename CNT<double>::template Result<Mat<M,N,E,CS,RS> >::Sub
1518 operator-(const double& l, const Mat<M,N,E,CS,RS>& r)
1519  { return CNT<double>::template Result<Mat<M,N,E,CS,RS> >::SubOp::perform(l,r); }
1520 
1521 template <int M, int N, class E, int CS, int RS> inline
1522 typename Mat<M,N,E,CS,RS>::template Result<long double>::Sub
1523 operator-(const Mat<M,N,E,CS,RS>& l, const long double& r)
1524  { return Mat<M,N,E,CS,RS>::template Result<long double>::SubOp::perform(l,r); }
1525 template <int M, int N, class E, int CS, int RS> inline
1526 typename CNT<long double>::template Result<Mat<M,N,E,CS,RS> >::Sub
1527 operator-(const long double& l, const Mat<M,N,E,CS,RS>& r)
1528  { return CNT<long double>::template Result<Mat<M,N,E,CS,RS> >::SubOp::perform(l,r); }
1529 
1530 // m = m-int, int-m // just convert int to m's precision float
1531 template <int M, int N, class E, int CS, int RS> inline
1532 typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Sub
1533 operator-(const Mat<M,N,E,CS,RS>& l, int r) {return l - (typename CNT<E>::Precision)r;}
1534 template <int M, int N, class E, int CS, int RS> inline
1535 typename CNT<typename CNT<E>::Precision>::template Result<Mat<M,N,E,CS,RS> >::Sub
1536 operator-(int l, const Mat<M,N,E,CS,RS>& r) {return (typename CNT<E>::Precision)l - r;}
1537 
1538 
1539 // Complex, conjugate, and negator are all easy to templatize.
1540 
1541 // m = m-complex, complex-m
1542 template <int M, int N, class E, int CS, int RS, class R> inline
1543 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Sub
1544 operator-(const Mat<M,N,E,CS,RS>& l, const std::complex<R>& r)
1545  { return Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::SubOp::perform(l,r); }
1546 template <int M, int N, class E, int CS, int RS, class R> inline
1547 typename CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::Sub
1548 operator-(const std::complex<R>& l, const Mat<M,N,E,CS,RS>& r)
1549  { return CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::SubOp::perform(l,r); }
1550 
1551 // m = m-conjugate, conjugate-m (convert conjugate->complex)
1552 template <int M, int N, class E, int CS, int RS, class R> inline
1553 typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Sub
1554 operator-(const Mat<M,N,E,CS,RS>& l, const conjugate<R>& r) {return l-(std::complex<R>)r;}
1555 template <int M, int N, class E, int CS, int RS, class R> inline
1556 typename CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::Sub
1557 operator-(const conjugate<R>& l, const Mat<M,N,E,CS,RS>& r) {return (std::complex<R>)l-r;}
1558 
1559 // m = m-negator, negator-m: convert negator to standard number
1560 template <int M, int N, class E, int CS, int RS, class R> inline
1561 typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Sub
1562 operator-(const Mat<M,N,E,CS,RS>& l, const negator<R>& r) {return l-(typename negator<R>::StdNumber)(R)r;}
1563 template <int M, int N, class E, int CS, int RS, class R> inline
1564 typename CNT<R>::template Result<Mat<M,N,E,CS,RS> >::Sub
1565 operator-(const negator<R>& l, const Mat<M,N,E,CS,RS>& r) {return (typename negator<R>::StdNumber)(R)l-r;}
1566 
1567 
1568 // Mat I/O
1569 template <int M, int N, class E, int CS, int RS, class CHAR, class TRAITS> inline
1570 std::basic_ostream<CHAR,TRAITS>&
1571 operator<<(std::basic_ostream<CHAR,TRAITS>& o, const Mat<M,N,E,CS,RS>& m) {
1572  for (int i=0;i<M;++i) {
1573  o << std::endl << "[";
1574  for (int j=0;j<N;++j)
1575  o << (j>0?",":"") << m(i,j);
1576  o << "]";
1577  }
1578  if (M) o << std::endl;
1579  return o;
1580 }
1581 
1582 template <int M, int N, class E, int CS, int RS, class CHAR, class TRAITS> inline
1583 std::basic_istream<CHAR,TRAITS>&
1584 operator>>(std::basic_istream<CHAR,TRAITS>& is, Mat<M,N,E,CS,RS>& m) {
1585  // TODO: not sure how to do Vec input yet
1586  assert(false);
1587  return is;
1588 }
1589 
1590 } //namespace SimTK
1591 
1592 
1593 #endif //SimTK_SIMMATRIX_SMALLMATRIX_MAT_H_