#include #include #include #include #include #include #include #include #include using namespace ipc; using namespace ipc::rigid; RigidBody rb_from_displacements( const Eigen::MatrixXd& vertices, const Eigen::MatrixXi& edges, Pose pose_t1) { static int id = 0; // move vertices so they center of mass is at 0,0 VectorMax3d x = compute_center_of_mass(vertices, edges); Eigen::MatrixXd centered_vertices = vertices.rowwise() - x.transpose(); // set position so current vertices match input Pose pose_t0 = Pose::Zero(vertices.cols()); pose_t0.position = x; // set previous_step position to: pose_t1.position += pose_t0.position; int ndof = pose_t0.ndof(); auto rb = RigidBody( vertices, edges, pose_t0, /*velocity=*/Pose::Zero(vertices.cols()), /*force=*/Pose::Zero(vertices.cols()), /*density=*/1, /*dof=*/VectorXb::Zero(ndof), /*oriented=*/false, /*group=*/id++); rb.pose = pose_t1; return rb; } TEST_CASE( "Rigid Body Problem Functional", "[RB][RB-Problem][RB-Problem-functional]") { Eigen::MatrixXd vertices(4, 2); int dim = vertices.cols(); Eigen::MatrixXi edges(4, 2); Pose rb1_pose_t1 = Pose::Zero(dim); Pose rb2_pose_t1 = Pose::Zero(dim); Eigen::MatrixXd expected(4, 2); vertices << -0.5, -0.5, 0.5, -0.5, 0.5, 0.5, -0.5, 0.5; edges << 0, 1, 1, 2, 2, 3, 3, 0; // expected displacement of nodes double dx = 0.0; double mass = 4.0; // ∑ mᵢ = 4 * 1.0 double moment_inertia = 8.0 * 0.5 * 0.5; // ∑ mᵢ ||rᵢ||² = 4 * 1.0 * 2.0 * (0.5)**2 SECTION("Translation Case") { rb1_pose_t1.position << 0.5, 0.5; rb2_pose_t1.position << 1.0, 1.0; dx = 0.5 * mass * (rb1_pose_t1.position.squaredNorm() + rb2_pose_t1.position.squaredNorm()); } SECTION("90 Deg Rotation Case") { rb1_pose_t1.rotation << 0.5 * igl::PI; rb2_pose_t1.rotation << igl::PI; dx = 0.5 * moment_inertia * (rb1_pose_t1.rotation.squaredNorm() + rb2_pose_t1.rotation.squaredNorm()); } std::vector rbs = { { rb_from_displacements(vertices, edges, rb1_pose_t1), rb_from_displacements(vertices, edges, rb2_pose_t1) } }; SplitDistanceBarrierRBProblem rbp; rbp.init(rbs); // displacement cases Eigen::VectorXd x = rbp.poses_to_dofs({ { rb1_pose_t1, rb2_pose_t1 } }); double fx = rbp.compute_energy_term(x); CHECK(fx == Approx(0.0)); x = rbp.poses_to_dofs({ { 2 * rb1_pose_t1, 2 * rb2_pose_t1 } }); fx = rbp.compute_energy_term(x); CHECK(fx == Approx(dx)); } TEST_CASE( "Rigid Body Problem Gradient", "[RB][RB-Problem][RB-Problem-gradient]") { Eigen::MatrixXd vertices(4, 2); int dim = vertices.cols(); Eigen::MatrixXi edges(4, 2); Pose vel_1 = Pose::Zero(dim), vel_2 = Pose::Zero(dim); Eigen::MatrixXd expected(4, 2); vertices << -0.5, -0.5, 0.5, -0.5, 0.5, 0.5, -0.5, 0.5; edges << 0, 1, 1, 2, 2, 3, 3, 0; SECTION("Translation Case") { vel_1.position << 0.5, 0.5; vel_2.position << 1.0, 1.0; } SECTION("90 Deg Rotation Case") { vel_1.rotation << 0.5 * igl::PI; vel_2.rotation << igl::PI; } SECTION("Translation and Rotation Case") { vel_1.position << 0.5, 0.5; vel_1.rotation << 0.5 * igl::PI; vel_2.position << 1.0, 1.0; vel_2.rotation << igl::PI; } std::vector rbs; rbs.push_back(rb_from_displacements(vertices, edges, vel_1)); rbs.push_back(rb_from_displacements(vertices, edges, vel_2)); SplitDistanceBarrierRBProblem rbp; rbp.init(rbs); // displacement cases Eigen::VectorXd x(2 * Pose::dim_to_ndof(dim)); x << vel_1.dof(), vel_2.dof(); Eigen::VectorXd grad_fx; double fx = rbp.compute_energy_term(x, grad_fx); Eigen::VectorXd grad_fx_approx = eval_grad_energy_approx(rbp, x); CHECK(fd::compare_gradient(grad_fx, grad_fx_approx)); } TEST_CASE("Rigid Body Problem Hessian", "[RB][RB-Problem][RB-Problem-hessian]") { Eigen::MatrixXd vertices(4, 2); int dim = vertices.cols(); Eigen::MatrixXi edges(4, 2); Pose vel_1 = Pose::Zero(dim), vel_2 = Pose::Zero(dim); Eigen::MatrixXd expected(4, 2); vertices << -0.5, -0.5, 0.5, -0.5, 0.5, 0.5, -0.5, 0.5; edges << 0, 1, 1, 2, 2, 3, 3, 0; SECTION("Translation Case") { vel_1.position << 0.5, 0.5; vel_2.position << 1.0, 1.0; } SECTION("90 Deg Rotation Case") { vel_1.rotation << 0.5 * igl::PI; vel_2.rotation << igl::PI; } SECTION("Translation and Rotation Case") { vel_1.position << 0.5, 0.5; vel_1.rotation << 0.5 * igl::PI; vel_2.position << 1.0, 1.0; vel_2.rotation << igl::PI; } std::vector rbs; rbs.push_back(rb_from_displacements(vertices, edges, vel_1)); rbs.push_back(rb_from_displacements(vertices, edges, vel_2)); SplitDistanceBarrierRBProblem rbp; rbp.init(rbs); // displacement cases Eigen::VectorXd x(2 * Pose::dim_to_ndof(dim)); x << vel_1.dof(), vel_2.dof(); Eigen::VectorXd grad_fx; Eigen::SparseMatrix hess_fx; rbp.compute_energy_term(x, grad_fx, hess_fx); Eigen::MatrixXd hess_fx_approx = eval_hess_energy_approx(rbp, x); CHECK(fd::compare_jacobian(hess_fx.toDense(), hess_fx_approx)); } TEST_CASE("dof -> poses -> dof", "[RB][RB-Problem]") { Eigen::MatrixXd vertices(4, 2); int dim = vertices.cols(); Eigen::MatrixXi edges(4, 2); Pose vel_1 = Pose::Zero(dim), vel_2 = Pose::Zero(dim); Eigen::MatrixXd expected(4, 2); vertices << -0.5, -0.5, 0.5, -0.5, 0.5, 0.5, -0.5, 0.5; edges << 0, 1, 1, 2, 2, 3, 3, 0; SECTION("Translation Case") { vel_1.position << 0.5, 0.5; vel_2.position << 1.0, 1.0; } SECTION("90 Deg Rotation Case") { vel_1.rotation << 0.5 * igl::PI; vel_2.rotation << igl::PI; } SECTION("Translation and Rotation Case") { vel_1.position << 0.5, 0.5; vel_1.rotation << 0.5 * igl::PI; vel_2.position << 1.0, 1.0; vel_2.rotation << igl::PI; } std::vector rbs = { { rb_from_displacements(vertices, edges, vel_1), rb_from_displacements(vertices, edges, vel_2) } }; SplitDistanceBarrierRBProblem rbp; rbp.init(rbs); for (int i = 0; i < 100; i++) { Eigen::VectorXd expected_dof = Eigen::VectorXd::Random(3 * rbs.size()); Eigen::VectorXd actual_dof = rbp.poses_to_dofs(rbp.dofs_to_poses(expected_dof)); REQUIRE(expected_dof.size() == actual_dof.size()); CHECK((expected_dof - actual_dof).squaredNorm() == Approx(0.0)); } } // TODO: Add 3D RB test