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.
79 lines
2.5 KiB
79 lines
2.5 KiB
2 years ago
|
#include <array>
|
||
|
|
#include <iomanip>
|
||
|
|
#include <iostream>
|
||
|
|
|
||
|
|
#include <catch2/catch.hpp>
|
||
|
|
|
||
|
|
#include <igl/PI.h>
|
||
|
|
|
||
|
|
#include <physics/rigid_body_assembler.hpp>
|
||
|
|
|
||
|
|
// ---------------------------------------------------
|
||
|
|
// Tests
|
||
|
|
// ---------------------------------------------------
|
||
|
|
using namespace ipc;
|
||
|
|
using namespace ipc::rigid;
|
||
|
|
|
||
|
|
RigidBody simple_rigid_body(
|
||
|
|
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=*/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("Rigid Body System Transform", "[RB][RB-System][RB-System-transform]")
|
||
|
|
{
|
||
|
|
Eigen::MatrixXd vertices(4, 2);
|
||
|
|
Eigen::MatrixXi edges(4, 2);
|
||
|
|
Pose<double> velocity = Pose<double>::Zero(vertices.cols());
|
||
|
|
Pose<double> rb1_pose_t1 = Pose<double>::Zero(vertices.cols());
|
||
|
|
Pose<double> rb2_pose_t1 = Pose<double>::Zero(vertices.cols());
|
||
|
|
|
||
|
|
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")
|
||
|
|
{
|
||
|
|
rb1_pose_t1.position << 0.5, 0.5;
|
||
|
|
rb2_pose_t1.position << 1.0, 1.0;
|
||
|
|
|
||
|
|
// expected displacements
|
||
|
|
expected.resize(8, 2);
|
||
|
|
expected.block(0, 0, 4, 2) =
|
||
|
|
rb1_pose_t1.position.transpose().replicate(4, 1);
|
||
|
|
expected.block(4, 0, 4, 2) =
|
||
|
|
rb2_pose_t1.position.transpose().replicate(4, 1);
|
||
|
|
}
|
||
|
|
|
||
|
|
SECTION("90 Deg Rotation Case")
|
||
|
|
{
|
||
|
|
rb1_pose_t1.rotation << 0.5 * igl::PI;
|
||
|
|
rb2_pose_t1.rotation << igl::PI;
|
||
|
|
expected.resize(8, 2);
|
||
|
|
expected.block(0, 0, 4, 2) << 1.0, 0.0, 0.0, 1.0, -1.0, 0.0, 0.0, -1.0;
|
||
|
|
expected.block(4, 0, 4, 2) << 1.0, 1.0, -1.0, 1.0, -1.0, -1.0, 1.0,
|
||
|
|
-1.0;
|
||
|
|
}
|
||
|
|
|
||
|
|
std::vector<RigidBody> rbs;
|
||
|
|
rbs.push_back(simple_rigid_body(vertices, edges, velocity));
|
||
|
|
rbs.push_back(simple_rigid_body(vertices, edges, velocity));
|
||
|
|
RigidBodyAssembler assembler;
|
||
|
|
assembler.init(rbs);
|
||
|
|
|
||
|
|
Poses<double> poses = { { rb1_pose_t1, rb2_pose_t1 } };
|
||
|
|
|
||
|
|
/// compute displacements between current and given positions
|
||
|
|
/// TODO: update test to not need displacements
|
||
|
|
Eigen::MatrixXd actual =
|
||
|
|
assembler.world_vertices(poses) - assembler.world_vertices();
|
||
|
|
CHECK((expected - actual).squaredNorm() < 1E-6);
|
||
|
|
}
|