1#ifndef SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_
2#define SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_
38template <
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)
51template <
int M,
class EL,
int CSL,
int RSL,
class ER,
int RSR>
inline
57template <
int M,
class EL,
int RSL,
class ER,
int CSR,
int RSR>
inline
62template <
int M,
class EL,
int RSL,
class ER,
int CSR,
int RSR>
inline
84template <
int M,
class E1,
int S1,
class E2,
int S2>
inline
85typename CNT<typename CNT<E1>::THerm>::template Result<E2>::Mul
90template <
class E1,
int S1,
class E2,
int S2>
inline
91typename CNT<typename CNT<E1>::THerm>::template Result<E2>::Mul
98template <
int N,
class E1,
int S1,
class E2,
int S2>
inline
99typename 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]);
104template <
class E1,
int S1,
class E2,
int S2>
inline
105typename CNT<E1>::template Result<E2>::Mul
112template <
int N,
class E1,
int S1,
class E2,
int S2>
inline
113typename CNT<E1>::template Result<E2>::Mul
117template <
int M,
class E1,
int S1,
class E2,
int S2>
inline
118typename CNT<E1>::template Result<E2>::Mul
122template <
int N,
class E1,
int S1,
class E2,
int S2>
inline
123typename CNT<E1>::template Result<E2>::Mul
145template <
int M,
class E1,
int S1,
class E2,
int S2>
inline
146Mat<M,M, typename CNT<E1>::template Result<typename CNT<E2>::THerm>::Mul>
149 for (
int i=0; i<M; ++i)
155template <
int M,
class E1,
int S1,
class E2,
int S2>
inline
163template <
int M,
class E1,
int S1,
class E2,
int S2>
inline
164Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
168template <
int M,
class E1,
int S1,
class E2,
int S2>
inline
169Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
173template <
int M,
class E1,
int S1,
class E2,
int S2>
inline
174Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
182template <
int M,
int N,
class ME,
int CS,
int RS,
class E,
int S>
inline
186 for (
int i=0; i<M; ++i)
192template <
int M,
class E,
int S,
int N,
class ME,
int CS,
int RS>
inline
193typename Row<M,E,S>::template Result<Mat<M,N,ME,CS,RS> >::Mul
196 for (
int i=0; i<N; ++i)
204template <
int N,
class ME,
int RS,
class E,
int S>
inline
205typename 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)
221template <
class ME,
int RS,
class E,
int S>
inline
222typename SymMat<1,ME,RS>::template Result<Vec<1,E,S> >::Mul
225 result[0] = m.
getDiag()[0]*v[0];
230template <
class ME,
int RS,
class E,
int S>
inline
231typename SymMat<2,ME,RS>::template Result<Vec<2,E,S> >::Mul
240template <
class ME,
int RS,
class E,
int S>
inline
241typename SymMat<3,ME,RS>::template Result<Vec<3,E,S> >::Mul
251template <
int M,
class E,
int S,
class ME,
int RS>
inline
252typename 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)
268template <
class E,
int S,
class ME,
int RS>
inline
269typename Row<1,E,S>::template Result<SymMat<1,ME,RS> >::Mul
272 result[0] = r[0]*m.
getDiag()[0];
277template <
class E,
int S,
class ME,
int RS>
inline
278typename Row<2,E,S>::template Result<SymMat<2,ME,RS> >::Mul
287template <
class E,
int S,
class ME,
int RS>
inline
288typename Row<3,E,S>::template Result<SymMat<3,ME,RS> >::Mul
303template <
int M,
class E1,
int S1,
int N,
class E2,
int S2>
inline
309template <
int M,
class E1,
int S1,
int MM,
int NN,
class E2,
int CS2,
int RS2>
inline
313 ::MulOpNonConforming::perform(v,m);
316template <
int M,
class E1,
int S1,
int MM,
class E2,
int RS2>
inline
320 ::MulOpNonConforming::perform(v,m);
323template <
int M,
class E1,
int S1,
int MM,
class E2,
int S2>
inline
327 ::MulOpNonConforming::perform(v1,v2);
334template <
int M,
class E,
int S,
int MM,
int NN,
class ME,
int CS,
int RS>
inline
335typename Row<M,E,S>::template Result<Mat<MM,NN,ME,CS,RS> >::MulNon
338 ::MulOpNonConforming::perform(r,m);
341template <
int N,
class E1,
int S1,
int M,
class E2,
int S2>
inline
342typename Row<N,E1,S1>::template Result<Vec<M,E2,S2> >::MulNon
345 ::MulOpNonConforming::perform(r,v);
348template <
int N1,
class E1,
int S1,
int N2,
class E2,
int S2>
inline
349typename Row<N1,E1,S1>::template Result<Row<N2,E2,S2> >::MulNon
352 ::MulOpNonConforming::perform(r1,r2);
358template <
int M,
int N,
class ME,
int CS,
int RS,
int MM,
class E,
int S>
inline
362 ::MulOpNonConforming::perform(m,v);
365template <
int M,
int N,
class ME,
int CS,
int RS,
int NN,
class E,
int S>
inline
369 ::MulOpNonConforming::perform(m,r);
373template <
int M,
int N,
class ME,
int CS,
int RS,
int Dim,
class E,
int S>
inline
377 ::MulOpNonConforming::perform(m,sy);
411template <
class E1,
int S1,
class E2,
int S2>
inline
412Vec<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]);
417template <
class E1,
int S1,
class E2,
int S2>
inline
418Vec<3,typename CNT<E1>::template Result<E2>::Mul>
422template <
class E1,
int S1,
class E2,
int S2>
inline
423Row<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]);
428template <
class E1,
int S1,
class E2,
int S2>
inline
429Row<3,typename CNT<E1>::template Result<E2>::Mul>
433template <
class E1,
int S1,
class E2,
int S2>
inline
434Row<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]);
439template <
class E1,
int S1,
class E2,
int S2>
inline
440Row<3,typename CNT<E1>::template Result<E2>::Mul>
444template <
class E1,
int S1,
class E2,
int S2>
inline
445Row<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]);
450template <
class E1,
int S1,
class E2,
int S2>
inline
451Row<3,typename CNT<E1>::template Result<E2>::Mul>
465template <
class E1,
int S1,
int N,
class E2,
int CS,
int RS>
inline
466Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
469 for (
int j=0; j < N; ++j)
470 result(j) = v % m(j);
473template <
class E1,
int S1,
int N,
class E2,
int CS,
int RS>
inline
474Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
479template <
class E1,
int S1,
int N,
class E2,
int S2,
int S3>
inline
480Row< N,Vec<3,typename CNT<E1>::template Result<E2>::Mul> >
483 for (
int j=0; j < N; ++j)
484 result(j) = v % m(j);
488template <
class E1,
int S1,
class E2,
int S2,
int S3>
inline
489Row< 3,Vec<3,typename CNT<E1>::template Result<E2>::Mul> >
492 for (
int j=0; j < 3; ++j)
493 result(j) = v % m(j);
496template <
class E1,
int S1,
int N,
class E2,
int S2,
int S3>
inline
497Row< N,Vec<3,typename CNT<E1>::template Result<E2>::Mul> >
499{
return cross(v,m); }
500template <
class E1,
int S1,
class E2,
int S2,
int S3>
inline
501Row< 3,Vec<3,typename CNT<E1>::template Result<E2>::Mul> >
503{
return cross(v,m); }
508template<
class EV,
int SV,
class EM,
int RS>
inline
509Mat<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 );
523template <
class EV,
int SV,
class EM,
int RS>
inline
524Mat<3,3,typename CNT<EV>::template Result<EM>::Mul>
528template <
class E1,
int S1,
int N,
class E2,
int CS,
int RS>
inline
529Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
533template <
class E1,
int S1,
int N,
class E2,
int CS,
int RS>
inline
534Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
538template<
class EV,
int SV,
class EM,
int RS>
inline
539Mat<3,3,typename CNT<EV>::template Result<EM>::Mul>
543template<
class EV,
int SV,
class EM,
int RS>
inline
544Mat<3,3,typename CNT<EV>::template Result<EM>::Mul>
548template <
int M,
class EM,
int CS,
int RS,
class EV,
int S>
inline
549Mat<M,3,typename CNT<EM>::template Result<EV>::Mul>
552 for (
int i=0; i < M; ++i)
553 result[i] = m[i] % v;
556template <
int M,
class EM,
int CS,
int RS,
class EV,
int S>
inline
557Mat<M,3,typename CNT<EM>::template Result<EV>::Mul>
563template<
class EM,
int RS,
class EV,
int SV>
inline
564Mat<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 );
578template<
class EM,
int RS,
class EV,
int SV>
inline
579Mat<3,3,typename CNT<EM>::template Result<EV>::Mul>
583template <
int M,
class EM,
int CS,
int RS,
class ER,
int S>
inline
584Mat<M,3,typename CNT<EM>::template Result<ER>::Mul>
588template <
int M,
class EM,
int CS,
int RS,
class ER,
int S>
inline
589Mat<M,3,typename CNT<EM>::template Result<ER>::Mul>
593template<
class EM,
int RS,
class EV,
int SV>
inline
594Mat<3,3,typename CNT<EM>::template Result<EV>::Mul>
598template<
class EM,
int RS,
class EV,
int SV>
inline
599Mat<3,3,typename CNT<EM>::template Result<EV>::Mul>
606template <
class E1,
int S1,
class E2,
int S2>
inline
607typename CNT<E1>::template Result<E2>::Mul
609 return a[0]*b[1]-a[1]*b[0];
611template <
class E1,
int S1,
class E2,
int S2>
inline
612typename CNT<E1>::template Result<E2>::Mul
615template <
class E1,
int S1,
class E2,
int S2>
inline
616typename CNT<E1>::template Result<E2>::Mul
618 return a[0]*b[1]-a[1]*b[0];
620template <
class E1,
int S1,
class E2,
int S2>
inline
621typename CNT<E1>::template Result<E2>::Mul
624template <
class E1,
int S1,
class E2,
int S2>
inline
625typename CNT<E1>::template Result<E2>::Mul
627 return a[0]*b[1]-a[1]*b[0];
629template <
class E1,
int S1,
class E2,
int S2>
inline
630typename CNT<E1>::template Result<E2>::Mul
633template <
class E1,
int S1,
class E2,
int S2>
inline
634typename CNT<E1>::template Result<E2>::Mul
636 return a[0]*b[1]-a[1]*b[0];
638template <
class E1,
int S1,
class E2,
int S2>
inline
639typename CNT<E1>::template Result<E2>::Mul
647template <
class E,
int S>
inline
656template <
class E,
int S>
inline
667template <
class E,
int S>
inline
670template <
class E,
int S>
inline
676template <
class E,
int S>
inline
681template <
class E,
int S>
inline
687template <
class E,
int S>
inline
690template <
class E,
int S>
inline
715template <
class E,
int S>
inline
725 nx*v[2], ny*v[2], xx+yy );
729template <
class E,
int S>
inline
741 -v[0]*z, -v[1]*z, xx+yy );
744template <
class E,
int S>
inline
746template <
class E,
int S>
inline
753template <
class E,
int CS,
int RS>
inline
759template <
class E,
int RS>
inline
765template <
class E,
int CS,
int RS>
inline
769 return E(m(0,0)*m(1,1) - m(0,1)*m(1,0));
773template <
class E,
int RS>
inline
783template <
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)));
791template <
class E,
int RS>
inline
814template <
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));
833template <
int M,
class E,
int RS>
inline
842template <
class E,
int CS,
int RS>
inline
858template <
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);
898template <
class E,
int CS,
int RS>
inline
905template <
class E,
int RS>
inline
912template <
class E,
int CS,
int RS>
inline
914 const E d (
det(m) );
917 E(-ood*m(1,0)),
E( ood*m(0,0)));
921template <
class E,
int RS>
inline
923 const E d (
det(s) );
926 E(-ood*s(1,0)),
E(ood*s(0,0)));
933template <
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) );
967template <
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) );
995template <
int M,
class E,
int CS,
int RS>
inline
1002template <
int M,
int N,
class ELT,
int CS,
int RS>
inline
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition ExceptionMacros.h:285
#define SimTK_ASSERT1(cond, msg, a1)
Definition ExceptionMacros.h:374
Specialized information about Composite Numerical Types which allows us to define appropriate templat...
Definition CompositeNumericalTypes.h:136
K::StdNumber StdNumber
Definition CompositeNumericalTypes.h:163
K::TInvert TInvert
Definition CompositeNumericalTypes.h:157
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition Mat.h:97
TInvert invert() const
Definition SmallMatrixMixed.h:1004
void set(int i, int j, const ELT &value)
Variant of indexing operator that's scripting friendly to set entry (i, j)
Definition Mat.h:1227
Mat< N, M, EInvert, N, 1 > TInvert
Definition Mat.h:171
This is a fixed-length row vector designed for no-overhead inline computation.
Definition Row.h:132
const TPosTrans & positionalTranspose() const
Definition Row.h:494
This is a small, fixed-size symmetric or Hermitian matrix designed for no-overhead inline computation...
Definition SymMat.h:87
const TDiag & diag() const
Definition SymMat.h:822
const TDiag & getDiag() const
Definition SymMat.h:818
const EHerm & getEltUpper(int i, int j) const
Definition SymMat.h:842
SymMat< M, EInvert, 1 > TInvert
Definition SymMat.h:162
const E & getEltDiag(int i) const
Definition SymMat.h:834
const E & getEltLower(int i, int j) const
Definition SymMat.h:838
This is a fixed-length column vector designed for no-overhead inline computation.
Definition Vec.h:184
void set(int i, const E &value)
Variant of operator[] that's scripting friendly to set ith entry.
Definition Vec.h:995
negator<N>, where N is a number type (real, complex, conjugate), is represented in memory identically...
Definition negator.h:75
const Real E
e = Real(exp(1))
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition Assembler.h:37
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
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
Matrix_< E > operator*(const MatrixBase< E > &l, const typename CNT< E >::StdNumber &r)
Definition BigMatrix.h:605
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
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
CNT< typenameCNT< E1 >::THerm >::template Result< E2 >::Mul dot(const Vec< M, E1, S1 > &r, const Vec< M, E2, S2 > &v)
Definition SmallMatrixMixed.h:86
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
unsigned int sign(unsigned char u)
Definition Scalar.h:311
ELEM sum(const VectorBase< ELEM > &v)
Definition VectorMath.h:147
unsigned char square(unsigned char u)
Definition Scalar.h:349
bool operator==(const PhiMatrix &p1, const PhiMatrix &p2)
Definition SpatialAlgebra.h:791
E det(const Mat< 1, 1, E, CS, RS > &m)
Special case Mat 1x1 determinant. No computation.
Definition SmallMatrixMixed.h:754
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
bool operator!=(const L &left, const R &right)
Definition SimTKcommon/include/SimTKcommon/internal/common.h:641
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