00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #ifndef EIGEN_LGSM_LIE_TWIST_H
00011 #define EIGEN_LGSM_LIE_TWIST_H
00012
00013
00014
00015
00016
00028 namespace internal {
00029 template<class Derived>
00030 struct traits<TwistBase<Derived> > : traits<LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived> > {
00031 };
00032 }
00033
00034 template<class Derived>
00035 class TwistBase : public LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived>{
00036 protected:
00038 typedef LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived> Base;
00039 public:
00040
00041 EIGEN_DENSE_PUBLIC_INTERFACE(TwistBase)
00042
00043 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(TwistBase)
00044
00046 typedef typename Base::BaseType BaseType;
00048 typedef typename internal::traits<Derived>::Coefficients Coefficients;
00049
00051 inline Scalar rx() const { return this->getAngularVelocity().x(); }
00053 inline Scalar ry() const { return this->getAngularVelocity().y(); }
00055 inline Scalar rz() const { return this->getAngularVelocity().z(); }
00057 inline Scalar vx() const { return this->getLinearVelocity().x(); }
00059 inline Scalar vy() const { return this->getLinearVelocity().y(); }
00061 inline Scalar vz() const { return this->getLinearVelocity().z(); }
00062
00064 inline Scalar& rx() { return this->getAngularVelocity().x(); }
00066 inline Scalar& ry() { return this->getAngularVelocity().y(); }
00068 inline Scalar& rz() { return this->getAngularVelocity().z(); }
00070 inline Scalar& vx() { return this->getLinearVelocity().x(); }
00072 inline Scalar& vy() { return this->getLinearVelocity().y(); }
00074 inline Scalar& vz() { return this->getLinearVelocity().z(); }
00075
00077 typedef Matrix<Scalar, 3, 1> LinearVelocity;
00079 typedef LieAlgebra<Matrix<Scalar, 3, 1> > AngularVelocity;
00080
00082 inline Map<AngularVelocity> getAngularVelocity(){ return Map<AngularVelocity>(this->derived().get().template head<3>().data()); }
00084 inline Map<const AngularVelocity> getAngularVelocity() const {return Map<const AngularVelocity>(this->derived().get().template head<3>().data()); }
00086 inline Map<LinearVelocity> getLinearVelocity() { return Map<LinearVelocity>(this->derived().get().template tail<3>().data()); }
00088 inline Map<const LinearVelocity> getLinearVelocity() const { return Map<const LinearVelocity>(this->derived().get().template tail<3>().data()); }
00089
00090 template<class RotationDerived> inline Twist<Scalar> changeFrame(const LieGroupBase<Quaternion<Scalar>, RotationDerived>&) const;
00091
00092 template<class OtherDerived> inline Twist<Scalar> changePoint(const MatrixBase<OtherDerived>& point) const;
00093 };
00094
00095
00096 template<class Derived>
00097 template<class RotationDerived>
00098 inline Twist<typename internal::traits<TwistBase<Derived> >::Scalar> TwistBase<Derived>::changeFrame(const LieGroupBase<Quaternion<Scalar>, RotationDerived>& rot) const {
00099 return Twist<Scalar>(rot*this->getAngularVelocity(),
00100 rot*this->getLinearVelocity());
00101 }
00102
00103 template<class Derived>
00104 template<class OtherDerived>
00105 inline Twist<typename internal::traits<TwistBase<Derived> >::Scalar> TwistBase<Derived>::changePoint(const MatrixBase<OtherDerived>& point) const {
00106 return Twist<Scalar>(this->getAngularVelocity(),
00107 this->getLinearVelocity() + this->getAngularVelocity().cross(point) );
00108 }
00109
00110
00111
00112
00113
00114 namespace internal {
00115 template<typename Scalar>
00116 struct traits<Twist<Scalar> >
00117 : public traits<LieAlgebra<Matrix<Scalar, 6, 1> > >
00118 {
00119 typedef Displacement<Scalar> Group;
00120 };
00121
00122 template<typename Scalar, int Options>
00123 struct traits<Map<Twist<Scalar>, Options> >
00124 : public traits<Map<LieAlgebra<Matrix<Scalar, 6, 1> >, Options > >
00125 {
00126 typedef Displacement<Scalar> Group;
00127 };
00128 }
00129
00140 template<typename _Scalar>
00141 class Twist : public TwistBase<Twist<_Scalar> >{
00142 protected:
00143 typedef TwistBase<Twist<_Scalar> > Base;
00144 public:
00145
00146 EIGEN_DENSE_PUBLIC_INTERFACE(Twist)
00147
00148 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Twist)
00149
00151 typedef typename Base::BaseType BaseType;
00153 typedef typename internal::traits<Twist>::Coefficients Coefficients;
00154
00155
00157 inline Twist() : m_coeffs() {}
00159 inline Twist(const Twist& g) : m_coeffs(g.get() ) {}
00161 inline Twist(const BaseType& g) : m_coeffs(g) {}
00163 inline Twist(Scalar rx, Scalar ry, Scalar rz, Scalar vx, Scalar vy, Scalar vz) {
00164 m_coeffs[0] = rx;
00165 m_coeffs[1] = ry;
00166 m_coeffs[2] = rz;
00167 m_coeffs[3] = vx;
00168 m_coeffs[4] = vy;
00169 m_coeffs[5] = vz;
00170 }
00172 template<typename OtherDerived>
00173 EIGEN_STRONG_INLINE Twist(const MatrixBase<OtherDerived>& other)
00174 : m_coeffs(other)
00175 {
00176 }
00178
00179
00180
00181
00182
00183
00185 template<class LinearVelocityDerived, class AngularVelocityDerived>
00186 inline Twist(const LieAlgebraBase<Matrix<Scalar, 3, 1>, AngularVelocityDerived>& r, const MatrixBase<LinearVelocityDerived>& v) {
00187 this->getLinearVelocity() = v;
00188 this->getAngularVelocity() = r;
00189 }
00191 inline Twist(const typename Base::AngularVelocity& r, const typename Base::LinearVelocity& v) {
00192 this->getLinearVelocity() = v;
00193 this->getAngularVelocity() = r;
00194 }
00196 inline Coefficients& get() { return m_coeffs; }
00198 inline const Coefficients& get() const { return m_coeffs; }
00199
00200 protected:
00202 Coefficients m_coeffs;
00203 };
00204
00206 typedef Twist<double> Twistd;
00208 typedef Twist<float> Twistf;
00209
00210
00211
00212
00213
00214
00226 template<typename _Scalar, int MapOptions, typename StrideType>
00227 class Map<Twist<_Scalar>, MapOptions, StrideType> : public TwistBase<Map<Twist<_Scalar>, MapOptions, StrideType> >{
00228 protected:
00229 typedef TwistBase<Map<Twist<_Scalar> > > Base;
00230 public:
00231 EIGEN_DENSE_PUBLIC_INTERFACE(Map)
00232 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
00233 typedef typename internal::traits<Map>::Coefficients Coefficients;
00234
00235 inline Map(const Twist<Scalar>& d) : m_coeffs(d.get()) {};
00236 template<int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
00237 inline Map(const Array<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& g) : m_coeffs(g.data()) {};
00238
00239 inline Map(Scalar* data) : m_coeffs(data) {};
00240 inline Map(const Map& m) : m_coeffs(m.get()) {};
00241
00242 inline Coefficients& get() { return m_coeffs; }
00243 inline const Coefficients& get() const { return m_coeffs; }
00244
00245 protected:
00246 Coefficients m_coeffs;
00247 };
00248
00249 #endif
00250