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