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.
104 lines
3.5 KiB
104 lines
3.5 KiB
#include <catch2/catch.hpp>
|
|
|
|
#include <logger.hpp>
|
|
#include <opt/distance_barrier_constraint.hpp>
|
|
|
|
using namespace ipc;
|
|
using namespace ipc::rigid;
|
|
|
|
// TEST_CASE(
|
|
// "Distance Barrier Constraint",
|
|
// "[opt][ccd][DistanceBarrier][DistanceBarrierConstraint]")
|
|
// {
|
|
// DistanceBarrierConstraint barrier;
|
|
//
|
|
// double barrier_epsilon = GENERATE(0.5, 1.0, 5.0);
|
|
//
|
|
// barrier.detection_method = ipc::rigid::DetectionMethod::BRUTE_FORCE;
|
|
// barrier.initial_barrier_activation_distance = barrier_epsilon;
|
|
// barrier.barrier_activation_distance(barrier_epsilon);
|
|
// barrier.active_constraint_scale = 10000;
|
|
//
|
|
// Eigen::MatrixX2d vertices(4, 2);
|
|
// Eigen::MatrixX2i edges(2, 2);
|
|
// Poses<double> poses_t1(2, Pose<double>::Zero(/*dim=*/2));
|
|
//
|
|
// edges.row(0) << 0, 1;
|
|
// edges.row(1) << 0, 1;
|
|
//
|
|
// // initial configuration
|
|
// // |3 edge_1
|
|
// // |2
|
|
// //
|
|
// // 0-------1 edge_0
|
|
// //
|
|
// vertices.row(0) << -0.5, 0.0;
|
|
// vertices.row(1) << 0.5, 0.0;
|
|
// vertices.row(2) << 0.0, -0.25;
|
|
// vertices.row(3) << 0.0, 0.25;
|
|
//
|
|
// auto expected_barrier_func = [&](double d) {
|
|
// #ifdef USE_DISTANCE_SQUARED
|
|
// return barrier.distance_barrier<double>(d * d, barrier_epsilon);
|
|
// #else
|
|
// return barrier.distance_barrier<double>(d, barrier_epsilon);
|
|
// #endif
|
|
// };
|
|
//
|
|
// Eigen::VectorXd expected_barrier = Eigen::VectorXd(4);
|
|
// SECTION("No displacements")
|
|
// {
|
|
// poses_t1[1].position << 0.0, 0.75;
|
|
// expected_barrier << expected_barrier_func(0.5),
|
|
// expected_barrier_func(1.0), expected_barrier_func(0.5 * sqrt(2)),
|
|
// expected_barrier_func(0.5 * sqrt(2));
|
|
// }
|
|
//
|
|
// SECTION("Left displacements")
|
|
// {
|
|
// poses_t1[1].position << -0.5, 0.75;
|
|
//
|
|
// expected_barrier << expected_barrier_func(0.5),
|
|
// expected_barrier_func(1.0), expected_barrier_func(0.5),
|
|
// expected_barrier_func(sqrt(0.5 * 0.5 + 1));
|
|
// }
|
|
//
|
|
// SECTION("Farther Left displacements")
|
|
// {
|
|
// poses_t1[1].position << -1.0, 0.75;
|
|
//
|
|
// expected_barrier << expected_barrier_func(0.5 * sqrt(2)),
|
|
// expected_barrier_func(sqrt(0.5 * 0.5 + 1.0)),
|
|
// expected_barrier_func(0.5 * sqrt(2)),
|
|
// expected_barrier_func(sqrt(0.5 * 0.5 + 1.5 * 1.5));
|
|
// }
|
|
//
|
|
// Eigen::VectorXd actual_barrier;
|
|
// // use brute force so we know the order
|
|
// RigidBody bodyA = RigidBody(
|
|
// vertices.topRows(2), edges.topRows(1), Pose<double>::Zero(/*dim=*/2),
|
|
// /*velocity=*/Pose<double>::Zero(/*dim=*/2),
|
|
// /*force=*/Pose<double>::Zero(/*dim=*/2), /*density=*/1.0,
|
|
// /*is_dof_fixed=*/VectorXb::Zero(3), /*is_oriented=*/false,
|
|
// /*group=*/0);
|
|
// RigidBody bodyB = RigidBody(
|
|
// vertices.bottomRows(2), edges.bottomRows(1), Pose<double>(0, 0.75,
|
|
// 0),
|
|
// /*velocity=*/Pose<double>::Zero(/*dim=*/2),
|
|
// /*force=*/Pose<double>::Zero(/*dim=*/2), /*density=*/1.0,
|
|
// /*is_dof_fixed=*/VectorXb::Zero(3), /*is_oriented=*/false,
|
|
// /*group=*/1);
|
|
//
|
|
// RigidBodyAssembler rbs;
|
|
// rbs.init({ { bodyA, bodyB } });
|
|
//
|
|
// barrier.initialize();
|
|
// barrier.compute_constraints(rbs, poses_t1, actual_barrier);
|
|
//
|
|
// REQUIRE(actual_barrier.size() == expected_barrier.size());
|
|
// CAPTURE(logger::fmt_eigen(actual_barrier));
|
|
// for (int i = 0; i < expected_barrier.size(); i++) {
|
|
// CAPTURE(i);
|
|
// CHECK(actual_barrier[i] == Approx(expected_barrier[i]));
|
|
// }
|
|
// }
|
|
|