Simbody  3.4
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
SymMat.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMMATRIX_SMALLMATRIX_SYMMAT_H_
2 #define SimTK_SIMMATRIX_SMALLMATRIX_SYMMAT_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-13 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 
32 #include "SimTKcommon/internal/common.h"
33 
34 namespace SimTK {
35 
87 template <int M, class ELT, int RS> class SymMat {
88  typedef ELT E;
89  typedef typename CNT<E>::TNeg ENeg;
90  typedef typename CNT<E>::TWithoutNegator EWithoutNegator;
91  typedef typename CNT<E>::TReal EReal;
92  typedef typename CNT<E>::TImag EImag;
93  typedef typename CNT<E>::TComplex EComplex;
94  typedef typename CNT<E>::THerm EHerm;
95  typedef typename CNT<E>::TPosTrans EPosTrans;
96  typedef typename CNT<E>::TSqHermT ESqHermT;
97  typedef typename CNT<E>::TSqTHerm ESqTHerm;
98 
99  typedef typename CNT<E>::TSqrt ESqrt;
100  typedef typename CNT<E>::TAbs EAbs;
101  typedef typename CNT<E>::TStandard EStandard;
102  typedef typename CNT<E>::TInvert EInvert;
103  typedef typename CNT<E>::TNormalize ENormalize;
104 
105  typedef typename CNT<E>::Scalar EScalar;
106  typedef typename CNT<E>::ULessScalar EULessScalar;
107  typedef typename CNT<E>::Number ENumber;
108  typedef typename CNT<E>::StdNumber EStdNumber;
109  typedef typename CNT<E>::Precision EPrecision;
110  typedef typename CNT<E>::ScalarNormSq EScalarNormSq;
111 
112 public:
113 
114  enum {
115  NRows = M,
116  NCols = M,
117  NDiagElements = M,
118  NLowerElements = (M*(M-1))/2,
119  NPackedElements = NDiagElements+NLowerElements,
120  NActualElements = RS * NPackedElements,
121  NActualScalars = CNT<E>::NActualScalars * NActualElements,
122  RowSpacing = RS,
123  ColSpacing = NActualElements,
124  ImagOffset = NTraits<ENumber>::ImagOffset,
125  RealStrideFactor = 1, // composite types don't change size when
126  // cast from complex to real or imaginary
127  ArgDepth = ((int)CNT<E>::ArgDepth < (int)MAX_RESOLVED_DEPTH
128  ? CNT<E>::ArgDepth + 1
129  : MAX_RESOLVED_DEPTH),
130  IsScalar = 0,
131  IsULessScalar = 0,
132  IsNumber = 0,
133  IsStdNumber = 0,
134  IsPrecision = 0,
135  SignInterpretation = CNT<E>::SignInterpretation
136  };
137 
138  typedef SymMat<M,E,RS> T;
139  typedef SymMat<M,ENeg,RS> TNeg;
140  typedef SymMat<M,EWithoutNegator,RS> TWithoutNegator;
141 
142  typedef SymMat<M,EReal,RS*CNT<E>::RealStrideFactor>
143  TReal;
144  typedef SymMat<M,EImag,RS*CNT<E>::RealStrideFactor>
145  TImag;
146  typedef SymMat<M,EComplex,RS> TComplex;
147  typedef T THerm; // These two are opposite of what you might expect
148  typedef SymMat<M,EHerm,RS> TPosTrans;
149  typedef E TElement;
150  typedef Vec<M,E,RS> TDiag; // storage type for the diagonal elements
151  typedef Vec<(M*(M-1))/2,E,RS> TLower; // storage type for the below-diag elements
152  typedef Vec<(M*(M-1))/2,EHerm,RS> TUpper; // cast TLower to this for upper elements
153  typedef Vec<(M*(M+1))/2,E,RS> TAsVec; // the whole SymMat as a single Vec
154 
155  // These are the results of calculations, so are returned in new, packed
156  // memory. Be sure to refer to element types here which are also packed.
157  typedef Row<M,E,1> TRow; // packed since we have to copy
158  typedef Vec<M,E,1> TCol;
159  typedef SymMat<M,ESqrt,1> TSqrt;
160  typedef SymMat<M,EAbs,1> TAbs;
161  typedef SymMat<M,EStandard,1> TStandard;
162  typedef SymMat<M,EInvert,1> TInvert;
163  typedef SymMat<M,ENormalize,1> TNormalize;
164  typedef SymMat<M,ESqHermT,1> TSqHermT; // ~Mat*Mat
165  typedef SymMat<M,ESqTHerm,1> TSqTHerm; // Mat*~Mat
166  typedef SymMat<M,E,1> TPacked; // no extra row spacing for new data
167 
168  typedef EScalar Scalar;
169  typedef EULessScalar ULessScalar;
170  typedef ENumber Number;
171  typedef EStdNumber StdNumber;
172  typedef EPrecision Precision;
173  typedef EScalarNormSq ScalarNormSq;
174 
175  static int size() { return (M*(M+1))/2; }
176  static int nrow() { return M; }
177  static int ncol() { return M; }
178 
179  // Scalar norm square is sum( squares of all scalars ). The off-diagonals
180  // come up twice.
181  ScalarNormSq scalarNormSqr() const {
182  return getDiag().scalarNormSqr() + 2.*getLower().scalarNormSqr();
183  }
184 
185  // sqrt() is elementwise square root; that is, the return value has the same
186  // dimension as this SymMat but with each element replaced by whatever it thinks
187  // its square root is.
188  TSqrt sqrt() const {
189  return TSqrt(getAsVec().sqrt());
190  }
191 
192  // abs() is elementwise absolute value; that is, the return value has the same
193  // dimension as this SymMat but with each element replaced by whatever it thinks
194  // its absolute value is.
195  TAbs abs() const {
196  return TAbs(getAsVec().abs());
197  }
198 
199  TStandard standardize() const {
200  return TStandard(getAsVec().standardize());
201  }
202 
203  EStandard trace() const {return getDiag().sum();}
204 
205  // This gives the resulting SymMat type when (m[i] op P) is applied to each element of m.
206  // It is a SymMat of dimension M, spacing 1, and element types which are the regular
207  // composite result of E op P. Typically P is a scalar type but it doesn't have to be.
208  template <class P> struct EltResult {
209  typedef SymMat<M, typename CNT<E>::template Result<P>::Mul, 1> Mul;
210  typedef SymMat<M, typename CNT<E>::template Result<P>::Dvd, 1> Dvd;
211  typedef SymMat<M, typename CNT<E>::template Result<P>::Add, 1> Add;
212  typedef SymMat<M, typename CNT<E>::template Result<P>::Sub, 1> Sub;
213  };
214 
215  // This is the composite result for m op P where P is some kind of appropriately shaped
216  // non-scalar type.
217  template <class P> struct Result {
218  typedef MulCNTs<M,M,ArgDepth,SymMat,ColSpacing,RowSpacing,
219  CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
220  P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOp;
221  typedef typename MulOp::Type Mul;
222 
223  typedef MulCNTsNonConforming<M,M,ArgDepth,SymMat,ColSpacing,RowSpacing,
224  CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
225  P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOpNonConforming;
226  typedef typename MulOpNonConforming::Type MulNon;
227 
228 
229  typedef DvdCNTs<M,M,ArgDepth,SymMat,ColSpacing,RowSpacing,
230  CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
231  P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> DvdOp;
232  typedef typename DvdOp::Type Dvd;
233 
234  typedef AddCNTs<M,M,ArgDepth,SymMat,ColSpacing,RowSpacing,
235  CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
236  P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> AddOp;
237  typedef typename AddOp::Type Add;
238 
239  typedef SubCNTs<M,M,ArgDepth,SymMat,ColSpacing,RowSpacing,
240  CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
241  P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> SubOp;
242  typedef typename SubOp::Type Sub;
243  };
244 
245  // Shape-preserving element substitution (always packed)
246  template <class P> struct Substitute {
247  typedef SymMat<M,P> Type;
248  };
249 
252  SymMat(){
253  #ifndef NDEBUG
254  setToNaN();
255  #endif
256  }
257 
259  SymMat(const SymMat& src) {
260  updAsVec() = src.getAsVec();
261  }
262 
264  SymMat& operator=(const SymMat& src) {
265  updAsVec() = src.getAsVec();
266  return *this;
267  }
268 
279  template <class EE, int CSS, int RSS>
280  explicit SymMat(const Mat<M,M,EE,CSS,RSS>& m)
281  { setFromSymmetric(m); }
282 
286  template <class EE, int CSS, int RSS>
287  SymMat& setFromLower(const Mat<M,M,EE,CSS,RSS>& m) {
288  this->updDiag() = m.diag().real();
289  for (int j=0; j<M; ++j)
290  for (int i=j+1; i<M; ++i)
291  this->updEltLower(i,j) = m(i,j);
292  return *this;
293  }
294 
302  template <class EE, int CSS, int RSS>
303  SymMat& setFromUpper(const Mat<M,M,EE,CSS,RSS>& m) {
304  this->updDiag() = m.diag().real();
305  for (int j=0; j<M; ++j)
306  for (int i=j+1; i<M; ++i)
307  this->updEltLower(i,j) = m(j,i);
308  return *this;
309  }
310 
316  template <class EE, int CSS, int RSS>
317  SymMat& setFromSymmetric(const Mat<M,M,EE,CSS,RSS>& m) {
318  SimTK_ERRCHK1(m.isNumericallySymmetric(), "SymMat::setFromSymmetric()",
319  "The allegedly symmetric source matrix was not symmetric to within "
320  "a tolerance of %g.", m.getDefaultTolerance());
321  this->updDiag() = m.diag().real();
322  for (int j=0; j<M; ++j)
323  for (int i=j+1; i<M; ++i)
324  this->updEltLower(i,j) =
325  (m(i,j) + CNT<EE>::transpose(m(j,i)))/2;
326  return *this;
327  }
328 
331  template <int RSS> SymMat(const SymMat<M,E,RSS>& src)
332  { updAsVec() = src.getAsVec(); }
333 
336  template <int RSS> SymMat(const SymMat<M,ENeg,RSS>& src)
337  { updAsVec() = src.getAsVec(); }
338 
342  template <class EE, int RSS> explicit SymMat(const SymMat<M,EE,RSS>& src)
343  { updAsVec() = src.getAsVec(); }
344 
345  // Construction using an element repeats that element on the diagonal
346  // but sets the rest of the matrix to zero.
347  explicit SymMat(const E& e) {
348  updDiag() = CNT<E>::real(e);
349  for (int i=0; i < NLowerElements; ++i) updlowerE(i) = E(0);
350  }
351 
352  // Construction using a negated element is just like construction from
353  // the element.
354  explicit SymMat(const ENeg& e) {
355  updDiag() = CNT<ENeg>::real(e);
356  for (int i=0; i < NLowerElements; ++i) updlowerE(i) = E(0);
357  }
358 
359  // Given an int, turn it into a suitable floating point number
360  // and then feed that to the above single-element constructor.
361  explicit SymMat(int i)
362  { new (this) SymMat(E(Precision(i))); }
363 
377 
378  SymMat(const E& e0,
379  const E& e1,const E& e2)
380  { assert(M==2); TDiag& d=updDiag(); TLower& l=updLower();
381  d[0]=CNT<E>::real(e0); d[1]=CNT<E>::real(e2);
382  l[0]=e1; }
383 
384  SymMat(const E& e0,
385  const E& e1,const E& e2,
386  const E& e3,const E& e4, const E& e5)
387  { assert(M==3); TDiag& d=updDiag(); TLower& l=updLower();
388  d[0]=CNT<E>::real(e0);d[1]=CNT<E>::real(e2);d[2]=CNT<E>::real(e5);
389  l[0]=e1;l[1]=e3;
390  l[2]=e4; }
391 
392  SymMat(const E& e0,
393  const E& e1,const E& e2,
394  const E& e3,const E& e4, const E& e5,
395  const E& e6,const E& e7, const E& e8, const E& e9)
396  { assert(M==4); TDiag& d=updDiag(); TLower& l=updLower();
397  d[0]=CNT<E>::real(e0);d[1]=CNT<E>::real(e2);d[2]=CNT<E>::real(e5);d[3]=CNT<E>::real(e9);
398  l[0]=e1;l[1]=e3;l[2]=e6;
399  l[3]=e4;l[4]=e7;
400  l[5]=e8; }
401 
402  SymMat(const E& e0,
403  const E& e1, const E& e2,
404  const E& e3, const E& e4, const E& e5,
405  const E& e6, const E& e7, const E& e8, const E& e9,
406  const E& e10,const E& e11, const E& e12, const E& e13, const E& e14)
407  { assert(M==5); TDiag& d=updDiag(); TLower& l=updLower();
408  d[0]=CNT<E>::real(e0);d[1]=CNT<E>::real(e2);d[2]=CNT<E>::real(e5);d[3]=CNT<E>::real(e9);d[4]=CNT<E>::real(e14);
409  l[0]=e1;l[1]=e3;l[2]=e6;l[3]=e10;
410  l[4]=e4;l[5]=e7;l[6]=e11;
411  l[7]=e8;l[8]=e12;
412  l[9]=e13; }
413 
414  SymMat(const E& e0,
415  const E& e1, const E& e2,
416  const E& e3, const E& e4, const E& e5,
417  const E& e6, const E& e7, const E& e8, const E& e9,
418  const E& e10,const E& e11, const E& e12, const E& e13, const E& e14,
419  const E& e15,const E& e16, const E& e17, const E& e18, const E& e19, const E& e20)
420  { assert(M==6); TDiag& d=updDiag(); TLower& l=updLower();
421  d[0]=CNT<E>::real(e0);d[1]=CNT<E>::real(e2);d[2]=CNT<E>::real(e5);
422  d[3]=CNT<E>::real(e9);d[4]=CNT<E>::real(e14);d[5]=CNT<E>::real(e20);
423  l[0] =e1; l[1] =e3; l[2] =e6; l[3]=e10; l[4]=e15;
424  l[5] =e4; l[6] =e7; l[7] =e11; l[8]=e16;
425  l[9] =e8; l[10]=e12;l[11]=e17;
426  l[12]=e13;l[13]=e18;
427  l[14]=e19; }
428 
429  // Construction from a pointer to anything assumes we're pointing
430  // at a packed element list of the right length, providing the
431  // lower triangle in row order, so a b c d e f means
432  // a
433  // b c
434  // d e f
435  // g h i j
436  // This has to be mapped to our diagonal/lower layout, which in
437  // the above example will be:
438  // [a c f j][b d g e h i]
439  //
440  // In the input layout, the i'th row begins at element i(i+1)/2,
441  // so diagonals are at i(i+1)/2 + i, while lower
442  // elements (i,j; i>j) are at i(i+1)/2 + j.
443  template <class EE> explicit SymMat(const EE* p) {
444  assert(p);
445  for (int i=0; i<M; ++i) {
446  const int rowStart = (i*(i+1))/2;
447  updDiag()[i] = CNT<EE>::real(p[rowStart + i]);
448  for (int j=0; j<i; ++j)
449  updEltLower(i,j) = p[rowStart + j];
450  }
451  }
452 
453  // This is the same thing except as an assignment to pointer rather
454  // than a constructor from pointer.
455  template <class EE> SymMat& operator=(const EE* p) {
456  assert(p);
457  for (int i=0; i<M; ++i) {
458  const int rowStart = (i*(i+1))/2;
459  updDiag()[i] = CNT<EE>::real(p[rowStart + i]);
460  for (int j=0; j<i; ++j)
461  updEltLower(i,j) = p[rowStart + j];
462  }
463  return *this;
464  }
465 
466  // Assignment works similarly to copy -- if the lengths match,
467  // go element-by-element. Otherwise, zero and then copy to each
468  // diagonal element.
469  template <class EE, int RSS> SymMat& operator=(const SymMat<M,EE,RSS>& mm) {
470  updAsVec() = mm.getAsVec();
471  return *this;
472  }
473 
474 
475  // Conforming assignment ops
476  template <class EE, int RSS> SymMat&
477  operator+=(const SymMat<M,EE,RSS>& mm) {
478  updAsVec() += mm.getAsVec();
479  return *this;
480  }
481  template <class EE, int RSS> SymMat&
482  operator+=(const SymMat<M,negator<EE>,RSS>& mm) {
483  updAsVec() -= -mm.getAsVec(); // negation of negator is free
484  return *this;
485  }
486 
487  template <class EE, int RSS> SymMat&
488  operator-=(const SymMat<M,EE,RSS>& mm) {
489  updAsVec() -= mm.getAsVec();
490  return *this;
491  }
492  template <class EE, int RSS> SymMat&
493  operator-=(const SymMat<M,negator<EE>,RSS>& mm) {
494  updAsVec() += -mm.getAsVec(); // negation of negator is free
495  return *this;
496  }
497 
498  // In place matrix multiply can only be done when the RHS matrix is the same
499  // size and the elements are also *= compatible.
500  template <class EE, int RSS> SymMat&
501  operator*=(const SymMat<M,EE,RSS>& mm) {
502  assert(false); // TODO
503  return *this;
504  }
505 
506  // Conforming binary ops with 'this' on left, producing new packed result.
507  // Cases: sy=sy+-sy, m=sy+-m, m=sy*sy, m=sy*m, v=sy*v
508 
509  // sy= this + sy
510  template <class E2, int RS2>
511  typename Result<SymMat<M,E2,RS2> >::Add
512  conformingAdd(const SymMat<M,E2,RS2>& r) const {
513  return typename Result<SymMat<M,E2,RS2> >::Add
514  (getAsVec().conformingAdd(r.getAsVec()));
515  }
516  // m= this - m
517  template <class E2, int RS2>
518  typename Result<SymMat<M,E2,RS2> >::Sub
519  conformingSubtract(const SymMat<M,E2,RS2>& r) const {
520  return typename Result<SymMat<M,E2,RS2> >::Sub
521  (getAsVec().conformingSubtract(r.getAsVec()));
522  }
523 
524  // symmetric * symmetric produces a full result
525  // m= this * s
526  // TODO: this is not a good implementation
527  template <class E2, int RS2>
528  typename Result<SymMat<M,E2,RS2> >::Mul
529  conformingMultiply(const SymMat<M,E2,RS2>& s) const {
530  typename Result<SymMat<M,E2,RS2> >::Mul result;
531  for (int j=0;j<M;++j)
532  for (int i=0;i<M;++i)
533  result(i,j) = (*this)[i] * s(j);
534  return result;
535  }
536 
537  // sy= this .* sy
538  template <class E2, int RS2>
539  typename EltResult<E2>::Mul
540  elementwiseMultiply(const SymMat<M,E2,RS2>& r) const {
541  return typename EltResult<E2>::Mul
542  (getAsVec().elementwiseMultiply(r.getAsVec()));
543  }
544 
545  // sy= this ./ sy
546  template <class E2, int RS2>
547  typename EltResult<E2>::Dvd
548  elementwiseDivide(const SymMat<M,E2,RS2>& r) const {
549  return typename EltResult<E2>::Dvd
550  (getAsVec().elementwiseDivide(r.getAsVec()));
551  }
552 
553  // TODO: need the rest of the SymMat operators
554 
555  // Must be i >= j.
556  const E& operator()(int i,int j) const
557  { return i==j ? getDiag()[i] : getEltLower(i,j); }
558  E& operator()(int i,int j)
559  { return i==j ? updDiag()[i] : updEltLower(i,j); }
560 
561  // These are slow for a symmetric matrix, requiring copying and
562  // possibly floating point operations for conjugation.
563  TRow operator[](int i) const {return row(i);}
564  TCol operator()(int j) const {return col(j);}
565 
566 
567  // This is the scalar Frobenius norm.
568  ScalarNormSq normSqr() const { return scalarNormSqr(); }
569  typename CNT<ScalarNormSq>::TSqrt
570  norm() const { return CNT<ScalarNormSq>::sqrt(scalarNormSqr()); }
571 
583  TNormalize normalize() const {
584  if (CNT<E>::IsScalar) {
585  return castAwayNegatorIfAny() / (SignInterpretation*norm());
586  } else {
587  TNormalize elementwiseNormalized;
588  // punt to the equivalent Vec to get the elements normalized
589  elementwiseNormalized.updAsVec() = getAsVec().normalize();
590  return elementwiseNormalized;
591  }
592  }
593 
594  TInvert invert() const {assert(false); return TInvert();} // TODO default inversion
595 
596  const SymMat& operator+() const { return *this; }
597  const TNeg& operator-() const { return negate(); }
598  TNeg& operator-() { return updNegate(); }
599  const THerm& operator~() const { return transpose(); }
600  THerm& operator~() { return updTranspose(); }
601 
602  const TNeg& negate() const { return *reinterpret_cast<const TNeg*>(this); }
603  TNeg& updNegate() { return *reinterpret_cast<TNeg*>(this); }
604 
605  const THerm& transpose() const { return *reinterpret_cast<const THerm*>(this); }
606  THerm& updTranspose() { return *reinterpret_cast<THerm*>(this); }
607 
608  const TPosTrans& positionalTranspose() const
609  { return *reinterpret_cast<const TPosTrans*>(this); }
610  TPosTrans& updPositionalTranspose()
611  { return *reinterpret_cast<TPosTrans*>(this); }
612 
613  const TReal& real() const { return *reinterpret_cast<const TReal*>(this); }
614  TReal& real() { return *reinterpret_cast< TReal*>(this); }
615 
616  // Had to contort these routines to get them through VC++ 7.net
617  const TImag& imag() const {
618  const int offs = ImagOffset;
619  const EImag* p = reinterpret_cast<const EImag*>(this);
620  return *reinterpret_cast<const TImag*>(p+offs);
621  }
622  TImag& imag() {
623  const int offs = ImagOffset;
624  EImag* p = reinterpret_cast<EImag*>(this);
625  return *reinterpret_cast<TImag*>(p+offs);
626  }
627 
628  const TWithoutNegator& castAwayNegatorIfAny() const {return *reinterpret_cast<const TWithoutNegator*>(this);}
629  TWithoutNegator& updCastAwayNegatorIfAny() {return *reinterpret_cast<TWithoutNegator*>(this);}
630 
631  // These are elementwise binary operators, (this op ee) by default but (ee op this) if
632  // 'FromLeft' appears in the name. The result is a packed SymMat<M> but the element type
633  // may change. These are mostly used to implement global operators.
634  // We call these "scalar" operators but actually the "scalar" can be a composite type.
635 
636  //TODO: consider converting 'e' to Standard Numbers as precalculation and changing
637  // return type appropriately.
638  template <class EE> SymMat<M, typename CNT<E>::template Result<EE>::Mul>
639  scalarMultiply(const EE& e) const {
640  SymMat<M, typename CNT<E>::template Result<EE>::Mul> result;
641  result.updAsVec() = getAsVec().scalarMultiply(e);
642  return result;
643  }
644  template <class EE> SymMat<M, typename CNT<EE>::template Result<E>::Mul>
645  scalarMultiplyFromLeft(const EE& e) const {
646  SymMat<M, typename CNT<EE>::template Result<E>::Mul> result;
647  result.updAsVec() = getAsVec().scalarMultiplyFromLeft(e);
648  return result;
649  }
650 
651  // TODO: should precalculate and store 1/e, while converting to Standard Numbers. Note
652  // that return type should change appropriately.
653  template <class EE> SymMat<M, typename CNT<E>::template Result<EE>::Dvd>
654  scalarDivide(const EE& e) const {
655  SymMat<M, typename CNT<E>::template Result<EE>::Dvd> result;
656  result.updAsVec() = getAsVec().scalarDivide(e);
657  return result;
658  }
659  template <class EE> SymMat<M, typename CNT<EE>::template Result<E>::Dvd>
660  scalarDivideFromLeft(const EE& e) const {
661  SymMat<M, typename CNT<EE>::template Result<E>::Dvd> result;
662  result.updAsVec() = getAsVec().scalarDivideFromLeft(e);
663  return result;
664  }
665 
666  // Additive ops involving a scalar update only the diagonal
667  template <class EE> SymMat<M, typename CNT<E>::template Result<EE>::Add>
668  scalarAdd(const EE& e) const {
669  SymMat<M, typename CNT<E>::template Result<EE>::Add> result(*this);
670  result.updDiag() += e;
671  return result;
672  }
673  // Add is commutative, so no 'FromLeft'.
674 
675  template <class EE> SymMat<M, typename CNT<E>::template Result<EE>::Sub>
676  scalarSubtract(const EE& e) const {
677  SymMat<M, typename CNT<E>::template Result<EE>::Sub> result(*this);
678  result.diag() -= e;
679  return result;
680  }
681  // This is s-m; negate m and add s to diagonal
682  // TODO: Should do something clever with negation here.
683  template <class EE> SymMat<M, typename CNT<EE>::template Result<E>::Sub>
684  scalarSubtractFromLeft(const EE& e) const {
685  SymMat<M, typename CNT<EE>::template Result<E>::Sub> result(-(*this));
686  result.diag() += e;
687  return result;
688  }
689 
690  // Generic assignments for any element type not listed explicitly, including scalars.
691  // These are done repeatedly for each element and only work if the operation can
692  // be performed leaving the original element type.
693  template <class EE> SymMat& operator =(const EE& e) {return scalarEq(e);}
694  template <class EE> SymMat& operator+=(const EE& e) {return scalarPlusEq(e);}
695  template <class EE> SymMat& operator-=(const EE& e) {return scalarMinusEq(e);}
696  template <class EE> SymMat& operator*=(const EE& e) {return scalarTimesEq(e);}
697  template <class EE> SymMat& operator/=(const EE& e) {return scalarDivideEq(e);}
698 
699  // Generalized scalar assignment & computed assignment methods. These will work
700  // for any assignment-compatible element, not just scalars.
701  template <class EE> SymMat& scalarEq(const EE& ee)
702  { updDiag() = ee; updLower() = E(0); return *this; }
703  template <class EE> SymMat& scalarPlusEq(const EE& ee)
704  { updDiag() += ee; return *this; }
705  template <class EE> SymMat& scalarMinusEq(const EE& ee)
706  { updDiag() -= ee; return *this; }
707 
708  // this is m = s-m; negate off diagonal, do d=s-d for each diagonal element d
709  template <class EE> SymMat& scalarMinusEqFromLeft(const EE& ee)
710  { updLower() *= E(-1); updDiag().scalarMinusEqFromLeft(ee); return *this; }
711 
712  template <class EE> SymMat& scalarTimesEq(const EE& ee)
713  { updAsVec() *= ee; return *this; }
714  template <class EE> SymMat& scalarTimesEqFromLeft(const EE& ee)
715  { updAsVec().scalarTimesEqFromLeft(ee); return *this; }
716  template <class EE> SymMat& scalarDivideEq(const EE& ee)
717  { updAsVec() /= ee; return *this; }
718  template <class EE> SymMat& scalarDivideEqFromLeft(const EE& ee)
719  { updAsVec().scalarDivideEqFromLeft(ee); return *this; }
720 
721  void setToNaN() { updAsVec().setToNaN(); }
722  void setToZero() { updAsVec().setToZero(); }
723 
724  // These assume we are given a pointer to d[0] of a SymMat<M,E,RS> like this one.
725  static const SymMat& getAs(const ELT* p) {return *reinterpret_cast<const SymMat*>(p);}
726  static SymMat& updAs(ELT* p) {return *reinterpret_cast<SymMat*>(p);}
727 
728  // Note packed spacing
729  static TPacked getNaN() {
730  return TPacked(CNT<typename TPacked::TDiag>::getNaN(),
731  CNT<typename TPacked::TLower>::getNaN());
732  }
733 
735  bool isNaN() const {return getAsVec().isNaN();}
736 
739  bool isInf() const {return getAsVec().isInf();}
740 
742  bool isFinite() const {return getAsVec().isFinite();}
743 
746  static double getDefaultTolerance() {return M*CNT<ELT>::getDefaultTolerance();}
747 
750  template <class E2, int RS2>
751  bool isNumericallyEqual(const SymMat<M,E2,RS2>& m, double tol) const {
752  return getAsVec().isNumericallyEqual(m.getAsVec(), tol);
753  }
754 
758  template <class E2, int RS2>
759  bool isNumericallyEqual(const SymMat<M,E2,RS2>& m) const {
760  const double tol = std::max(getDefaultTolerance(),m.getDefaultTolerance());
761  return isNumericallyEqual(m, tol);
762  }
763 
769  bool isNumericallyEqual
770  (const ELT& e,
771  double tol = getDefaultTolerance()) const
772  {
773  if (!diag().isNumericallyEqual(e, tol))
774  return false;
775  return getLower().isNumericallyEqual(ELT(0), tol);
776  }
777 
778  // Rows and columns have to be copied and Hermitian elements have to
779  // be conjugated at a floating point cost. This isn't the best way
780  // to work with a symmetric matrix.
781  TRow row(int i) const {
782  SimTK_INDEXCHECK(i,M,"SymMat::row[i]");
783  TRow rowi;
784  // Columns left of diagonal are lower.
785  for (int j=0; j<i; ++j)
786  rowi[j] = getEltLower(i,j);
787  rowi[i] = getEltDiag(i);
788  for (int j=i+1; j<M; ++j)
789  rowi[j] = getEltUpper(i,j); // conversion from EHerm to E may cost flops
790  return rowi;
791  }
792 
793  TCol col(int j) const {
794  SimTK_INDEXCHECK(j,M,"SymMat::col(j)");
795  TCol colj;
796  // Rows above diagonal are upper (with conjugated elements).
797  for (int i=0; i<j; ++i)
798  colj[i] = getEltUpper(i,j); // conversion from EHerm to E may cost flops
799  colj[j] = getEltDiag(j);
800  for (int i=j+1; i<M; ++i)
801  colj[i] = getEltLower(i,j);
802  return colj;
803  }
804 
810  E elt(int i, int j) const {
811  SimTK_INDEXCHECK(i,M,"SymMat::elt(i,j)");
812  SimTK_INDEXCHECK(j,M,"SymMat::elt(i,j)");
813  if (i>j) return getEltLower(i,j); // copy element
814  else if (i==j) return getEltDiag(i); // copy element
815  else return getEltUpper(i,j); // conversion from EHerm to E may cost flops
816  }
817 
818  const TDiag& getDiag() const {return TDiag::getAs(d);}
819  TDiag& updDiag() {return TDiag::updAs(d);}
820 
821  // Conventional synonym
822  const TDiag& diag() const {return getDiag();}
823  TDiag& diag() {return updDiag();}
824 
825  const TLower& getLower() const {return TLower::getAs(&d[M*RS]);}
826  TLower& updLower() {return TLower::updAs(&d[M*RS]);}
827 
828  const TUpper& getUpper() const {return reinterpret_cast<const TUpper&>(getLower());}
829  TUpper& updUpper() {return reinterpret_cast< TUpper&>(updLower());}
830 
831  const TAsVec& getAsVec() const {return TAsVec::getAs(d);}
832  TAsVec& updAsVec() {return TAsVec::updAs(d);}
833 
834  const E& getEltDiag(int i) const {return getDiag()[i];}
835  E& updEltDiag(int i) {return updDiag()[i];}
836 
837  // must be i > j
838  const E& getEltLower(int i, int j) const {return getLower()[lowerIx(i,j)];}
839  E& updEltLower(int i, int j) {return updLower()[lowerIx(i,j)];}
840 
841  // must be i < j
842  const EHerm& getEltUpper(int i, int j) const {return getUpper()[lowerIx(j,i)];}
843  EHerm& updEltUpper(int i, int j) {return updUpper()[lowerIx(j,i)];}
844 
846  TRow colSum() const {
847  TRow temp(~getDiag());
848  for (int i = 1; i < M; ++i)
849  for (int j = 0; j < i; ++j) {
850  const E& value = getEltLower(i, j);;
851  temp[j] += value;
852  temp[i] += E(reinterpret_cast<const EHerm&>(value));
853  }
854  return temp;
855  }
858  TRow sum() const {return colSum();}
859 
862  TCol rowSum() const {
863  TCol temp(getDiag());
864  for (int i = 1; i < M; ++i)
865  for (int j = 0; j < i; ++j) {
866  const E& value = getEltLower(i, j);;
867  temp[i] += value;
868  temp[j] += E(reinterpret_cast<const EHerm&>(value));
869  }
870  return temp;
871  }
872 private:
873  E d[NActualElements];
874 
875  // This utility doesn't turn lower or upper into a Vec which could turn
876  // out to have zero length if this is a 1x1 matrix.
877  const E& getlowerE(int i) const {return d[(M+i)*RS];}
878  E& updlowerE(int i) {return d[(M+i)*RS];}
879 
880  SymMat(const TDiag& di, const TLower& low) {
881  updDiag() = di; updLower() = low;
882  }
883 
884  explicit SymMat(const TAsVec& v) {
885  TAsVec::updAs(d) = v;
886  }
887 
888  // Convert i,j with i>j to an index in "lower". This also gives the
889  // right index for "upper" with i and j reversed.
890  static int lowerIx(int i, int j) {
891  assert(0 <= j && j < i && i < M);
892  return (i-j-1) + j*(M-1) - (j*(j-1))/2;
893  }
894 
895  template <int MM, class EE, int RSS> friend class SymMat;
896 };
897 
899 // Global operators involving two symmetric matrices. //
900 // sy=sy+sy, sy=sy-sy, m=sy*sy, sy==sy, sy!=sy //
902 
903 // m3 = m1 + m2 where all m's have the same dimension M.
904 template <int M, class E1, int S1, class E2, int S2> inline
905 typename SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >::Add
906 operator+(const SymMat<M,E1,S1>& l, const SymMat<M,E2,S2>& r) {
907  return SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >
908  ::AddOp::perform(l,r);
909 }
910 
911 // m3 = m1 - m2 where all m's have the same dimension M.
912 template <int M, class E1, int S1, class E2, int S2> inline
913 typename SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >::Sub
914 operator-(const SymMat<M,E1,S1>& l, const SymMat<M,E2,S2>& r) {
915  return SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >
916  ::SubOp::perform(l,r);
917 }
918 
919 // m3 = m1 * m2 where all m's have the same dimension M.
920 // The result will not be symmetric.
921 template <int M, class E1, int S1, class E2, int S2> inline
922 typename SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >::Mul
923 operator*(const SymMat<M,E1,S1>& l, const SymMat<M,E2,S2>& r) {
924  return SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >
925  ::MulOp::perform(l,r);
926 }
927 
928 // bool = m1 == m2, m1 and m2 have the same dimension M
929 template <int M, class E1, int S1, class E2, int S2> inline bool
930 operator==(const SymMat<M,E1,S1>& l, const SymMat<M,E2,S2>& r) {
931  return l.getAsVec() == r.getAsVec();
932 }
933 
934 // bool = m1 == m2, m1 and m2 have the same dimension M
935 template <int M, class E1, int S1, class E2, int S2> inline bool
936 operator!=(const SymMat<M,E1,S1>& l, const SymMat<M,E2,S2>& r) {return !(l==r);}
937 
939 // Global operators involving a SymMat and a scalar. //
941 
942 // SCALAR MULTIPLY
943 
944 // m = m*real, real*m
945 template <int M, class E, int S> inline
946 typename SymMat<M,E,S>::template Result<float>::Mul
947 operator*(const SymMat<M,E,S>& l, const float& r)
948  { return SymMat<M,E,S>::template Result<float>::MulOp::perform(l,r); }
949 template <int M, class E, int S> inline
950 typename SymMat<M,E,S>::template Result<float>::Mul
951 operator*(const float& l, const SymMat<M,E,S>& r) {return r*l;}
952 
953 template <int M, class E, int S> inline
954 typename SymMat<M,E,S>::template Result<double>::Mul
955 operator*(const SymMat<M,E,S>& l, const double& r)
956  { return SymMat<M,E,S>::template Result<double>::MulOp::perform(l,r); }
957 template <int M, class E, int S> inline
958 typename SymMat<M,E,S>::template Result<double>::Mul
959 operator*(const double& l, const SymMat<M,E,S>& r) {return r*l;}
960 
961 template <int M, class E, int S> inline
962 typename SymMat<M,E,S>::template Result<long double>::Mul
963 operator*(const SymMat<M,E,S>& l, const long double& r)
964  { return SymMat<M,E,S>::template Result<long double>::MulOp::perform(l,r); }
965 template <int M, class E, int S> inline
966 typename SymMat<M,E,S>::template Result<long double>::Mul
967 operator*(const long double& l, const SymMat<M,E,S>& r) {return r*l;}
968 
969 // m = m*int, int*m -- just convert int to m's precision float
970 template <int M, class E, int S> inline
971 typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Mul
972 operator*(const SymMat<M,E,S>& l, int r) {return l * (typename CNT<E>::Precision)r;}
973 template <int M, class E, int S> inline
974 typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Mul
975 operator*(int l, const SymMat<M,E,S>& r) {return r * (typename CNT<E>::Precision)l;}
976 
977 // Complex, conjugate, and negator are all easy to templatize.
978 
979 // m = m*complex, complex*m
980 template <int M, class E, int S, class R> inline
981 typename SymMat<M,E,S>::template Result<std::complex<R> >::Mul
982 operator*(const SymMat<M,E,S>& l, const std::complex<R>& r)
983  { return SymMat<M,E,S>::template Result<std::complex<R> >::MulOp::perform(l,r); }
984 template <int M, class E, int S, class R> inline
985 typename SymMat<M,E,S>::template Result<std::complex<R> >::Mul
986 operator*(const std::complex<R>& l, const SymMat<M,E,S>& r) {return r*l;}
987 
988 // m = m*conjugate, conjugate*m (convert conjugate->complex)
989 template <int M, class E, int S, class R> inline
990 typename SymMat<M,E,S>::template Result<std::complex<R> >::Mul
991 operator*(const SymMat<M,E,S>& l, const conjugate<R>& r) {return l*(std::complex<R>)r;}
992 template <int M, class E, int S, class R> inline
993 typename SymMat<M,E,S>::template Result<std::complex<R> >::Mul
994 operator*(const conjugate<R>& l, const SymMat<M,E,S>& r) {return r*(std::complex<R>)l;}
995 
996 // m = m*negator, negator*m: convert negator to standard number
997 template <int M, class E, int S, class R> inline
998 typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Mul
999 operator*(const SymMat<M,E,S>& l, const negator<R>& r) {return l * (typename negator<R>::StdNumber)(R)r;}
1000 template <int M, class E, int S, class R> inline
1001 typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Mul
1002 operator*(const negator<R>& l, const SymMat<M,E,S>& r) {return r * (typename negator<R>::StdNumber)(R)l;}
1003 
1004 
1005 // SCALAR DIVIDE. This is a scalar operation when the scalar is on the right,
1006 // but when it is on the left it means scalar * pseudoInverse(mat), which is
1007 // a similar symmetric matrix.
1008 
1009 // m = m/real, real/m
1010 template <int M, class E, int S> inline
1011 typename SymMat<M,E,S>::template Result<float>::Dvd
1012 operator/(const SymMat<M,E,S>& l, const float& r)
1013  { return SymMat<M,E,S>::template Result<float>::DvdOp::perform(l,r); }
1014 template <int M, class E, int S> inline
1015 typename CNT<float>::template Result<SymMat<M,E,S> >::Dvd
1016 operator/(const float& l, const SymMat<M,E,S>& r)
1017  { return CNT<float>::template Result<SymMat<M,E,S> >::DvdOp::perform(l,r); }
1018 
1019 template <int M, class E, int S> inline
1020 typename SymMat<M,E,S>::template Result<double>::Dvd
1021 operator/(const SymMat<M,E,S>& l, const double& r)
1022  { return SymMat<M,E,S>::template Result<double>::DvdOp::perform(l,r); }
1023 template <int M, class E, int S> inline
1024 typename CNT<double>::template Result<SymMat<M,E,S> >::Dvd
1025 operator/(const double& l, const SymMat<M,E,S>& r)
1026  { return CNT<double>::template Result<SymMat<M,E,S> >::DvdOp::perform(l,r); }
1027 
1028 template <int M, class E, int S> inline
1029 typename SymMat<M,E,S>::template Result<long double>::Dvd
1030 operator/(const SymMat<M,E,S>& l, const long double& r)
1031  { return SymMat<M,E,S>::template Result<long double>::DvdOp::perform(l,r); }
1032 template <int M, class E, int S> inline
1033 typename CNT<long double>::template Result<SymMat<M,E,S> >::Dvd
1034 operator/(const long double& l, const SymMat<M,E,S>& r)
1035  { return CNT<long double>::template Result<SymMat<M,E,S> >::DvdOp::perform(l,r); }
1036 
1037 // m = m/int, int/m -- just convert int to m's precision float
1038 template <int M, class E, int S> inline
1039 typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Dvd
1040 operator/(const SymMat<M,E,S>& l, int r) {return l / (typename CNT<E>::Precision)r;}
1041 template <int M, class E, int S> inline
1042 typename CNT<typename CNT<E>::Precision>::template Result<SymMat<M,E,S> >::Dvd
1043 operator/(int l, const SymMat<M,E,S>& r) {return (typename CNT<E>::Precision)l / r;}
1044 
1045 
1046 // Complex, conjugate, and negator are all easy to templatize.
1047 
1048 // m = m/complex, complex/m
1049 template <int M, class E, int S, class R> inline
1050 typename SymMat<M,E,S>::template Result<std::complex<R> >::Dvd
1051 operator/(const SymMat<M,E,S>& l, const std::complex<R>& r)
1052  { return SymMat<M,E,S>::template Result<std::complex<R> >::DvdOp::perform(l,r); }
1053 template <int M, class E, int S, class R> inline
1054 typename CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::Dvd
1055 operator/(const std::complex<R>& l, const SymMat<M,E,S>& r)
1056  { return CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::DvdOp::perform(l,r); }
1057 
1058 // m = m/conjugate, conjugate/m (convert conjugate->complex)
1059 template <int M, class E, int S, class R> inline
1060 typename SymMat<M,E,S>::template Result<std::complex<R> >::Dvd
1061 operator/(const SymMat<M,E,S>& l, const conjugate<R>& r) {return l/(std::complex<R>)r;}
1062 template <int M, class E, int S, class R> inline
1063 typename CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::Dvd
1064 operator/(const conjugate<R>& l, const SymMat<M,E,S>& r) {return (std::complex<R>)l/r;}
1065 
1066 // m = m/negator, negator/m: convert negator to number
1067 template <int M, class E, int S, class R> inline
1068 typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Dvd
1069 operator/(const SymMat<M,E,S>& l, const negator<R>& r) {return l/(typename negator<R>::StdNumber)(R)r;}
1070 template <int M, class E, int S, class R> inline
1071 typename CNT<R>::template Result<SymMat<M,E,S> >::Dvd
1072 operator/(const negator<R>& l, const SymMat<M,E,S>& r) {return (typename negator<R>::StdNumber)(R)l/r;}
1073 
1074 
1075 // Add and subtract are odd as scalar ops. They behave as though the
1076 // scalar stands for a conforming matrix whose diagonal elements are that,
1077 // scalar and then a normal matrix add or subtract is done.
1078 
1079 // SCALAR ADD
1080 
1081 // m = m+real, real+m
1082 template <int M, class E, int S> inline
1083 typename SymMat<M,E,S>::template Result<float>::Add
1084 operator+(const SymMat<M,E,S>& l, const float& r)
1085  { return SymMat<M,E,S>::template Result<float>::AddOp::perform(l,r); }
1086 template <int M, class E, int S> inline
1087 typename SymMat<M,E,S>::template Result<float>::Add
1088 operator+(const float& l, const SymMat<M,E,S>& r) {return r+l;}
1089 
1090 template <int M, class E, int S> inline
1091 typename SymMat<M,E,S>::template Result<double>::Add
1092 operator+(const SymMat<M,E,S>& l, const double& r)
1093  { return SymMat<M,E,S>::template Result<double>::AddOp::perform(l,r); }
1094 template <int M, class E, int S> inline
1095 typename SymMat<M,E,S>::template Result<double>::Add
1096 operator+(const double& l, const SymMat<M,E,S>& r) {return r+l;}
1097 
1098 template <int M, class E, int S> inline
1099 typename SymMat<M,E,S>::template Result<long double>::Add
1100 operator+(const SymMat<M,E,S>& l, const long double& r)
1101  { return SymMat<M,E,S>::template Result<long double>::AddOp::perform(l,r); }
1102 template <int M, class E, int S> inline
1103 typename SymMat<M,E,S>::template Result<long double>::Add
1104 operator+(const long double& l, const SymMat<M,E,S>& r) {return r+l;}
1105 
1106 // m = m+int, int+m -- just convert int to m's precision float
1107 template <int M, class E, int S> inline
1108 typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Add
1109 operator+(const SymMat<M,E,S>& l, int r) {return l + (typename CNT<E>::Precision)r;}
1110 template <int M, class E, int S> inline
1111 typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Add
1112 operator+(int l, const SymMat<M,E,S>& r) {return r + (typename CNT<E>::Precision)l;}
1113 
1114 // Complex, conjugate, and negator are all easy to templatize.
1115 
1116 // m = m+complex, complex+m
1117 template <int M, class E, int S, class R> inline
1118 typename SymMat<M,E,S>::template Result<std::complex<R> >::Add
1119 operator+(const SymMat<M,E,S>& l, const std::complex<R>& r)
1120  { return SymMat<M,E,S>::template Result<std::complex<R> >::AddOp::perform(l,r); }
1121 template <int M, class E, int S, class R> inline
1122 typename SymMat<M,E,S>::template Result<std::complex<R> >::Add
1123 operator+(const std::complex<R>& l, const SymMat<M,E,S>& r) {return r+l;}
1124 
1125 // m = m+conjugate, conjugate+m (convert conjugate->complex)
1126 template <int M, class E, int S, class R> inline
1127 typename SymMat<M,E,S>::template Result<std::complex<R> >::Add
1128 operator+(const SymMat<M,E,S>& l, const conjugate<R>& r) {return l+(std::complex<R>)r;}
1129 template <int M, class E, int S, class R> inline
1130 typename SymMat<M,E,S>::template Result<std::complex<R> >::Add
1131 operator+(const conjugate<R>& l, const SymMat<M,E,S>& r) {return r+(std::complex<R>)l;}
1132 
1133 // m = m+negator, negator+m: convert negator to standard number
1134 template <int M, class E, int S, class R> inline
1135 typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Add
1136 operator+(const SymMat<M,E,S>& l, const negator<R>& r) {return l + (typename negator<R>::StdNumber)(R)r;}
1137 template <int M, class E, int S, class R> inline
1138 typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Add
1139 operator+(const negator<R>& l, const SymMat<M,E,S>& r) {return r + (typename negator<R>::StdNumber)(R)l;}
1140 
1141 // SCALAR SUBTRACT -- careful, not commutative.
1142 
1143 // m = m-real, real-m
1144 template <int M, class E, int S> inline
1145 typename SymMat<M,E,S>::template Result<float>::Sub
1146 operator-(const SymMat<M,E,S>& l, const float& r)
1147  { return SymMat<M,E,S>::template Result<float>::SubOp::perform(l,r); }
1148 template <int M, class E, int S> inline
1149 typename CNT<float>::template Result<SymMat<M,E,S> >::Sub
1150 operator-(const float& l, const SymMat<M,E,S>& r)
1151  { return CNT<float>::template Result<SymMat<M,E,S> >::SubOp::perform(l,r); }
1152 
1153 template <int M, class E, int S> inline
1154 typename SymMat<M,E,S>::template Result<double>::Sub
1155 operator-(const SymMat<M,E,S>& l, const double& r)
1156  { return SymMat<M,E,S>::template Result<double>::SubOp::perform(l,r); }
1157 template <int M, class E, int S> inline
1158 typename CNT<double>::template Result<SymMat<M,E,S> >::Sub
1159 operator-(const double& l, const SymMat<M,E,S>& r)
1160  { return CNT<double>::template Result<SymMat<M,E,S> >::SubOp::perform(l,r); }
1161 
1162 template <int M, class E, int S> inline
1163 typename SymMat<M,E,S>::template Result<long double>::Sub
1164 operator-(const SymMat<M,E,S>& l, const long double& r)
1165  { return SymMat<M,E,S>::template Result<long double>::SubOp::perform(l,r); }
1166 template <int M, class E, int S> inline
1167 typename CNT<long double>::template Result<SymMat<M,E,S> >::Sub
1168 operator-(const long double& l, const SymMat<M,E,S>& r)
1169  { return CNT<long double>::template Result<SymMat<M,E,S> >::SubOp::perform(l,r); }
1170 
1171 // m = m-int, int-m // just convert int to m's precision float
1172 template <int M, class E, int S> inline
1173 typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Sub
1174 operator-(const SymMat<M,E,S>& l, int r) {return l - (typename CNT<E>::Precision)r;}
1175 template <int M, class E, int S> inline
1176 typename CNT<typename CNT<E>::Precision>::template Result<SymMat<M,E,S> >::Sub
1177 operator-(int l, const SymMat<M,E,S>& r) {return (typename CNT<E>::Precision)l - r;}
1178 
1179 
1180 // Complex, conjugate, and negator are all easy to templatize.
1181 
1182 // m = m-complex, complex-m
1183 template <int M, class E, int S, class R> inline
1184 typename SymMat<M,E,S>::template Result<std::complex<R> >::Sub
1185 operator-(const SymMat<M,E,S>& l, const std::complex<R>& r)
1186  { return SymMat<M,E,S>::template Result<std::complex<R> >::SubOp::perform(l,r); }
1187 template <int M, class E, int S, class R> inline
1188 typename CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::Sub
1189 operator-(const std::complex<R>& l, const SymMat<M,E,S>& r)
1190  { return CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::SubOp::perform(l,r); }
1191 
1192 // m = m-conjugate, conjugate-m (convert conjugate->complex)
1193 template <int M, class E, int S, class R> inline
1194 typename SymMat<M,E,S>::template Result<std::complex<R> >::Sub
1195 operator-(const SymMat<M,E,S>& l, const conjugate<R>& r) {return l-(std::complex<R>)r;}
1196 template <int M, class E, int S, class R> inline
1197 typename CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::Sub
1198 operator-(const conjugate<R>& l, const SymMat<M,E,S>& r) {return (std::complex<R>)l-r;}
1199 
1200 // m = m-negator, negator-m: convert negator to standard number
1201 template <int M, class E, int S, class R> inline
1202 typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Sub
1203 operator-(const SymMat<M,E,S>& l, const negator<R>& r) {return l-(typename negator<R>::StdNumber)(R)r;}
1204 template <int M, class E, int S, class R> inline
1205 typename CNT<R>::template Result<SymMat<M,E,S> >::Sub
1206 operator-(const negator<R>& l, const SymMat<M,E,S>& r) {return (typename negator<R>::StdNumber)(R)l-r;}
1207 
1208 
1209 // SymMat I/O
1210 template <int M, class E, int RS, class CHAR, class TRAITS> inline
1211 std::basic_ostream<CHAR,TRAITS>&
1212 operator<<(std::basic_ostream<CHAR,TRAITS>& o, const SymMat<M,E,RS>& m) {
1213  for (int i=0;i<M;++i) {
1214  o << std::endl << "[";
1215  for (int j=0; j<=i; ++j)
1216  o << (j>0?" ":"") << m(i,j);
1217  for (int j=i+1; j<M; ++j)
1218  o << " *";
1219  o << "]";
1220  }
1221  if (M) o << std::endl;
1222  return o;
1223 }
1224 
1225 template <int M, class E, int RS, class CHAR, class TRAITS> inline
1226 std::basic_istream<CHAR,TRAITS>&
1227 operator>>(std::basic_istream<CHAR,TRAITS>& is, SymMat<M,E,RS>& m) {
1228  // TODO: not sure how to do input yet
1229  assert(false);
1230  return is;
1231 }
1232 
1233 } //namespace SimTK
1234 
1235 
1236 #endif //SimTK_SIMMATRIX_SMALLMATRIX_SYMMAT_H_