00001 #ifndef SimTK_SIMMATRIX_SMALLMATRIX_VEC_H_
00002 #define SimTK_SIMMATRIX_SMALLMATRIX_VEC_H_
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00039 #include "SimTKcommon/internal/common.h"
00040
00041 namespace SimTK {
00042
00043
00044
00045
00046 namespace Impl {
00047
00048
00049
00050
00051 template <class E1, int S1, class E2, int S2> void
00052 conformingAdd(const Vec<1,E1,S1>& r1, const Vec<1,E2,S2>& r2, Vec<1,typename CNT<E1>::template Result<E2>::Add>& result) {
00053 result[0] = r1[0] + r2[0];
00054 }
00055 template <int N, class E1, int S1, class E2, int S2> void
00056 conformingAdd(const Vec<N,E1,S1>& r1, const Vec<N,E2,S2>& r2, Vec<N,typename CNT<E1>::template Result<E2>::Add>& result) {
00057 conformingAdd(reinterpret_cast<const Vec<N-1,E1,S1>&>(r1), reinterpret_cast<const Vec<N-1,E2,S2>&>(r2), reinterpret_cast<Vec<N-1,typename CNT<E1>::template Result<E2>::Add>&>(result));
00058 result[N-1] = r1[N-1] + r2[N-1];
00059 }
00060 template <class E1, int S1, class E2, int S2> void
00061 conformingSubtract(const Vec<1,E1,S1>& r1, const Vec<1,E2,S2>& r2, Vec<1,typename CNT<E1>::template Result<E2>::Sub>& result) {
00062 result[0] = r1[0] - r2[0];
00063 }
00064 template <int N, class E1, int S1, class E2, int S2> void
00065 conformingSubtract(const Vec<N,E1,S1>& r1, const Vec<N,E2,S2>& r2, Vec<N,typename CNT<E1>::template Result<E2>::Sub>& result) {
00066 conformingSubtract(reinterpret_cast<const Vec<N-1,E1,S1>&>(r1), reinterpret_cast<const Vec<N-1,E2,S2>&>(r2), reinterpret_cast<Vec<N-1,typename CNT<E1>::template Result<E2>::Sub>&>(result));
00067 result[N-1] = r1[N-1] - r2[N-1];
00068 }
00069 template <class E1, int S1, class E2, int S2> void
00070 copy(Vec<1,E1,S1>& r1, const Vec<1,E2,S2>& r2) {
00071 r1[0] = r2[0];
00072 }
00073 template <int N, class E1, int S1, class E2, int S2> void
00074 copy(Vec<N,E1,S1>& r1, const Vec<N,E2,S2>& r2) {
00075 copy(reinterpret_cast<Vec<N-1,E1,S1>&>(r1), reinterpret_cast<const Vec<N-1,E2,S2>&>(r2));
00076 r1[N-1] = r2[N-1];
00077 }
00078
00079 }
00080
00082 template <int M, class ELT, int STRIDE>
00083 class Vec {
00084 typedef ELT E;
00085 typedef typename CNT<E>::TNeg ENeg;
00086 typedef typename CNT<E>::TWithoutNegator EWithoutNegator;
00087 typedef typename CNT<E>::TReal EReal;
00088 typedef typename CNT<E>::TImag EImag;
00089 typedef typename CNT<E>::TComplex EComplex;
00090 typedef typename CNT<E>::THerm EHerm;
00091 typedef typename CNT<E>::TPosTrans EPosTrans;
00092 typedef typename CNT<E>::TSqHermT ESqHermT;
00093 typedef typename CNT<E>::TSqTHerm ESqTHerm;
00094
00095 typedef typename CNT<E>::TSqrt ESqrt;
00096 typedef typename CNT<E>::TAbs EAbs;
00097 typedef typename CNT<E>::TStandard EStandard;
00098 typedef typename CNT<E>::TInvert EInvert;
00099 typedef typename CNT<E>::TNormalize ENormalize;
00100
00101 typedef typename CNT<E>::Scalar EScalar;
00102 typedef typename CNT<E>::ULessScalar EULessScalar;
00103 typedef typename CNT<E>::Number ENumber;
00104 typedef typename CNT<E>::StdNumber EStdNumber;
00105 typedef typename CNT<E>::Precision EPrecision;
00106 typedef typename CNT<E>::ScalarNormSq EScalarNormSq;
00107
00108 public:
00109
00110 enum {
00111 NRows = M,
00112 NCols = 1,
00113 NPackedElements = M,
00114 NActualElements = M * STRIDE,
00115 NActualScalars = CNT<E>::NActualScalars * NActualElements,
00116 RowSpacing = STRIDE,
00117 ColSpacing = NActualElements,
00118 ImagOffset = NTraits<ENumber>::ImagOffset,
00119 RealStrideFactor = 1,
00120
00121 ArgDepth = ((int)CNT<E>::ArgDepth < (int)MAX_RESOLVED_DEPTH
00122 ? CNT<E>::ArgDepth + 1
00123 : MAX_RESOLVED_DEPTH),
00124 IsScalar = 0,
00125 IsULessScalar = 0,
00126 IsNumber = 0,
00127 IsStdNumber = 0,
00128 IsPrecision = 0,
00129 SignInterpretation = CNT<E>::SignInterpretation
00130 };
00131
00132
00133
00134 typedef Vec<M,E,STRIDE> T;
00135 typedef Vec<M,ENeg,STRIDE> TNeg;
00136 typedef Vec<M,EWithoutNegator,STRIDE> TWithoutNegator;
00137
00138 typedef Vec<M,EReal,STRIDE*CNT<E>::RealStrideFactor>
00139 TReal;
00140 typedef Vec<M,EImag,STRIDE*CNT<E>::RealStrideFactor>
00141 TImag;
00142 typedef Vec<M,EComplex,STRIDE> TComplex;
00143 typedef Row<M,EHerm,STRIDE> THerm;
00144 typedef Row<M,E,STRIDE> TPosTrans;
00145 typedef E TElement;
00146 typedef E TRow;
00147 typedef Vec TCol;
00148
00149
00150
00151 typedef Vec<M,ESqrt,1> TSqrt;
00152 typedef Vec<M,EAbs,1> TAbs;
00153 typedef Vec<M,EStandard,1> TStandard;
00154 typedef Row<M,EInvert,1> TInvert;
00155 typedef Vec<M,ENormalize,1> TNormalize;
00156
00157 typedef ESqHermT TSqHermT;
00158 typedef SymMat<M,ESqTHerm> TSqTHerm;
00159
00160
00161
00162 typedef EScalar Scalar;
00163 typedef EULessScalar ULessScalar;
00164 typedef ENumber Number;
00165 typedef EStdNumber StdNumber;
00166 typedef EPrecision Precision;
00167 typedef EScalarNormSq ScalarNormSq;
00168
00169 int size() const { return M; }
00170 int nrow() const { return M; }
00171 int ncol() const { return 1; }
00172
00173
00174
00175 ScalarNormSq scalarNormSqr() const {
00176 ScalarNormSq sum(0);
00177 for(int i=0;i<M;++i) sum += CNT<E>::scalarNormSqr(d[i*STRIDE]);
00178 return sum;
00179 }
00180
00181
00182
00183
00184 TSqrt sqrt() const {
00185 TSqrt vsqrt;
00186 for(int i=0;i<M;++i) vsqrt[i] = CNT<E>::sqrt(d[i*STRIDE]);
00187 return vsqrt;
00188 }
00189
00190
00191
00192
00193 TAbs abs() const {
00194 TAbs vabs;
00195 for(int i=0;i<M;++i) vabs[i] = CNT<E>::abs(d[i*STRIDE]);
00196 return vabs;
00197 }
00198
00199 TStandard standardize() const {
00200 TStandard vstd;
00201 for(int i=0;i<M;++i) vstd[i] = CNT<E>::standardize(d[i*STRIDE]);
00202 return vstd;
00203 }
00204
00205
00206
00207 EStandard sum() const {
00208 E sum(0);
00209 for (int i=0;i<M;++i) sum += d[i*STRIDE];
00210 return CNT<E>::standardize(sum);
00211 }
00212
00213
00214
00215
00216
00217 template <class P> struct EltResult {
00218 typedef Vec<M, typename CNT<E>::template Result<P>::Mul, 1> Mul;
00219 typedef Vec<M, typename CNT<E>::template Result<P>::Dvd, 1> Dvd;
00220 typedef Vec<M, typename CNT<E>::template Result<P>::Add, 1> Add;
00221 typedef Vec<M, typename CNT<E>::template Result<P>::Sub, 1> Sub;
00222 };
00223
00224
00225
00226 template <class P> struct Result {
00227 typedef MulCNTs<M,1,ArgDepth,Vec,ColSpacing,RowSpacing,
00228 CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
00229 P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOp;
00230 typedef typename MulOp::Type Mul;
00231
00232 typedef MulCNTsNonConforming<M,1,ArgDepth,Vec,ColSpacing,RowSpacing,
00233 CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
00234 P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOpNonConforming;
00235 typedef typename MulOpNonConforming::Type MulNon;
00236
00237 typedef DvdCNTs<M,1,ArgDepth,Vec,ColSpacing,RowSpacing,
00238 CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
00239 P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> DvdOp;
00240 typedef typename DvdOp::Type Dvd;
00241
00242 typedef AddCNTs<M,1,ArgDepth,Vec,ColSpacing,RowSpacing,
00243 CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
00244 P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> AddOp;
00245 typedef typename AddOp::Type Add;
00246
00247 typedef SubCNTs<M,1,ArgDepth,Vec,ColSpacing,RowSpacing,
00248 CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
00249 P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> SubOp;
00250 typedef typename SubOp::Type Sub;
00251 };
00252
00253
00254 template <class P> struct Substitute {
00255 typedef Vec<M,P> Type;
00256 };
00257
00258
00259
00260 Vec(){
00261 #ifndef NDEBUG
00262 setToNaN();
00263 #endif
00264 }
00265
00266
00267
00268
00269 Vec(const Vec& src) {
00270 Impl::copy(*this, src);
00271 }
00272 Vec& operator=(const Vec& src) {
00273 Impl::copy(*this, src);
00274 return *this;
00275 }
00276
00277
00278
00279 template <int SS> Vec(const Vec<M,E,SS>& src) {
00280 Impl::copy(*this, src);
00281 }
00282
00283
00284
00285
00286 template <int SS> Vec(const Vec<M,ENeg,SS>& src) {
00287 Impl::copy(*this, src);
00288 }
00289
00290
00291
00292 template <class EE, int SS> explicit Vec(const Vec<M,EE,SS>& vv) {
00293 Impl::copy(*this, vv);
00294 }
00295
00296
00297 explicit Vec(const ELT& e)
00298 { for (int i=0;i<M;++i) d[i*STRIDE]=e; }
00299
00300
00301 Vec(const E& e0,const E& e1)
00302 { assert(M==2);(*this)[0]=e0;(*this)[1]=e1; }
00303 Vec(const E& e0,const E& e1,const E& e2)
00304 { assert(M==3);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2; }
00305 Vec(const E& e0,const E& e1,const E& e2,const E& e3)
00306 { assert(M==4);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;(*this)[3]=e3; }
00307 Vec(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4)
00308 { assert(M==5);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;
00309 (*this)[3]=e3;(*this)[4]=e4; }
00310 Vec(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,const E& e5)
00311 { assert(M==6);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;
00312 (*this)[3]=e3;(*this)[4]=e4;(*this)[5]=e5; }
00313 Vec(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,const E& e5, const E& e6)
00314 { assert(M==7);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;
00315 (*this)[3]=e3;(*this)[4]=e4;(*this)[5]=e5;(*this)[6]=e6; }
00316 Vec(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,const E& e5, const E& e6, const E& e7)
00317 { assert(M==8);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;
00318 (*this)[3]=e3;(*this)[4]=e4;(*this)[5]=e5;(*this)[6]=e6;(*this)[7]=e7; }
00319 Vec(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,const E& e5, const E& e6, const E& e7, const E& e8)
00320 { assert(M==9);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;
00321 (*this)[3]=e3;(*this)[4]=e4;(*this)[5]=e5;(*this)[6]=e6;(*this)[7]=e7;(*this)[8]=e8; }
00322
00323
00324
00325 template <class EE> explicit Vec(const EE* p)
00326 { assert(p); for(int i=0;i<M;++i) d[i*STRIDE]=p[i]; }
00327 template <class EE> Vec& operator=(const EE* p)
00328 { assert(p); for(int i=0;i<M;++i) d[i*STRIDE]=p[i]; return *this; }
00329
00330
00331 template <class EE, int SS> Vec& operator=(const Vec<M,EE,SS>& vv) {
00332 Impl::copy(*this, vv);
00333 return *this;
00334 }
00335 template <class EE, int SS> Vec& operator+=(const Vec<M,EE,SS>& r)
00336 { for(int i=0;i<M;++i) d[i*STRIDE] += r[i]; return *this; }
00337 template <class EE, int SS> Vec& operator+=(const Vec<M,negator<EE>,SS>& r)
00338 { for(int i=0;i<M;++i) d[i*STRIDE] -= -(r[i]); return *this; }
00339 template <class EE, int SS> Vec& operator-=(const Vec<M,EE,SS>& r)
00340 { for(int i=0;i<M;++i) d[i*STRIDE] -= r[i]; return *this; }
00341 template <class EE, int SS> Vec& operator-=(const Vec<M,negator<EE>,SS>& r)
00342 { for(int i=0;i<M;++i) d[i*STRIDE] += -(r[i]); return *this; }
00343
00344
00345
00346 template <class EE, int SS> Vec<M,typename CNT<E>::template Result<EE>::Add>
00347 conformingAdd(const Vec<M,EE,SS>& r) const {
00348 Vec<M,typename CNT<E>::template Result<EE>::Add> result;
00349 Impl::conformingAdd(*this, r, result);
00350 return result;
00351 }
00352 template <class EE, int SS> Vec<M,typename CNT<E>::template Result<EE>::Sub>
00353 conformingSubtract(const Vec<M,EE,SS>& r) const {
00354 Vec<M,typename CNT<E>::template Result<EE>::Sub> result;
00355 Impl::conformingSubtract(*this, r, result);
00356 return result;
00357 }
00358 template <class EE, int SS> Mat<M,M,typename CNT<E>::template Result<EE>::Mul>
00359 conformingMultiply(const Row<M,EE,SS>& r) const {
00360 Mat<M,M,typename CNT<E>::template Result<EE>::Mul> result;
00361 for (int j=0;j<M;++j) result(j) = scalarMultiply(r(j));
00362 return result;
00363 }
00364
00365 const E& operator[](int i) const { assert(0 <= i && i < M); return d[i*STRIDE]; }
00366 E& operator[](int i) { assert(0 <= i && i < M); return d[i*STRIDE]; }
00367 const E& operator()(int i) const { return (*this)[i]; }
00368 E& operator()(int i) { return (*this)[i]; }
00369
00370 ScalarNormSq normSqr() const { return scalarNormSqr(); }
00371 typename CNT<ScalarNormSq>::TSqrt
00372 norm() const { return CNT<ScalarNormSq>::sqrt(scalarNormSqr()); }
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385 TNormalize normalize() const {
00386 if (CNT<E>::IsScalar) {
00387 return castAwayNegatorIfAny() / (SignInterpretation*norm());
00388 } else {
00389 TNormalize elementwiseNormalized;
00390 for (int i=0; i<M; ++i)
00391 elementwiseNormalized[i] = CNT<E>::normalize((*this)[i]);
00392 return elementwiseNormalized;
00393 }
00394 }
00395
00396 TInvert invert() const {assert(false); return TInvert();}
00397
00398 const Vec& operator+() const { return *this; }
00399 const TNeg& operator-() const { return negate(); }
00400 TNeg& operator-() { return updNegate(); }
00401 const THerm& operator~() const { return transpose(); }
00402 THerm& operator~() { return updTranspose(); }
00403
00404 const TNeg& negate() const { return *reinterpret_cast<const TNeg*>(this); }
00405 TNeg& updNegate() { return *reinterpret_cast< TNeg*>(this); }
00406
00407 const THerm& transpose() const { return *reinterpret_cast<const THerm*>(this); }
00408 THerm& updTranspose() { return *reinterpret_cast< THerm*>(this); }
00409
00410 const TPosTrans& positionalTranspose() const
00411 { return *reinterpret_cast<const TPosTrans*>(this); }
00412 TPosTrans& updPositionalTranspose()
00413 { return *reinterpret_cast<TPosTrans*>(this); }
00414
00415 const TReal& real() const { return *reinterpret_cast<const TReal*>(this); }
00416 TReal& real() { return *reinterpret_cast< TReal*>(this); }
00417
00418
00419 const TImag& imag() const {
00420 const int offs = ImagOffset;
00421 const EImag* p = reinterpret_cast<const EImag*>(this);
00422 return *reinterpret_cast<const TImag*>(p+offs);
00423 }
00424 TImag& imag() {
00425 const int offs = ImagOffset;
00426 EImag* p = reinterpret_cast<EImag*>(this);
00427 return *reinterpret_cast<TImag*>(p+offs);
00428 }
00429
00430 const TWithoutNegator& castAwayNegatorIfAny() const {return *reinterpret_cast<const TWithoutNegator*>(this);}
00431 TWithoutNegator& updCastAwayNegatorIfAny() {return *reinterpret_cast<TWithoutNegator*>(this);}
00432
00433
00434
00435
00436
00437
00438
00439
00440 template <class EE> Vec<M, typename CNT<E>::template Result<EE>::Mul>
00441 scalarMultiply(const EE& e) const {
00442 Vec<M, typename CNT<E>::template Result<EE>::Mul> result;
00443 for (int i=0; i<M; ++i) result[i] = (*this)[i] * e;
00444 return result;
00445 }
00446 template <class EE> Vec<M, typename CNT<EE>::template Result<E>::Mul>
00447 scalarMultiplyFromLeft(const EE& e) const {
00448 Vec<M, typename CNT<EE>::template Result<E>::Mul> result;
00449 for (int i=0; i<M; ++i) result[i] = e * (*this)[i];
00450 return result;
00451 }
00452
00453
00454
00455 template <class EE> Vec<M, typename CNT<E>::template Result<EE>::Dvd>
00456 scalarDivide(const EE& e) const {
00457 Vec<M, typename CNT<E>::template Result<EE>::Dvd> result;
00458 for (int i=0; i<M; ++i) result[i] = (*this)[i] / e;
00459 return result;
00460 }
00461 template <class EE> Vec<M, typename CNT<EE>::template Result<E>::Dvd>
00462 scalarDivideFromLeft(const EE& e) const {
00463 Vec<M, typename CNT<EE>::template Result<E>::Dvd> result;
00464 for (int i=0; i<M; ++i) result[i] = e / (*this)[i];
00465 return result;
00466 }
00467
00468 template <class EE> Vec<M, typename CNT<E>::template Result<EE>::Add>
00469 scalarAdd(const EE& e) const {
00470 Vec<M, typename CNT<E>::template Result<EE>::Add> result;
00471 for (int i=0; i<M; ++i) result[i] = (*this)[i] + e;
00472 return result;
00473 }
00474
00475
00476 template <class EE> Vec<M, typename CNT<E>::template Result<EE>::Sub>
00477 scalarSubtract(const EE& e) const {
00478 Vec<M, typename CNT<E>::template Result<EE>::Sub> result;
00479 for (int i=0; i<M; ++i) result[i] = (*this)[i] - e;
00480 return result;
00481 }
00482 template <class EE> Vec<M, typename CNT<EE>::template Result<E>::Sub>
00483 scalarSubtractFromLeft(const EE& e) const {
00484 Vec<M, typename CNT<EE>::template Result<E>::Sub> result;
00485 for (int i=0; i<M; ++i) result[i] = e - (*this)[i];
00486 return result;
00487 }
00488
00489
00490
00491
00492 template <class EE> Vec& operator =(const EE& e) {return scalarEq(e);}
00493 template <class EE> Vec& operator+=(const EE& e) {return scalarPlusEq(e);}
00494 template <class EE> Vec& operator-=(const EE& e) {return scalarMinusEq(e);}
00495 template <class EE> Vec& operator*=(const EE& e) {return scalarTimesEq(e);}
00496 template <class EE> Vec& operator/=(const EE& e) {return scalarDivideEq(e);}
00497
00498
00499
00500 template <class EE> Vec& scalarEq(const EE& ee)
00501 { for(int i=0;i<M;++i) d[i*STRIDE] = ee; return *this; }
00502
00503 template <class EE> Vec& scalarPlusEq(const EE& ee)
00504 { for(int i=0;i<M;++i) d[i*STRIDE] += ee; return *this; }
00505
00506 template <class EE> Vec& scalarMinusEq(const EE& ee)
00507 { for(int i=0;i<M;++i) d[i*STRIDE] -= ee; return *this; }
00508 template <class EE> Vec& scalarMinusEqFromLeft(const EE& ee)
00509 { for(int i=0;i<M;++i) d[i*STRIDE] = ee - d[i*STRIDE]; return *this; }
00510
00511 template <class EE> Vec& scalarTimesEq(const EE& ee)
00512 { for(int i=0;i<M;++i) d[i*STRIDE] *= ee; return *this; }
00513 template <class EE> Vec& scalarTimesEqFromLeft(const EE& ee)
00514 { for(int i=0;i<M;++i) d[i*STRIDE] = ee * d[i*STRIDE]; return *this; }
00515
00516 template <class EE> Vec& scalarDivideEq(const EE& ee)
00517 { for(int i=0;i<M;++i) d[i*STRIDE] /= ee; return *this; }
00518 template <class EE> Vec& scalarDivideEqFromLeft(const EE& ee)
00519 { for(int i=0;i<M;++i) d[i*STRIDE] = ee / d[i*STRIDE]; return *this; }
00520
00521 void setToNaN() {
00522 (*this) = CNT<ELT>::getNaN();
00523 }
00524
00525 void setToZero() {
00526 (*this) = ELT(0);
00527 }
00528
00529
00530
00531 template <int MM>
00532 const Vec<MM,ELT,STRIDE>& getSubVec(int i) const {
00533 assert(0 <= i && i + MM <= M);
00534 return Vec<MM,ELT,STRIDE>::getAs(&(*this)[i]);
00535 }
00536 template <int MM>
00537 Vec<MM,ELT,STRIDE>& updSubVec(int i) {
00538 assert(0 <= i && i + MM <= M);
00539 return Vec<MM,ELT,STRIDE>::updAs(&(*this)[i]);
00540 }
00541
00542
00543
00544
00545 Vec<M-1,ELT,1> drop1(int p) const {
00546 assert(0 <= p && p < M);
00547 Vec<M-1,ELT,1> out;
00548 int nxt=0;
00549 for (int i=0; i<M-1; ++i, ++nxt) {
00550 if (nxt==p) ++nxt;
00551 out[i] = (*this)[nxt];
00552 }
00553 return out;
00554 }
00555
00556
00557
00558
00559 template <class EE> Vec<M+1,ELT,1> append1(const EE& v) const {
00560 Vec<M+1,ELT,1> out;
00561 Vec<M,ELT,1>::updAs(&out[0]) = (*this);
00562 out[M] = v;
00563 return out;
00564 }
00565
00566
00567
00568
00569
00570
00571
00572 template <class EE> Vec<M+1,ELT,1> insert1(int p, const EE& v) const {
00573 assert(0 <= p && p <= M);
00574 if (p==M) return append1(v);
00575 Vec<M+1,ELT,1> out;
00576 int nxt=0;
00577 for (int i=0; i<M; ++i, ++nxt) {
00578 if (i==p) out[nxt++] = v;
00579 out[nxt] = (*this)[i];
00580 }
00581 return out;
00582 }
00583
00584
00585 static const Vec& getAs(const ELT* p) {return *reinterpret_cast<const Vec*>(p);}
00586 static Vec& updAs(ELT* p) {return *reinterpret_cast<Vec*>(p);}
00587
00588
00589 template <int MM>
00590 static const Vec& getSubVec(const Vec<MM,ELT,STRIDE>& v, int i) {
00591 assert(0 <= i && i + M <= MM);
00592 return getAs(&v[i]);
00593 }
00594 template <int MM>
00595 static Vec& updSubVec(Vec<MM,ELT,STRIDE>& v, int i) {
00596 assert(0 <= i && i + M <= MM);
00597 return updAs(&v[i]);
00598 }
00599
00600 static Vec<M,ELT,1> getNaN() { return Vec<M,ELT,1>(CNT<ELT>::getNaN()); }
00601
00603 bool isNaN() const {
00604 for (int i=0; i<M; ++i)
00605 if (CNT<ELT>::isNaN((*this)[i]))
00606 return true;
00607 return false;
00608 }
00609
00612 bool isInf() const {
00613 bool seenInf = false;
00614 for (int i=0; i<M; ++i) {
00615 const ELT& e = (*this)[i];
00616 if (!CNT<ELT>::isFinite(e)) {
00617 if (!CNT<ELT>::isInf(e))
00618 return false;
00619 seenInf = true;
00620 }
00621 }
00622 return seenInf;
00623 }
00624
00626 bool isFinite() const {
00627 for (int i=0; i<M; ++i)
00628 if (!CNT<ELT>::isFinite((*this)[i]))
00629 return false;
00630 return true;
00631 }
00632
00635 static double getDefaultTolerance() {return CNT<ELT>::getDefaultTolerance();}
00636
00639 template <class E2, int RS2>
00640 bool isNumericallyEqual(const Vec<M,E2,RS2>& v, double tol) const {
00641 for (int i=0; i<M; ++i)
00642 if (!CNT<ELT>::isNumericallyEqual((*this)[i], v[i], tol))
00643 return false;
00644 return true;
00645 }
00646
00650 template <class E2, int RS2>
00651 bool isNumericallyEqual(const Vec<M,E2,RS2>& v) const {
00652 const double tol = std::max(getDefaultTolerance(),v.getDefaultTolerance());
00653 return isNumericallyEqual(v, tol);
00654 }
00655
00660 bool isNumericallyEqual
00661 (const ELT& e,
00662 double tol = getDefaultTolerance()) const
00663 {
00664 for (int i=0; i<M; ++i)
00665 if (!CNT<ELT>::isNumericallyEqual((*this)[i], e, tol))
00666 return false;
00667 return true;
00668 }
00669 private:
00670 ELT d[NActualElements];
00671 };
00672
00674
00675
00677
00678
00679 template <int M, class E1, int S1, class E2, int S2> inline
00680 typename Vec<M,E1,S1>::template Result< Vec<M,E2,S2> >::Add
00681 operator+(const Vec<M,E1,S1>& l, const Vec<M,E2,S2>& r) {
00682 return Vec<M,E1,S1>::template Result< Vec<M,E2,S2> >
00683 ::AddOp::perform(l,r);
00684 }
00685
00686
00687 template <int M, class E1, int S1, class E2, int S2> inline
00688 typename Vec<M,E1,S1>::template Result< Vec<M,E2,S2> >::Sub
00689 operator-(const Vec<M,E1,S1>& l, const Vec<M,E2,S2>& r) {
00690 return Vec<M,E1,S1>::template Result< Vec<M,E2,S2> >
00691 ::SubOp::perform(l,r);
00692 }
00693
00695 template <int M, class E1, int S1, class E2, int S2> inline bool
00696 operator==(const Vec<M,E1,S1>& l, const Vec<M,E2,S2>& r)
00697 { for (int i=0; i < M; ++i) if (l[i] != r[i]) return false;
00698 return true; }
00700 template <int M, class E1, int S1, class E2, int S2> inline bool
00701 operator!=(const Vec<M,E1,S1>& l, const Vec<M,E2,S2>& r) {return !(l==r);}
00702
00704 template <int M, class E1, int S1, class E2> inline bool
00705 operator==(const Vec<M,E1,S1>& v, const E2& e)
00706 { for (int i=0; i < M; ++i) if (v[i] != e) return false;
00707 return true; }
00709 template <int M, class E1, int S1, class E2> inline bool
00710 operator!=(const Vec<M,E1,S1>& v, const E2& e) {return !(v==e);}
00711
00712
00714 template <int M, class E1, int S1, class E2, int S2> inline bool
00715 operator<(const Vec<M,E1,S1>& l, const Vec<M,E2,S2>& r)
00716 { for (int i=0; i < M; ++i) if (l[i] >= r[i]) return false;
00717 return true; }
00719 template <int M, class E1, int S1, class E2> inline bool
00720 operator<(const Vec<M,E1,S1>& v, const E2& e)
00721 { for (int i=0; i < M; ++i) if (v[i] >= e) return false;
00722 return true; }
00723
00725 template <int M, class E1, int S1, class E2, int S2> inline bool
00726 operator>(const Vec<M,E1,S1>& l, const Vec<M,E2,S2>& r)
00727 { for (int i=0; i < M; ++i) if (l[i] <= r[i]) return false;
00728 return true; }
00730 template <int M, class E1, int S1, class E2> inline bool
00731 operator>(const Vec<M,E1,S1>& v, const E2& e)
00732 { for (int i=0; i < M; ++i) if (v[i] <= e) return false;
00733 return true; }
00734
00737 template <int M, class E1, int S1, class E2, int S2> inline bool
00738 operator<=(const Vec<M,E1,S1>& l, const Vec<M,E2,S2>& r)
00739 { for (int i=0; i < M; ++i) if (l[i] > r[i]) return false;
00740 return true; }
00743 template <int M, class E1, int S1, class E2> inline bool
00744 operator<=(const Vec<M,E1,S1>& v, const E2& e)
00745 { for (int i=0; i < M; ++i) if (v[i] > e) return false;
00746 return true; }
00747
00750 template <int M, class E1, int S1, class E2, int S2> inline bool
00751 operator>=(const Vec<M,E1,S1>& l, const Vec<M,E2,S2>& r)
00752 { for (int i=0; i < M; ++i) if (l[i] < r[i]) return false;
00753 return true; }
00756 template <int M, class E1, int S1, class E2> inline bool
00757 operator>=(const Vec<M,E1,S1>& v, const E2& e)
00758 { for (int i=0; i < M; ++i) if (v[i] < e) return false;
00759 return true; }
00760
00762
00764
00765
00766
00767
00768
00769
00770
00771
00772 template <int M, class E, int S> inline
00773 typename Vec<M,E,S>::template Result<float>::Mul
00774 operator*(const Vec<M,E,S>& l, const float& r)
00775 { return Vec<M,E,S>::template Result<float>::MulOp::perform(l,r); }
00776 template <int M, class E, int S> inline
00777 typename Vec<M,E,S>::template Result<float>::Mul
00778 operator*(const float& l, const Vec<M,E,S>& r) {return r*l;}
00779
00780 template <int M, class E, int S> inline
00781 typename Vec<M,E,S>::template Result<double>::Mul
00782 operator*(const Vec<M,E,S>& l, const double& r)
00783 { return Vec<M,E,S>::template Result<double>::MulOp::perform(l,r); }
00784 template <int M, class E, int S> inline
00785 typename Vec<M,E,S>::template Result<double>::Mul
00786 operator*(const double& l, const Vec<M,E,S>& r) {return r*l;}
00787
00788 template <int M, class E, int S> inline
00789 typename Vec<M,E,S>::template Result<long double>::Mul
00790 operator*(const Vec<M,E,S>& l, const long double& r)
00791 { return Vec<M,E,S>::template Result<long double>::MulOp::perform(l,r); }
00792 template <int M, class E, int S> inline
00793 typename Vec<M,E,S>::template Result<long double>::Mul
00794 operator*(const long double& l, const Vec<M,E,S>& r) {return r*l;}
00795
00796
00797 template <int M, class E, int S> inline
00798 typename Vec<M,E,S>::template Result<typename CNT<E>::Precision>::Mul
00799 operator*(const Vec<M,E,S>& l, int r) {return l * (typename CNT<E>::Precision)r;}
00800 template <int M, class E, int S> inline
00801 typename Vec<M,E,S>::template Result<typename CNT<E>::Precision>::Mul
00802 operator*(int l, const Vec<M,E,S>& r) {return r * (typename CNT<E>::Precision)l;}
00803
00804
00805
00806
00807 template <int M, class E, int S, class R> inline
00808 typename Vec<M,E,S>::template Result<std::complex<R> >::Mul
00809 operator*(const Vec<M,E,S>& l, const std::complex<R>& r)
00810 { return Vec<M,E,S>::template Result<std::complex<R> >::MulOp::perform(l,r); }
00811 template <int M, class E, int S, class R> inline
00812 typename Vec<M,E,S>::template Result<std::complex<R> >::Mul
00813 operator*(const std::complex<R>& l, const Vec<M,E,S>& r) {return r*l;}
00814
00815
00816 template <int M, class E, int S, class R> inline
00817 typename Vec<M,E,S>::template Result<std::complex<R> >::Mul
00818 operator*(const Vec<M,E,S>& l, const conjugate<R>& r) {return l*(std::complex<R>)r;}
00819 template <int M, class E, int S, class R> inline
00820 typename Vec<M,E,S>::template Result<std::complex<R> >::Mul
00821 operator*(const conjugate<R>& l, const Vec<M,E,S>& r) {return r*(std::complex<R>)l;}
00822
00823
00824 template <int M, class E, int S, class R> inline
00825 typename Vec<M,E,S>::template Result<typename negator<R>::StdNumber>::Mul
00826 operator*(const Vec<M,E,S>& l, const negator<R>& r) {return l * (typename negator<R>::StdNumber)(R)r;}
00827 template <int M, class E, int S, class R> inline
00828 typename Vec<M,E,S>::template Result<typename negator<R>::StdNumber>::Mul
00829 operator*(const negator<R>& l, const Vec<M,E,S>& r) {return r * (typename negator<R>::StdNumber)(R)l;}
00830
00831
00832
00833
00834
00835
00836 template <int M, class E, int S> inline
00837 typename Vec<M,E,S>::template Result<float>::Dvd
00838 operator/(const Vec<M,E,S>& l, const float& r)
00839 { return Vec<M,E,S>::template Result<float>::DvdOp::perform(l,r); }
00840 template <int M, class E, int S> inline
00841 typename CNT<float>::template Result<Vec<M,E,S> >::Dvd
00842 operator/(const float& l, const Vec<M,E,S>& r)
00843 { return CNT<float>::template Result<Vec<M,E,S> >::DvdOp::perform(l,r); }
00844
00845 template <int M, class E, int S> inline
00846 typename Vec<M,E,S>::template Result<double>::Dvd
00847 operator/(const Vec<M,E,S>& l, const double& r)
00848 { return Vec<M,E,S>::template Result<double>::DvdOp::perform(l,r); }
00849 template <int M, class E, int S> inline
00850 typename CNT<double>::template Result<Vec<M,E,S> >::Dvd
00851 operator/(const double& l, const Vec<M,E,S>& r)
00852 { return CNT<double>::template Result<Vec<M,E,S> >::DvdOp::perform(l,r); }
00853
00854 template <int M, class E, int S> inline
00855 typename Vec<M,E,S>::template Result<long double>::Dvd
00856 operator/(const Vec<M,E,S>& l, const long double& r)
00857 { return Vec<M,E,S>::template Result<long double>::DvdOp::perform(l,r); }
00858 template <int M, class E, int S> inline
00859 typename CNT<long double>::template Result<Vec<M,E,S> >::Dvd
00860 operator/(const long double& l, const Vec<M,E,S>& r)
00861 { return CNT<long double>::template Result<Vec<M,E,S> >::DvdOp::perform(l,r); }
00862
00863
00864 template <int M, class E, int S> inline
00865 typename Vec<M,E,S>::template Result<typename CNT<E>::Precision>::Dvd
00866 operator/(const Vec<M,E,S>& l, int r) {return l / (typename CNT<E>::Precision)r;}
00867 template <int M, class E, int S> inline
00868 typename CNT<typename CNT<E>::Precision>::template Result<Vec<M,E,S> >::Dvd
00869 operator/(int l, const Vec<M,E,S>& r) {return (typename CNT<E>::Precision)l / r;}
00870
00871
00872
00873
00874
00875 template <int M, class E, int S, class R> inline
00876 typename Vec<M,E,S>::template Result<std::complex<R> >::Dvd
00877 operator/(const Vec<M,E,S>& l, const std::complex<R>& r)
00878 { return Vec<M,E,S>::template Result<std::complex<R> >::DvdOp::perform(l,r); }
00879 template <int M, class E, int S, class R> inline
00880 typename CNT<std::complex<R> >::template Result<Vec<M,E,S> >::Dvd
00881 operator/(const std::complex<R>& l, const Vec<M,E,S>& r)
00882 { return CNT<std::complex<R> >::template Result<Vec<M,E,S> >::DvdOp::perform(l,r); }
00883
00884
00885 template <int M, class E, int S, class R> inline
00886 typename Vec<M,E,S>::template Result<std::complex<R> >::Dvd
00887 operator/(const Vec<M,E,S>& l, const conjugate<R>& r) {return l/(std::complex<R>)r;}
00888 template <int M, class E, int S, class R> inline
00889 typename CNT<std::complex<R> >::template Result<Vec<M,E,S> >::Dvd
00890 operator/(const conjugate<R>& l, const Vec<M,E,S>& r) {return (std::complex<R>)l/r;}
00891
00892
00893 template <int M, class E, int S, class R> inline
00894 typename Vec<M,E,S>::template Result<typename negator<R>::StdNumber>::Dvd
00895 operator/(const Vec<M,E,S>& l, const negator<R>& r) {return l/(typename negator<R>::StdNumber)(R)r;}
00896 template <int M, class E, int S, class R> inline
00897 typename CNT<R>::template Result<Vec<M,E,S> >::Dvd
00898 operator/(const negator<R>& l, const Vec<M,E,S>& r) {return (typename negator<R>::StdNumber)(R)l/r;}
00899
00900
00901
00902
00903
00904
00905
00906
00907
00908 template <int M, class E, int S> inline
00909 typename Vec<M,E,S>::template Result<float>::Add
00910 operator+(const Vec<M,E,S>& l, const float& r)
00911 { return Vec<M,E,S>::template Result<float>::AddOp::perform(l,r); }
00912 template <int M, class E, int S> inline
00913 typename Vec<M,E,S>::template Result<float>::Add
00914 operator+(const float& l, const Vec<M,E,S>& r) {return r+l;}
00915
00916 template <int M, class E, int S> inline
00917 typename Vec<M,E,S>::template Result<double>::Add
00918 operator+(const Vec<M,E,S>& l, const double& r)
00919 { return Vec<M,E,S>::template Result<double>::AddOp::perform(l,r); }
00920 template <int M, class E, int S> inline
00921 typename Vec<M,E,S>::template Result<double>::Add
00922 operator+(const double& l, const Vec<M,E,S>& r) {return r+l;}
00923
00924 template <int M, class E, int S> inline
00925 typename Vec<M,E,S>::template Result<long double>::Add
00926 operator+(const Vec<M,E,S>& l, const long double& r)
00927 { return Vec<M,E,S>::template Result<long double>::AddOp::perform(l,r); }
00928 template <int M, class E, int S> inline
00929 typename Vec<M,E,S>::template Result<long double>::Add
00930 operator+(const long double& l, const Vec<M,E,S>& r) {return r+l;}
00931
00932
00933 template <int M, class E, int S> inline
00934 typename Vec<M,E,S>::template Result<typename CNT<E>::Precision>::Add
00935 operator+(const Vec<M,E,S>& l, int r) {return l + (typename CNT<E>::Precision)r;}
00936 template <int M, class E, int S> inline
00937 typename Vec<M,E,S>::template Result<typename CNT<E>::Precision>::Add
00938 operator+(int l, const Vec<M,E,S>& r) {return r + (typename CNT<E>::Precision)l;}
00939
00940
00941
00942
00943 template <int M, class E, int S, class R> inline
00944 typename Vec<M,E,S>::template Result<std::complex<R> >::Add
00945 operator+(const Vec<M,E,S>& l, const std::complex<R>& r)
00946 { return Vec<M,E,S>::template Result<std::complex<R> >::AddOp::perform(l,r); }
00947 template <int M, class E, int S, class R> inline
00948 typename Vec<M,E,S>::template Result<std::complex<R> >::Add
00949 operator+(const std::complex<R>& l, const Vec<M,E,S>& r) {return r+l;}
00950
00951
00952 template <int M, class E, int S, class R> inline
00953 typename Vec<M,E,S>::template Result<std::complex<R> >::Add
00954 operator+(const Vec<M,E,S>& l, const conjugate<R>& r) {return l+(std::complex<R>)r;}
00955 template <int M, class E, int S, class R> inline
00956 typename Vec<M,E,S>::template Result<std::complex<R> >::Add
00957 operator+(const conjugate<R>& l, const Vec<M,E,S>& r) {return r+(std::complex<R>)l;}
00958
00959
00960 template <int M, class E, int S, class R> inline
00961 typename Vec<M,E,S>::template Result<typename negator<R>::StdNumber>::Add
00962 operator+(const Vec<M,E,S>& l, const negator<R>& r) {return l + (typename negator<R>::StdNumber)(R)r;}
00963 template <int M, class E, int S, class R> inline
00964 typename Vec<M,E,S>::template Result<typename negator<R>::StdNumber>::Add
00965 operator+(const negator<R>& l, const Vec<M,E,S>& r) {return r + (typename negator<R>::StdNumber)(R)l;}
00966
00967
00968
00969
00970 template <int M, class E, int S> inline
00971 typename Vec<M,E,S>::template Result<float>::Sub
00972 operator-(const Vec<M,E,S>& l, const float& r)
00973 { return Vec<M,E,S>::template Result<float>::SubOp::perform(l,r); }
00974 template <int M, class E, int S> inline
00975 typename CNT<float>::template Result<Vec<M,E,S> >::Sub
00976 operator-(const float& l, const Vec<M,E,S>& r)
00977 { return CNT<float>::template Result<Vec<M,E,S> >::SubOp::perform(l,r); }
00978
00979 template <int M, class E, int S> inline
00980 typename Vec<M,E,S>::template Result<double>::Sub
00981 operator-(const Vec<M,E,S>& l, const double& r)
00982 { return Vec<M,E,S>::template Result<double>::SubOp::perform(l,r); }
00983 template <int M, class E, int S> inline
00984 typename CNT<double>::template Result<Vec<M,E,S> >::Sub
00985 operator-(const double& l, const Vec<M,E,S>& r)
00986 { return CNT<double>::template Result<Vec<M,E,S> >::SubOp::perform(l,r); }
00987
00988 template <int M, class E, int S> inline
00989 typename Vec<M,E,S>::template Result<long double>::Sub
00990 operator-(const Vec<M,E,S>& l, const long double& r)
00991 { return Vec<M,E,S>::template Result<long double>::SubOp::perform(l,r); }
00992 template <int M, class E, int S> inline
00993 typename CNT<long double>::template Result<Vec<M,E,S> >::Sub
00994 operator-(const long double& l, const Vec<M,E,S>& r)
00995 { return CNT<long double>::template Result<Vec<M,E,S> >::SubOp::perform(l,r); }
00996
00997
00998 template <int M, class E, int S> inline
00999 typename Vec<M,E,S>::template Result<typename CNT<E>::Precision>::Sub
01000 operator-(const Vec<M,E,S>& l, int r) {return l - (typename CNT<E>::Precision)r;}
01001 template <int M, class E, int S> inline
01002 typename CNT<typename CNT<E>::Precision>::template Result<Vec<M,E,S> >::Sub
01003 operator-(int l, const Vec<M,E,S>& r) {return (typename CNT<E>::Precision)l - r;}
01004
01005
01006
01007
01008
01009 template <int M, class E, int S, class R> inline
01010 typename Vec<M,E,S>::template Result<std::complex<R> >::Sub
01011 operator-(const Vec<M,E,S>& l, const std::complex<R>& r)
01012 { return Vec<M,E,S>::template Result<std::complex<R> >::SubOp::perform(l,r); }
01013 template <int M, class E, int S, class R> inline
01014 typename CNT<std::complex<R> >::template Result<Vec<M,E,S> >::Sub
01015 operator-(const std::complex<R>& l, const Vec<M,E,S>& r)
01016 { return CNT<std::complex<R> >::template Result<Vec<M,E,S> >::SubOp::perform(l,r); }
01017
01018
01019 template <int M, class E, int S, class R> inline
01020 typename Vec<M,E,S>::template Result<std::complex<R> >::Sub
01021 operator-(const Vec<M,E,S>& l, const conjugate<R>& r) {return l-(std::complex<R>)r;}
01022 template <int M, class E, int S, class R> inline
01023 typename CNT<std::complex<R> >::template Result<Vec<M,E,S> >::Sub
01024 operator-(const conjugate<R>& l, const Vec<M,E,S>& r) {return (std::complex<R>)l-r;}
01025
01026
01027 template <int M, class E, int S, class R> inline
01028 typename Vec<M,E,S>::template Result<typename negator<R>::StdNumber>::Sub
01029 operator-(const Vec<M,E,S>& l, const negator<R>& r) {return l-(typename negator<R>::StdNumber)(R)r;}
01030 template <int M, class E, int S, class R> inline
01031 typename CNT<R>::template Result<Vec<M,E,S> >::Sub
01032 operator-(const negator<R>& l, const Vec<M,E,S>& r) {return (typename negator<R>::StdNumber)(R)l-r;}
01033
01034
01035 template <int M, class E, int S, class CHAR, class TRAITS> inline
01036 std::basic_ostream<CHAR,TRAITS>&
01037 operator<<(std::basic_ostream<CHAR,TRAITS>& o, const Vec<M,E,S>& v) {
01038 o << "~[" << v[0]; for(int i=1;i<M;++i) o<<','<<v[i]; o<<']'; return o;
01039 }
01040
01041 template <int M, class E, int S, class CHAR, class TRAITS> inline
01042 std::basic_istream<CHAR,TRAITS>&
01043 operator>>(std::basic_istream<CHAR,TRAITS>& is, Vec<M,E,S>& v) {
01044
01045 assert(false);
01046 return is;
01047 }
01048
01049 }
01050
01051
01052 #endif //SimTK_SIMMATRIX_SMALLMATRIX_VEC_H_