10 #ifndef SPATIAL_DYN_EIGEN_SPATIAL_MOTION_H_
11 #define SPATIAL_DYN_EIGEN_SPATIAL_MOTION_H_
13 #include "spatial_motion_base.h"
17 template<
typename _Scalar,
int _Cols,
int _Options,
int _MaxCols>
18 class SpatialMotion :
public Eigen::PlainObjectBase<SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>> {
21 typedef Eigen::PlainObjectBase<SpatialMotion> Base;
22 enum { Options = _Options };
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
40 using Base::const_cast_derived;
43 typedef typename Base::PacketScalar PacketScalar;
45 typedef typename Base::PlainObject PlainObject;
59 EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible<_Scalar>::value);
61 template<
typename OtherDerived>
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);
67 template<
typename DerivedLin,
typename DerivedAng>
69 const Eigen::EigenBase<DerivedAng>& ang);
74 EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable<_Scalar>::value);
76 template<
typename OtherDerived>
77 SpatialMotion& operator=(
const Eigen::DenseBase<OtherDerived>& other);
79 template<
typename OtherDerived>
80 SpatialMotion& operator=(
const Eigen::EigenBase<OtherDerived>& other);
82 template<
typename OtherDerived>
83 SpatialMotion& operator=(
const Eigen::ReturnByValue<OtherDerived>& func);
89 Eigen::Index innerStride()
const;
90 Eigen::Index outerStride()
const;
93 using Base::m_storage;
97 template<
typename _Scalar,
int _Cols,
int _Options,
int _MaxCols>
100 Base::_check_template_params();
101 EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
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);
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() {
114 Base::resize(rows, cols);
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) {}
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);
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()) {}
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;
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;
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);
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) {
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);
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);
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);
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);
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) {
207 template<
typename _Scalar,
int _Cols,
int _Options,
int _MaxCols>
208 inline Eigen::Index SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::innerStride()
const {
212 template<
typename _Scalar,
int _Cols,
int _Options,
int _MaxCols>
213 inline Eigen::Index SpatialMotion<_Scalar, _Cols, _Options, _MaxCols>::outerStride()
const {
Definition: spatial_motion.h:18
Definition: discrete_dynamics.cc:21