Operators.h
Go to the documentation of this file.
1 /*
2  * Copyright 2012-2020 CNRS-UM LIRMM, CNRS-AIST JRL
3  */
4 
5 #pragma once
6 
7 #include "SpaceVecAlg"
8 
9 namespace sva
10 {
11 
12 // Operators implementation
13 
14 namespace sva_internal
15 {
16 
17 template<typename Derived1, typename Derived2, typename Derived3>
18 inline void colwiseCrossEq(const Eigen::MatrixBase<Derived1> & m1,
19  const Eigen::MatrixBase<Derived2> & m2,
20  Eigen::MatrixBase<Derived3> const & result)
21 {
22  Eigen::MatrixBase<Derived3> & result_nc = const_cast<Eigen::MatrixBase<Derived3> &>(result);
23  result_nc.row(0) = (m1.row(1) * m2.coeff(2) - m1.row(2) * m2.coeff(1));
24  result_nc.row(1) = (m1.row(2) * m2.coeff(0) - m1.row(0) * m2.coeff(2));
25  result_nc.row(2) = (m1.row(0) * m2.coeff(1) - m1.row(1) * m2.coeff(0));
26 }
27 
28 template<typename Derived1, typename Derived2, typename Derived3>
29 inline void colwiseCrossPlusEq(const Eigen::MatrixBase<Derived1> & m1,
30  const Eigen::MatrixBase<Derived2> & m2,
31  Eigen::MatrixBase<Derived3> const & result)
32 {
33  Eigen::MatrixBase<Derived3> & result_nc = const_cast<Eigen::MatrixBase<Derived3> &>(result);
34  result_nc.row(0) += (m1.row(1) * m2.coeff(2) - m1.row(2) * m2.coeff(1));
35  result_nc.row(1) += (m1.row(2) * m2.coeff(0) - m1.row(0) * m2.coeff(2));
36  result_nc.row(2) += (m1.row(0) * m2.coeff(1) - m1.row(1) * m2.coeff(0));
37 }
38 
39 template<typename Derived1, typename Derived2, typename Derived3>
40 inline void colwiseCrossMinusEq(const Eigen::MatrixBase<Derived1> & m1,
41  const Eigen::MatrixBase<Derived2> & m2,
42  Eigen::MatrixBase<Derived3> const & result)
43 {
44  Eigen::MatrixBase<Derived3> & result_nc = const_cast<Eigen::MatrixBase<Derived3> &>(result);
45  result_nc.row(0) -= (m1.row(1) * m2.coeff(2) - m1.row(2) * m2.coeff(1));
46  result_nc.row(1) -= (m1.row(2) * m2.coeff(0) - m1.row(0) * m2.coeff(2));
47  result_nc.row(2) -= (m1.row(0) * m2.coeff(1) - m1.row(1) * m2.coeff(0));
48 }
49 
50 template<typename Derived1, typename Derived2, typename Derived3>
51 inline void colwiseLeftMultEq(const Eigen::MatrixBase<Derived1> & m1,
52  const Eigen::MatrixBase<Derived2> & m2,
53  Eigen::MatrixBase<Derived3> const & result)
54 {
55  Eigen::MatrixBase<Derived3> & result_nc = const_cast<Eigen::MatrixBase<Derived3> &>(result);
56  for(typename Derived1::Index i = 0; i < m1.cols(); ++i)
57  {
58  result_nc.col(i) = m2 * m1.col(i);
59  }
60 }
61 
62 } // namespace sva_internal
63 
64 template<typename Derived>
65 inline Eigen::Block<Derived, 3, Eigen::Dynamic> motionAngular(Eigen::MatrixBase<Derived> & mv)
66 {
67  return Eigen::Block<Derived, 3, Eigen::Dynamic>(mv.derived(), 0, 0, 3, mv.cols());
68 }
69 
70 template<typename Derived>
71 inline Eigen::Block<const Derived, 3, Eigen::Dynamic> motionAngular(const Eigen::MatrixBase<Derived> & mv)
72 {
73  return Eigen::Block<const Derived, 3, Eigen::Dynamic>(mv.derived(), 0, 0, 3, mv.cols());
74 }
75 
76 template<typename Derived>
77 inline Eigen::Block<Derived, 3, Eigen::Dynamic> motionLinear(Eigen::MatrixBase<Derived> & mv)
78 {
79  return Eigen::Block<Derived, 3, Eigen::Dynamic>(mv.derived(), 3, 0, 3, mv.cols());
80 }
81 
82 template<typename Derived>
83 inline Eigen::Block<const Derived, 3, Eigen::Dynamic> motionLinear(const Eigen::MatrixBase<Derived> & mv)
84 {
85  return Eigen::Block<const Derived, 3, Eigen::Dynamic>(mv.derived(), 3, 0, 3, mv.cols());
86 }
87 
88 template<typename Derived>
89 inline Eigen::Block<Derived, 3, Eigen::Dynamic> forceCouple(Eigen::MatrixBase<Derived> & mv)
90 {
91  return Eigen::Block<Derived, 3, Eigen::Dynamic>(mv.derived(), 0, 0, 3, mv.cols());
92 }
93 
94 template<typename Derived>
95 inline Eigen::Block<const Derived, 3, Eigen::Dynamic> forceCouple(const Eigen::MatrixBase<Derived> & mv)
96 {
97  return Eigen::Block<const Derived, 3, Eigen::Dynamic>(mv.derived(), 0, 0, 3, mv.cols());
98 }
99 
100 template<typename Derived>
101 inline Eigen::Block<Derived, 3, Eigen::Dynamic> forceForce(Eigen::MatrixBase<Derived> & mv)
102 {
103  return Eigen::Block<Derived, 3, Eigen::Dynamic>(mv.derived(), 3, 0, 3, mv.cols());
104 }
105 
106 template<typename Derived>
107 inline Eigen::Block<const Derived, 3, Eigen::Dynamic> forceForce(const Eigen::MatrixBase<Derived> & mv)
108 {
109  return Eigen::Block<const Derived, 3, Eigen::Dynamic>(mv.derived(), 3, 0, 3, mv.cols());
110 }
111 
112 template<typename T>
113 inline MotionVec<T> MotionVec<T>::cross(const MotionVec<T> & mv2) const
114 {
115  return MotionVec<T>(angular().cross(mv2.angular()), angular().cross(mv2.linear()) + linear().cross(mv2.angular()));
116 }
117 
118 template<typename T>
119 template<typename Derived>
120 inline void MotionVec<T>::cross(const Eigen::MatrixBase<Derived> & mv2, Eigen::MatrixBase<Derived> & result) const
121 {
122  static_assert(Derived::RowsAtCompileTime == 6, "the matrix must have exactly 6 rows");
123  static_assert(std::is_same<typename Derived::Scalar, T>::value, "motion vec and matrix must be the same type");
124 
125  sva_internal::colwiseCrossEq(motionAngular(mv2), -angular_, motionAngular(result));
126 
127  sva_internal::colwiseCrossEq(motionLinear(mv2), -angular_, motionLinear(result));
128  sva_internal::colwiseCrossMinusEq(motionAngular(mv2), linear_, motionLinear(result));
129 }
130 
131 template<typename T>
132 inline ForceVec<T> MotionVec<T>::crossDual(const ForceVec<T> & fv2) const
133 {
134  return ForceVec<T>(angular().cross(fv2.couple()) + linear().cross(fv2.force()), angular().cross(fv2.force()));
135 }
136 
137 template<typename T>
138 template<typename Derived>
139 inline void MotionVec<T>::crossDual(const Eigen::MatrixBase<Derived> & fv2, Eigen::MatrixBase<Derived> & result) const
140 {
141  static_assert(Derived::RowsAtCompileTime == 6, "the matrix must have exactly 6 rows");
142  static_assert(std::is_same<typename Derived::Scalar, T>::value, "motion vec and matrix must be the same type");
143 
144  sva_internal::colwiseCrossEq(forceCouple(fv2), -angular_, forceCouple(result));
145  sva_internal::colwiseCrossMinusEq(forceForce(fv2), linear_, forceCouple(result));
146 
147  sva_internal::colwiseCrossEq(forceForce(fv2), -angular_, forceForce(result));
148 }
149 
150 template<typename T>
151 inline T MotionVec<T>::dot(const sva::ForceVec<T> & fv2) const
152 {
153  return angular().dot(fv2.couple()) + linear().dot(fv2.force());
154 }
155 
156 template<typename T>
157 inline ForceVec<T> RBInertia<T>::operator*(const MotionVec<T> & mv) const
158 {
159  return ForceVec<T>(inertia() * mv.angular() + momentum().cross(mv.linear()),
160  mass() * mv.linear() - momentum().cross(mv.angular()));
161 }
162 
163 template<typename T>
164 template<typename Derived>
165 inline void RBInertia<T>::mul(const Eigen::MatrixBase<Derived> & mv, Eigen::MatrixBase<Derived> & result) const
166 {
167  static_assert(Derived::RowsAtCompileTime == 6, "the matrix must have exactly 6 rows");
168  static_assert(std::is_same<typename Derived::Scalar, T>::value, "motion vec and matrix must be the same type");
169 
170  forceCouple(result).noalias() = inertia() * motionAngular(mv);
171  sva_internal::colwiseCrossMinusEq(motionLinear(mv), momentum(), forceCouple(result));
172 
173  forceForce(result).noalias() = motionLinear(mv) * mass();
174  sva_internal::colwiseCrossPlusEq(motionAngular(mv), momentum(), forceForce(result));
175 }
176 
177 template<typename T>
178 inline ABInertia<T> ABInertia<T>::operator+(const RBInertia<T> & rbI) const
179 {
180  Eigen::Matrix3<T> M, I;
181  M.template triangularView<Eigen::Lower>() = massMatrix() + Eigen::Matrix3<T>::Identity() * rbI.mass();
182  I.template triangularView<Eigen::Lower>() = inertia() + rbI.inertia();
183  return ABInertia<T>(M, gInertia() + vector3ToCrossMatrix(rbI.momentum()), I);
184 }
185 
186 template<typename T>
187 inline ForceVec<T> ABInertia<T>::operator*(const MotionVec<T> & mv) const
188 {
189  return ForceVec<T>(inertia() * mv.angular() + gInertia() * mv.linear(),
190  massMatrix() * mv.linear() + gInertia().transpose() * mv.angular());
191 }
192 
193 template<typename T>
194 template<typename Derived>
195 inline void ABInertia<T>::mul(const Eigen::MatrixBase<Derived> & mv, Eigen::MatrixBase<Derived> & result) const
196 {
197  static_assert(Derived::RowsAtCompileTime == 6, "the matrix must have exactly 6 rows");
198  static_assert(std::is_same<typename Derived::Scalar, T>::value, "motion vec and matrix must be the same type");
199 
200  forceCouple(result).noalias() = inertia() * motionAngular(mv);
201  forceCouple(result).noalias() += gInertia() * motionLinear(mv);
202 
203  forceForce(result).noalias() = inertia() * motionLinear(mv);
204  forceForce(result).noalias() += gInertia().transpose() * motionAngular(mv);
205 }
206 
207 template<typename T>
208 inline MotionVec<T> PTransform<T>::operator*(const MotionVec<T> & mv) const
209 {
210  return MotionVec<T>(angularMul(mv), linearMul(mv));
211 }
212 
213 template<typename T>
214 inline Eigen::Vector3<T> PTransform<T>::angularMul(const MotionVec<T> & mv) const
215 {
216  const Eigen::Matrix3<T> & E = rotation();
217  return (E * mv.angular()).eval();
218 }
219 
220 template<typename T>
221 inline Eigen::Vector3<T> PTransform<T>::linearMul(const MotionVec<T> & mv) const
222 {
223  const Eigen::Matrix3<T> & E = rotation();
224  const Eigen::Vector3<T> & r = translation();
225  return (E * (mv.linear() - r.cross(mv.angular()))).eval();
226 }
227 
228 template<typename T>
229 template<typename Derived>
230 inline void PTransform<T>::mul(const Eigen::MatrixBase<Derived> & mv, Eigen::MatrixBase<Derived> & result) const
231 {
232  static_assert(Derived::RowsAtCompileTime == 6, "the matrix must have exactly 6 rows");
233  static_assert(std::is_same<typename Derived::Scalar, T>::value, "motion vec and matrix must be the same type");
234 
235  const Eigen::Matrix3<T> & E = rotation();
236  const Eigen::Vector3<T> & r = translation();
237 
238  motionAngular(result).noalias() = E * motionAngular(mv);
239 
240  motionLinear(result).noalias() = motionLinear(mv);
241  sva_internal::colwiseCrossPlusEq(motionAngular(mv), r, motionLinear(result));
242  sva_internal::colwiseLeftMultEq(motionLinear(result), E, motionLinear(result));
243 }
244 
245 template<typename T>
246 inline MotionVec<T> PTransform<T>::invMul(const MotionVec<T> & mv) const
247 {
248  const Eigen::Matrix3<T> & E = rotation();
249  const Eigen::Vector3<T> & r = translation();
250  MotionVec<T> ret;
251  ret.angular_.noalias() = E.transpose() * mv.angular_;
252  ret.linear_.noalias() = E.transpose() * mv.linear_;
253  ret.linear_.noalias() += r.cross(ret.angular_);
254  return ret;
255 }
256 
257 template<typename T>
258 inline Eigen::Vector3<T> PTransform<T>::angularInvMul(const MotionVec<T> & mv) const
259 {
260  const Eigen::Matrix3<T> & E = rotation();
261  return (E.transpose() * mv.angular()).eval();
262 }
263 
264 template<typename T>
265 inline Eigen::Vector3<T> PTransform<T>::linearInvMul(const MotionVec<T> & mv) const
266 {
267  const Eigen::Matrix3<T> & E = rotation();
268  const Eigen::Vector3<T> & r = translation();
269  return (E.transpose() * mv.linear() + r.cross(E.transpose() * mv.angular())).eval();
270 }
271 
272 template<typename T>
273 template<typename Derived>
274 inline void PTransform<T>::invMul(const Eigen::MatrixBase<Derived> & mv, Eigen::MatrixBase<Derived> & result) const
275 {
276  static_assert(Derived::RowsAtCompileTime == 6, "the matrix must have exactly 6 rows");
277  static_assert(std::is_same<typename Derived::Scalar, T>::value, "motion vec and matrix must be the same type");
278 
279  const Eigen::Matrix3<T> & E = rotation();
280  const Eigen::Vector3<T> & r = translation();
281 
282  motionAngular(result).noalias() = E.transpose() * motionAngular(mv);
283 
284  motionLinear(result).noalias() = E.transpose() * motionLinear(mv);
285  sva_internal::colwiseCrossMinusEq(motionAngular(result), r, motionLinear(result));
286 }
287 
288 template<typename T>
289 inline ForceVec<T> PTransform<T>::dualMul(const ForceVec<T> & fv) const
290 {
291  return ForceVec<T>(coupleDualMul(fv), forceDualMul(fv));
292 }
293 
294 template<typename T>
295 inline Eigen::Vector3<T> PTransform<T>::coupleDualMul(const ForceVec<T> & fv) const
296 {
297  const Eigen::Matrix3<T> & E = rotation();
298  const Eigen::Vector3<T> & r = translation();
299  return (E * (fv.couple() - r.cross(fv.force()))).eval();
300 }
301 
302 template<typename T>
303 inline Eigen::Vector3<T> PTransform<T>::forceDualMul(const ForceVec<T> & fv) const
304 {
305  const Eigen::Matrix3<T> & E = rotation();
306  return (E * fv.force()).eval();
307 }
308 
309 template<typename T>
310 template<typename Derived>
311 inline void PTransform<T>::dualMul(const Eigen::MatrixBase<Derived> & fv, Eigen::MatrixBase<Derived> & result) const
312 {
313  static_assert(Derived::RowsAtCompileTime == 6, "the matrix must have exactly 6 rows");
314  static_assert(std::is_same<typename Derived::Scalar, T>::value, "force vec and matrix must be the same type");
315 
316  const Eigen::Matrix3<T> & E = rotation();
317  const Eigen::Vector3<T> & r = translation();
318 
319  forceCouple(result).noalias() = forceCouple(fv);
320  sva_internal::colwiseCrossPlusEq(forceForce(fv), r, forceCouple(result));
321  sva_internal::colwiseLeftMultEq(forceCouple(result), E, forceCouple(result));
322 
323  forceForce(result).noalias() = E * forceForce(fv);
324 }
325 
326 template<typename T>
327 inline ForceVec<T> PTransform<T>::transMul(const ForceVec<T> & fv) const
328 {
329  const Eigen::Matrix3<T> & E = rotation();
330  const Eigen::Vector3<T> & r = translation();
331  ForceVec<T> ret;
332  ret.force_.noalias() = E.transpose() * fv.force_;
333  ret.couple_.noalias() = E.transpose() * fv.couple_;
334  ret.couple_.noalias() += r.cross(ret.force_);
335  return ret;
336 }
337 
338 template<typename T>
339 inline Eigen::Vector3<T> PTransform<T>::coupleTransMul(const ForceVec<T> & fv) const
340 {
341  const Eigen::Matrix3<T> & E = rotation();
342  const Eigen::Vector3<T> & r = translation();
343 
344  return (E.transpose() * fv.couple_ + r.cross(E.transpose() * fv.force_)).eval();
345 }
346 
347 template<typename T>
348 inline Eigen::Vector3<T> PTransform<T>::forceTransMul(const ForceVec<T> & fv) const
349 {
350  const Eigen::Matrix3<T> & E = rotation();
351  return (E.transpose() * fv.force_).eval();
352 }
353 
354 template<typename T>
355 template<typename Derived>
356 inline void PTransform<T>::transMul(const Eigen::MatrixBase<Derived> & fv, Eigen::MatrixBase<Derived> & result) const
357 {
358  static_assert(Derived::RowsAtCompileTime == 6, "the matrix must have exactly 6 rows");
359  static_assert(std::is_same<typename Derived::Scalar, T>::value, "force vec and matrix must be the same type");
360 
361  const Eigen::Matrix3<T> & E = rotation();
362  const Eigen::Vector3<T> & r = translation();
363 
364  forceForce(result).noalias() = E.transpose() * forceForce(fv);
365 
366  forceCouple(result).noalias() = E.transpose() * forceCouple(fv);
367  sva_internal::colwiseCrossMinusEq(forceForce(result), r, forceCouple(result));
368 }
369 
370 template<typename T>
371 inline RBInertia<T> PTransform<T>::dualMul(const RBInertia<T> & rbI) const
372 {
373  const Eigen::Matrix3<T> & E = rotation();
374  const Eigen::Vector3<T> & r = translation();
375  Eigen::Matrix3<T> I;
376  I.template triangularView<Eigen::Lower>() =
377  E
378  * (rbI.inertia() + vector3ToCrossMatrix(r) * Eigen::vector3ToCrossMatrix<T>(rbI.momentum())
379  + Eigen::vector3ToCrossMatrix<T>(rbI.momentum() - rbI.mass() * r) * Eigen::vector3ToCrossMatrix<T>(r))
380  * E.transpose();
381  return RBInertia<T>(rbI.mass(), E * (rbI.momentum() - rbI.mass() * r), I);
382 }
383 
384 template<typename T>
385 inline RBInertia<T> PTransform<T>::transMul(const RBInertia<T> & rbI) const
386 {
387  const Eigen::Matrix3<T> & E = rotation();
388  const Eigen::Vector3<T> & r = translation();
389  Eigen::Matrix3<T> I;
390  I.template triangularView<Eigen::Lower>() =
391  E.transpose() * rbI.inertia() * E
392  - Eigen::vector3ToCrossMatrix<T>(r) * Eigen::vector3ToCrossMatrix<T>(E.transpose() * rbI.momentum())
393  - Eigen::vector3ToCrossMatrix<T>(E.transpose() * rbI.momentum() + rbI.mass() * r)
394  * Eigen::vector3ToCrossMatrix<T>(r);
395  return RBInertia<T>(rbI.mass(), E.transpose() * rbI.momentum() + rbI.mass() * r, I);
396 }
397 
398 template<typename T>
399 inline ABInertia<T> PTransform<T>::dualMul(const ABInertia<T> & rbI) const
400 {
401  const Eigen::Matrix3<T> & E = rotation();
402  const Eigen::Vector3<T> & r = translation();
403 
404  Eigen::Matrix3<T> massM = rbI.massMatrix();
405  Eigen::Matrix3<T> rCross = vector3ToCrossMatrix(r);
406  Eigen::Matrix3<T> tmpI = rbI.gInertia() - rCross * massM;
407 
408  Eigen::Matrix3<T> M, I;
409  M.template triangularView<Eigen::Lower>() = E * massM * E.transpose();
410  I.template triangularView<Eigen::Lower>() =
411  E * (rbI.inertia() - rCross * rbI.gInertia().transpose() + (tmpI * rCross)) * E.transpose();
412 
413  return ABInertia<T>(M, E * (tmpI)*E.transpose(), I);
414 }
415 
416 template<typename T>
417 inline ABInertia<T> PTransform<T>::transMul(const ABInertia<T> & rbI) const
418 {
419  const Eigen::Matrix3<T> & E = rotation();
420  const Eigen::Vector3<T> & r = translation();
421 
422  Eigen::Matrix3<T> Mp(E.transpose() * rbI.massMatrix() * E);
423  Eigen::Matrix3<T> Hp(E.transpose() * rbI.gInertia() * E);
424  Eigen::Matrix3<T> rCross(vector3ToCrossMatrix(r));
425 
426  Eigen::Matrix3<T> M, I;
427  M.template triangularView<Eigen::Lower>() = Mp;
428  I.template triangularView<Eigen::Lower>() =
429  (E.transpose() * rbI.inertia() * E + rCross * Hp.transpose() - (Hp + rCross * Mp) * rCross);
430  return ABInertia<T>(M, Hp + rCross * Mp, I);
431 }
432 
433 } // namespace sva
sva::MotionVec
Definition: fwd.h:11
sva::PTransform::invMul
MotionVec< T > invMul(const MotionVec< T > &mv) const
Definition: Operators.h:246
Eigen::Matrix3
Matrix< T, 3, 3 > Matrix3
Definition: EigenTypedef.h:20
SpaceVecAlg
sva::sva_internal::colwiseLeftMultEq
void colwiseLeftMultEq(const Eigen::MatrixBase< Derived1 > &m1, const Eigen::MatrixBase< Derived2 > &m2, Eigen::MatrixBase< Derived3 > const &result)
Definition: Operators.h:51
sva::MotionVec::crossDual
ForceVec< T > crossDual(const ForceVec< T > &fv2) const
Definition: Operators.h:132
sva::ForceVec
Definition: ForceVec.h:19
sva::forceForce
Eigen::Block< Derived, 3, Eigen::Dynamic > forceForce(Eigen::MatrixBase< Derived > &mv)
Definition: Operators.h:101
sva::PTransform::coupleTransMul
Eigen::Vector3< T > coupleTransMul(const ForceVec< T > &fv) const
Definition: Operators.h:339
sva::RBInertia::mass
T mass() const
Definition: RBInertia.h:56
sva::ABInertia::operator+
ABInertia< T > operator+(const ABInertia< T > &rbI) const
Definition: ABInertia.h:104
sva::sva_internal::colwiseCrossMinusEq
void colwiseCrossMinusEq(const Eigen::MatrixBase< Derived1 > &m1, const Eigen::MatrixBase< Derived2 > &m2, Eigen::MatrixBase< Derived3 > const &result)
Definition: Operators.h:40
sva::PTransform::forceTransMul
Eigen::Vector3< T > forceTransMul(const ForceVec< T > &fv) const
Definition: Operators.h:348
sva
Definition: ABInertia.h:10
sva::MotionVec::dot
T dot(const ForceVec< T > &fv2) const
Definition: Operators.h:151
sva::PTransform::mul
void mul(const Eigen::MatrixBase< Derived > &mv, Eigen::MatrixBase< Derived > &result) const
Definition: Operators.h:230
sva::MotionVec::angular
const vector3_t & angular() const
Definition: MotionVec.h:51
sva::ForceVec::couple
vector3_t & couple()
Definition: ForceVec.h:55
sva::PTransform::dualMul
ForceVec< T > dualMul(const ForceVec< T > &fv) const
Definition: Operators.h:289
Eigen::vector3ToCrossMatrix
Matrix3< T > vector3ToCrossMatrix(const Vector3< T > &vec)
Definition: EigenUtility.h:16
sva::PTransform::linearInvMul
Eigen::Vector3< T > linearInvMul(const MotionVec< T > &mv) const
Definition: Operators.h:265
Eigen::Vector3
Matrix< T, 3, 1 > Vector3
Definition: EigenTypedef.h:18
sva::ABInertia::gInertia
const matrix3_t & gInertia() const
Definition: ABInertia.h:69
sva::motionLinear
Eigen::Block< Derived, 3, Eigen::Dynamic > motionLinear(Eigen::MatrixBase< Derived > &mv)
Definition: Operators.h:77
sva::PTransform::angularMul
Eigen::Vector3< T > angularMul(const MotionVec< T > &mv) const
Definition: Operators.h:214
sva::forceCouple
Eigen::Block< Derived, 3, Eigen::Dynamic > forceCouple(Eigen::MatrixBase< Derived > &mv)
Definition: Operators.h:89
sva::PTransform::linearMul
Eigen::Vector3< T > linearMul(const MotionVec< T > &mv) const
Definition: Operators.h:221
sva::ABInertia::operator*
ABInertia< T > operator*(T2 scalar) const
Definition: ABInertia.h:142
sva::RBInertia::inertia
matrix3_t inertia() const
Definition: RBInertia.h:74
sva::MotionVec::linear
const vector3_t & linear() const
Definition: MotionVec.h:63
sva::PTransform::operator*
PTransform< T > operator*(const PTransform< T > &pt) const
Definition: PTransform.h:188
sva::ABInertia::massMatrix
matrix3_t massMatrix() const
Definition: ABInertia.h:60
sva::RBInertia
Definition: fwd.h:17
sva::sva_internal::colwiseCrossPlusEq
void colwiseCrossPlusEq(const Eigen::MatrixBase< Derived1 > &m1, const Eigen::MatrixBase< Derived2 > &m2, Eigen::MatrixBase< Derived3 > const &result)
Definition: Operators.h:29
sva::ForceVec::force
vector3_t & force()
Definition: ForceVec.h:77
sva::RBInertia::momentum
const vector3_t & momentum() const
Definition: RBInertia.h:62
sva::ABInertia
Definition: ABInertia.h:18
sva::motionAngular
Eigen::Block< Derived, 3, Eigen::Dynamic > motionAngular(Eigen::MatrixBase< Derived > &mv)
Definition: Operators.h:65
sva::PTransform::coupleDualMul
Eigen::Vector3< T > coupleDualMul(const ForceVec< T > &fv) const
Definition: Operators.h:295
sva::ABInertia::mul
void mul(const Eigen::MatrixBase< Derived > &mv, Eigen::MatrixBase< Derived > &result) const
Definition: Operators.h:195
sva::RBInertia::operator*
RBInertia< T > operator*(T2 scalar) const
Definition: RBInertia.h:134
sva::RBInertia::mul
void mul(const Eigen::MatrixBase< Derived > &mv, Eigen::MatrixBase< Derived > &result) const
Definition: Operators.h:165
sva::PTransform::angularInvMul
Eigen::Vector3< T > angularInvMul(const MotionVec< T > &mv) const
Definition: Operators.h:258
sva::PTransform::transMul
ForceVec< T > transMul(const ForceVec< T > &fv) const
Definition: Operators.h:327
sva::ABInertia::inertia
matrix3_t inertia() const
Definition: ABInertia.h:81
sva::MotionVec::cross
MotionVec< T > cross(const MotionVec< T > &mv2) const
Definition: Operators.h:113
sva::PTransform::forceDualMul
Eigen::Vector3< T > forceDualMul(const ForceVec< T > &fv) const
Definition: Operators.h:303
sva::sva_internal::colwiseCrossEq
void colwiseCrossEq(const Eigen::MatrixBase< Derived1 > &m1, const Eigen::MatrixBase< Derived2 > &m2, Eigen::MatrixBase< Derived3 > const &result)
Definition: Operators.h:18