00001
00002
00003
00004
00005
00006
00007 #ifndef SimTK_TRANSFORM_H
00008 #define SimTK_TRANSFORM_H
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
00035
00036
00037
00038
00039
00040
00041
00042 #include "SimTKcommon/SmallMatrix.h"
00043 #include "SimTKcommon/internal/BigMatrix.h"
00044 #include "SimTKcommon/internal/UnitVec.h"
00045 #include "SimTKcommon/internal/Quaternion.h"
00046 #include "SimTKcommon/internal/Rotation.h"
00047
00048 #include <iosfwd>
00049
00050
00051
00052
00053 namespace SimTK {
00054
00055
00056
00057 template <class P> class Transform_;
00058 template <class P> class InverseTransform_;
00059
00060 typedef Transform_<Real> Transform;
00061 typedef Transform_<float> fTransform;
00062 typedef Transform_<double> dTransform;
00063
00064
00065
00120
00121 template <class P>
00122 class Transform_ {
00123 public:
00125 Transform_() : R_BF(), p_BF(0) { }
00126
00128 Transform_( const Rotation_<P>& R, const Vec<3,P>& p ) : R_BF(R), p_BF(p) { }
00129
00132 Transform_( const Rotation_<P>& R ) : R_BF(R), p_BF(0) { }
00133
00136 Transform_( const Vec<3,P>& p ) : R_BF(), p_BF(p) { }
00137
00138
00139
00147
00148 inline Transform_& operator=( const InverseTransform_<P>& X );
00149
00153 Transform_& set( const Rotation_<P>& R, const Vec<3,P>& p ) { p_BF=p; R_BF=R; return *this; }
00154
00158 Transform_& setToZero() { R_BF.setRotationToIdentityMatrix(); p_BF = P(0); return *this; }
00159
00164 Transform_& setToNaN() { R_BF.setRotationToNaN(); p_BF.setToNaN(); return *this; }
00165
00168 const InverseTransform_<P>& invert() const { return *reinterpret_cast<const InverseTransform_<P>*>(this); }
00169
00172 InverseTransform_<P>& updInvert() { return *reinterpret_cast<InverseTransform_<P>*>(this); }
00173
00175 const InverseTransform_<P>& operator~() const {return invert();}
00176
00178 InverseTransform_<P>& operator~() {return updInvert();}
00179
00182 Transform_ compose(const Transform_& X_FY) const {
00183 return Transform_( R_BF * X_FY.R(), p_BF + R_BF * X_FY.p() );
00184 }
00185
00191
00192 inline Transform_ compose( const InverseTransform_<P>& X_FY ) const;
00193
00197 Vec<3,P> xformFrameVecToBase( const Vec<3,P>& vF ) const {return R_BF*vF;}
00198
00202 Vec<3,P> xformBaseVecToFrame( const Vec<3,P>& vB ) const { return ~R_BF*vB; }
00203
00207 Vec<3,P> shiftFrameStationToBase( const Vec<3,P>& sF ) const
00208 { return p_BF + xformFrameVecToBase(sF); }
00209
00213 Vec<3,P> shiftBaseStationToFrame( const Vec<3,P>& sB ) const
00214 { return xformBaseVecToFrame(sB - p_BF); }
00215
00217 const Rotation_<P>& R() const { return R_BF; }
00218
00220 Rotation_<P>& updR() { return R_BF; }
00221
00224 const typename Rotation_<P>::ColType& x() const { return R().x(); }
00227 const typename Rotation_<P>::ColType& y() const { return R().y(); }
00230 const typename Rotation_<P>::ColType& z() const { return R().z(); }
00231
00234 const InverseRotation_<P>& RInv() const { return ~R_BF; }
00235
00238 InverseRotation_<P>& updRInv() { return ~R_BF; }
00239
00241 const Vec<3,P>& p() const { return p_BF; }
00242
00245 Vec<3,P>& updP() { return p_BF; }
00246
00251 Transform_<P>& setP( const Vec<3,P>& p ) { p_BF=p; return *this; }
00252
00256 Vec<3,P> pInv() const { return -(~R_BF*p_BF); }
00257
00265 Transform_<P>& setPInv( const Vec<3,P>& p_FB ) { p_BF = -(R_BF*p_FB); return *this; }
00266
00270 const Mat<3,4,P>& asMat34() const { return Mat<3,4,P>::getAs(reinterpret_cast<const P*>(this)); }
00271
00273 Mat<3,4,P> toMat34() const { return asMat34(); }
00274
00276 Mat<4,4,P> toMat44() const {
00277 Mat<4,4,P> tmp;
00278 tmp.template updSubMat<3,4>(0,0) = asMat34();
00279 tmp[3] = Row<4,P>(0,0,0,1);
00280 return tmp;
00281 }
00282
00283
00284 const Vec<3,P>& T() const {return p();}
00285 Vec<3,P>& updT() {return updP();}
00286
00287 private:
00288
00289 Rotation_<P> R_BF;
00290 Vec<3,P> p_BF;
00291 };
00292
00293
00294
00306
00307 template <class P>
00308 class InverseTransform_ {
00309 public:
00311 InverseTransform_() : R_FB(), p_FB(0) { }
00312
00313
00314
00316 operator Transform_<P>() const { return Transform_<P>( R(), p() ); }
00317
00318
00319
00320
00321
00322
00323
00324 InverseTransform_& operator=( const Transform_<P>& X ) {
00325
00326
00327
00328 p_FB = X.pInv();
00329 R_FB = X.RInv();
00330 return *this;
00331 }
00332
00333
00334 const Transform_<P>& invert() const { return *reinterpret_cast<const Transform_<P>*>(this); }
00335 Transform_<P>& updInvert() { return *reinterpret_cast<Transform_<P>*>(this); }
00336
00337
00338 const Transform_<P>& operator~() const { return invert(); }
00339 Transform_<P>& operator~() { return updInvert(); }
00340
00341
00342
00343 Transform_<P> compose(const Transform_<P>& X_FY) const {
00344 return Transform_<P>( ~R_FB * X_FY.R(), ~R_FB *(X_FY.p() - p_FB) );
00345 }
00346
00347
00348
00349 Transform_<P> compose(const InverseTransform_<P>& X_FY) const {
00350 return Transform_<P>( ~R_FB * X_FY.R(), ~R_FB *(X_FY.p() - p_FB) );
00351 }
00352
00353
00354
00355 Vec<3,P> xformFrameVecToBase(const Vec<3,P>& vF) const {return ~R_FB*vF;}
00356 Vec<3,P> xformBaseVecToFrame(const Vec<3,P>& vB) const {return R_FB*vB;}
00357
00358
00359 Vec<3,P> shiftFrameStationToBase(const Vec<3,P>& sF) const { return ~R_FB*(sF-p_FB); }
00360 Vec<3,P> shiftBaseStationToFrame(const Vec<3,P>& sB) const { return R_FB*sB + p_FB; }
00361
00362 const InverseRotation_<P>& R() const {return ~R_FB;}
00363 InverseRotation_<P>& updR() {return ~R_FB;}
00364
00365 const typename InverseRotation_<P>::ColType& x() const {return R().x();}
00366 const typename InverseRotation_<P>::ColType& y() const {return R().y();}
00367 const typename InverseRotation_<P>::ColType& z() const {return R().z();}
00368
00369 const Rotation_<P>& RInv() const {return R_FB;}
00370 Rotation_<P>& updRInv() {return R_FB;}
00371
00375 Vec<3,P> p() const { return -(~R_FB*p_FB); }
00376
00377
00378
00379
00380
00381
00382 void setP( const Vec<3,P>& p_BF ) { p_FB = -(R_FB*p_BF); }
00383
00384
00385 const Vec<3,P>& pInv() const { return p_FB; }
00386 void setPInv( const Vec<3,P>& p ) { p_FB = p; }
00387
00390 Mat<3,4,P> toMat34() const { return Transform_<P>(*this).asMat34(); }
00391
00393 Mat<4,4,P> toMat44() const { return Transform_<P>(*this).toMat44(); }
00394
00395
00396 Vec<3,P> T() const {return p();}
00397
00398 private:
00399
00400
00401 Rotation_<P> R_FB;
00402 Vec<3,P> p_FB;
00403 };
00404
00405
00410
00411 template <class P, int S> inline Vec<3,P>
00412 operator*(const Transform_<P>& X_BF, const Vec<3,P,S>& s_F) {return X_BF.shiftFrameStationToBase(s_F);}
00413 template <class P, int S> inline Vec<3,P>
00414 operator*(const InverseTransform_<P>& X_BF, const Vec<3,P,S>& s_F) {return X_BF.shiftFrameStationToBase(s_F);}
00415 template <class P, int S> inline Vec<3,P>
00416 operator*(const Transform_<P>& X_BF, const Vec<3,negator<P>,S>& s_F) {return X_BF*Vec<3,P>(s_F);}
00417 template <class P, int S> inline Vec<3,P>
00418 operator*(const InverseTransform_<P>& X_BF, const Vec<3,negator<P>,S>& s_F) {return X_BF*Vec<3,P>(s_F);}
00420
00421
00426
00427 template <class P, int S> inline Vec<4,P>
00428 operator*(const Transform_<P>& X_BF, const Vec<4,P,S>& a_F) {
00429 assert(a_F[3]==0 || a_F[3]==1);
00430 const Vec<3,P,S>& v_F = Vec<3,P,S>::getAs(&a_F[0]);
00431
00432 Vec<4,P> out;
00433 if( a_F[3] == 0 ) { Vec<3,P>::updAs(&out[0]) = X_BF.xformFrameVecToBase(v_F); out[3] = 0; }
00434 else { Vec<3,P>::updAs(&out[0]) = X_BF.shiftFrameStationToBase(v_F); out[3] = 1; }
00435 return out;
00436 }
00437
00438 template <class P, int S> inline Vec<4,P>
00439 operator*(const InverseTransform_<P>& X_BF, const Vec<4,P,S>& a_F ) {
00440 assert(a_F[3]==0 || a_F[3]==1);
00441 const Vec<3,P,S>& v_F = Vec<3,P,S>::getAs(&a_F[0]);
00442
00443 Vec<4,P> out;
00444 if( a_F[3] == 0 ) { Vec<3,P>::updAs(&out[0]) = X_BF.xformFrameVecToBase(v_F); out[3] = 0; }
00445 else { Vec<3,P>::updAs(&out[0]) = X_BF.shiftFrameStationToBase(v_F); out[3] = 1; }
00446 return out;
00447 }
00448 template <class P, int S> inline Vec<4,P>
00449 operator*(const Transform_<P>& X_BF, const Vec<4,negator<P>,S>& s_F) {return X_BF*Vec<4,P>(s_F);}
00450 template <class P, int S> inline Vec<4,P>
00451 operator*(const InverseTransform_<P>& X_BF, const Vec<4,negator<P>,S>& s_F) {return X_BF*Vec<4,P>(s_F);}
00453
00454
00457
00458 template <class P, class E> inline Vector_<E>
00459 operator*(const Transform_<P>& X, const VectorBase<E>& v) {
00460 Vector_<E> result(v.size());
00461 for (int i = 0; i < v.size(); ++i)
00462 result[i] = X*v[i];
00463 return result;
00464 }
00465 template <class P, class E> inline Vector_<E>
00466 operator*(const VectorBase<E>& v, const Transform_<P>& X) {
00467 Vector_<E> result(v.size());
00468 for (int i = 0; i < v.size(); ++i)
00469 result[i] = X*v[i];
00470 return result;
00471 }
00472 template <class P, class E> inline RowVector_<E>
00473 operator*(const Transform_<P>& X, const RowVectorBase<E>& v) {
00474 RowVector_<E> result(v.size());
00475 for (int i = 0; i < v.size(); ++i)
00476 result[i] = X*v[i];
00477 return result;
00478 }
00479 template <class P, class E> inline RowVector_<E>
00480 operator*(const RowVectorBase<E>& v, const Transform_<P>& X) {
00481 RowVector_<E> result(v.size());
00482 for (int i = 0; i < v.size(); ++i)
00483 result[i] = X*v[i];
00484 return result;
00485 }
00486 template <class P, class E> inline Matrix_<E>
00487 operator*(const Transform_<P>& X, const MatrixBase<E>& v) {
00488 Matrix_<E> result(v.nrow(), v.ncol());
00489 for (int i = 0; i < v.nrow(); ++i)
00490 for (int j = 0; j < v.ncol(); ++j)
00491 result(i, j) = X*v(i, j);
00492 return result;
00493 }
00494 template <class P, class E> inline Matrix_<E>
00495 operator*(const MatrixBase<E>& v, const Transform_<P>& X) {
00496 Matrix_<E> result(v.nrow(), v.ncol());
00497 for (int i = 0; i < v.nrow(); ++i)
00498 for (int j = 0; j < v.ncol(); ++j)
00499 result(i, j) = X*v(i, j);
00500 return result;
00501 }
00502 template <class P, int N, class E, int S> inline Vec<N,E>
00503 operator*(const Transform_<P>& X, const Vec<N,E,S>& v) {
00504 Vec<N,E> result;
00505 for (int i = 0; i < N; ++i)
00506 result[i] = X*v[i];
00507 return result;
00508 }
00509 template <class P, int N, class E, int S> inline Vec<N,E>
00510 operator*(const Vec<N,E,S>& v, const Transform_<P>& X) {
00511 Vec<N,E> result;
00512 for (int i = 0; i < N; ++i)
00513 result[i] = X*v[i];
00514 return result;
00515 }
00516 template <class P, int N, class E, int S> inline Row<N,E>
00517 operator*(const Transform_<P>& X, const Row<N,E,S>& v) {
00518 Row<N,E> result;
00519 for (int i = 0; i < N; ++i)
00520 result[i] = X*v[i];
00521 return result;
00522 }
00523 template <class P, int N, class E, int S> inline Row<N,E>
00524 operator*(const Row<N,E,S>& v, const Transform_<P>& X) {
00525 Row<N,E> result;
00526 for (int i = 0; i < N; ++i)
00527 result[i] = X*v[i];
00528 return result;
00529 }
00530 template <class P, int M, int N, class E, int CS, int RS> inline Mat<M,N,E>
00531 operator*(const Transform_<P>& X, const Mat<M,N,E,CS,RS>& v) {
00532 Mat<M,N,E> result;
00533 for (int i = 0; i < M; ++i)
00534 for (int j = 0; j < N; ++j)
00535 result(i, j) = X*v(i, j);
00536 return result;
00537 }
00538 template <class P, int M, int N, class E, int CS, int RS> inline Mat<M,N,E>
00539 operator*(const Mat<M,N,E,CS,RS>& v, const Transform_<P>& X) {
00540 Mat<M,N,E> result;
00541 for (int i = 0; i < M; ++i)
00542 for (int j = 0; j < N; ++j)
00543 result(i, j) = X*v(i, j);
00544 return result;
00545 }
00546
00548
00549
00550
00551 template <class P> inline Transform_<P>&
00552 Transform_<P>::operator=( const InverseTransform_<P>& X ) {
00553
00554
00555 p_BF = X.p();
00556 R_BF = X.R();
00557 return *this;
00558 }
00559
00560 template <class P> inline Transform_<P>
00561 Transform_<P>::compose( const InverseTransform_<P>& X_FY ) const {
00562 return Transform_<P>( R_BF * X_FY.R(), p_BF + R_BF * X_FY.p() );
00563 }
00564
00567
00568 template <class P> inline Transform_<P>
00569 operator*(const Transform_<P>& X1, const Transform_<P>& X2) {return X1.compose(X2);}
00570 template <class P> inline Transform_<P>
00571 operator*(const Transform_<P>& X1, const InverseTransform_<P>& X2) {return X1.compose(X2);}
00572 template <class P> inline Transform_<P>
00573 operator*(const InverseTransform_<P>& X1, const Transform_<P>& X2) {return X1.compose(X2);}
00574 template <class P> inline Transform_<P>
00575 operator*(const InverseTransform_<P>& X1, const InverseTransform_<P>& X2) {return X1.compose(X2);}
00577
00580
00581 template <class P> inline bool
00582 operator==(const Transform_<P>& X1, const Transform_<P>& X2) {return X1.R()==X2.R() && X1.p()==X2.p();}
00583 template <class P> inline bool
00584 operator==(const InverseTransform_<P>& X1, const InverseTransform_<P>& X2) {return X1.R()==X2.R() && X1.p()==X2.p();}
00585 template <class P> inline bool
00586 operator==(const Transform_<P>& X1, const InverseTransform_<P>& X2) {return X1.R()==X2.R() && X1.p()==X2.p();}
00587 template <class P> inline bool
00588 operator==(const InverseTransform_<P>& X1, const Transform_<P>& X2) {return X1.R()==X2.R() && X1.p()==X2.p();}
00590
00592 template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream&
00593 operator<<(std::ostream&, const Transform_<P>&);
00595 template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream&
00596 operator<<(std::ostream&, const InverseTransform_<P>&);
00597
00598
00599
00600
00601 }
00602
00603
00604 #endif // SimTK_TRANSFORM_H_
00605
00606