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 class InverseTransform;
00058
00059
00060
00119
00120 class Transform {
00121 public:
00123 Transform() : R_BF(), p_BF(0) { }
00124
00126 Transform( const Rotation& R, const Vec3& p ) : R_BF(R), p_BF(p) { }
00127
00130 Transform( const Rotation& R ) : R_BF(R), p_BF(0) { }
00131
00134 Transform( const Vec3& p ) : R_BF(), p_BF(p) { }
00135
00136
00137
00145
00146 inline Transform& operator=( const InverseTransform& X );
00147
00151 Transform& set( const Rotation& R, const Vec3& p ) { p_BF=p; R_BF=R; return *this; }
00152
00156 Transform& setToZero() { R_BF.setRotationToIdentityMatrix(); p_BF = 0.; return *this; }
00157
00162 Transform& setToNaN() { R_BF.setRotationToNaN(); p_BF.setToNaN(); return *this; }
00163
00166 const InverseTransform& invert() const { return *reinterpret_cast<const InverseTransform*>(this); }
00167
00170 InverseTransform& updInvert() { return *reinterpret_cast<InverseTransform*>(this); }
00171
00173 const InverseTransform& operator~() const {return invert();}
00174
00176 InverseTransform& operator~() {return updInvert();}
00177
00180 Transform compose(const Transform& X_FY) const {
00181 return Transform( R_BF * X_FY.R(), p_BF + R_BF * X_FY.p() );
00182 }
00183
00189
00190 inline Transform compose( const InverseTransform& X_FY ) const;
00191
00195 Vec3 xformFrameVecToBase( const Vec3& vF ) const {return R_BF*vF;}
00196
00200 Vec3 xformBaseVecToFrame( const Vec3& vB ) const { return ~R_BF*vB; }
00201
00205 Vec3 shiftFrameStationToBase( const Vec3& sF ) const { return p_BF + xformFrameVecToBase(sF); }
00206
00210 Vec3 shiftBaseStationToFrame( const Vec3& sB ) const { return xformBaseVecToFrame(sB - p_BF); }
00211
00213 const Rotation& R() const { return R_BF; }
00214
00216 Rotation& updR() { return R_BF; }
00217
00220 const Rotation::ColType& x() const { return R().x(); }
00223 const Rotation::ColType& y() const { return R().y(); }
00226 const Rotation::ColType& z() const { return R().z(); }
00227
00230 const InverseRotation& RInv() const { return ~R_BF; }
00231
00234 InverseRotation& updRInv() { return ~R_BF; }
00235
00237 const Vec3& p() const { return p_BF; }
00238
00241 Vec3& updP() { return p_BF; }
00242
00247 Transform& setP( const Vec3& p ) { p_BF=p; return *this; }
00248
00252 Vec3 pInv() const { return -(~R_BF*p_BF); }
00253
00261 Transform& setPInv( const Vec3& p_FB ) { p_BF = -(R_BF*p_FB); return *this; }
00262
00266 const Mat34& asMat34() const { return Mat34::getAs(reinterpret_cast<const Real*>(this)); }
00267
00269 Mat34 toMat34() const { return asMat34(); }
00270
00272 Mat44 toMat44() const {
00273 Mat44 tmp;
00274 tmp.updSubMat<3,4>(0,0) = asMat34();
00275 tmp[3] = Row4(0,0,0,1);
00276 return tmp;
00277 }
00278
00279
00280 const Vec3& T() const {return p();}
00281 Vec3& updT() {return updP();}
00282
00283 private:
00284
00285 Rotation R_BF;
00286 Vec3 p_BF;
00287 };
00288
00289
00290
00303
00304 class InverseTransform {
00305 public:
00306 InverseTransform() : R_FB(), p_FB(0) { }
00307
00308
00309
00310 operator Transform() const { return Transform( R(), p() ); }
00311
00312
00313
00314
00315
00316
00317
00318 InverseTransform& operator=( const Transform& X ) {
00319
00320
00321
00322 p_FB = X.pInv();
00323 R_FB = X.RInv();
00324 return *this;
00325 }
00326
00327
00328 const Transform& invert() const { return *reinterpret_cast<const Transform*>(this); }
00329 Transform& updInvert() { return *reinterpret_cast<Transform*>(this); }
00330
00331
00332 const Transform& operator~() const { return invert(); }
00333 Transform& operator~() { return updInvert(); }
00334
00335
00336
00337 Transform compose(const Transform& X_FY) const {
00338 return Transform( ~R_FB * X_FY.R(), ~R_FB *(X_FY.p() - p_FB) );
00339 }
00340
00341
00342
00343 Transform compose(const InverseTransform& X_FY) const {
00344 return Transform( ~R_FB * X_FY.R(), ~R_FB *(X_FY.p() - p_FB) );
00345 }
00346
00347
00348
00349 Vec3 xformFrameVecToBase(const Vec3& vF) const {return ~R_FB*vF;}
00350 Vec3 xformBaseVecToFrame(const Vec3& vB) const {return R_FB*vB;}
00351
00352
00353 Vec3 shiftFrameStationToBase(const Vec3& sF) const { return ~R_FB*(sF-p_FB); }
00354 Vec3 shiftBaseStationToFrame(const Vec3& sB) const { return R_FB*sB + p_FB; }
00355
00356 const InverseRotation& R() const {return ~R_FB;}
00357 InverseRotation& updR() {return ~R_FB;}
00358
00359 const InverseRotation::ColType& x() const {return R().x();}
00360 const InverseRotation::ColType& y() const {return R().y();}
00361 const InverseRotation::ColType& z() const {return R().z();}
00362
00363 const Rotation& RInv() const {return R_FB;}
00364 Rotation& updRInv() {return R_FB;}
00365
00366
00367 Vec3 p() const { return -(~R_FB*p_FB); }
00368
00369
00370
00371
00372
00373
00374 void setP( const Vec3& p_BF ) { p_FB = -(R_FB*p_BF); }
00375
00376
00377 const Vec3& pInv() const { return p_FB; }
00378 void setPInv( const Vec3& p ) { p_FB = p; }
00379
00382 Mat34 toMat34() const { return Transform(*this).asMat34(); }
00383
00385 Mat44 toMat44() const { return Transform(*this).toMat44(); }
00386
00387
00388 Vec3 T() const {return p();}
00389
00390 private:
00391
00392
00393 Rotation R_FB;
00394 Vec3 p_FB;
00395 };
00396
00397
00400 inline Vec3 operator*( const Transform& X_BF, const Vec3& s_F ) { return X_BF.shiftFrameStationToBase(s_F); }
00401 inline Vec3 operator*( const InverseTransform& X_BF, const Vec3& s_F ) { return X_BF.shiftFrameStationToBase(s_F); }
00402
00406
00407 inline Vec4 operator*( const Transform& X_BF, const Vec4& a_F ) {
00408 assert(a_F[3]==0 || a_F[3]==1);
00409 const Vec3& v_F = Vec3::getAs(&a_F[0]);
00410
00411 Vec4 out;
00412 if( a_F[3] == 0 ) { Vec3::updAs(&out[0]) = X_BF.xformFrameVecToBase(v_F); out[3] = 0; }
00413 else { Vec3::updAs(&out[0]) = X_BF.shiftFrameStationToBase(v_F); out[3] = 1; }
00414 return out;
00415 }
00416
00417
00418 inline Vec4 operator*( const InverseTransform& X_BF, const Vec4& a_F ) {
00419 assert(a_F[3]==0 || a_F[3]==1);
00420 const Vec3& v_F = Vec3::getAs(&a_F[0]);
00421
00422 Vec4 out;
00423 if( a_F[3] == 0 ) { Vec3::updAs(&out[0]) = X_BF.xformFrameVecToBase(v_F); out[3] = 0; }
00424 else { Vec3::updAs(&out[0]) = X_BF.shiftFrameStationToBase(v_F); out[3] = 1; }
00425 return out;
00426 }
00427
00429
00430 template <class E>
00431 inline Vector_<E> operator*(const Transform& X, const VectorBase<E>& v) {
00432 Vector_<E> result(v.size());
00433 for (int i = 0; i < v.size(); ++i)
00434 result[i] = X*v[i];
00435 return result;
00436 }
00437 template <class E>
00438 inline Vector_<E> operator*(const VectorBase<E>& v, const Transform& X) {
00439 Vector_<E> result(v.size());
00440 for (int i = 0; i < v.size(); ++i)
00441 result[i] = X*v[i];
00442 return result;
00443 }
00444 template <class E>
00445 inline RowVector_<E> operator*(const Transform& X, const RowVectorBase<E>& v) {
00446 RowVector_<E> result(v.size());
00447 for (int i = 0; i < v.size(); ++i)
00448 result[i] = X*v[i];
00449 return result;
00450 }
00451 template <class E>
00452 inline RowVector_<E> operator*(const RowVectorBase<E>& v, const Transform& X) {
00453 RowVector_<E> result(v.size());
00454 for (int i = 0; i < v.size(); ++i)
00455 result[i] = X*v[i];
00456 return result;
00457 }
00458 template <class E>
00459 inline Matrix_<E> operator*(const Transform& X, const MatrixBase<E>& v) {
00460 Matrix_<E> result(v.nrow(), v.ncol());
00461 for (int i = 0; i < v.nrow(); ++i)
00462 for (int j = 0; j < v.ncol(); ++j)
00463 result(i, j) = X*v(i, j);
00464 return result;
00465 }
00466 template <class E>
00467 inline Matrix_<E> operator*(const MatrixBase<E>& v, const Transform& X) {
00468 Matrix_<E> result(v.nrow(), v.ncol());
00469 for (int i = 0; i < v.nrow(); ++i)
00470 for (int j = 0; j < v.ncol(); ++j)
00471 result(i, j) = X*v(i, j);
00472 return result;
00473 }
00474 template <int N, class E>
00475 inline Vec<N,E> operator*(const Transform& X, const Vec<N,E>& v) {
00476 Vec<N,E> result;
00477 for (int i = 0; i < N; ++i)
00478 result[i] = X*v[i];
00479 return result;
00480 }
00481 template <int N, class E>
00482 inline Vec<N,E> operator*(const Vec<N,E>& v, const Transform& X) {
00483 Vec<N,E> result;
00484 for (int i = 0; i < N; ++i)
00485 result[i] = X*v[i];
00486 return result;
00487 }
00488 template <int N, class E>
00489 inline Row<N,E> operator*(const Transform& X, const Row<N,E>& v) {
00490 Row<N,E> result;
00491 for (int i = 0; i < N; ++i)
00492 result[i] = X*v[i];
00493 return result;
00494 }
00495 template <int N, class E>
00496 inline Row<N,E> operator*(const Row<N,E>& v, const Transform& X) {
00497 Row<N,E> result;
00498 for (int i = 0; i < N; ++i)
00499 result[i] = X*v[i];
00500 return result;
00501 }
00502 template <int M, int N, class E>
00503 inline Mat<M,N,E> operator*(const Transform& X, const Mat<M,N,E>& v) {
00504 Mat<M,N,E> result;
00505 for (int i = 0; i < M; ++i)
00506 for (int j = 0; j < N; ++j)
00507 result(i, j) = X*v(i, j);
00508 return result;
00509 }
00510 template <int M, int N, class E>
00511 inline Mat<M,N,E> operator*(const Mat<M,N,E>& v, const Transform& X) {
00512 Mat<M,N,E> result;
00513 for (int i = 0; i < M; ++i)
00514 for (int j = 0; j < N; ++j)
00515 result(i, j) = X*v(i, j);
00516 return result;
00517 }
00518
00520
00521
00522 inline Transform& Transform::operator=( const InverseTransform& X ) {
00523
00524
00525 p_BF = X.p();
00526 R_BF = X.R();
00527 return *this;
00528 }
00529
00530 inline Transform Transform::compose( const InverseTransform& X_FY ) const {
00531 return Transform( R_BF * X_FY.R(), p_BF + R_BF * X_FY.p() );
00532 }
00533
00534 inline Transform operator*( const Transform& X1, const Transform& X2 ) { return X1.compose(X2); }
00535 inline Transform operator*( const Transform& X1, const InverseTransform& X2 ) { return X1.compose(X2); }
00536 inline Transform operator*( const InverseTransform& X1, const Transform& X2 ) { return X1.compose(X2); }
00537 inline Transform operator*( const InverseTransform& X1, const InverseTransform& X2 ) { return X1.compose(X2); }
00538
00539 inline bool operator==( const Transform& X1, const Transform& X2 ) { return X1.R()==X2.R() && X1.p()==X2.p(); }
00540 inline bool operator==( const InverseTransform& X1, const InverseTransform& X2 ) { return X1.R()==X2.R() && X1.p()==X2.p(); }
00541 inline bool operator==( const Transform& X1, const InverseTransform& X2 ) { return X1.R()==X2.R() && X1.p()==X2.p(); }
00542 inline bool operator==( const InverseTransform& X1, const Transform& X2 ) { return X1.R()==X2.R() && X1.p()==X2.p(); }
00543
00544 SimTK_SimTKCOMMON_EXPORT std::ostream& operator<<( std::ostream& o, const Transform& );
00545
00546
00547
00548 }
00549
00550
00551 #endif // SimTK_TRANSFORM_H_
00552
00553