24 #include <boost/random.hpp>
25 #include <Eigen/Geometry>
29 #include <boost/test/unit_test.hpp>
30 #include <boost/utility/binary.hpp>
42 #include <metapod/tools/spatial/lti.hh>
43 #include <metapod/tools/spatial/rm-general.hh>
45 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(metapod::Spatial::ltI<double>)
46 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(metapod::Spatial::RotationMatrixTpl<double>)
49 const metapod::Spatial::ltI<double> &
S,
50 const metapod::Spatial::RotationMatrixTpl<double> & R,
51 metapod::Spatial::ltI<double> & res)
53 res =
R.rotTSymmetricMatrix(
S);
58 void timeSelfAdj(
const Eigen::Matrix3d &
A,
const Eigen::Matrix3d & Sdense, Eigen::Matrix3d & ASA)
60 typedef Eigen::SelfAdjointView<const Eigen::Matrix3d, Eigen::Upper> Sym3;
62 ASA.triangularView<Eigen::Upper>() =
A *
S *
A.transpose();
65 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
79 Matrix3
M = Matrix3::Random();
80 M =
M *
M.transpose();
96 Matrix3
M = Matrix3::Random();
97 M =
M *
M.transpose();
115 for (
int i = 0;
i < 100; ++
i)
117 Matrix3
M = Matrix3::Random();
118 M =
M *
M.transpose();
121 BOOST_CHECK_GT((
v.transpose() * (
S *
v))[0], 0);
133 (Symmetric3::Vector3::Constant(1.) + Symmetric3::Vector3::Random());
147 p = Vector3::UnitY();
150 p = Vector3::UnitZ();
153 Matrix3 vx =
skew(
v);
154 Matrix3 vxvx2 = (vx * vx).eval();
160 double m = Eigen::internal::random<double>() + 1;
162 (
S -
m * Symmetric3::SkewSquare(
v)).matrix().
isApprox(
S.matrix() -
m * vxvx2, 1e-12));
165 S -= Symmetric3::SkewSquare(
v);
169 S -=
m * Symmetric3::SkewSquare(
v);
175 Matrix3
M = Matrix3::Random();
176 M =
M *
M.transpose();
178 for (
int i = 0;
i < 3; ++
i)
179 for (
int j = 0; j < 3; ++j)
180 BOOST_CHECK_SMALL(
S(
i, j) -
M(
i, j), Eigen::NumTraits<double>::dummy_precision());
187 Matrix3
R = (Eigen::Quaterniond(Eigen::Matrix<double, 4, 1>::Random())).normalized().matrix();
200 double kinetic_ref =
v.transpose() *
S.matrix() *
v;
201 double kinetic =
S.vtiv(
v);
202 BOOST_CHECK_SMALL(kinetic_ref - kinetic, 1e-12);
209 Matrix3 Vcross =
skew(
v);
210 Matrix3 M_ref(Vcross *
S.matrix());
223 Matrix3 Vcross =
skew(
v);
224 Matrix3 M_ref(
S.matrix() * Vcross);
264 const size_t NBT = 100000;
267 std::vector<Symmetric3> Sres(NBT);
268 std::vector<Matrix3> Rs(NBT);
269 for (
size_t i = 0;
i < NBT; ++
i)
270 Rs[
i] = (Eigen::Quaterniond(Eigen::Matrix<double, 4, 1>::Random())).normalized().matrix();
272 std::cout <<
"Pinocchio: ";
279 timer.
toc(std::cout, NBT);
290 typedef Eigen::Matrix3d Matrix3;
291 typedef Eigen::SelfAdjointView<Matrix3, Eigen::Upper> Sym3;
293 Matrix3
M = Matrix3::Random();
297 BOOST_CHECK((Scp - Scp.transpose()).isApprox(Matrix3::Zero(), 1e-16));
300 Matrix3 M2 = Matrix3::Random();
301 M.triangularView<Eigen::Upper>() = M2;
303 Matrix3
A = Matrix3::Random(), ASA1, ASA2;
304 ASA1.triangularView<Eigen::Upper>() =
A *
S *
A.transpose();
308 Matrix3 Masa1 = ASA1.selfadjointView<Eigen::Upper>();
309 Matrix3 Masa2 = ASA2.selfadjointView<Eigen::Upper>();
313 const size_t NBT = 100000;
314 std::vector<Eigen::Matrix3d> Sres(NBT);
315 std::vector<Eigen::Matrix3d> Rs(NBT);
316 for (
size_t i = 0;
i < NBT; ++
i)
317 Rs[
i] = (Eigen::Quaterniond(Eigen::Matrix<double, 4, 1>::Random())).normalized().matrix();
319 std::cout <<
"Eigen: ";
326 timer.
toc(std::cout, NBT);
349 BOOST_AUTO_TEST_SUITE_END()