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.
 
 
 
 
 

255 lines
7.2 KiB

#include <array>
#include <iomanip>
#include <iostream>
#include <catch2/catch.hpp>
#include <finitediff.hpp>
#include <igl/PI.h>
#include <physics/mass.hpp>
#include <problems/split_distance_barrier_rb_problem.hpp>
#include <utils/not_implemented_error.hpp>
using namespace ipc;
using namespace ipc::rigid;
RigidBody rb_from_displacements(
const Eigen::MatrixXd& vertices,
const Eigen::MatrixXi& edges,
Pose<double> 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<double> pose_t0 = Pose<double>::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<double>::Zero(vertices.cols()),
/*force=*/Pose<double>::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<double> rb1_pose_t1 = Pose<double>::Zero(dim);
Pose<double> rb2_pose_t1 = Pose<double>::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<RigidBody> 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<double>({ { rb1_pose_t1, rb2_pose_t1 } });
double fx = rbp.compute_energy_term(x);
CHECK(fx == Approx(0.0));
x = rbp.poses_to_dofs<double>({ { 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<double> vel_1 = Pose<double>::Zero(dim),
vel_2 = Pose<double>::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<RigidBody> 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<double>::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<double> vel_1 = Pose<double>::Zero(dim),
vel_2 = Pose<double>::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<RigidBody> 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<double>::dim_to_ndof(dim));
x << vel_1.dof(), vel_2.dof();
Eigen::VectorXd grad_fx;
Eigen::SparseMatrix<double> 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<double> vel_1 = Pose<double>::Zero(dim),
vel_2 = Pose<double>::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<RigidBody> 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