Simbody  3.5
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules 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 
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,
118  NLowerElements = (M*(M-1))/2,
125  RealStrideFactor = 1, // composite types don't change size when
126  // cast from complex to real or imaginary
128  ? CNT<E>::ArgDepth + 1
130  IsScalar = 0,
132  IsNumber = 0,
136  };
137 
138  typedef SymMat<M,E,RS> T;
141 
147  typedef T THerm; // These two are opposite of what you might expect
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;
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 {
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,
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>
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>
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>
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&
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&
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&
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
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
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
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
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
571 
583  TNormalize normalize() const {
584  if (CNT<E>::IsScalar) {
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 
609  { return *reinterpret_cast<const TPosTrans*>(this); }
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() {
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 
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
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
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
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
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_
Matrix_< E > operator/(const MatrixBase< E > &l, const typename CNT< E >::StdNumber &r)
Definition: BigMatrix.h:613
SymMat< M, EInvert, 1 > TInvert
Definition: SymMat.h:162
const E & getEltDiag(int i) const
Definition: SymMat.h:834
Definition: SymMat.h:134
SymMat< M, ENormalize, 1 > TNormalize
Definition: SymMat.h:163
SymMat< M, ESqTHerm, 1 > TSqTHerm
Definition: SymMat.h:165
SymMat< M, typename CNT< EE >::template Result< E >::Sub > scalarSubtractFromLeft(const EE &e) const
Definition: SymMat.h:684
EPrecision Precision
Definition: SymMat.h:172
SymMat< M, EAbs, 1 > TAbs
Definition: SymMat.h:160
SymMat & setFromUpper(const Mat< M, M, EE, CSS, RSS > &m)
Create a new SymMat of this type from a square Mat of the right size, looking only at upper elements ...
Definition: SymMat.h:303
SymMat & operator/=(const EE &e)
Definition: SymMat.h:697
ScalarNormSq scalarNormSqr() const
Definition: SymMat.h:181
SymMat()
Default construction initializes to NaN when debugging but is left uninitialized otherwise.
Definition: SymMat.h:252
EltResult< E2 >::Mul elementwiseMultiply(const SymMat< M, E2, RS2 > &r) const
Definition: SymMat.h:540
SymMat(const ENeg &e)
Definition: SymMat.h:354
T THerm
Definition: SymMat.h:147
const TDiag & getDiag() const
Definition: SymMat.h:818
SymMat< M, ESqrt, 1 > TSqrt
Definition: SymMat.h:159
ScalarNormSq scalarNormSqr() const
Scalar norm square is sum( conjugate squares of all underlying scalars ), where conjugate square of s...
Definition: Vec.h:325
THerm & updTranspose()
Definition: SymMat.h:606
SymMat< M, EHerm, RS > TPosTrans
Definition: SymMat.h:148
Definition: SymMat.h:208
bool isNaN() const
Return true if any element of this SymMat contains a NaN anywhere.
Definition: SymMat.h:735
SymMat & scalarMinusEq(const EE &ee)
Definition: SymMat.h:705
EStandard trace() const
Definition: SymMat.h:203
SymMat & operator+=(const EE &e)
Definition: SymMat.h:694
SymMat(const SymMat< M, E, RSS > &src)
This is an implicit conversion from a SymMat of the same length and element type but with different s...
Definition: SymMat.h:331
static const SymMat & getAs(const ELT *p)
Definition: SymMat.h:725
SymMat & scalarEq(const EE &ee)
Definition: SymMat.h:701
DvdOp::Type Dvd
Definition: SymMat.h:232
Vec & scalarMinusEqFromLeft(const EE &ee)
Definition: Vec.h:787
TSqrt sqrt() const
Definition: SymMat.h:188
TInvert invert() const
Definition: SymMat.h:594
TUpper & updUpper()
Definition: SymMat.h:829
Definition: SymMat.h:123
static const THerm & transpose(const K &t)
Definition: CompositeNumericalTypes.h:216
static int ncol()
Definition: SymMat.h:177
SymMat< M, typename CNT< EE >::template Result< E >::Dvd > scalarDivideFromLeft(const EE &e) const
Definition: SymMat.h:660
SymMat< M, EWithoutNegator, RS > TWithoutNegator
Definition: SymMat.h:140
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
K::TSqrt TSqrt
Definition: CompositeNumericalTypes.h:154
TAbs abs() const
Definition: SymMat.h:195
Definition: SymMat.h:119
static TSqrt sqrt(const K &t)
Definition: CompositeNumericalTypes.h:239
Vec< M, typename CNT< E >::template Result< EE >::Mul > scalarMultiply(const EE &e) const
Definition: Vec.h:722
Definition: SymMat.h:121
static Vec & updAs(ELT *p)
Recast a writable ordinary C++ array E[] to a writable Vec; assumes compatible length...
Definition: Vec.h:906
const TDiag & diag() const
Definition: SymMat.h:822
bool isFinite() const
Return true if no element contains an Infinity or a NaN.
Definition: SymMat.h:742
SymMat & operator-=(const EE &e)
Definition: SymMat.h:695
void setToZero()
Definition: SymMat.h:722
EScalarNormSq ScalarNormSq
Definition: SymMat.h:173
SymMat & scalarDivideEq(const EE &ee)
Definition: SymMat.h:716
Definition: SymMat.h:132
SymMat< M, E, RS > T
Definition: SymMat.h:138
SymMat< M, typename CNT< E >::template Result< EE >::Add > scalarAdd(const EE &e) const
Definition: SymMat.h:668
SymMat< M, EStandard, 1 > TStandard
Definition: SymMat.h:161
SymMat & scalarTimesEq(const EE &ee)
Definition: SymMat.h:712
#define SimTK_ERRCHK1(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:326
ScalarNormSq normSqr() const
Definition: SymMat.h:568
const TWithoutNegator & castAwayNegatorIfAny() const
Definition: SymMat.h:628
TRow row(int i) const
Definition: SymMat.h:781
MulOp::Type Mul
Definition: SymMat.h:221
void setToNaN()
Definition: SymMat.h:721
Result< SymMat< M, E2, RS2 > >::Mul conformingMultiply(const SymMat< M, E2, RS2 > &s) const
Definition: SymMat.h:529
SymMat(const E &e0, const E &e1, const E &e2, const E &e3, const E &e4, const E &e5, const E &e6, const E &e7, const E &e8, const E &e9, const E &e10, const E &e11, const E &e12, const E &e13, const E &e14)
Definition: SymMat.h:402
const TLower & getLower() const
Definition: SymMat.h:825
EStdNumber StdNumber
Definition: SymMat.h:171
EStandard sum() const
Sum just adds up all the elements into a single return element that is the same type as this Vec's el...
Definition: Vec.h:364
const TPosTrans & positionalTranspose() const
Definition: SymMat.h:608
Definition: SymMat.h:246
std::basic_istream< CHAR, TRAITS > & operator>>(std::basic_istream< CHAR, TRAITS > &is, conjugate< R > &c)
Definition: conjugate.h:800
Vec<(M *(M+1))/2, E, RS > TAsVec
Definition: SymMat.h:153
bool isNumericallyEqual(const Vec< M, E2, RS2 > &v, double tol) const
Test whether this vector is numerically equal to some other vector with the same shape, using a specified tolerance.
Definition: Vec.h:954
TPosTrans & updPositionalTranspose()
Definition: SymMat.h:610
This is a small, fixed-size symmetric or Hermitian matrix designed for no-overhead inline computation...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:592
Definition: CompositeNumericalTypes.h:120
static double getDefaultTolerance()
Definition: CompositeNumericalTypes.h:269
SymMat(const SymMat< M, EE, RSS > &src)
Construct a SymMat from a SymMat of the same dimensions, with any element type and spacing...
Definition: SymMat.h:342
SymMat< M, typename CNT< E >::template Result< P >::Sub, 1 > Sub
Definition: SymMat.h:212
TCol operator()(int j) const
Definition: SymMat.h:564
AddCNTs< M, M, ArgDepth, SymMat, ColSpacing, RowSpacing, CNT< P >::NRows, CNT< P >::NCols, CNT< P >::ArgDepth, P, CNT< P >::ColSpacing, CNT< P >::RowSpacing > AddOp
Definition: SymMat.h:236
SymMat(const EE *p)
Definition: SymMat.h:443
SymMat< M, typename CNT< E >::template Result< P >::Add, 1 > Add
Definition: SymMat.h:211
Definition: SymMat.h:115
EltResult< E2 >::Dvd elementwiseDivide(const SymMat< M, E2, RS2 > &r) const
Definition: SymMat.h:548
bool operator==(const PhiMatrix &p1, const PhiMatrix &p2)
Definition: SpatialAlgebra.h:774
bool isNumericallyEqual(const SymMat< M, E2, RS2 > &m) const
Test whether this matrix is numerically equal to some other matrix with the same shape, using a default tolerance which is the looser of the default tolerances of the two objects being compared.
Definition: SymMat.h:759
void setToNaN()
Set every scalar in this Vec to NaN; this is the default initial value in Debug builds, but not in Release.
Definition: Vec.h:810
SymMat & scalarPlusEq(const EE &ee)
Definition: SymMat.h:703
SymMat(const E &e0, const E &e1, const E &e2, const E &e3, const E &e4, const E &e5, const E &e6, const E &e7, const E &e8, const E &e9)
Definition: SymMat.h:392
SymMat & scalarDivideEqFromLeft(const EE &ee)
Definition: SymMat.h:718
static const Vec & getAs(const ELT *p)
Recast an ordinary C++ array E[] to a const Vec; assumes compatible length, stride, and packing.
Definition: Vec.h:902
const TNeg & operator-() const
Definition: SymMat.h:597
static SymMat & updAs(ELT *p)
Definition: SymMat.h:726
Definition: SymMat.h:116
SubOp::Type Sub
Definition: SymMat.h:242
EHerm & updEltUpper(int i, int j)
Definition: SymMat.h:843
Definition: SymMat.h:125
E elt(int i, int j) const
Return a value for any element of a symmetric matrix, even those in the upper triangle which aren't a...
Definition: SymMat.h:810
TNormalize normalize() const
If the elements of this Vec are scalars, the result is what you get by dividing each element by the n...
Definition: Vec.h:621
SymMat(const E &e)
Definition: SymMat.h:347
bool isNumericallySymmetric(double tol=getDefaultTolerance()) const
A Matrix is symmetric (actually Hermitian) if it is square and each element (i,j) is the Hermitian tr...
Definition: Mat.h:1172
CNT< ScalarNormSq >::TSqrt norm() const
Definition: SymMat.h:570
SymMat(const E &e0, const E &e1, const E &e2, const E &e3, const E &e4, const E &e5)
Definition: SymMat.h:384
bool isNumericallyEqual(const SymMat< M, E2, RS2 > &m, double tol) const
Test whether this matrix is numerically equal to some other matrix with the same shape, using a specified tolerance.
Definition: SymMat.h:751
SymMat< M, EImag, RS *CNT< E >::RealStrideFactor > TImag
Definition: SymMat.h:145
SymMat & operator-=(const SymMat< M, EE, RSS > &mm)
Definition: SymMat.h:488
const THerm & transpose() const
Definition: SymMat.h:605
SymMat & operator+=(const SymMat< M, EE, RSS > &mm)
Definition: SymMat.h:477
static int size()
Definition: SymMat.h:175
TImag & imag()
Definition: SymMat.h:622
K::Precision Precision
Definition: CompositeNumericalTypes.h:164
bool isInf() const
Return true if any element of this Vec contains a +Infinity or -Infinity somewhere but no element con...
Definition: Vec.h:925
SymMat< M, typename CNT< E >::template Result< EE >::Mul > scalarMultiply(const EE &e) const
Definition: SymMat.h:639
MulCNTs< M, M, ArgDepth, SymMat, ColSpacing, RowSpacing, CNT< P >::NRows, CNT< P >::NCols, CNT< P >::ArgDepth, P, CNT< P >::ColSpacing, CNT< P >::RowSpacing > MulOp
Definition: SymMat.h:220
Definition: SymMat.h:118
Matrix_< E > operator*(const MatrixBase< E > &l, const typename CNT< E >::StdNumber &r)
Definition: BigMatrix.h:605
const SymMat & operator+() const
Definition: SymMat.h:596
SymMat(const E &e0, const E &e1, const E &e2)
A bevy of constructors from individual exact-match elements IN ROW ORDER, giving the LOWER TRIANGLE...
Definition: SymMat.h:378
TRow colSum() const
Returns a row vector (Row) containing the column sums of this matrix.
Definition: SymMat.h:846
Definition: SymMat.h:120
const EHerm & getEltUpper(int i, int j) const
Definition: SymMat.h:842
Definition: SymMat.h:124
TRow operator[](int i) const
Definition: SymMat.h:563
SymMat< M, typename CNT< E >::template Result< P >::Mul, 1 > Mul
Definition: SymMat.h:209
bool isNaN() const
Return true if any element of this Vec contains a NaN anywhere.
Definition: Vec.h:916
Definition: SymMat.h:122
Vec< M, E, RS > TDiag
Definition: SymMat.h:150
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
bool isFinite() const
Return true if no element of this Vec contains an Infinity or a NaN anywhere.
Definition: Vec.h:940
TStandard standardize() const
Definition: SymMat.h:199
Vec<(M *(M-1))/2, E, RS > TLower
Definition: SymMat.h:151
TLower & updLower()
Definition: SymMat.h:826
SymMat & operator=(const EE *p)
Definition: SymMat.h:455
SymMat< M, EComplex, RS > TComplex
Definition: SymMat.h:146
TRow sum() const
This is an alternate name for colSum(); behaves like the Matlab function of the same name...
Definition: SymMat.h:858
SymMat & scalarMinusEqFromLeft(const EE &ee)
Definition: SymMat.h:709
TDiag & updDiag()
Definition: SymMat.h:819
const TDiag & diag() const
Select main diagonal (of largest leading square if rectangular) and return it as a read-only view (as...
Definition: Mat.h:798
SubCNTs< M, M, ArgDepth, SymMat, ColSpacing, RowSpacing, CNT< P >::NRows, CNT< P >::NCols, CNT< P >::ArgDepth, P, CNT< P >::ColSpacing, CNT< P >::RowSpacing > SubOp
Definition: SymMat.h:241
Definition: SymMat.h:135
SymMat & operator=(const SymMat< M, EE, RSS > &mm)
Definition: SymMat.h:469
E & updEltDiag(int i)
Definition: SymMat.h:835
negator, where N is a number type (real, complex, conjugate), is represented in memory identically...
Definition: String.h:44
Definition: SymMat.h:133
EULessScalar ULessScalar
Definition: SymMat.h:169
This is a fixed-length row vector designed for no-overhead inline computation.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:590
static double getDefaultTolerance()
For approximate comparisions, the default tolerance to use for a matrix is its shortest dimension tim...
Definition: Mat.h:1121
Vec<(M *(M-1))/2, EHerm, RS > TUpper
Definition: SymMat.h:152
SymMat & setFromSymmetric(const Mat< M, M, EE, CSS, RSS > &m)
Create a new SymMat of this type from a square Mat of the right size, that is expected to be symmetri...
Definition: SymMat.h:317
Definition: SymMat.h:131
bool isInf() const
Return true if any element of this SymMat contains a +Inf or -Inf somewhere but no element contains a...
Definition: SymMat.h:739
TDiag & diag()
Definition: SymMat.h:823
bool operator!=(const conjugate< R > &a, const float &b)
Definition: conjugate.h:859
Definition: SymMat.h:117
NTraits< N >::StdNumber StdNumber
Definition: negator.h:107
Row< M, E, 1 > TRow
Definition: SymMat.h:157
Specialized information about Composite Numerical Types which allows us to define appropriate templat...
Definition: CompositeNumericalTypes.h:136
Vec< M, E, 1 > TCol
Definition: SymMat.h:158
static int nrow()
Definition: SymMat.h:176
SymMat & operator=(const SymMat &src)
Copy assignment; no harm if source and this are the same matrix.
Definition: SymMat.h:264
THerm & operator~()
Definition: SymMat.h:600
TReal & real()
Definition: SymMat.h:614
EScalar Scalar
Definition: SymMat.h:168
const TUpper & getUpper() const
Definition: SymMat.h:828
TNeg & updNegate()
Definition: SymMat.h:603
E TElement
Definition: SymMat.h:149
SymMat & operator+=(const SymMat< M, negator< EE >, RSS > &mm)
Definition: SymMat.h:482
SymMat< M, typename CNT< E >::template Result< EE >::Sub > scalarSubtract(const EE &e) const
Definition: SymMat.h:676
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:591
Mandatory first inclusion for any Simbody source or header file.
Vec< M, typename CNT< E >::template Result< EE >::Dvd > scalarDivide(const EE &e) const
Definition: Vec.h:737
const TAsVec & getAsVec() const
Definition: SymMat.h:831
Vec & scalarTimesEqFromLeft(const EE &ee)
Definition: Vec.h:791
Vec< M, typename CNT< EE >::template Result< E >::Mul > scalarMultiplyFromLeft(const EE &e) const
Definition: Vec.h:728
TNormalize normalize() const
There is no conventional meaning for normalize() applied to a matrix.
Definition: SymMat.h:583
E & updEltLower(int i, int j)
Definition: SymMat.h:839
Definition: SymMat.h:217
SymMat(const E &e0, const E &e1, const E &e2, const E &e3, const E &e4, const E &e5, const E &e6, const E &e7, const E &e8, const E &e9, const E &e10, const E &e11, const E &e12, const E &e13, const E &e14, const E &e15, const E &e16, const E &e17, const E &e18, const E &e19, const E &e20)
Definition: SymMat.h:414
#define SimTK_INDEXCHECK(ix, ub, where)
Definition: ExceptionMacros.h:145
TWithoutNegator & updCastAwayNegatorIfAny()
Definition: SymMat.h:629
SymMat< M, EReal, RS *CNT< E >::RealStrideFactor > TReal
Definition: SymMat.h:143
SymMat< M, typename CNT< E >::template Result< EE >::Dvd > scalarDivide(const EE &e) const
Definition: SymMat.h:654
ENumber Number
Definition: SymMat.h:170
const THerm & operator~() const
Definition: SymMat.h:599
AddOp::Type Add
Definition: SymMat.h:237
TCol col(int j) const
Definition: SymMat.h:793
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:589
const TReal & real() const
Return a reference to the real portion of this Vec if it has complex elements; otherwise the type doe...
Definition: Vec.h:679
DvdCNTs< M, M, ArgDepth, SymMat, ColSpacing, RowSpacing, CNT< P >::NRows, CNT< P >::NCols, CNT< P >::ArgDepth, P, CNT< P >::ColSpacing, CNT< P >::RowSpacing > DvdOp
Definition: SymMat.h:231
const E & getEltLower(int i, int j) const
Definition: SymMat.h:838
SymMat & scalarTimesEqFromLeft(const EE &ee)
Definition: SymMat.h:714
SymMat< M, P > Type
Definition: SymMat.h:247
const TNeg & negate() const
Definition: SymMat.h:602
Result< SymMat< M, E2, RS2 > >::Add conformingAdd(const SymMat< M, E2, RS2 > &r) const
Definition: SymMat.h:512
SimTK::conjugate should be instantiated only for float, double, long double.
Definition: String.h:45
SymMat< M, typename CNT< E >::template Result< P >::Dvd, 1 > Dvd
Definition: SymMat.h:210
MulCNTsNonConforming< M, M, ArgDepth, SymMat, ColSpacing, RowSpacing, CNT< P >::NRows, CNT< P >::NCols, CNT< P >::ArgDepth, P, CNT< P >::ColSpacing, CNT< P >::RowSpacing > MulOpNonConforming
Definition: SymMat.h:225
Vec< M, typename CNT< EE >::template Result< E >::Dvd > scalarDivideFromLeft(const EE &e) const
Definition: Vec.h:743
void setToZero()
Set every scalar in this Vec to zero.
Definition: Vec.h:815
SymMat(const Mat< M, M, EE, CSS, RSS > &m)
This is an explicit conversion from square Mat of right size, assuming that the source matrix is symm...
Definition: SymMat.h:280
SymMat(int i)
Definition: SymMat.h:361
SymMat & operator*=(const SymMat< M, EE, RSS > &mm)
Definition: SymMat.h:501
const E & operator()(int i, int j) const
Definition: SymMat.h:556
static TPacked getNaN()
Definition: SymMat.h:729
const TImag & imag() const
Definition: SymMat.h:617
SymMat< M, typename CNT< EE >::template Result< E >::Mul > scalarMultiplyFromLeft(const EE &e) const
Definition: SymMat.h:645
TNeg & operator-()
Definition: SymMat.h:598
MulOpNonConforming::Type MulNon
Definition: SymMat.h:226
SymMat(const SymMat &src)
Copy constructor.
Definition: SymMat.h:259
Result< SymMat< M, E2, RS2 > >::Sub conformingSubtract(const SymMat< M, E2, RS2 > &r) const
Definition: SymMat.h:519
E & operator()(int i, int j)
Definition: SymMat.h:558
SymMat< M, ESqHermT, 1 > TSqHermT
Definition: SymMat.h:164
SymMat(const SymMat< M, ENeg, RSS > &src)
This is an implicit conversion from a SymMat of the same length and negated element type...
Definition: SymMat.h:336
Vec & scalarDivideEqFromLeft(const EE &ee)
Definition: Vec.h:795
const TReal & real() const
Definition: SymMat.h:613
static const TReal & real(const T &t)
Definition: CompositeNumericalTypes.h:203
Definition: SymMat.h:130
Definition: SymMat.h:127
static double getDefaultTolerance()
For approximate comparisions, the default tolerance to use for a matrix is its shortest dimension tim...
Definition: SymMat.h:746
Definition: negator.h:64
SymMat< M, E, 1 > TPacked
Definition: SymMat.h:166
TCol rowSum() const
Returns a column vector (Vec) containing the row sums of this matrix.
Definition: SymMat.h:862
SymMat< M, ENeg, RS > TNeg
Definition: SymMat.h:139
TAsVec & updAsVec()
Definition: SymMat.h:832
SymMat & operator-=(const SymMat< M, negator< EE >, RSS > &mm)
Definition: SymMat.h:493
SymMat & setFromLower(const Mat< M, M, EE, CSS, RSS > &m)
Create a new SymMat of this type from a square Mat of the right size, looking only at lower elements ...
Definition: SymMat.h:287
SymMat & operator*=(const EE &e)
Definition: SymMat.h:696