30 #ifndef _RL_MATH_TRANSFORM_H_
31 #define _RL_MATH_TRANSFORM_H_
33 #define EIGEN_MATRIXBASE_PLUGIN <rl/math/MatrixBaseAddons.h>
35 #include <Eigen/Geometry>
44 typedef ::Eigen::Transform< Real, 3, ::Eigen::Affine >
Transform;
50 template<
typename Matrix1,
typename Matrix2,
typename Real >
53 distance(
const Matrix1& t1,
const Matrix2& t2,
const Real& weight = 1.0)
59 ::std::pow(t2(0, 3) - t1(0, 3), 2) +
60 ::std::pow(t2(1, 3) - t1(1, 3), 2) +
61 ::std::pow(t2(2, 3) - t1(2, 3), 2) +
62 weight * ::std::pow(q1.angularDistance(q2), 2)
66 template<
typename Matrix1,
typename Real,
typename Matrix2 >
76 t2(0, 0) = t1(0, 0) - c * t1(1, 0) + b * t1(2, 0);
77 t2(0, 1) = t1(0, 1) - c * t1(1, 1) + b * t1(2, 1);
78 t2(0, 2) = t1(0, 2) - c * t1(1, 2) + b * t1(2, 2);
79 t2(0, 3) = t1(0, 3) + x;
80 t2(1, 0) = t1(1, 0) + c * t1(0, 0) - a * t1(2, 0);
81 t2(1, 1) = t1(1, 1) + c * t1(0, 1) - a * t1(2, 1);
82 t2(1, 2) = t1(1, 2) + c * t1(0, 2) - a * t1(2, 2);
83 t2(1, 3) = t1(1, 3) + y;
84 t2(2, 0) = t1(2, 0) - b * t1(0, 0) + a * t1(1, 0);
85 t2(2, 1) = t1(2, 1) - b * t1(0, 1) + a * t1(1, 1);
86 t2(2, 2) = t1(2, 2) - b * t1(0, 2) + a * t1(1, 2);
87 t2(2, 3) = t1(2, 3) + z;
94 template<
typename Matrix1,
typename Vector,
typename Matrix2 >
101 fromDelta(t1, xyzabc(0), xyzabc(1), xyzabc(2), xyzabc(3), xyzabc(4), xyzabc(5), t2);
104 template<
typename Matrix,
typename Real >
115 a = (t(2, 1) - t(1, 2)) / 2.0f;
116 b = (t(0, 2) - t(2, 0)) / 2.0f;
117 c = (t(1, 0) - t(0, 1)) / 2.0f;
120 template<
typename Matrix,
typename Vector >
127 getDelta(t, xyzabc(0), xyzabc(1), xyzabc(2), xyzabc(3), xyzabc(4), xyzabc(5));
130 template<
typename Matrix,
typename Real >
138 assert(::std::abs(t(2, 0)) <= ::std::numeric_limits<Real>::epsilon());
142 theta = ::std::atan2(t(1, 0), t(0, 0));
144 if (::std::abs(t(0, 0)) <= ::std::numeric_limits<Real>::epsilon())
146 a = t(1, 3) / t(1, 0);
150 a = t(0, 3) / t(0, 0);
153 alpha = ::std::atan2(t(2, 1), t(2, 2));
156 template<
typename Real,
typename Matrix >
182 template<
typename Vector,
typename Matrix >
189 setDelta(xyzabc(0), xyzabc(1), xyzabc(2), xyzabc(3), xyzabc(4), xyzabc(5), t);
192 template<
typename Real,
typename Matrix >
200 Real cosAlpha = ::std::cos(alpha);
201 Real cosTheta = ::std::cos(theta);
202 Real sinAlpha = ::std::sin(alpha);
203 Real sinTheta = ::std::sin(theta);
209 t(0, 1) = -cosAlpha * sinTheta;
210 t(1, 1) = cosAlpha * cosTheta;
213 t(0, 2) = sinAlpha * sinTheta;
214 t(1, 2) = -sinAlpha * cosTheta;
217 t(0, 3) = a * cosTheta;
218 t(1, 3) = a * sinTheta;
223 template<
typename Matrix1,
typename Matrix2,
typename Real >
233 x = t2(0, 3) - t1(0, 3);
234 y = t2(1, 3) - t1(1, 3);
235 z = t2(2, 3) - t1(2, 3);
236 a = (t1(1, 0) * t2(2, 0) - t1(2, 0) * t2(1, 0) + t1(1, 1) * t2(2, 1) - t1(2, 1) * t2(1, 1) + t1(1, 2) * t2(2, 2) - t1(2, 2) * t2(1, 2)) / 2.0f;
237 b = (t1(2, 0) * t2(0, 0) - t1(0, 0) * t2(2, 0) + t1(2, 1) * t2(0, 1) - t1(0, 1) * t2(2, 1) + t1(2, 2) * t2(0, 2) - t1(0, 2) * t2(2, 2)) / 2.0f;
238 c = (t1(0, 0) * t2(1, 0) - t1(1, 0) * t2(0, 0) + t1(0, 1) * t2(1, 1) - t1(1, 1) * t2(0, 1) + t1(0, 2) * t2(1, 2) - t1(1, 2) * t2(0, 2)) / 2.0f;
241 template<
typename Matrix1,
typename Matrix2,
typename Vector >
248 toDelta(t1, t2, xyzabc(0), xyzabc(1), xyzabc(2), xyzabc(3), xyzabc(4), xyzabc(5));
254 #endif // _RL_MATH_TRANSFORM_H_