You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
122 lines
3.5 KiB
122 lines
3.5 KiB
// Test Pose
|
|
#include <catch2/catch.hpp>
|
|
|
|
#include <Eigen/Geometry>
|
|
#include <igl/PI.h>
|
|
|
|
#include <autodiff/autodiff_types.hpp>
|
|
#include <interval/interval.hpp>
|
|
#include <physics/pose.hpp>
|
|
|
|
TEST_CASE("Poses to dofs", "[physics][pose]")
|
|
{
|
|
using namespace ipc::rigid;
|
|
int dim = GENERATE(2, 3);
|
|
int num_bodies = GENERATE(0, 1, 2, 3, 10, 1000);
|
|
Eigen::VectorXd dofs =
|
|
Eigen::VectorXd::Random(num_bodies * Pose<double>::dim_to_ndof(dim));
|
|
Poses<double> poses = Pose<double>::dofs_to_poses(dofs, dim);
|
|
Eigen::VectorXd returned_dofs = Pose<double>::poses_to_dofs(poses);
|
|
CHECK((dofs - returned_dofs).squaredNorm() == Approx(0));
|
|
}
|
|
|
|
TEST_CASE("Cast poses", "[physics][pose]")
|
|
{
|
|
using namespace ipc::rigid;
|
|
int dim = GENERATE(2, 3);
|
|
int num_bodies = GENERATE(0, 1, 2, 3, 10, 1000);
|
|
|
|
Eigen::VectorXf dof =
|
|
Eigen::VectorXf::Random(num_bodies * Pose<float>::dim_to_ndof(dim));
|
|
Poses<float> expected_posesf = Pose<float>::dofs_to_poses(dof, dim);
|
|
|
|
Poses<float> actual_posesf = cast<float>(cast<double>(expected_posesf));
|
|
|
|
CHECK(
|
|
(Pose<float>::poses_to_dofs(expected_posesf)
|
|
- Pose<float>::poses_to_dofs(actual_posesf))
|
|
.squaredNorm()
|
|
== Approx(0.0));
|
|
}
|
|
|
|
TEST_CASE("SE(3) ↦ SO(3)", "[physics][pose]")
|
|
{
|
|
using namespace ipc::rigid;
|
|
double angle;
|
|
Eigen::Vector3d axis;
|
|
|
|
SECTION("zero")
|
|
{
|
|
angle = 0;
|
|
axis = Eigen::Vector3d::Random();
|
|
}
|
|
SECTION("random")
|
|
{
|
|
angle = GENERATE(take(100, random(0.0, 2 * igl::PI)));
|
|
axis = Eigen::Vector3d::Random();
|
|
}
|
|
axis.normalize();
|
|
|
|
Pose<double> p = Pose<double>::Zero(3);
|
|
p.rotation = angle * axis;
|
|
Eigen::Matrix3d R_actual = p.construct_rotation_matrix();
|
|
Eigen::Matrix3d R_expected =
|
|
Eigen::AngleAxisd(angle, axis).toRotationMatrix();
|
|
CHECK((R_actual - R_expected).norm() == Approx(0).margin(1e-12));
|
|
}
|
|
|
|
TEST_CASE("∇²(SE(3) ↦ SO(3))", "[!benchmark][physics][pose]")
|
|
{
|
|
using namespace ipc::rigid;
|
|
typedef ipc::rigid::AutodiffType<Eigen::Dynamic, 12> Diff;
|
|
Diff::activate(12);
|
|
|
|
Pose<double> p;
|
|
p.position = Eigen::Vector3d::Zero();
|
|
p.rotation = Eigen::Vector3d(0, igl::PI, 0);
|
|
|
|
BENCHMARK("Compute R") { return p.construct_rotation_matrix(); };
|
|
|
|
Pose<Diff::DDouble1> d1p;
|
|
d1p.position = Diff::d1vars(0, Eigen::Vector3d::Zero());
|
|
d1p.rotation = Diff::d1vars(3, Eigen::Vector3d(0, igl::PI, 0));
|
|
|
|
BENCHMARK("Compute R DDouble1") { return d1p.construct_rotation_matrix(); };
|
|
|
|
Pose<Diff::DDouble2> d2p;
|
|
d2p.position = Diff::d2vars(0, Eigen::Vector3d::Zero());
|
|
d2p.rotation = Diff::d2vars(3, Eigen::Vector3d(0, igl::PI, 0));
|
|
|
|
BENCHMARK("Compute R DDouble2") { return d2p.construct_rotation_matrix(); };
|
|
}
|
|
|
|
TEST_CASE("Interval SE(3) ↦ SO(3)", "[!benchmark][physics][pose]")
|
|
{
|
|
using namespace ipc::rigid;
|
|
double angle;
|
|
Eigen::Vector3d axis;
|
|
|
|
SECTION("zero")
|
|
{
|
|
angle = 0;
|
|
axis = Eigen::Vector3d::Random();
|
|
}
|
|
SECTION("random")
|
|
{
|
|
angle = GENERATE(take(1, random(0.0, 2 * igl::PI)));
|
|
axis = Eigen::Vector3d::Random();
|
|
}
|
|
axis.normalize();
|
|
|
|
Pose<double> p = Pose<double>::Zero(3);
|
|
p.rotation = angle * axis;
|
|
BENCHMARK("Double SE(3) ↦ SO(3)")
|
|
{
|
|
Eigen::Matrix3d R = p.construct_rotation_matrix();
|
|
};
|
|
Pose<ipc::rigid::Interval> pI = p.cast<ipc::rigid::Interval>();
|
|
BENCHMARK("Interval SE(3) ↦ SO(3)")
|
|
{
|
|
Matrix3I R = pI.construct_rotation_matrix();
|
|
};
|
|
}
|
|
|