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.
 
 
 
 
 

75 lines
2.2 KiB

// Test the rigid body class.
#include <array>
#include <iomanip>
#include <iostream>
#include <catch2/catch.hpp>
#include <igl/PI.h>
#include <physics/rigid_body.hpp>
// ---------------------------------------------------
// Tests
// ---------------------------------------------------
using namespace ipc;
using namespace ipc::rigid;
RigidBody
simple(Eigen::MatrixXd& vertices, Eigen::MatrixXi& edges, Pose<double> velocity)
{
static int id = 0;
int ndof = Pose<double>::dim_to_ndof(vertices.cols());
return RigidBody(
vertices, edges, Pose<double>::Zero(vertices.cols()), velocity,
/*force=*/Pose<double>::Zero(vertices.cols()), /*density=*/1.0,
/*is_dof_fixed=*/VectorXb::Zero(ndof), /*oriented=*/false,
/*group=*/id++);
}
TEST_CASE("2D Rigid Body Transform", "[RB][RB-transform]")
{
// Test vertices positions for given rb position
Eigen::MatrixXd vertices_t0(4, 2);
Eigen::MatrixXi edges(4, 2);
Pose<double> velocity = Pose<double>::Zero(vertices_t0.cols());
Pose<double> rb_step = Pose<double>::Zero(vertices_t0.cols());
Eigen::MatrixXd vertices_step(4, 2), expected(4, 2);
vertices_t0 << -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")
{
rb_step.position << 0.5, 0.5;
rb_step.rotation << 0.0;
vertices_step = rb_step.position.transpose().replicate(4, 1);
}
SECTION("90 Deg Rotation Case")
{
rb_step.position << 0.0, 0.0;
rb_step.rotation << 0.5 * igl::PI;
vertices_step << 1.0, 0.0, 0.0, 1.0, -1.0, 0.0, 0.0, -1.0;
}
SECTION("Translation and Rotation Case")
{
rb_step.position << 0.5, 0.5;
rb_step.rotation << 0.5 * igl::PI;
vertices_step << 1.0, 0.0, 0.0, 1.0, -1.0, 0.0, 0.0, -1.0;
vertices_step += rb_step.position.transpose().replicate(4, 1);
}
expected = vertices_t0 + vertices_step;
auto rb = simple(vertices_t0, edges, velocity);
Pose<double> gamma_t1(
rb.pose.position + rb_step.position,
rb.pose.rotation + rb_step.rotation);
Eigen::MatrixXd actual = rb.world_vertices<double>(gamma_t1);
CHECK((expected - actual).squaredNorm() < 1E-6);
}
// TODO: Add 3D RB test