1 #ifndef SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_ 2 #define SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_ 38 template <
int M,
class EL,
int CSL,
int RSL,
class ER,
int RSR>
inline 40 for (
int i=0; i<M; ++i) {
41 if (l(i,i) != r.
getDiag()[i])
return false;
42 for (
int j=0; j<i; ++j)
44 for (
int j=i+1; j<M; ++j)
51 template <
int M,
class EL,
int CSL,
int RSL,
class ER,
int RSR>
inline 57 template <
int M,
class EL,
int RSL,
class ER,
int CSR,
int RSR>
inline 62 template <
int M,
class EL,
int RSL,
class ER,
int CSR,
int RSR>
inline 84 template <
int M,
class E1,
int S1,
class E2,
int S2>
inline 90 template <
class E1,
int S1,
class E2,
int S2>
inline 98 template <
int N,
class E1,
int S1,
class E2,
int S2>
inline 99 typename CNT<E1>::template Result<E2>::Mul
101 typename CNT<E1>::template Result<E2>::Mul
sum(
reinterpret_cast<const Row<N-1,E1,S1
>&>(r)*
reinterpret_cast<const Vec<N-1,E2,S2
>&>(v) + r[N-1]*v[N-1]);
104 template <
class E1,
int S1,
class E2,
int S2>
inline 105 typename CNT<E1>::template Result<E2>::Mul
112 template <
int N,
class E1,
int S1,
class E2,
int S2>
inline 113 typename CNT<E1>::template Result<E2>::Mul
117 template <
int M,
class E1,
int S1,
class E2,
int S2>
inline 118 typename CNT<E1>::template Result<E2>::Mul
122 template <
int N,
class E1,
int S1,
class E2,
int S2>
inline 123 typename CNT<E1>::template Result<E2>::Mul
145 template <
int M,
class E1,
int S1,
class E2,
int S2>
inline 146 Mat<M,M, typename CNT<E1>::template Result<typename CNT<E2>::THerm>::Mul>
149 for (
int i=0; i<M; ++i)
155 template <
int M,
class E1,
int S1,
class E2,
int S2>
inline 156 typename Vec<M,E1,S1>::template Result<Row<M,E2,S2> >::Mul
163 template <
int M,
class E1,
int S1,
class E2,
int S2>
inline 164 Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
168 template <
int M,
class E1,
int S1,
class E2,
int S2>
inline 169 Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
173 template <
int M,
class E1,
int S1,
class E2,
int S2>
inline 174 Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
182 template <
int M,
int N,
class ME,
int CS,
int RS,
class E,
int S>
inline 183 typename Mat<M,N,ME,CS,RS>::template Result<Vec<N,E,S> >::Mul
186 for (
int i=0; i<M; ++i)
192 template <
int M,
class E,
int S,
int N,
class ME,
int CS,
int RS>
inline 193 typename Row<M,E,S>::template Result<Mat<M,N,ME,CS,RS> >::Mul
196 for (
int i=0; i<N; ++i)
204 template <
int N,
class ME,
int RS,
class E,
int S>
inline 205 typename SymMat<N,ME,RS>::template Result<Vec<N,E,S> >::Mul
208 for (
int i=0; i<N; ++i) {
209 result[i] = m.
getDiag()[i]*v[i];
210 for (
int j=0; j<i; ++j)
212 for (
int j=i+1; j<N; ++j)
221 template <
class ME,
int RS,
class E,
int S>
inline 222 typename SymMat<1,ME,RS>::template Result<Vec<1,E,S> >::Mul
225 result[0] = m.
getDiag()[0]*v[0];
230 template <
class ME,
int RS,
class E,
int S>
inline 231 typename SymMat<2,ME,RS>::template Result<Vec<2,E,S> >::Mul
240 template <
class ME,
int RS,
class E,
int S>
inline 241 typename SymMat<3,ME,RS>::template Result<Vec<3,E,S> >::Mul
251 template <
int M,
class E,
int S,
class ME,
int RS>
inline 252 typename Row<M,E,S>::template Result<SymMat<M,ME,RS> >::Mul
255 for (
int j=0; j<M; ++j) {
256 result[j] = r[j]*m.
getDiag()[j];
257 for (
int i=0; i<j; ++i)
259 for (
int i=j+1; i<M; ++i)
268 template <
class E,
int S,
class ME,
int RS>
inline 269 typename Row<1,E,S>::template Result<SymMat<1,ME,RS> >::Mul
272 result[0] = r[0]*m.
getDiag()[0];
277 template <
class E,
int S,
class ME,
int RS>
inline 278 typename Row<2,E,S>::template Result<SymMat<2,ME,RS> >::Mul
287 template <
class E,
int S,
class ME,
int RS>
inline 288 typename Row<3,E,S>::template Result<SymMat<3,ME,RS> >::Mul
303 template <
int M,
class E1,
int S1,
int N,
class E2,
int S2>
inline 304 typename Vec<M,E1,S1>::template Result<Row<N,E2,S2> >::MulNon
309 template <
int M,
class E1,
int S1,
int MM,
int NN,
class E2,
int CS2,
int RS2>
inline 310 typename Vec<M,E1,S1>::template Result<Mat<MM,NN,E2,CS2,RS2> >::MulNon
313 ::MulOpNonConforming::perform(v,m);
316 template <
int M,
class E1,
int S1,
int MM,
class E2,
int RS2>
inline 317 typename Vec<M,E1,S1>::template Result<SymMat<MM,E2,RS2> >::MulNon
320 ::MulOpNonConforming::perform(v,m);
323 template <
int M,
class E1,
int S1,
int MM,
class E2,
int S2>
inline 324 typename Vec<M,E1,S1>::template Result<Vec<MM,E2,S2> >::MulNon
327 ::MulOpNonConforming::perform(v1,v2);
334 template <
int M,
class E,
int S,
int MM,
int NN,
class ME,
int CS,
int RS>
inline 335 typename Row<M,E,S>::template Result<Mat<MM,NN,ME,CS,RS> >::MulNon
338 ::MulOpNonConforming::perform(r,m);
341 template <
int N,
class E1,
int S1,
int M,
class E2,
int S2>
inline 342 typename Row<N,E1,S1>::template Result<Vec<M,E2,S2> >::MulNon
345 ::MulOpNonConforming::perform(r,v);
348 template <
int N1,
class E1,
int S1,
int N2,
class E2,
int S2>
inline 349 typename Row<N1,E1,S1>::template Result<Row<N2,E2,S2> >::MulNon
352 ::MulOpNonConforming::perform(r1,r2);
358 template <
int M,
int N,
class ME,
int CS,
int RS,
int MM,
class E,
int S>
inline 359 typename Mat<M,N,ME,CS,RS>::template Result<Vec<MM,E,S> >::MulNon
362 ::MulOpNonConforming::perform(m,v);
365 template <
int M,
int N,
class ME,
int CS,
int RS,
int NN,
class E,
int S>
inline 366 typename Mat<M,N,ME,CS,RS>::template Result<Row<NN,E,S> >::MulNon
369 ::MulOpNonConforming::perform(m,r);
373 template <
int M,
int N,
class ME,
int CS,
int RS,
int Dim,
class E,
int S>
inline 374 typename Mat<M,N,ME,CS,RS>::template Result<SymMat<Dim,E,S> >::MulNon
377 ::MulOpNonConforming::perform(m,sy);
411 template <
class E1,
int S1,
class E2,
int S2>
inline 412 Vec<3,typename CNT<E1>::template Result<E2>::Mul>
415 (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]);
417 template <
class E1,
int S1,
class E2,
int S2>
inline 418 Vec<3,typename CNT<E1>::template Result<E2>::Mul>
422 template <
class E1,
int S1,
class E2,
int S2>
inline 423 Row<3,typename CNT<E1>::template Result<E2>::Mul>
426 (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]);
428 template <
class E1,
int S1,
class E2,
int S2>
inline 429 Row<3,typename CNT<E1>::template Result<E2>::Mul>
433 template <
class E1,
int S1,
class E2,
int S2>
inline 434 Row<3,typename CNT<E1>::template Result<E2>::Mul>
437 (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]);
439 template <
class E1,
int S1,
class E2,
int S2>
inline 440 Row<3,typename CNT<E1>::template Result<E2>::Mul>
444 template <
class E1,
int S1,
class E2,
int S2>
inline 445 Row<3,typename CNT<E1>::template Result<E2>::Mul>
448 (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]);
450 template <
class E1,
int S1,
class E2,
int S2>
inline 451 Row<3,typename CNT<E1>::template Result<E2>::Mul>
465 template <
class E1,
int S1,
int N,
class E2,
int CS,
int RS>
inline 466 Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
469 for (
int j=0; j < N; ++j)
470 result(j) = v % m(j);
473 template <
class E1,
int S1,
int N,
class E2,
int CS,
int RS>
inline 474 Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
479 template <
class E1,
int S1,
int N,
class E2,
int S2,
int S3>
inline 480 Row< N,Vec<3,typename CNT<E1>::template Result<E2>::Mul> >
483 for (
int j=0; j < N; ++j)
484 result(j) = v % m(j);
488 template <
class E1,
int S1,
class E2,
int S2,
int S3>
inline 489 Row< 3,Vec<3,typename CNT<E1>::template Result<E2>::Mul> >
492 for (
int j=0; j < 3; ++j)
493 result(j) = v % m(j);
496 template <
class E1,
int S1,
int N,
class E2,
int S2,
int S3>
inline 497 Row< N,Vec<3,typename CNT<E1>::template Result<E2>::Mul> >
499 {
return cross(v,m); }
500 template <
class E1,
int S1,
class E2,
int S2,
int S3>
inline 501 Row< 3,Vec<3,typename CNT<E1>::template Result<E2>::Mul> >
503 {
return cross(v,m); }
508 template<
class EV,
int SV,
class EM,
int RS>
inline 509 Mat<3,3,typename CNT<EV>::template Result<EM>::Mul>
511 const EV& x=v[0];
const EV& y=v[1];
const EV& z=v[2];
513 const EM& b=s(1,0);
const EM& d=s(1,1);
514 const EM& c=s(2,0);
const EM& e=s(2,1);
const EM& f=s(2,2);
517 const EResult xe=x*e, yc=y*c, zb=z*b;
519 ( yc-zb, y*e-z*d, y*f-z*e,
520 z*a-x*c, zb-xe, z*c-x*f,
521 x*b-y*a, x*d-y*b, xe-yc );
523 template <
class EV,
int SV,
class EM,
int RS>
inline 524 Mat<3,3,typename CNT<EV>::template Result<EM>::Mul>
528 template <
class E1,
int S1,
int N,
class E2,
int CS,
int RS>
inline 529 Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
533 template <
class E1,
int S1,
int N,
class E2,
int CS,
int RS>
inline 534 Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
538 template<
class EV,
int SV,
class EM,
int RS>
inline 539 Mat<3,3,typename CNT<EV>::template Result<EM>::Mul>
543 template<
class EV,
int SV,
class EM,
int RS>
inline 544 Mat<3,3,typename CNT<EV>::template Result<EM>::Mul>
548 template <
int M,
class EM,
int CS,
int RS,
class EV,
int S>
inline 549 Mat<M,3,typename CNT<EM>::template Result<EV>::Mul>
552 for (
int i=0; i < M; ++i)
553 result[i] = m[i] % v;
556 template <
int M,
class EM,
int CS,
int RS,
class EV,
int S>
inline 557 Mat<M,3,typename CNT<EM>::template Result<EV>::Mul>
563 template<
class EM,
int RS,
class EV,
int SV>
inline 564 Mat<3,3,typename CNT<EM>::template Result<EV>::Mul>
566 const EV& x=v[0];
const EV& y=v[1];
const EV& z=v[2];
568 const EM& b=s(1,0);
const EM& d=s(1,1);
569 const EM& c=s(2,0);
const EM& e=s(2,1);
const EM& f=s(2,2);
572 const EResult xe=x*e, yc=y*c, zb=z*b;
574 ( zb-yc, x*c-z*a, y*a-x*b,
575 z*d-y*e, xe-zb, y*b-x*d,
576 z*e-y*f, x*f-z*c, yc-xe );
578 template<
class EM,
int RS,
class EV,
int SV>
inline 579 Mat<3,3,typename CNT<EM>::template Result<EV>::Mul>
583 template <
int M,
class EM,
int CS,
int RS,
class ER,
int S>
inline 584 Mat<M,3,typename CNT<EM>::template Result<ER>::Mul>
588 template <
int M,
class EM,
int CS,
int RS,
class ER,
int S>
inline 589 Mat<M,3,typename CNT<EM>::template Result<ER>::Mul>
593 template<
class EM,
int RS,
class EV,
int SV>
inline 594 Mat<3,3,typename CNT<EM>::template Result<EV>::Mul>
598 template<
class EM,
int RS,
class EV,
int SV>
inline 599 Mat<3,3,typename CNT<EM>::template Result<EV>::Mul>
606 template <
class E1,
int S1,
class E2,
int S2>
inline 607 typename CNT<E1>::template Result<E2>::Mul
609 return a[0]*b[1]-a[1]*b[0];
611 template <
class E1,
int S1,
class E2,
int S2>
inline 612 typename CNT<E1>::template Result<E2>::Mul
615 template <
class E1,
int S1,
class E2,
int S2>
inline 616 typename CNT<E1>::template Result<E2>::Mul
618 return a[0]*b[1]-a[1]*b[0];
620 template <
class E1,
int S1,
class E2,
int S2>
inline 621 typename CNT<E1>::template Result<E2>::Mul
624 template <
class E1,
int S1,
class E2,
int S2>
inline 625 typename CNT<E1>::template Result<E2>::Mul
627 return a[0]*b[1]-a[1]*b[0];
629 template <
class E1,
int S1,
class E2,
int S2>
inline 630 typename CNT<E1>::template Result<E2>::Mul
633 template <
class E1,
int S1,
class E2,
int S2>
inline 634 typename CNT<E1>::template Result<E2>::Mul
636 return a[0]*b[1]-a[1]*b[0];
638 template <
class E1,
int S1,
class E2,
int S2>
inline 639 typename CNT<E1>::template Result<E2>::Mul
647 template <
class E,
int S>
inline 656 template <
class E,
int S>
inline 667 template <
class E,
int S>
inline 670 template <
class E,
int S>
inline 676 template <
class E,
int S>
inline 681 template <
class E,
int S>
inline 687 template <
class E,
int S>
inline 690 template <
class E,
int S>
inline 715 template <
class E,
int S>
inline 725 nx*v[2], ny*v[2], xx+yy );
729 template <
class E,
int S>
inline 741 -v[0]*z, -v[1]*z, xx+yy );
744 template <
class E,
int S>
inline 746 template <
class E,
int S>
inline 753 template <
class E,
int CS,
int RS>
inline 759 template <
class E,
int RS>
inline 765 template <
class E,
int CS,
int RS>
inline 769 return E(m(0,0)*m(1,1) - m(0,1)*m(1,0));
773 template <
class E,
int RS>
inline 783 template <
class E,
int CS,
int RS>
inline 785 return E( m(0,0)*(m(1,1)*m(2,2)-m(1,2)*m(2,1))
786 - m(0,1)*(m(1,0)*m(2,2)-m(1,2)*m(2,0))
787 + m(0,2)*(m(1,0)*m(2,1)-m(1,1)*m(2,0)));
791 template <
class E,
int RS>
inline 814 template <
int M,
class E,
int CS,
int RS>
inline 819 const Mat<M-1,M,
E,CS,RS>& m2 = m.template getSubMat<M-1,M>(1,0);
820 for (
int j=0; j < M; ++j) {
822 result +=
sign*m(0,j)*
det(m2.dropCol(j));
833 template <
int M,
class E,
int RS>
inline 842 template <
class E,
int CS,
int RS>
inline 858 template <
int M,
class E,
int CS,
int RS>
inline 871 Raw* rawData =
reinterpret_cast<Raw*
>(&inv(0,0));
876 Lapack::getrf<Raw>(M,M,rawData,M,&ipiv[0],info);
877 SimTK_ASSERT1(info>=0,
"Argument %d to Lapack getrf routine was bad", -info);
879 "Matrix is singular so can't be inverted (Lapack getrf info=%d).", info);
889 Lapack::getri<Raw>(M,rawData,M,&ipiv[0],&work[0],M,info);
890 SimTK_ASSERT1(info>=0,
"Argument %d to Lapack getri routine was bad", -info);
892 "Matrix is singular so can't be inverted (Lapack getri info=%d).", info);
898 template <
class E,
int CS,
int RS>
inline 905 template <
class E,
int RS>
inline 912 template <
class E,
int CS,
int RS>
inline 914 const E d (
det(m) );
917 E(-ood*m(1,0)),
E( ood*m(0,0)));
921 template <
class E,
int RS>
inline 923 const E d (
det(s) );
926 E(-ood*s(1,0)),
E(ood*s(0,0)));
933 template <
class E,
int CS,
int RS>
inline 938 const E d00 (m(1,1)*m(2,2)-m(1,2)*m(2,1)),
939 nd01(m(1,2)*m(2,0)-m(1,0)*m(2,2)),
940 d02 (m(1,0)*m(2,1)-m(1,1)*m(2,0));
943 const E d (m(0,0)*d00 + m(0,1)*nd01 + m(0,2)*d02);
949 const E nd10(m(0,2)*m(2,1)-m(0,1)*m(2,2)),
950 d11 (m(0,0)*m(2,2)-m(0,2)*m(2,0)),
951 nd12(m(0,1)*m(2,0)-m(0,0)*m(2,1)),
952 d20 (m(0,1)*m(1,2)-m(0,2)*m(1,1)),
953 nd21(m(0,2)*m(1,0)-m(0,0)*m(1,2)),
954 d22 (m(0,0)*m(1,1)-m(0,1)*m(1,0));
957 (
E(ood* d00),
E(ood*nd10),
E(ood* d20),
958 E(ood*nd01),
E(ood* d11),
E(ood*nd21),
959 E(ood* d02),
E(ood*nd12),
E(ood* d22) );
967 template <
class E,
int RS>
inline 972 const E d00 (s(1,1)*s(2,2)-s(1,2)*s(2,1)),
973 nd01(s(1,2)*s(2,0)-s(1,0)*s(2,2)),
974 d02 (s(1,0)*s(2,1)-s(1,1)*s(2,0));
977 const E d (s(0,0)*d00 + s(0,1)*nd01 + s(0,2)*d02);
983 const E d11 (s(0,0)*s(2,2)-s(0,2)*s(2,0)),
984 nd12(s(0,1)*s(2,0)-s(0,0)*s(2,1)),
985 d22 (s(0,0)*s(1,1)-s(0,1)*s(1,0));
989 E(ood*nd01),
E(ood* d11),
990 E(ood* d02),
E(ood*nd12),
E(ood* d22) );
995 template <
int M,
class E,
int CS,
int RS>
inline 1002 template <
int M,
int N,
class ELT,
int CS,
int RS>
inline 1011 #endif //SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_ const TPosTrans & positionalTranspose() const
Definition: Row.h:494
Vec< 3, typename CNT< E1 >::template Result< E2 >::Mul > cross(const Vec< 3, E1, S1 > &a, const Vec< 3, E2, S2 > &b)
Definition: SmallMatrixMixed.h:413
Vec< 3, typename CNT< E1 >::template Result< E2 >::Mul > operator%(const Vec< 3, E1, S1 > &a, const Vec< 3, E2, S2 > &b)
Definition: SmallMatrixMixed.h:419
Mat< M, M, typename CNT< E1 >::template Result< typename CNT< E2 >::THerm >::Mul > outer(const Vec< M, E1, S1 > &v, const Vec< M, E2, S2 > &w)
Definition: SmallMatrixMixed.h:147
This is a small, fixed-size symmetric or Hermitian matrix designed for no-overhead inline computation...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:621
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
negator<N>, where N is a number type (real, complex, conjugate), is represented in memory identically...
Definition: String.h:44
const EHerm & getEltUpper(int i, int j) const
Definition: SymMat.h:842
const TDiag & getDiag() const
Definition: SymMat.h:818
bool operator==(const PhiMatrix &p1, const PhiMatrix &p2)
Definition: SpatialAlgebra.h:791
SymMat< 3, E > crossMatSq(const Vec< 3, E, S > &v)
Calculate matrix S(v) such that S(v)*w = -v % (v % w) = (v % w) % v.
Definition: SmallMatrixMixed.h:717
unsigned char square(unsigned char u)
Definition: Scalar.h:349
#define SimTK_ASSERT1(cond, msg, a1)
Definition: ExceptionMacros.h:374
const E & getEltLower(int i, int j) const
Definition: SymMat.h:838
ELEM sum(const VectorBase< ELEM > &v)
Definition: VectorMath.h:147
const E & getEltDiag(int i) const
Definition: SymMat.h:834
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:618
TInvert invert() const
Definition: SmallMatrixMixed.h:1004
Matrix_< E > operator*(const MatrixBase< E > &l, const typename CNT< E >::StdNumber &r)
Definition: BigMatrix.h:605
K::TInvert TInvert
Definition: CompositeNumericalTypes.h:157
E det(const Mat< 1, 1, E, CS, RS > &m)
Special case Mat 1x1 determinant. No computation.
Definition: SmallMatrixMixed.h:754
K::StdNumber StdNumber
Definition: CompositeNumericalTypes.h:163
Mat< 3, 3, E > crossMat(const Vec< 3, E, S > &v)
Calculate matrix M(v) such that M(v)*w = v % w.
Definition: SmallMatrixMixed.h:649
Specialized information about Composite Numerical Types which allows us to define appropriate templat...
Definition: CompositeNumericalTypes.h:136
CNT< typename CNT< E1 >::THerm >::template Result< E2 >::Mul dot(const Vec< M, E1, S1 > &r, const Vec< M, E2, S2 > &v)
Definition: SmallMatrixMixed.h:86
SymMat< M, EInvert, 1 > TInvert
Definition: SymMat.h:162
This is a fixed-length row vector designed for no-overhead inline computation.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:619
const Real E
e = Real(exp(1))
Mat< N, M, EInvert, N, 1 > TInvert
Definition: Mat.h:171
unsigned int sign(unsigned char u)
Definition: Scalar.h:311
Mat< 1, 1, E, CS, RS >::TInvert inverse(const Mat< 1, 1, E, CS, RS > &m)
Specialized 1x1 Mat inverse: costs one divide.
Definition: SmallMatrixMixed.h:899
bool operator!=(const L &left, const R &right)
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:641
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:620
Mat< 1, 1, E, CS, RS >::TInvert lapackInverse(const Mat< 1, 1, E, CS, RS > &m)
Specialized 1x1 lapackInverse(): costs one divide.
Definition: SmallMatrixMixed.h:843
K::THerm THerm
Definition: CompositeNumericalTypes.h:144
const TDiag & diag() const
Definition: SymMat.h:822