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
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
|
|
|