30 MOTION_TYPEDEF_TPL(Derived);
48 angular().setRandom();
52 ActionMatrixType toActionMatrix_impl()
const
63 ActionMatrixType toDualActionMatrix_impl()
const
74 HomogeneousMatrixType toHomogeneousMatrix_impl()
const
76 HomogeneousMatrixType M;
86 return linear() ==
other.linear() && angular() ==
other.angular();
92 return other.derived() == derived();
99 return derived().set(
other.derived());
104 return derived().set(
other.derived());
107 template<
typename D2>
110 linear() =
other.linear();
111 angular() =
other.angular();
115 template<
typename D2>
118 other.derived().setTo(derived());
122 template<
typename V6>
123 Derived & operator=(
const Eigen::MatrixBase<V6> & v)
132 MotionPlain operator-()
const
134 return derived().__opposite__();
136 template<
typename M1>
139 return derived().__plus__(v.derived());
141 template<
typename M1>
144 return derived().__minus__(v.derived());
147 template<
typename M1>
150 return derived().__pequ__(v.derived());
152 template<
typename M1>
155 v.derived().addTo(derived());
159 template<
typename M1>
162 return derived().__mequ__(v.derived());
165 MotionPlain __opposite__()
const
167 return MotionPlain(-linear(), -angular());
170 template<
typename M1>
173 return MotionPlain(linear() + v.linear(), angular() + v.angular());
176 template<
typename M1>
179 return MotionPlain(linear() - v.linear(), angular() - v.angular());
182 template<
typename M1>
185 linear() += v.linear();
186 angular() += v.angular();
190 template<
typename M1>
193 linear() -= v.linear();
194 angular() -= v.angular();
198 template<
typename OtherScalar>
201 return MotionPlain(
alpha * linear(),
alpha * angular());
204 template<
typename OtherScalar>
210 template<
typename F1>
213 return phi.linear().dot(linear()) +
phi.angular().dot(angular());
217 typename MotionAlgebraAction<D, Derived>::ReturnType cross_impl(
const D & d)
const
219 return d.motionAction(derived());
222 template<
typename M1,
typename M2>
225 mout.linear() = v.linear().cross(angular()) + v.angular().cross(linear());
226 mout.angular() = v.angular().cross(angular());
229 template<
typename M1>
233 motionAction(v, res);
237 template<
typename M2>
240 const Scalar &
prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
242 return derived().isApprox_impl(
m2,
prec);
245 template<
typename D2>
248 const Scalar &
prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
250 return linear().isApprox(
m2.linear(),
prec) && angular().isApprox(
m2.angular(),
prec);
253 bool isZero_impl(
const Scalar &
prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
255 return linear().isZero(
prec) && angular().isZero(
prec);
258 template<
typename S2,
int O2,
typename D2>
261 v.angular().noalias() = m.rotation() * angular();
262 v.linear().noalias() = m.rotation() * linear() + m.translation().cross(v.angular());
265 template<
typename S2,
int O2>
266 typename SE3GroupAction<Derived>::ReturnType se3Action_impl(
const SE3Tpl<S2, O2> & m)
const
268 typename SE3GroupAction<Derived>::ReturnType res;
269 se3Action_impl(m, res);
273 template<
typename S2,
int O2,
typename D2>
276 v.linear().noalias() =
277 m.rotation().transpose() * (linear() - m.translation().cross(angular()));
278 v.angular().noalias() = m.rotation().transpose() * angular();
281 template<
typename S2,
int O2>
282 typename SE3GroupAction<Derived>::ReturnType
285 typename SE3GroupAction<Derived>::ReturnType res;
286 se3ActionInverse_impl(m, res);
290 void disp_impl(std::ostream &
os)
const
292 os <<
" v = " << linear().transpose() << std::endl
293 <<
" w = " << angular().transpose() << std::endl;
299 return derived().ref();
305 MotionDense(
const MotionDense &) =
delete;
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...