spatial-dyn
spatial_motion.h
1 
10 #ifndef SPATIAL_DYN_EIGEN_SPATIAL_MOTION_H_
11 #define SPATIAL_DYN_EIGEN_SPATIAL_MOTION_H_
12 
13 #include "spatial_motion_base.h"
14 
15 namespace spatial_dyn {
16 
17 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
18 class SpatialMotion : public Eigen::PlainObjectBase<SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>> {
19 
20  public:
21  typedef Eigen::PlainObjectBase<SpatialMotion> Base;
22  enum { Options = _Options };
23 
24  // EIGEN_GENERIC_PUBLIC_INTERFACE(SpatialMotion)
25  typedef typename Eigen::internal::traits<SpatialMotion>::Scalar Scalar;
26  typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
27  typedef typename Base::CoeffReturnType CoeffReturnType;
28  typedef typename Eigen::internal::ref_selector<SpatialMotion>::type Nested;
29  typedef typename Eigen::internal::traits<SpatialMotion>::StorageKind StorageKind;
30  typedef typename Eigen::internal::traits<SpatialMotion>::StorageIndex StorageIndex;
31  enum CompileTimeTraits {
32  RowsAtCompileTime = Eigen::internal::traits<SpatialMotion>::RowsAtCompileTime,
33  ColsAtCompileTime = Eigen::internal::traits<SpatialMotion>::ColsAtCompileTime,
34  Flags = Eigen::internal::traits<SpatialMotion>::Flags,
35  SizeAtCompileTime = Base::SizeAtCompileTime,
36  MaxSizeAtCompileTime = Base::MaxSizeAtCompileTime,
37  IsVectorAtCompileTime = Base::IsVectorAtCompileTime
38  };
39  using Base::derived;
40  using Base::const_cast_derived;
41 
42  // EIGEN_DENSE_PUBLIC_INTERFACE(SpatialMotion)
43  typedef typename Base::PacketScalar PacketScalar;
44 
45  typedef typename Base::PlainObject PlainObject;
46 
47  using Base::base;
48  using Base::coeffRef;
49 
50  SpatialMotion();
51 
52  SpatialMotion(double cols);
53 
54  SpatialMotion(double rows, double cols);
55 
56  SpatialMotion(const SpatialMotion& other);
57 
59  EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible<_Scalar>::value);
60 
61  template<typename OtherDerived>
62  SpatialMotion(const Eigen::EigenBase<OtherDerived>& other);
63 
64  SpatialMotion(const _Scalar& lin_x, const _Scalar& lin_y, const _Scalar& lin_z,
65  const _Scalar& ang_x, const _Scalar& ang_y, const _Scalar& ang_z);
66 
67  template<typename DerivedLin, typename DerivedAng>
68  SpatialMotion(const Eigen::EigenBase<DerivedLin>& lin,
69  const Eigen::EigenBase<DerivedAng>& ang);
70 
71  SpatialMotion& operator=(const SpatialMotion& other);
72 
73  SpatialMotion& operator=(SpatialMotion&& other)
74  EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable<_Scalar>::value);
75 
76  template<typename OtherDerived>
77  SpatialMotion& operator=(const Eigen::DenseBase<OtherDerived>& other);
78 
79  template<typename OtherDerived>
80  SpatialMotion& operator=(const Eigen::EigenBase<OtherDerived>& other);
81 
82  template<typename OtherDerived>
83  SpatialMotion& operator=(const Eigen::ReturnByValue<OtherDerived>& func);
84 
85  SpatialMotion& operator=(const _Scalar &value);
86 
87  SpatialMotion& operator*=(const Eigen::Isometry3d& T);
88 
89  Eigen::Index innerStride() const;
90  Eigen::Index outerStride() const;
91 
92  protected:
93  using Base::m_storage;
94 
95 };
96 
97 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
99  : SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::Base() {
100  Base::_check_template_params();
101  EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
102 }
103 
104 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
105 inline SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::SpatialMotion(double cols)
106  : SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::Base() {
107  Base::resize(6, cols);
108 }
109 
110 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
111 inline SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::SpatialMotion(double rows, double cols)
112  : SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::Base() {
113  assert(rows == 6);
114  Base::resize(rows, cols);
115 }
116 
117 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
118 inline SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::SpatialMotion(const SpatialMotion& other)
119  : SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::Base(other) {}
120 
121 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
122 inline SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::SpatialMotion(SpatialMotion&& other)
123 EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible<_Scalar>::value)
124  : SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::Base(std::move(other)) {
125  Base::_check_template_params();
126  if (ColsAtCompileTime != Eigen::Dynamic) Base::_set_noalias(other);
127 }
128 
129 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
130 template<typename OtherDerived>
131 inline SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::SpatialMotion(
132  const Eigen::EigenBase<OtherDerived>& other)
133  : Base(other.derived()) {}
134 
135 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
136 inline SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::SpatialMotion(
137  const _Scalar& lin_x, const _Scalar& lin_y, const _Scalar& lin_z,
138  const _Scalar& ang_x, const _Scalar& ang_y, const _Scalar& ang_z) {
139  m_storage.data()[0] = lin_x;
140  m_storage.data()[1] = lin_y;
141  m_storage.data()[2] = lin_z;
142  m_storage.data()[3] = ang_x;
143  m_storage.data()[4] = ang_y;
144  m_storage.data()[5] = ang_z;
145 }
146 
147 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
148 template<typename DerivedLin, typename DerivedAng>
149 inline SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::SpatialMotion(
150  const Eigen::EigenBase<DerivedLin>& lin, const Eigen::EigenBase<DerivedAng>& ang) {
151  this->linear() = lin;
152  this->angular() = ang;
153 }
154 
155 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
156 inline SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>&
157 SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::operator=(const SpatialMotion& other) {
158  return Base::_set(other);
159 }
160 
161 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
162 inline SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>&
163 SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::operator=(SpatialMotion&& other)
164 EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable<_Scalar>::value) {
165  other.swap(*this);
166  return *this;
167 }
168 
169 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
170 template<typename OtherDerived>
171 inline SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>&
172 SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::operator=(
173  const Eigen::DenseBase<OtherDerived>& other) {
174  return Base::_set(other);
175 }
176 
177 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
178 template<typename OtherDerived>
179 inline SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>&
180 SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::operator=(
181  const Eigen::EigenBase<OtherDerived>& other) {
182  return Base::operator=(other);
183 }
184 
185 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
186 template<typename OtherDerived>
187 inline SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>&
188 SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::operator=(
189  const Eigen::ReturnByValue<OtherDerived>& func) {
190  return Base::operator=(func);
191 }
192 
193 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
194 inline SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>&
195 SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::operator=(const _Scalar& value) {
196  Base::setConstant(value);
197  return *this;
198 }
199 
200 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
201 inline SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>&
202 SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::operator*=(const Eigen::Isometry3d& T) {
203  *this = T * (*this);
204  return *this;
205 }
206 
207 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
208 inline Eigen::Index SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::innerStride() const {
209  return 1;
210 }
211 
212 template<typename _Scalar, int _Cols, int _Options, int _MaxCols>
213 inline Eigen::Index SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::outerStride() const {
214  return 6;
215 }
216 
217 } // namespace spatial_dyn
218 
219 #endif // SPATIAL_DYN_EIGEN_SPATIAL_MOTION_H_
Definition: spatial_motion.h:18
Definition: discrete_dynamics.cc:21