Simbody  3.5
SymMat.h
Go to the documentation of this file.
00001 #ifndef SimTK_SIMMATRIX_SMALLMATRIX_SYMMAT_H_
00002 #define SimTK_SIMMATRIX_SMALLMATRIX_SYMMAT_H_
00003 
00004 /* -------------------------------------------------------------------------- *
00005  *                       Simbody(tm): SimTKcommon                             *
00006  * -------------------------------------------------------------------------- *
00007  * This is part of the SimTK biosimulation toolkit originating from           *
00008  * Simbios, the NIH National Center for Physics-Based Simulation of           *
00009  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
00010  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
00011  *                                                                            *
00012  * Portions copyright (c) 2005-13 Stanford University and the Authors.        *
00013  * Authors: Michael Sherman                                                   *
00014  * Contributors:                                                              *
00015  *                                                                            *
00016  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
00017  * not use this file except in compliance with the License. You may obtain a  *
00018  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
00019  *                                                                            *
00020  * Unless required by applicable law or agreed to in writing, software        *
00021  * distributed under the License is distributed on an "AS IS" BASIS,          *
00022  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
00023  * See the License for the specific language governing permissions and        *
00024  * limitations under the License.                                             *
00025  * -------------------------------------------------------------------------- */
00026 
00032 #include "SimTKcommon/internal/common.h"
00033 
00034 namespace SimTK {
00035 
00087 template <int M, class ELT, int RS> class SymMat {
00088     typedef ELT                                 E;
00089     typedef typename CNT<E>::TNeg               ENeg;
00090     typedef typename CNT<E>::TWithoutNegator    EWithoutNegator;
00091     typedef typename CNT<E>::TReal              EReal;
00092     typedef typename CNT<E>::TImag              EImag;
00093     typedef typename CNT<E>::TComplex           EComplex;
00094     typedef typename CNT<E>::THerm              EHerm;
00095     typedef typename CNT<E>::TPosTrans          EPosTrans;
00096     typedef typename CNT<E>::TSqHermT           ESqHermT;
00097     typedef typename CNT<E>::TSqTHerm           ESqTHerm;
00098 
00099     typedef typename CNT<E>::TSqrt              ESqrt;
00100     typedef typename CNT<E>::TAbs               EAbs;
00101     typedef typename CNT<E>::TStandard          EStandard;
00102     typedef typename CNT<E>::TInvert            EInvert;
00103     typedef typename CNT<E>::TNormalize         ENormalize;
00104 
00105     typedef typename CNT<E>::Scalar             EScalar;
00106     typedef typename CNT<E>::ULessScalar        EULessScalar;
00107     typedef typename CNT<E>::Number             ENumber;
00108     typedef typename CNT<E>::StdNumber          EStdNumber;
00109     typedef typename CNT<E>::Precision          EPrecision;
00110     typedef typename CNT<E>::ScalarNormSq       EScalarNormSq;
00111 
00112 public:
00113 
00114     enum {
00115         NRows               = M,
00116         NCols               = M,
00117         NDiagElements       = M,
00118         NLowerElements      = (M*(M-1))/2,
00119         NPackedElements     = NDiagElements+NLowerElements,
00120         NActualElements     = RS * NPackedElements,
00121         NActualScalars      = CNT<E>::NActualScalars * NActualElements,
00122         RowSpacing          = RS,
00123         ColSpacing          = NActualElements,
00124         ImagOffset          = NTraits<ENumber>::ImagOffset,
00125         RealStrideFactor    = 1, // composite types don't change size when
00126                                  // cast from complex to real or imaginary
00127         ArgDepth            = ((int)CNT<E>::ArgDepth < (int)MAX_RESOLVED_DEPTH 
00128                                 ? CNT<E>::ArgDepth + 1 
00129                                 : MAX_RESOLVED_DEPTH),
00130         IsScalar            = 0,
00131         IsULessScalar       = 0,
00132         IsNumber            = 0,
00133         IsStdNumber         = 0,
00134         IsPrecision         = 0,
00135         SignInterpretation  = CNT<E>::SignInterpretation
00136     };
00137 
00138     typedef SymMat<M,E,RS>                  T;
00139     typedef SymMat<M,ENeg,RS>               TNeg;
00140     typedef SymMat<M,EWithoutNegator,RS>    TWithoutNegator;
00141 
00142     typedef SymMat<M,EReal,RS*CNT<E>::RealStrideFactor>          
00143                                             TReal;
00144     typedef SymMat<M,EImag,RS*CNT<E>::RealStrideFactor>          
00145                                             TImag;
00146     typedef SymMat<M,EComplex,RS>           TComplex;
00147     typedef T                               THerm;   // These two are opposite of what you might expect
00148     typedef SymMat<M,EHerm,RS>              TPosTrans;
00149     typedef E                               TElement;
00150     typedef Vec<M,E,RS>                     TDiag;   // storage type for the diagonal elements
00151     typedef Vec<(M*(M-1))/2,E,RS>           TLower;  // storage type for the below-diag elements
00152     typedef Vec<(M*(M-1))/2,EHerm,RS>       TUpper;  // cast TLower to this for upper elements
00153     typedef Vec<(M*(M+1))/2,E,RS>           TAsVec;  // the whole SymMat as a single Vec
00154 
00155     // These are the results of calculations, so are returned in new, packed
00156     // memory. Be sure to refer to element types here which are also packed.
00157     typedef Row<M,E,1>                  TRow; // packed since we have to copy
00158     typedef Vec<M,E,1>                  TCol;
00159     typedef SymMat<M,ESqrt,1>           TSqrt;
00160     typedef SymMat<M,EAbs,1>            TAbs;
00161     typedef SymMat<M,EStandard,1>       TStandard;
00162     typedef SymMat<M,EInvert,1>         TInvert;
00163     typedef SymMat<M,ENormalize,1>      TNormalize;
00164     typedef SymMat<M,ESqHermT,1>        TSqHermT; // ~Mat*Mat
00165     typedef SymMat<M,ESqTHerm,1>        TSqTHerm; // Mat*~Mat
00166     typedef SymMat<M,E,1>               TPacked;  // no extra row spacing for new data
00167     
00168     typedef EScalar                     Scalar;
00169     typedef EULessScalar                ULessScalar;
00170     typedef ENumber                     Number;
00171     typedef EStdNumber                  StdNumber;
00172     typedef EPrecision                  Precision;
00173     typedef EScalarNormSq               ScalarNormSq;
00174 
00175     static int size() { return (M*(M+1))/2; }
00176     static int nrow() { return M; }
00177     static int ncol() { return M; }
00178 
00179     // Scalar norm square is sum( squares of all scalars ). The off-diagonals
00180     // come up twice.
00181     ScalarNormSq scalarNormSqr() const { 
00182         return getDiag().scalarNormSqr() + 2.*getLower().scalarNormSqr();
00183     }
00184 
00185     // sqrt() is elementwise square root; that is, the return value has the same
00186     // dimension as this SymMat but with each element replaced by whatever it thinks
00187     // its square root is.
00188     TSqrt sqrt() const { 
00189         return TSqrt(getAsVec().sqrt());
00190     }
00191 
00192     // abs() is elementwise absolute value; that is, the return value has the same
00193     // dimension as this SymMat but with each element replaced by whatever it thinks
00194     // its absolute value is.
00195     TAbs abs() const { 
00196         return TAbs(getAsVec().abs());
00197     }
00198 
00199     TStandard standardize() const {
00200         return TStandard(getAsVec().standardize());
00201     }
00202 
00203     EStandard trace() const {return getDiag().sum();}
00204 
00205     // This gives the resulting SymMat type when (m[i] op P) is applied to each element of m.
00206     // It is a SymMat of dimension M, spacing 1, and element types which are the regular
00207     // composite result of E op P. Typically P is a scalar type but it doesn't have to be.
00208     template <class P> struct EltResult { 
00209         typedef SymMat<M, typename CNT<E>::template Result<P>::Mul, 1> Mul;
00210         typedef SymMat<M, typename CNT<E>::template Result<P>::Dvd, 1> Dvd;
00211         typedef SymMat<M, typename CNT<E>::template Result<P>::Add, 1> Add;
00212         typedef SymMat<M, typename CNT<E>::template Result<P>::Sub, 1> Sub;
00213     };
00214 
00215     // This is the composite result for m op P where P is some kind of appropriately shaped
00216     // non-scalar type.
00217     template <class P> struct Result { 
00218         typedef MulCNTs<M,M,ArgDepth,SymMat,ColSpacing,RowSpacing,
00219             CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
00220             P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOp;
00221         typedef typename MulOp::Type Mul;
00222 
00223         typedef MulCNTsNonConforming<M,M,ArgDepth,SymMat,ColSpacing,RowSpacing,
00224             CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
00225             P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOpNonConforming;
00226         typedef typename MulOpNonConforming::Type MulNon;
00227 
00228 
00229         typedef DvdCNTs<M,M,ArgDepth,SymMat,ColSpacing,RowSpacing,
00230             CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
00231             P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> DvdOp;
00232         typedef typename DvdOp::Type Dvd;
00233 
00234         typedef AddCNTs<M,M,ArgDepth,SymMat,ColSpacing,RowSpacing,
00235             CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
00236             P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> AddOp;
00237         typedef typename AddOp::Type Add;
00238 
00239         typedef SubCNTs<M,M,ArgDepth,SymMat,ColSpacing,RowSpacing,
00240             CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
00241             P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> SubOp;
00242         typedef typename SubOp::Type Sub;
00243     };
00244 
00245     // Shape-preserving element substitution (always packed)
00246     template <class P> struct Substitute {
00247         typedef SymMat<M,P> Type;
00248     };
00249 
00252     SymMat(){ 
00253     #ifndef NDEBUG
00254         setToNaN();
00255     #endif
00256     }
00257 
00259     SymMat(const SymMat& src) {
00260         updAsVec() = src.getAsVec();
00261     }
00262 
00264     SymMat& operator=(const SymMat& src) {
00265         updAsVec() = src.getAsVec();
00266         return *this;
00267     }
00268 
00279     template <class EE, int CSS, int RSS>
00280     explicit SymMat(const Mat<M,M,EE,CSS,RSS>& m)
00281     {   setFromSymmetric(m); }
00282 
00286     template <class EE, int CSS, int RSS>
00287     SymMat& setFromLower(const Mat<M,M,EE,CSS,RSS>& m) {
00288         this->updDiag() = m.diag().real();
00289         for (int j=0; j<M; ++j)
00290             for (int i=j+1; i<M; ++i)
00291                 this->updEltLower(i,j) = m(i,j);
00292         return *this;
00293     }
00294 
00302     template <class EE, int CSS, int RSS>
00303     SymMat& setFromUpper(const Mat<M,M,EE,CSS,RSS>& m) {
00304         this->updDiag() = m.diag().real();
00305         for (int j=0; j<M; ++j)
00306             for (int i=j+1; i<M; ++i)
00307                 this->updEltLower(i,j) = m(j,i);
00308         return *this;
00309     }
00310 
00316     template <class EE, int CSS, int RSS>
00317     SymMat& setFromSymmetric(const Mat<M,M,EE,CSS,RSS>& m) {
00318         SimTK_ERRCHK1(m.isNumericallySymmetric(), "SymMat::setFromSymmetric()",
00319             "The allegedly symmetric source matrix was not symmetric to within "
00320             "a tolerance of %g.", m.getDefaultTolerance());
00321         this->updDiag() = m.diag().real();
00322         for (int j=0; j<M; ++j)
00323             for (int i=j+1; i<M; ++i)
00324                 this->updEltLower(i,j) = 
00325                     (m(i,j) + CNT<EE>::transpose(m(j,i)))/2;
00326         return *this;
00327     }
00328 
00331     template <int RSS> SymMat(const SymMat<M,E,RSS>& src) 
00332       { updAsVec() = src.getAsVec(); }
00333 
00336     template <int RSS> SymMat(const SymMat<M,ENeg,RSS>& src)
00337       { updAsVec() = src.getAsVec(); }
00338 
00342     template <class EE, int RSS> explicit SymMat(const SymMat<M,EE,RSS>& src)
00343       { updAsVec() = src.getAsVec(); }
00344 
00345     // Construction using an element repeats that element on the diagonal
00346     // but sets the rest of the matrix to zero.
00347     explicit SymMat(const E& e) {
00348         updDiag() = CNT<E>::real(e); 
00349         for (int i=0; i < NLowerElements; ++i) updlowerE(i) = E(0); 
00350     }
00351 
00352     // Construction using a negated element is just like construction from
00353     // the element.
00354     explicit SymMat(const ENeg& e) {
00355         updDiag() = CNT<ENeg>::real(e); 
00356         for (int i=0; i < NLowerElements; ++i) updlowerE(i) = E(0); 
00357     }
00358 
00359     // Given an int, turn it into a suitable floating point number
00360     // and then feed that to the above single-element constructor.
00361     explicit SymMat(int i) 
00362       { new (this) SymMat(E(Precision(i))); }
00363 
00377 
00378     SymMat(const E& e0,
00379            const E& e1,const E& e2)
00380     {   assert(M==2); TDiag& d=updDiag(); TLower& l=updLower();
00381         d[0]=CNT<E>::real(e0); d[1]=CNT<E>::real(e2); 
00382         l[0]=e1; }
00383 
00384     SymMat(const E& e0,
00385            const E& e1,const E& e2,
00386            const E& e3,const E& e4, const E& e5)
00387     {   assert(M==3); TDiag& d=updDiag(); TLower& l=updLower();
00388         d[0]=CNT<E>::real(e0);d[1]=CNT<E>::real(e2);d[2]=CNT<E>::real(e5); 
00389         l[0]=e1;l[1]=e3;
00390         l[2]=e4; }
00391 
00392     SymMat(const E& e0,
00393            const E& e1,const E& e2,
00394            const E& e3,const E& e4, const E& e5,
00395            const E& e6,const E& e7, const E& e8, const E& e9)
00396     {   assert(M==4); TDiag& d=updDiag(); TLower& l=updLower();
00397         d[0]=CNT<E>::real(e0);d[1]=CNT<E>::real(e2);d[2]=CNT<E>::real(e5);d[3]=CNT<E>::real(e9); 
00398         l[0]=e1;l[1]=e3;l[2]=e6;
00399         l[3]=e4;l[4]=e7;
00400         l[5]=e8; }
00401 
00402     SymMat(const E& e0,
00403            const E& e1, const E& e2,
00404            const E& e3, const E& e4,  const E& e5,
00405            const E& e6, const E& e7,  const E& e8,  const E& e9,
00406            const E& e10,const E& e11, const E& e12, const E& e13, const E& e14)
00407     {   assert(M==5); TDiag& d=updDiag(); TLower& l=updLower();
00408         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); 
00409         l[0]=e1;l[1]=e3;l[2]=e6;l[3]=e10;
00410         l[4]=e4;l[5]=e7;l[6]=e11;
00411         l[7]=e8;l[8]=e12;
00412         l[9]=e13; }
00413 
00414     SymMat(const E& e0,
00415            const E& e1, const E& e2,
00416            const E& e3, const E& e4,  const E& e5,
00417            const E& e6, const E& e7,  const E& e8,  const E& e9,
00418            const E& e10,const E& e11, const E& e12, const E& e13, const E& e14,
00419            const E& e15,const E& e16, const E& e17, const E& e18, const E& e19, const E& e20)
00420     {   assert(M==6); TDiag& d=updDiag(); TLower& l=updLower();
00421         d[0]=CNT<E>::real(e0);d[1]=CNT<E>::real(e2);d[2]=CNT<E>::real(e5);
00422         d[3]=CNT<E>::real(e9);d[4]=CNT<E>::real(e14);d[5]=CNT<E>::real(e20); 
00423         l[0] =e1; l[1] =e3; l[2] =e6;  l[3]=e10; l[4]=e15;
00424         l[5] =e4; l[6] =e7; l[7] =e11; l[8]=e16;
00425         l[9] =e8; l[10]=e12;l[11]=e17;
00426         l[12]=e13;l[13]=e18;
00427         l[14]=e19; }
00428 
00429     // Construction from a pointer to anything assumes we're pointing
00430     // at a packed element list of the right length, providing the
00431     // lower triangle in row order, so a b c d e f means
00432     //      a
00433     //      b c
00434     //      d e f
00435     //      g h i j
00436     // This has to be mapped to our diagonal/lower layout, which in
00437     // the above example will be:
00438     //      [a c f j][b d g e h i]
00439     //
00440     // In the input layout, the i'th row begins at element i(i+1)/2,
00441     // so diagonals are at i(i+1)/2 + i, while lower
00442     // elements (i,j; i>j) are at i(i+1)/2 + j.
00443     template <class EE> explicit SymMat(const EE* p) {
00444         assert(p);
00445         for (int i=0; i<M; ++i) {
00446             const int rowStart = (i*(i+1))/2;
00447             updDiag()[i] = CNT<EE>::real(p[rowStart + i]);
00448             for (int j=0; j<i; ++j)
00449                 updEltLower(i,j) = p[rowStart + j];
00450         }
00451     }
00452 
00453     // This is the same thing except as an assignment to pointer rather
00454     // than a constructor from pointer.
00455     template <class EE> SymMat& operator=(const EE* p) {
00456         assert(p);
00457         for (int i=0; i<M; ++i) {
00458             const int rowStart = (i*(i+1))/2;
00459             updDiag()[i] = CNT<EE>::real(p[rowStart + i]);
00460             for (int j=0; j<i; ++j)
00461                 updEltLower(i,j) = p[rowStart + j];
00462         }
00463         return *this;
00464     }
00465 
00466     // Assignment works similarly to copy -- if the lengths match,
00467     // go element-by-element. Otherwise, zero and then copy to each 
00468     // diagonal element.
00469     template <class EE, int RSS> SymMat& operator=(const SymMat<M,EE,RSS>& mm) {
00470         updAsVec() = mm.getAsVec();
00471         return *this;
00472     }
00473 
00474 
00475     // Conforming assignment ops
00476     template <class EE, int RSS> SymMat& 
00477     operator+=(const SymMat<M,EE,RSS>& mm) {
00478         updAsVec() += mm.getAsVec();
00479         return *this;
00480     }
00481     template <class EE, int RSS> SymMat&
00482     operator+=(const SymMat<M,negator<EE>,RSS>& mm) {
00483         updAsVec() -= -mm.getAsVec();   // negation of negator is free
00484         return *this;
00485     }
00486 
00487     template <class EE, int RSS> SymMat&
00488     operator-=(const SymMat<M,EE,RSS>& mm) {
00489         updAsVec() -= mm.getAsVec();
00490         return *this;
00491     }
00492     template <class EE, int RSS> SymMat&
00493     operator-=(const SymMat<M,negator<EE>,RSS>& mm) {
00494         updAsVec() += -mm.getAsVec();   // negation of negator is free
00495         return *this;
00496     }
00497 
00498     // In place matrix multiply can only be done when the RHS matrix is the same
00499     // size and the elements are also *= compatible.
00500     template <class EE, int RSS> SymMat&
00501     operator*=(const SymMat<M,EE,RSS>& mm) {
00502         assert(false); // TODO
00503         return *this;
00504     }
00505 
00506     // Conforming binary ops with 'this' on left, producing new packed result.
00507     // Cases: sy=sy+-sy, m=sy+-m, m=sy*sy, m=sy*m, v=sy*v
00508 
00509     // sy= this + sy
00510     template <class E2, int RS2> 
00511     typename Result<SymMat<M,E2,RS2> >::Add
00512     conformingAdd(const SymMat<M,E2,RS2>& r) const {
00513         return typename Result<SymMat<M,E2,RS2> >::Add
00514             (getAsVec().conformingAdd(r.getAsVec()));
00515     }
00516     // m= this - m
00517     template <class E2, int RS2> 
00518     typename Result<SymMat<M,E2,RS2> >::Sub
00519     conformingSubtract(const SymMat<M,E2,RS2>& r) const {
00520         return typename Result<SymMat<M,E2,RS2> >::Sub
00521             (getAsVec().conformingSubtract(r.getAsVec()));
00522     }
00523 
00524     // symmetric * symmetric produces a full result
00525     // m= this * s
00526     // TODO: this is not a good implementation
00527     template <class E2, int RS2>
00528     typename Result<SymMat<M,E2,RS2> >::Mul
00529     conformingMultiply(const SymMat<M,E2,RS2>& s) const {
00530         typename Result<SymMat<M,E2,RS2> >::Mul result;
00531         for (int j=0;j<M;++j)
00532             for (int i=0;i<M;++i)
00533                 result(i,j) = (*this)[i] * s(j);
00534         return result;
00535     }
00536 
00537     // sy= this .* sy
00538     template <class E2, int RS2> 
00539     typename EltResult<E2>::Mul
00540     elementwiseMultiply(const SymMat<M,E2,RS2>& r) const {
00541         return typename EltResult<E2>::Mul
00542             (getAsVec().elementwiseMultiply(r.getAsVec()));
00543     }
00544 
00545     // sy= this ./ sy
00546     template <class E2, int RS2> 
00547     typename EltResult<E2>::Dvd
00548     elementwiseDivide(const SymMat<M,E2,RS2>& r) const {
00549         return typename EltResult<E2>::Dvd
00550             (getAsVec().elementwiseDivide(r.getAsVec()));
00551     }
00552 
00553     // TODO: need the rest of the SymMat operators
00554     
00555     // Must be i >= j.
00556     const E& operator()(int i,int j) const 
00557       { return i==j ? getDiag()[i] : getEltLower(i,j); }
00558     E& operator()(int i,int j)
00559       { return i==j ? updDiag()[i] : updEltLower(i,j); }
00560 
00561     // These are slow for a symmetric matrix, requiring copying and
00562     // possibly floating point operations for conjugation.
00563     TRow operator[](int i) const {return row(i);}
00564     TCol operator()(int j) const {return col(j);}
00565 
00566 
00567     // This is the scalar Frobenius norm.
00568     ScalarNormSq normSqr() const { return scalarNormSqr(); }
00569     typename CNT<ScalarNormSq>::TSqrt 
00570         norm() const { return CNT<ScalarNormSq>::sqrt(scalarNormSqr()); }
00571 
00583     TNormalize normalize() const {
00584         if (CNT<E>::IsScalar) {
00585             return castAwayNegatorIfAny() / (SignInterpretation*norm());
00586         } else {
00587             TNormalize elementwiseNormalized;
00588             // punt to the equivalent Vec to get the elements normalized
00589             elementwiseNormalized.updAsVec() = getAsVec().normalize();
00590             return elementwiseNormalized;
00591         }
00592     }
00593 
00594     TInvert invert() const {assert(false); return TInvert();} // TODO default inversion
00595 
00596     const SymMat& operator+() const { return *this; }
00597     const TNeg&   operator-() const { return negate(); }
00598     TNeg&         operator-()       { return updNegate(); }
00599     const THerm&  operator~() const { return transpose(); }
00600     THerm&        operator~()       { return updTranspose(); }
00601 
00602     const TNeg&  negate() const { return *reinterpret_cast<const TNeg*>(this); }
00603     TNeg&        updNegate()    { return *reinterpret_cast<TNeg*>(this); }
00604 
00605     const THerm& transpose()    const { return *reinterpret_cast<const THerm*>(this); }
00606     THerm&       updTranspose()       { return *reinterpret_cast<THerm*>(this); }
00607 
00608     const TPosTrans& positionalTranspose() const
00609         { return *reinterpret_cast<const TPosTrans*>(this); }
00610     TPosTrans&       updPositionalTranspose()
00611         { return *reinterpret_cast<TPosTrans*>(this); }
00612 
00613     const TReal& real() const { return *reinterpret_cast<const TReal*>(this); }
00614     TReal&       real()       { return *reinterpret_cast<      TReal*>(this); }
00615 
00616     // Had to contort these routines to get them through VC++ 7.net
00617     const TImag& imag()    const { 
00618         const int offs = ImagOffset;
00619         const EImag* p = reinterpret_cast<const EImag*>(this);
00620         return *reinterpret_cast<const TImag*>(p+offs);
00621     }
00622     TImag& imag() { 
00623         const int offs = ImagOffset;
00624         EImag* p = reinterpret_cast<EImag*>(this);
00625         return *reinterpret_cast<TImag*>(p+offs);
00626     }
00627 
00628     const TWithoutNegator& castAwayNegatorIfAny() const {return *reinterpret_cast<const TWithoutNegator*>(this);}
00629     TWithoutNegator&       updCastAwayNegatorIfAny()    {return *reinterpret_cast<TWithoutNegator*>(this);}
00630 
00631     // These are elementwise binary operators, (this op ee) by default but (ee op this) if
00632     // 'FromLeft' appears in the name. The result is a packed SymMat<M> but the element type
00633     // may change. These are mostly used to implement global operators.
00634     // We call these "scalar" operators but actually the "scalar" can be a composite type.
00635 
00636     //TODO: consider converting 'e' to Standard Numbers as precalculation and changing
00637     // return type appropriately.
00638     template <class EE> SymMat<M, typename CNT<E>::template Result<EE>::Mul>
00639     scalarMultiply(const EE& e) const {
00640         SymMat<M, typename CNT<E>::template Result<EE>::Mul> result;
00641         result.updAsVec() = getAsVec().scalarMultiply(e);
00642         return result;
00643     }
00644     template <class EE> SymMat<M, typename CNT<EE>::template Result<E>::Mul>
00645     scalarMultiplyFromLeft(const EE& e) const {
00646         SymMat<M, typename CNT<EE>::template Result<E>::Mul> result;
00647         result.updAsVec() = getAsVec().scalarMultiplyFromLeft(e);
00648         return result;
00649     }
00650 
00651     // TODO: should precalculate and store 1/e, while converting to Standard Numbers. Note
00652     // that return type should change appropriately.
00653     template <class EE> SymMat<M, typename CNT<E>::template Result<EE>::Dvd>
00654     scalarDivide(const EE& e) const {
00655         SymMat<M, typename CNT<E>::template Result<EE>::Dvd> result;
00656         result.updAsVec() = getAsVec().scalarDivide(e);
00657         return result;
00658     }
00659     template <class EE> SymMat<M, typename CNT<EE>::template Result<E>::Dvd>
00660     scalarDivideFromLeft(const EE& e) const {
00661         SymMat<M, typename CNT<EE>::template Result<E>::Dvd> result;
00662         result.updAsVec() = getAsVec().scalarDivideFromLeft(e);
00663         return result;
00664     }
00665 
00666     // Additive ops involving a scalar update only the diagonal
00667     template <class EE> SymMat<M, typename CNT<E>::template Result<EE>::Add>
00668     scalarAdd(const EE& e) const {
00669         SymMat<M, typename CNT<E>::template Result<EE>::Add> result(*this);
00670         result.updDiag() += e;
00671         return result;
00672     }
00673     // Add is commutative, so no 'FromLeft'.
00674 
00675     template <class EE> SymMat<M, typename CNT<E>::template Result<EE>::Sub>
00676     scalarSubtract(const EE& e) const {
00677         SymMat<M, typename CNT<E>::template Result<EE>::Sub> result(*this);
00678         result.diag() -= e;
00679         return result;
00680     }
00681     // This is s-m; negate m and add s to diagonal
00682     // TODO: Should do something clever with negation here.
00683     template <class EE> SymMat<M, typename CNT<EE>::template Result<E>::Sub>
00684     scalarSubtractFromLeft(const EE& e) const {
00685         SymMat<M, typename CNT<EE>::template Result<E>::Sub> result(-(*this));
00686         result.diag() += e;
00687         return result;
00688     }
00689 
00690     // Generic assignments for any element type not listed explicitly, including scalars.
00691     // These are done repeatedly for each element and only work if the operation can
00692     // be performed leaving the original element type.
00693     template <class EE> SymMat& operator =(const EE& e) {return scalarEq(e);}
00694     template <class EE> SymMat& operator+=(const EE& e) {return scalarPlusEq(e);}
00695     template <class EE> SymMat& operator-=(const EE& e) {return scalarMinusEq(e);}
00696     template <class EE> SymMat& operator*=(const EE& e) {return scalarTimesEq(e);}
00697     template <class EE> SymMat& operator/=(const EE& e) {return scalarDivideEq(e);}
00698 
00699     // Generalized scalar assignment & computed assignment methods. These will work
00700     // for any assignment-compatible element, not just scalars.
00701     template <class EE> SymMat& scalarEq(const EE& ee)
00702       { updDiag() = ee; updLower() = E(0); return *this; }
00703     template <class EE> SymMat& scalarPlusEq(const EE& ee)
00704       { updDiag() += ee; return *this; }
00705     template <class EE> SymMat& scalarMinusEq(const EE& ee)
00706       { updDiag() -= ee; return *this; }
00707 
00708     // this is m = s-m; negate off diagonal, do d=s-d for each diagonal element d
00709     template <class EE> SymMat& scalarMinusEqFromLeft(const EE& ee)
00710       { updLower() *= E(-1); updDiag().scalarMinusEqFromLeft(ee); return *this; }
00711 
00712     template <class EE> SymMat& scalarTimesEq(const EE& ee)
00713       { updAsVec() *= ee; return *this; }
00714     template <class EE> SymMat& scalarTimesEqFromLeft(const EE& ee)
00715       { updAsVec().scalarTimesEqFromLeft(ee); return *this; }
00716     template <class EE> SymMat& scalarDivideEq(const EE& ee)
00717       { updAsVec() /= ee; return *this; }
00718     template <class EE> SymMat& scalarDivideEqFromLeft(const EE& ee)
00719       { updAsVec().scalarDivideEqFromLeft(ee); return *this; } 
00720 
00721     void setToNaN()  { updAsVec().setToNaN();  }
00722     void setToZero() { updAsVec().setToZero(); }
00723 
00724     // These assume we are given a pointer to d[0] of a SymMat<M,E,RS> like this one.
00725     static const SymMat& getAs(const ELT* p)  {return *reinterpret_cast<const SymMat*>(p);}
00726     static SymMat&       updAs(ELT* p)        {return *reinterpret_cast<SymMat*>(p);}
00727 
00728     // Note packed spacing
00729     static TPacked getNaN() {
00730         return TPacked(CNT<typename TPacked::TDiag>::getNaN(),
00731                        CNT<typename TPacked::TLower>::getNaN());
00732     }
00733 
00735     bool isNaN() const {return getAsVec().isNaN();}
00736 
00739     bool isInf() const {return getAsVec().isInf();}
00740 
00742     bool isFinite() const {return getAsVec().isFinite();}
00743 
00746     static double getDefaultTolerance() {return M*CNT<ELT>::getDefaultTolerance();}
00747 
00750     template <class E2, int RS2>
00751     bool isNumericallyEqual(const SymMat<M,E2,RS2>& m, double tol) const {
00752         return getAsVec().isNumericallyEqual(m.getAsVec(), tol);
00753     }
00754 
00758     template <class E2, int RS2>
00759     bool isNumericallyEqual(const SymMat<M,E2,RS2>& m) const {
00760         const double tol = std::max(getDefaultTolerance(),m.getDefaultTolerance());
00761         return isNumericallyEqual(m, tol);
00762     }
00763 
00769     bool isNumericallyEqual
00770        (const ELT& e,
00771         double     tol = getDefaultTolerance()) const 
00772     {
00773         if (!diag().isNumericallyEqual(e, tol))
00774             return false;
00775         return getLower().isNumericallyEqual(ELT(0), tol);
00776     }
00777 
00778     // Rows and columns have to be copied and Hermitian elements have to
00779     // be conjugated at a floating point cost. This isn't the best way
00780     // to work with a symmetric matrix.
00781     TRow row(int i) const {
00782         SimTK_INDEXCHECK(i,M,"SymMat::row[i]");
00783         TRow rowi;
00784         // Columns left of diagonal are lower.
00785         for (int j=0; j<i; ++j)
00786             rowi[j] = getEltLower(i,j);
00787         rowi[i] = getEltDiag(i);
00788         for (int j=i+1; j<M; ++j)
00789             rowi[j] = getEltUpper(i,j); // conversion from EHerm to E may cost flops
00790         return rowi;
00791     }
00792 
00793     TCol col(int j) const {
00794         SimTK_INDEXCHECK(j,M,"SymMat::col(j)");
00795         TCol colj;
00796         // Rows above diagonal are upper (with conjugated elements).
00797         for (int i=0; i<j; ++i)
00798             colj[i] = getEltUpper(i,j); // conversion from EHerm to E may cost flops
00799         colj[j] = getEltDiag(j);
00800         for (int i=j+1; i<M; ++i)
00801             colj[i] = getEltLower(i,j);
00802         return colj;
00803     }
00804 
00810     E elt(int i, int j) const {
00811         SimTK_INDEXCHECK(i,M,"SymMat::elt(i,j)");
00812         SimTK_INDEXCHECK(j,M,"SymMat::elt(i,j)");
00813         if      (i>j)  return getEltLower(i,j); // copy element
00814         else if (i==j) return getEltDiag(i);    // copy element
00815         else           return getEltUpper(i,j); // conversion from EHerm to E may cost flops 
00816     }
00817 
00818     const TDiag&  getDiag()  const {return TDiag::getAs(d);}
00819     TDiag&        updDiag()        {return TDiag::updAs(d);}
00820 
00821     // Conventional synonym
00822     const TDiag& diag() const {return getDiag();}
00823     TDiag&       diag()       {return updDiag();}
00824 
00825     const TLower& getLower() const {return TLower::getAs(&d[M*RS]);}
00826     TLower&       updLower()       {return TLower::updAs(&d[M*RS]);}
00827 
00828     const TUpper& getUpper() const {return reinterpret_cast<const TUpper&>(getLower());}
00829     TUpper&       updUpper()       {return reinterpret_cast<      TUpper&>(updLower());}
00830 
00831     const TAsVec& getAsVec() const {return TAsVec::getAs(d);}
00832     TAsVec&       updAsVec()       {return TAsVec::updAs(d);}
00833 
00834     const E& getEltDiag(int i) const {return getDiag()[i];}
00835     E&       updEltDiag(int i)       {return updDiag()[i];}
00836 
00837     // must be i > j
00838     const E& getEltLower(int i, int j) const {return getLower()[lowerIx(i,j)];}
00839     E&       updEltLower(int i, int j)       {return updLower()[lowerIx(i,j)];}
00840 
00841     // must be i < j
00842     const EHerm& getEltUpper(int i, int j) const {return getUpper()[lowerIx(j,i)];}
00843     EHerm&       updEltUpper(int i, int j)       {return updUpper()[lowerIx(j,i)];}
00844     
00846     TRow colSum() const {
00847         TRow temp(~getDiag());
00848         for (int i = 1; i < M; ++i)
00849             for (int j = 0; j < i; ++j) {
00850                 const E& value = getEltLower(i, j);;
00851                 temp[j] += value;
00852                 temp[i] += E(reinterpret_cast<const EHerm&>(value));
00853             }
00854         return temp;
00855     }
00858     TRow sum() const {return colSum();}
00859 
00862     TCol rowSum() const {
00863         TCol temp(getDiag());
00864         for (int i = 1; i < M; ++i)
00865             for (int j = 0; j < i; ++j) {
00866                 const E& value = getEltLower(i, j);;
00867                 temp[i] += value;
00868                 temp[j] += E(reinterpret_cast<const EHerm&>(value));
00869             }
00870         return temp;
00871     }
00872 private:
00873     E d[NActualElements];
00874 
00875     // This utility doesn't turn lower or upper into a Vec which could turn
00876     // out to have zero length if this is a 1x1 matrix.
00877     const E& getlowerE(int i) const {return d[(M+i)*RS];}
00878     E& updlowerE(int i) {return d[(M+i)*RS];}
00879 
00880     SymMat(const TDiag& di, const TLower& low) {
00881         updDiag() = di; updLower() = low;
00882     }
00883 
00884     explicit SymMat(const TAsVec& v) {
00885         TAsVec::updAs(d) = v;
00886     }
00887     
00888     // Convert i,j with i>j to an index in "lower". This also gives the
00889     // right index for "upper" with i and j reversed.
00890     static int lowerIx(int i, int j) {
00891         assert(0 <= j && j < i && i < M);
00892         return (i-j-1) + j*(M-1) - (j*(j-1))/2;
00893     }
00894 
00895     template <int MM, class EE, int RSS> friend class SymMat;
00896 };
00897 
00899 // Global operators involving two symmetric matrices. //
00900 //   sy=sy+sy, sy=sy-sy, m=sy*sy, sy==sy, sy!=sy      //
00902 
00903 // m3 = m1 + m2 where all m's have the same dimension M. 
00904 template <int M, class E1, int S1, class E2, int S2> inline
00905 typename SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >::Add
00906 operator+(const SymMat<M,E1,S1>& l, const SymMat<M,E2,S2>& r) {
00907     return SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >
00908         ::AddOp::perform(l,r);
00909 }
00910 
00911 // m3 = m1 - m2 where all m's have the same dimension M. 
00912 template <int M, class E1, int S1, class E2, int S2> inline
00913 typename SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >::Sub
00914 operator-(const SymMat<M,E1,S1>& l, const SymMat<M,E2,S2>& r) {
00915     return SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >
00916         ::SubOp::perform(l,r);
00917 }
00918 
00919 // m3 = m1 * m2 where all m's have the same dimension M. 
00920 // The result will not be symmetric.
00921 template <int M, class E1, int S1, class E2, int S2> inline
00922 typename SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >::Mul
00923 operator*(const SymMat<M,E1,S1>& l, const SymMat<M,E2,S2>& r) {
00924     return SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >
00925         ::MulOp::perform(l,r);
00926 }
00927 
00928 // bool = m1 == m2, m1 and m2 have the same dimension M
00929 template <int M, class E1, int S1, class E2, int S2> inline bool
00930 operator==(const SymMat<M,E1,S1>& l, const SymMat<M,E2,S2>& r) {
00931     return l.getAsVec() == r.getAsVec();
00932 }
00933 
00934 // bool = m1 == m2, m1 and m2 have the same dimension M
00935 template <int M, class E1, int S1, class E2, int S2> inline bool
00936 operator!=(const SymMat<M,E1,S1>& l, const SymMat<M,E2,S2>& r) {return !(l==r);} 
00937 
00939 // Global operators involving a SymMat and a scalar. //
00941 
00942 // SCALAR MULTIPLY
00943 
00944 // m = m*real, real*m 
00945 template <int M, class E, int S> inline
00946 typename SymMat<M,E,S>::template Result<float>::Mul
00947 operator*(const SymMat<M,E,S>& l, const float& r)
00948   { return SymMat<M,E,S>::template Result<float>::MulOp::perform(l,r); }
00949 template <int M, class E, int S> inline
00950 typename SymMat<M,E,S>::template Result<float>::Mul
00951 operator*(const float& l, const SymMat<M,E,S>& r) {return r*l;}
00952 
00953 template <int M, class E, int S> inline
00954 typename SymMat<M,E,S>::template Result<double>::Mul
00955 operator*(const SymMat<M,E,S>& l, const double& r)
00956   { return SymMat<M,E,S>::template Result<double>::MulOp::perform(l,r); }
00957 template <int M, class E, int S> inline
00958 typename SymMat<M,E,S>::template Result<double>::Mul
00959 operator*(const double& l, const SymMat<M,E,S>& r) {return r*l;}
00960 
00961 template <int M, class E, int S> inline
00962 typename SymMat<M,E,S>::template Result<long double>::Mul
00963 operator*(const SymMat<M,E,S>& l, const long double& r)
00964   { return SymMat<M,E,S>::template Result<long double>::MulOp::perform(l,r); }
00965 template <int M, class E, int S> inline
00966 typename SymMat<M,E,S>::template Result<long double>::Mul
00967 operator*(const long double& l, const SymMat<M,E,S>& r) {return r*l;}
00968 
00969 // m = m*int, int*m -- just convert int to m's precision float
00970 template <int M, class E, int S> inline
00971 typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Mul
00972 operator*(const SymMat<M,E,S>& l, int r) {return l * (typename CNT<E>::Precision)r;}
00973 template <int M, class E, int S> inline
00974 typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Mul
00975 operator*(int l, const SymMat<M,E,S>& r) {return r * (typename CNT<E>::Precision)l;}
00976 
00977 // Complex, conjugate, and negator are all easy to templatize.
00978 
00979 // m = m*complex, complex*m
00980 template <int M, class E, int S, class R> inline
00981 typename SymMat<M,E,S>::template Result<std::complex<R> >::Mul
00982 operator*(const SymMat<M,E,S>& l, const std::complex<R>& r)
00983   { return SymMat<M,E,S>::template Result<std::complex<R> >::MulOp::perform(l,r); }
00984 template <int M, class E, int S, class R> inline
00985 typename SymMat<M,E,S>::template Result<std::complex<R> >::Mul
00986 operator*(const std::complex<R>& l, const SymMat<M,E,S>& r) {return r*l;}
00987 
00988 // m = m*conjugate, conjugate*m (convert conjugate->complex)
00989 template <int M, class E, int S, class R> inline
00990 typename SymMat<M,E,S>::template Result<std::complex<R> >::Mul
00991 operator*(const SymMat<M,E,S>& l, const conjugate<R>& r) {return l*(std::complex<R>)r;}
00992 template <int M, class E, int S, class R> inline
00993 typename SymMat<M,E,S>::template Result<std::complex<R> >::Mul
00994 operator*(const conjugate<R>& l, const SymMat<M,E,S>& r) {return r*(std::complex<R>)l;}
00995 
00996 // m = m*negator, negator*m: convert negator to standard number
00997 template <int M, class E, int S, class R> inline
00998 typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Mul
00999 operator*(const SymMat<M,E,S>& l, const negator<R>& r) {return l * (typename negator<R>::StdNumber)(R)r;}
01000 template <int M, class E, int S, class R> inline
01001 typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Mul
01002 operator*(const negator<R>& l, const SymMat<M,E,S>& r) {return r * (typename negator<R>::StdNumber)(R)l;}
01003 
01004 
01005 // SCALAR DIVIDE. This is a scalar operation when the scalar is on the right,
01006 // but when it is on the left it means scalar * pseudoInverse(mat), which is 
01007 // a similar symmetric matrix.
01008 
01009 // m = m/real, real/m 
01010 template <int M, class E, int S> inline
01011 typename SymMat<M,E,S>::template Result<float>::Dvd
01012 operator/(const SymMat<M,E,S>& l, const float& r)
01013   { return SymMat<M,E,S>::template Result<float>::DvdOp::perform(l,r); }
01014 template <int M, class E, int S> inline
01015 typename CNT<float>::template Result<SymMat<M,E,S> >::Dvd
01016 operator/(const float& l, const SymMat<M,E,S>& r)
01017   { return CNT<float>::template Result<SymMat<M,E,S> >::DvdOp::perform(l,r); }
01018 
01019 template <int M, class E, int S> inline
01020 typename SymMat<M,E,S>::template Result<double>::Dvd
01021 operator/(const SymMat<M,E,S>& l, const double& r)
01022   { return SymMat<M,E,S>::template Result<double>::DvdOp::perform(l,r); }
01023 template <int M, class E, int S> inline
01024 typename CNT<double>::template Result<SymMat<M,E,S> >::Dvd
01025 operator/(const double& l, const SymMat<M,E,S>& r)
01026   { return CNT<double>::template Result<SymMat<M,E,S> >::DvdOp::perform(l,r); }
01027 
01028 template <int M, class E, int S> inline
01029 typename SymMat<M,E,S>::template Result<long double>::Dvd
01030 operator/(const SymMat<M,E,S>& l, const long double& r)
01031   { return SymMat<M,E,S>::template Result<long double>::DvdOp::perform(l,r); }
01032 template <int M, class E, int S> inline
01033 typename CNT<long double>::template Result<SymMat<M,E,S> >::Dvd
01034 operator/(const long double& l, const SymMat<M,E,S>& r)
01035   { return CNT<long double>::template Result<SymMat<M,E,S> >::DvdOp::perform(l,r); }
01036 
01037 // m = m/int, int/m -- just convert int to m's precision float
01038 template <int M, class E, int S> inline
01039 typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Dvd
01040 operator/(const SymMat<M,E,S>& l, int r) {return l / (typename CNT<E>::Precision)r;}
01041 template <int M, class E, int S> inline
01042 typename CNT<typename CNT<E>::Precision>::template Result<SymMat<M,E,S> >::Dvd
01043 operator/(int l, const SymMat<M,E,S>& r) {return (typename CNT<E>::Precision)l / r;}
01044 
01045 
01046 // Complex, conjugate, and negator are all easy to templatize.
01047 
01048 // m = m/complex, complex/m
01049 template <int M, class E, int S, class R> inline
01050 typename SymMat<M,E,S>::template Result<std::complex<R> >::Dvd
01051 operator/(const SymMat<M,E,S>& l, const std::complex<R>& r)
01052   { return SymMat<M,E,S>::template Result<std::complex<R> >::DvdOp::perform(l,r); }
01053 template <int M, class E, int S, class R> inline
01054 typename CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::Dvd
01055 operator/(const std::complex<R>& l, const SymMat<M,E,S>& r)
01056   { return CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::DvdOp::perform(l,r); }
01057 
01058 // m = m/conjugate, conjugate/m (convert conjugate->complex)
01059 template <int M, class E, int S, class R> inline
01060 typename SymMat<M,E,S>::template Result<std::complex<R> >::Dvd
01061 operator/(const SymMat<M,E,S>& l, const conjugate<R>& r) {return l/(std::complex<R>)r;}
01062 template <int M, class E, int S, class R> inline
01063 typename CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::Dvd
01064 operator/(const conjugate<R>& l, const SymMat<M,E,S>& r) {return (std::complex<R>)l/r;}
01065 
01066 // m = m/negator, negator/m: convert negator to number
01067 template <int M, class E, int S, class R> inline
01068 typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Dvd
01069 operator/(const SymMat<M,E,S>& l, const negator<R>& r) {return l/(typename negator<R>::StdNumber)(R)r;}
01070 template <int M, class E, int S, class R> inline
01071 typename CNT<R>::template Result<SymMat<M,E,S> >::Dvd
01072 operator/(const negator<R>& l, const SymMat<M,E,S>& r) {return (typename negator<R>::StdNumber)(R)l/r;}
01073 
01074 
01075 // Add and subtract are odd as scalar ops. They behave as though the
01076 // scalar stands for a conforming matrix whose diagonal elements are that,
01077 // scalar and then a normal matrix add or subtract is done.
01078 
01079 // SCALAR ADD
01080 
01081 // m = m+real, real+m 
01082 template <int M, class E, int S> inline
01083 typename SymMat<M,E,S>::template Result<float>::Add
01084 operator+(const SymMat<M,E,S>& l, const float& r)
01085   { return SymMat<M,E,S>::template Result<float>::AddOp::perform(l,r); }
01086 template <int M, class E, int S> inline
01087 typename SymMat<M,E,S>::template Result<float>::Add
01088 operator+(const float& l, const SymMat<M,E,S>& r) {return r+l;}
01089 
01090 template <int M, class E, int S> inline
01091 typename SymMat<M,E,S>::template Result<double>::Add
01092 operator+(const SymMat<M,E,S>& l, const double& r)
01093   { return SymMat<M,E,S>::template Result<double>::AddOp::perform(l,r); }
01094 template <int M, class E, int S> inline
01095 typename SymMat<M,E,S>::template Result<double>::Add
01096 operator+(const double& l, const SymMat<M,E,S>& r) {return r+l;}
01097 
01098 template <int M, class E, int S> inline
01099 typename SymMat<M,E,S>::template Result<long double>::Add
01100 operator+(const SymMat<M,E,S>& l, const long double& r)
01101   { return SymMat<M,E,S>::template Result<long double>::AddOp::perform(l,r); }
01102 template <int M, class E, int S> inline
01103 typename SymMat<M,E,S>::template Result<long double>::Add
01104 operator+(const long double& l, const SymMat<M,E,S>& r) {return r+l;}
01105 
01106 // m = m+int, int+m -- just convert int to m's precision float
01107 template <int M, class E, int S> inline
01108 typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Add
01109 operator+(const SymMat<M,E,S>& l, int r) {return l + (typename CNT<E>::Precision)r;}
01110 template <int M, class E, int S> inline
01111 typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Add
01112 operator+(int l, const SymMat<M,E,S>& r) {return r + (typename CNT<E>::Precision)l;}
01113 
01114 // Complex, conjugate, and negator are all easy to templatize.
01115 
01116 // m = m+complex, complex+m
01117 template <int M, class E, int S, class R> inline
01118 typename SymMat<M,E,S>::template Result<std::complex<R> >::Add
01119 operator+(const SymMat<M,E,S>& l, const std::complex<R>& r)
01120   { return SymMat<M,E,S>::template Result<std::complex<R> >::AddOp::perform(l,r); }
01121 template <int M, class E, int S, class R> inline
01122 typename SymMat<M,E,S>::template Result<std::complex<R> >::Add
01123 operator+(const std::complex<R>& l, const SymMat<M,E,S>& r) {return r+l;}
01124 
01125 // m = m+conjugate, conjugate+m (convert conjugate->complex)
01126 template <int M, class E, int S, class R> inline
01127 typename SymMat<M,E,S>::template Result<std::complex<R> >::Add
01128 operator+(const SymMat<M,E,S>& l, const conjugate<R>& r) {return l+(std::complex<R>)r;}
01129 template <int M, class E, int S, class R> inline
01130 typename SymMat<M,E,S>::template Result<std::complex<R> >::Add
01131 operator+(const conjugate<R>& l, const SymMat<M,E,S>& r) {return r+(std::complex<R>)l;}
01132 
01133 // m = m+negator, negator+m: convert negator to standard number
01134 template <int M, class E, int S, class R> inline
01135 typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Add
01136 operator+(const SymMat<M,E,S>& l, const negator<R>& r) {return l + (typename negator<R>::StdNumber)(R)r;}
01137 template <int M, class E, int S, class R> inline
01138 typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Add
01139 operator+(const negator<R>& l, const SymMat<M,E,S>& r) {return r + (typename negator<R>::StdNumber)(R)l;}
01140 
01141 // SCALAR SUBTRACT -- careful, not commutative.
01142 
01143 // m = m-real, real-m 
01144 template <int M, class E, int S> inline
01145 typename SymMat<M,E,S>::template Result<float>::Sub
01146 operator-(const SymMat<M,E,S>& l, const float& r)
01147   { return SymMat<M,E,S>::template Result<float>::SubOp::perform(l,r); }
01148 template <int M, class E, int S> inline
01149 typename CNT<float>::template Result<SymMat<M,E,S> >::Sub
01150 operator-(const float& l, const SymMat<M,E,S>& r)
01151   { return CNT<float>::template Result<SymMat<M,E,S> >::SubOp::perform(l,r); }
01152 
01153 template <int M, class E, int S> inline
01154 typename SymMat<M,E,S>::template Result<double>::Sub
01155 operator-(const SymMat<M,E,S>& l, const double& r)
01156   { return SymMat<M,E,S>::template Result<double>::SubOp::perform(l,r); }
01157 template <int M, class E, int S> inline
01158 typename CNT<double>::template Result<SymMat<M,E,S> >::Sub
01159 operator-(const double& l, const SymMat<M,E,S>& r)
01160   { return CNT<double>::template Result<SymMat<M,E,S> >::SubOp::perform(l,r); }
01161 
01162 template <int M, class E, int S> inline
01163 typename SymMat<M,E,S>::template Result<long double>::Sub
01164 operator-(const SymMat<M,E,S>& l, const long double& r)
01165   { return SymMat<M,E,S>::template Result<long double>::SubOp::perform(l,r); }
01166 template <int M, class E, int S> inline
01167 typename CNT<long double>::template Result<SymMat<M,E,S> >::Sub
01168 operator-(const long double& l, const SymMat<M,E,S>& r)
01169   { return CNT<long double>::template Result<SymMat<M,E,S> >::SubOp::perform(l,r); }
01170 
01171 // m = m-int, int-m // just convert int to m's precision float
01172 template <int M, class E, int S> inline
01173 typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Sub
01174 operator-(const SymMat<M,E,S>& l, int r) {return l - (typename CNT<E>::Precision)r;}
01175 template <int M, class E, int S> inline
01176 typename CNT<typename CNT<E>::Precision>::template Result<SymMat<M,E,S> >::Sub
01177 operator-(int l, const SymMat<M,E,S>& r) {return (typename CNT<E>::Precision)l - r;}
01178 
01179 
01180 // Complex, conjugate, and negator are all easy to templatize.
01181 
01182 // m = m-complex, complex-m
01183 template <int M, class E, int S, class R> inline
01184 typename SymMat<M,E,S>::template Result<std::complex<R> >::Sub
01185 operator-(const SymMat<M,E,S>& l, const std::complex<R>& r)
01186   { return SymMat<M,E,S>::template Result<std::complex<R> >::SubOp::perform(l,r); }
01187 template <int M, class E, int S, class R> inline
01188 typename CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::Sub
01189 operator-(const std::complex<R>& l, const SymMat<M,E,S>& r)
01190   { return CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::SubOp::perform(l,r); }
01191 
01192 // m = m-conjugate, conjugate-m (convert conjugate->complex)
01193 template <int M, class E, int S, class R> inline
01194 typename SymMat<M,E,S>::template Result<std::complex<R> >::Sub
01195 operator-(const SymMat<M,E,S>& l, const conjugate<R>& r) {return l-(std::complex<R>)r;}
01196 template <int M, class E, int S, class R> inline
01197 typename CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::Sub
01198 operator-(const conjugate<R>& l, const SymMat<M,E,S>& r) {return (std::complex<R>)l-r;}
01199 
01200 // m = m-negator, negator-m: convert negator to standard number
01201 template <int M, class E, int S, class R> inline
01202 typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Sub
01203 operator-(const SymMat<M,E,S>& l, const negator<R>& r) {return l-(typename negator<R>::StdNumber)(R)r;}
01204 template <int M, class E, int S, class R> inline
01205 typename CNT<R>::template Result<SymMat<M,E,S> >::Sub
01206 operator-(const negator<R>& l, const SymMat<M,E,S>& r) {return (typename negator<R>::StdNumber)(R)l-r;}
01207 
01208 
01209 // SymMat I/O
01210 template <int M, class E, int RS, class CHAR, class TRAITS> inline
01211 std::basic_ostream<CHAR,TRAITS>&
01212 operator<<(std::basic_ostream<CHAR,TRAITS>& o, const SymMat<M,E,RS>& m) {
01213     for (int i=0;i<M;++i) {
01214         o << std::endl << "[";
01215         for (int j=0; j<=i; ++j)         
01216             o << (j>0?" ":"") << m(i,j);
01217         for (int j=i+1; j<M; ++j)
01218             o << " *";
01219         o << "]";
01220     }
01221     if (M) o << std::endl;
01222     return o; 
01223 }
01224 
01225 template <int M, class E, int RS, class CHAR, class TRAITS> inline
01226 std::basic_istream<CHAR,TRAITS>&
01227 operator>>(std::basic_istream<CHAR,TRAITS>& is, SymMat<M,E,RS>& m) {
01228     // TODO: not sure how to do input yet
01229     assert(false);
01230     return is;
01231 }
01232 
01233 } //namespace SimTK
01234 
01235 
01236 #endif //SimTK_SIMMATRIX_SMALLMATRIX_SYMMAT_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines