00001 #ifndef SimTK_SIMMATRIX_SPATIAL_ALGEBRA_H_
00002 #define SimTK_SIMMATRIX_SPATIAL_ALGEBRA_H_
00003
00004
00005
00006
00007
00008
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
00041 #include "SimTKcommon/SmallMatrix.h"
00042
00043 #include <iostream>
00044
00045 namespace SimTK {
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055 typedef Vec<2, Vec3> SpatialVec;
00056 typedef Row<2, Row3> SpatialRow;
00057 typedef Mat<2,2, Mat33> SpatialMat;
00058
00059
00060
00061
00062 using namespace SimTK;
00063
00064 class PhiMatrixTranspose;
00065
00066
00067 inline static Vec3
00068 unitVec(const Vec3& v) {
00069 Real tmpNorm = v.norm();
00070 if (tmpNorm < 1e-15) tmpNorm = 1e-15;
00071
00072 Vec3 ret = v/tmpNorm;
00073
00074 return ret;
00075 }
00076
00077 class PhiMatrix {
00078 public:
00079 typedef PhiMatrixTranspose TransposeType;
00080
00081 PhiMatrix() { setToNaN(); }
00082 PhiMatrix(const Vec3& l) : l_(l) {}
00083
00084 void setToZero() { l_ = 0.; }
00085 void setToNaN() { l_.setToNaN(); }
00086
00087 SpatialMat toSpatialMat() const {
00088 return SpatialMat(Mat33(1), crossMat(l_),
00089 Mat33(0), Mat33(1));
00090 }
00091
00092 const Vec3& l() const { return l_; }
00093 private:
00094 Vec3 l_;
00095 };
00096
00097 class PhiMatrixTranspose {
00098 public:
00099 PhiMatrixTranspose(const PhiMatrix& phi) : phi(phi) {}
00100
00101 SpatialMat toSpatialMat() const {
00102 return SpatialMat( Mat33(1) , Mat33(0),
00103 crossMat(-l()) , Mat33(1));
00104 }
00105
00106 const Vec3& l() const {return phi.l();}
00107 private:
00108 const PhiMatrix& phi;
00109 };
00110
00111 inline PhiMatrixTranspose
00112 transpose(const PhiMatrix& phi)
00113 {
00114 PhiMatrixTranspose ret(phi);
00115 return ret;
00116 }
00117
00118 inline PhiMatrixTranspose
00119 operator~(const PhiMatrix& phi) {return transpose(phi);}
00120
00121 inline SpatialVec
00122 operator*(const PhiMatrix& phi,
00123 const SpatialVec& v)
00124 {
00125 return SpatialVec(v[0] + phi.l() % v[1],
00126 v[1]);
00127 }
00128
00129 inline SpatialMat
00130 operator*(const PhiMatrix& phi,
00131 const SpatialMat& m)
00132 {
00133 const Mat33 x = crossMat(phi.l());
00134 return SpatialMat( m(0,0) + x*m(1,0), m(0,1) + x*m(1,1),
00135 m(1,0) , m(1,1));
00136 }
00137
00138 inline SpatialVec
00139 operator*(const PhiMatrixTranspose& phiT,
00140 const SpatialVec& v)
00141 {
00142 return SpatialVec(v[0],
00143 v[1] + v[0] % phiT.l());
00144 }
00145
00146
00147 inline SpatialMat
00148 operator*(const SpatialMat::THerm& m,
00149 const PhiMatrixTranspose& phiT)
00150 {
00151 const Mat33 x = crossMat(phiT.l());
00152 return SpatialMat( m(0,0) - m(0,1) * x, m(0,1),
00153 m(1,0) - m(1,1) * x, m(1,1) );
00154 }
00155
00156 inline SpatialMat
00157 operator*(const SpatialMat& m,
00158 const PhiMatrixTranspose& phiT)
00159 {
00160 const Mat33 x = crossMat(phiT.l());
00161 return SpatialMat( m(0,0) - m(0,1) * x, m(0,1),
00162 m(1,0) - m(1,1) * x, m(1,1) );
00163 }
00164
00165 inline bool
00166 operator==(const PhiMatrix& p1, const PhiMatrix& p2)
00167 {
00168 return p1.l() == p2.l();
00169 }
00170
00171 inline bool
00172 operator==(const PhiMatrixTranspose& p1, const PhiMatrixTranspose& p2)
00173 {
00174 return p1.l() == p2.l();
00175 }
00176 }
00177
00178 #endif // SimTK_SIMMATRIX_SPATIAL_ALGEBRA_H_