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 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 112 template <
int N,
class E1,
int S1,
class E2,
int S2>
inline 117 template <
int M,
class E1,
int S1,
class E2,
int S2>
inline 122 template <
int N,
class E1,
int S1,
class E2,
int S2>
inline 145 template <
int M,
class E1,
int S1,
class E2,
int S2>
inline 149 for (
int i=0; i<M; ++i)
155 template <
int M,
class E1,
int S1,
class E2,
int S2>
inline 163 template <
int M,
class E1,
int S1,
class E2,
int S2>
inline 168 template <
int M,
class E1,
int S1,
class E2,
int S2>
inline 173 template <
int M,
class E1,
int S1,
class E2,
int S2>
inline 182 template <
int M,
int N,
class ME,
int CS,
int RS,
class E,
int S>
inline 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 196 for (
int i=0; i<N; ++i)
204 template <
int N,
class ME,
int RS,
class E,
int S>
inline 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 225 result[0] = m.
getDiag()[0]*v[0];
230 template <
class ME,
int RS,
class E,
int S>
inline 240 template <
class ME,
int RS,
class E,
int S>
inline 251 template <
int M,
class E,
int S,
class ME,
int RS>
inline 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 272 result[0] = r[0]*m.
getDiag()[0];
277 template <
class E,
int S,
class ME,
int RS>
inline 287 template <
class E,
int S,
class ME,
int RS>
inline 303 template <
int M,
class E1,
int S1,
int N,
class E2,
int S2>
inline 309 template <
int M,
class E1,
int S1,
int MM,
int NN,
class E2,
int CS2,
int RS2>
inline 313 ::MulOpNonConforming::perform(v,m);
316 template <
int M,
class E1,
int S1,
int MM,
class E2,
int RS2>
inline 320 ::MulOpNonConforming::perform(v,m);
323 template <
int M,
class E1,
int S1,
int MM,
class E2,
int S2>
inline 327 ::MulOpNonConforming::perform(v1,v2);
334 template <
int M,
class E,
int S,
int MM,
int NN,
class ME,
int CS,
int RS>
inline 338 ::MulOpNonConforming::perform(r,m);
341 template <
int N,
class E1,
int S1,
int M,
class E2,
int S2>
inline 345 ::MulOpNonConforming::perform(r,v);
348 template <
int N1,
class E1,
int S1,
int N2,
class E2,
int S2>
inline 352 ::MulOpNonConforming::perform(r1,r2);
358 template <
int M,
int N,
class ME,
int CS,
int RS,
int MM,
class E,
int S>
inline 362 ::MulOpNonConforming::perform(m,v);
365 template <
int M,
int N,
class ME,
int CS,
int RS,
int NN,
class E,
int S>
inline 369 ::MulOpNonConforming::perform(m,r);
373 template <
int M,
int N,
class ME,
int CS,
int RS,
int Dim,
class E,
int S>
inline 377 ::MulOpNonConforming::perform(m,sy);
411 template <
class E1,
int S1,
class E2,
int S2>
inline 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 422 template <
class E1,
int S1,
class E2,
int S2>
inline 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 433 template <
class E1,
int S1,
class E2,
int S2>
inline 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 444 template <
class E1,
int S1,
class E2,
int S2>
inline 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 465 template <
class E1,
int S1,
int N,
class E2,
int CS,
int RS>
inline 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 479 template <
class E1,
int S1,
int N,
class E2,
int S2,
int S3>
inline 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 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 499 {
return cross(v,m); }
500 template <
class E1,
int S1,
class E2,
int S2,
int S3>
inline 503 {
return cross(v,m); }
508 template<
class EV,
int SV,
class EM,
int RS>
inline 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 528 template <
class E1,
int S1,
int N,
class E2,
int CS,
int RS>
inline 533 template <
class E1,
int S1,
int N,
class E2,
int CS,
int RS>
inline 538 template<
class EV,
int SV,
class EM,
int RS>
inline 543 template<
class EV,
int SV,
class EM,
int RS>
inline 548 template <
int M,
class EM,
int CS,
int RS,
class EV,
int S>
inline 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 563 template<
class EM,
int RS,
class EV,
int SV>
inline 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 583 template <
int M,
class EM,
int CS,
int RS,
class ER,
int S>
inline 588 template <
int M,
class EM,
int CS,
int RS,
class ER,
int S>
inline 593 template<
class EM,
int RS,
class EV,
int SV>
inline 598 template<
class EM,
int RS,
class EV,
int SV>
inline 606 template <
class E1,
int S1,
class E2,
int S2>
inline 609 return a[0]*b[1]-a[1]*b[0];
611 template <
class E1,
int S1,
class E2,
int S2>
inline 615 template <
class E1,
int S1,
class E2,
int S2>
inline 618 return a[0]*b[1]-a[1]*b[0];
620 template <
class E1,
int S1,
class E2,
int S2>
inline 624 template <
class E1,
int S1,
class E2,
int S2>
inline 627 return a[0]*b[1]-a[1]*b[0];
629 template <
class E1,
int S1,
class E2,
int S2>
inline 633 template <
class E1,
int S1,
class E2,
int S2>
inline 636 return a[0]*b[1]-a[1]*b[0];
638 template <
class E1,
int S1,
class E2,
int S2>
inline 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 714 template <
class E,
int S>
inline 724 nx*v[2], ny*v[2], xx+yy );
728 template <
class E,
int S>
inline 740 -v[0]*z, -v[1]*z, xx+yy );
743 template <
class E,
int S>
inline 745 template <
class E,
int S>
inline 752 template <
class E,
int CS,
int RS>
inline 758 template <
class E,
int RS>
inline 764 template <
class E,
int CS,
int RS>
inline 768 return E(m(0,0)*m(1,1) - m(0,1)*m(1,0));
772 template <
class E,
int RS>
inline 782 template <
class E,
int CS,
int RS>
inline 784 return E( m(0,0)*(m(1,1)*m(2,2)-m(1,2)*m(2,1))
785 - m(0,1)*(m(1,0)*m(2,2)-m(1,2)*m(2,0))
786 + m(0,2)*(m(1,0)*m(2,1)-m(1,1)*m(2,0)));
790 template <
class E,
int RS>
inline 813 template <
int M,
class E,
int CS,
int RS>
inline 818 const Mat<M-1,M,
E,CS,RS>& m2 = m.template getSubMat<M-1,M>(1,0);
819 for (
int j=0; j < M; ++j) {
821 result += sign*m(0,j)*
det(m2.dropCol(j));
832 template <
int M,
class E,
int RS>
inline 841 template <
class E,
int CS,
int RS>
inline 857 template <
int M,
class E,
int CS,
int RS>
inline 870 Raw* rawData =
reinterpret_cast<Raw*
>(&inv(0,0));
875 Lapack::getrf<Raw>(M,M,rawData,M,&ipiv[0],info);
876 SimTK_ASSERT1(info>=0,
"Argument %d to Lapack getrf routine was bad", -info);
878 "Matrix is singular so can't be inverted (Lapack getrf info=%d).", info);
888 Lapack::getri<Raw>(M,rawData,M,&ipiv[0],&work[0],M,info);
889 SimTK_ASSERT1(info>=0,
"Argument %d to Lapack getri routine was bad", -info);
891 "Matrix is singular so can't be inverted (Lapack getri info=%d).", info);
897 template <
class E,
int CS,
int RS>
inline 904 template <
class E,
int RS>
inline 911 template <
class E,
int CS,
int RS>
inline 913 const E d (
det(m) );
916 E(-ood*m(1,0)),
E( ood*m(0,0)));
920 template <
class E,
int RS>
inline 922 const E d (
det(s) );
925 E(-ood*s(1,0)),
E(ood*s(0,0)));
932 template <
class E,
int CS,
int RS>
inline 937 const E d00 (m(1,1)*m(2,2)-m(1,2)*m(2,1)),
938 nd01(m(1,2)*m(2,0)-m(1,0)*m(2,2)),
939 d02 (m(1,0)*m(2,1)-m(1,1)*m(2,0));
942 const E d (m(0,0)*d00 + m(0,1)*nd01 + m(0,2)*d02);
948 const E nd10(m(0,2)*m(2,1)-m(0,1)*m(2,2)),
949 d11 (m(0,0)*m(2,2)-m(0,2)*m(2,0)),
950 nd12(m(0,1)*m(2,0)-m(0,0)*m(2,1)),
951 d20 (m(0,1)*m(1,2)-m(0,2)*m(1,1)),
952 nd21(m(0,2)*m(1,0)-m(0,0)*m(1,2)),
953 d22 (m(0,0)*m(1,1)-m(0,1)*m(1,0));
956 (
E(ood* d00),
E(ood*nd10),
E(ood* d20),
957 E(ood*nd01),
E(ood* d11),
E(ood*nd21),
958 E(ood* d02),
E(ood*nd12),
E(ood* d22) );
966 template <
class E,
int RS>
inline 971 const E d00 (s(1,1)*s(2,2)-s(1,2)*s(2,1)),
972 nd01(s(1,2)*s(2,0)-s(1,0)*s(2,2)),
973 d02 (s(1,0)*s(2,1)-s(1,1)*s(2,0));
976 const E d (s(0,0)*d00 + s(0,1)*nd01 + s(0,2)*d02);
982 const E d11 (s(0,0)*s(2,2)-s(0,2)*s(2,0)),
983 nd12(s(0,1)*s(2,0)-s(0,0)*s(2,1)),
984 d22 (s(0,0)*s(1,1)-s(0,1)*s(1,0));
988 E(ood*nd01),
E(ood* d11),
989 E(ood* d02),
E(ood*nd12),
E(ood* d22) );
994 template <
int M,
class E,
int CS,
int RS>
inline 1001 template <
int M,
int N,
class ELT,
int CS,
int RS>
inline 1010 #endif //SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_ TInvert invert() const
Definition: SmallMatrixMixed.h:1003
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
const TDiag & getDiag() const
Definition: SymMat.h:818
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
const E & getEltLower(int i, int j) const
Definition: SymMat.h:838
This is a small, fixed-size symmetric or Hermitian matrix designed for no-overhead inline computation...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:608
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
bool operator==(const PhiMatrix &p1, const PhiMatrix &p2)
Definition: SpatialAlgebra.h:774
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:716
unsigned char square(unsigned char u)
Definition: Scalar.h:351
#define SimTK_ASSERT1(cond, msg, a1)
Definition: ExceptionMacros.h:375
ELEM sum(const VectorBase< ELEM > &v)
Definition: VectorMath.h:147
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:605
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
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:753
K::StdNumber StdNumber
Definition: CompositeNumericalTypes.h:163
bool operator!=(const conjugate< R > &a, const float &b)
Definition: conjugate.h:859
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:606
const TDiag & diag() const
Definition: SymMat.h:822
const Real E
e = Real(exp(1))
Mat< N, M, EInvert, N, 1 > TInvert
Definition: Mat.h:169
unsigned int sign(unsigned char u)
Definition: Scalar.h:311
const EHerm & getEltUpper(int i, int j) const
Definition: SymMat.h:842
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:898
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:607
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:842
const TPosTrans & positionalTranspose() const
Definition: Row.h:494
const E & getEltDiag(int i) const
Definition: SymMat.h:834