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.
851 lines
31 KiB
851 lines
31 KiB
#include <catch2/catch.hpp>
|
|
|
|
#include <ghc/fs_std.hpp> // filesystem
|
|
#include <igl/PI.h>
|
|
#include <igl/edges.h>
|
|
|
|
#include <ipc/distance/edge_edge.hpp>
|
|
|
|
// #include <ccd.hpp>
|
|
#include <ccd/piecewise_linear/time_of_impact.hpp>
|
|
#include <ccd/rigid/time_of_impact.hpp>
|
|
#include <constants.hpp>
|
|
#include <io/serialize_json.hpp>
|
|
|
|
using namespace ipc;
|
|
using namespace ipc::rigid;
|
|
|
|
const double TESTING_TOI_TOLERANCE = 1e-6;
|
|
|
|
void print_EE_obj(
|
|
const RigidBody& bodyA,
|
|
const Pose<double>& bodyA_pose_t0,
|
|
const Pose<double>& bodyA_pose_t1,
|
|
int edgeA_id,
|
|
const RigidBody& bodyB,
|
|
const Pose<double>& bodyB_pose_t0,
|
|
const Pose<double>& bodyB_pose_t1,
|
|
int edgeB_id,
|
|
int n = 100)
|
|
{
|
|
fmt::print("# Edge 1 vertices\n");
|
|
for (int i = 0; i < n + 1; i++) {
|
|
Pose<double> pose =
|
|
Pose<double>::interpolate(bodyA_pose_t0, bodyA_pose_t1, i / n);
|
|
std::cout
|
|
<< "v "
|
|
<< bodyA.world_vertex(pose, bodyA.edges(edgeA_id, 0)).transpose()
|
|
<< std::endl;
|
|
std::cout
|
|
<< "v "
|
|
<< bodyA.world_vertex(pose, bodyA.edges(edgeA_id, 1)).transpose()
|
|
<< std::endl;
|
|
}
|
|
fmt::print("# Edge 2 vertices\n");
|
|
for (int i = 0; i < n + 1; i++) {
|
|
Pose<double> pose =
|
|
Pose<double>::interpolate(bodyB_pose_t0, bodyB_pose_t1, i / n);
|
|
std::cout
|
|
<< "v "
|
|
<< bodyB.world_vertex(pose, bodyB.edges(edgeB_id, 0)).transpose()
|
|
<< std::endl;
|
|
std::cout
|
|
<< "v "
|
|
<< bodyB.world_vertex(pose, bodyB.edges(edgeB_id, 1)).transpose()
|
|
<< std::endl;
|
|
}
|
|
fmt::print("# Edge 1 surface\n");
|
|
for (int i = 0; i < 4 * n + 2; i += 2) {
|
|
if (i == 2 * n) {
|
|
fmt::print("# Edge 2 surface\n");
|
|
continue;
|
|
}
|
|
fmt::print("f {} {} {} {}\n", i + 1, i + 2, i + 4, i + 3);
|
|
}
|
|
}
|
|
|
|
RigidBody create_body(
|
|
const Eigen::MatrixXd& vertices,
|
|
const Eigen::MatrixXi& edges,
|
|
const Eigen::MatrixXi& faces)
|
|
{
|
|
static int id = 0;
|
|
int dim = vertices.cols();
|
|
Pose<double> pose = Pose<double>::Zero(dim);
|
|
RigidBody rb = RigidBody(
|
|
vertices, edges, faces, pose,
|
|
/*velocity=*/Pose<double>::Zero(pose.dim()),
|
|
/*force=*/Pose<double>::Zero(pose.dim()),
|
|
/*denisty=*/1.0,
|
|
/*is_dof_fixed=*/VectorMax6b::Zero(pose.ndof()),
|
|
/*oriented=*/false,
|
|
/*group_id=*/id++);
|
|
rb.vertices = vertices; // Cancel out the inertial rotation for testing
|
|
rb.pose.position.setZero();
|
|
rb.pose.rotation.setZero();
|
|
return rb;
|
|
}
|
|
|
|
RigidBody
|
|
create_body(const Eigen::MatrixXd& vertices, const Eigen::MatrixXi& edges)
|
|
{
|
|
return create_body(vertices, edges, Eigen::MatrixXi());
|
|
}
|
|
|
|
TEST_CASE("Rigid edge-vertex time of impact", "[ccd][rigid_toi][edge_vertex]")
|
|
{
|
|
int dim = GENERATE(2);
|
|
int ndof = Pose<double>::dim_to_ndof(dim);
|
|
int pos_ndof = Pose<double>::dim_to_pos_ndof(dim);
|
|
int rot_ndof = Pose<double>::dim_to_rot_ndof(dim);
|
|
|
|
Eigen::MatrixXd bodyA_vertices(2, dim);
|
|
bodyA_vertices.row(0) << -1, 0;
|
|
bodyA_vertices.row(1) << 1, 0;
|
|
Eigen::MatrixXd bodyB_vertices(2, dim);
|
|
bodyB_vertices.row(0) << -2, 0;
|
|
bodyB_vertices.row(1) << 2, 0;
|
|
|
|
Eigen::MatrixXi bodyA_edges(1, 2);
|
|
bodyA_edges.row(0) << 0, 1;
|
|
Eigen::MatrixXi bodyB_edges(1, 2);
|
|
int edge_id0 = GENERATE(0, 1); // Test both orders
|
|
bodyB_edges.row(0) << edge_id0, edge_id0 == 0 ? 1 : 0;
|
|
|
|
Pose<double> bodyA_pose_t0(
|
|
Eigen::Vector3d(0, 0.5, 0).head(pos_ndof),
|
|
Eigen::Vector3d(0, 0, 0).head(rot_ndof));
|
|
Pose<double> bodyB_pose_t0 = Pose<double>::Zero(dim);
|
|
Pose<double> bodyA_pose_t1 = Pose<double>::Zero(dim);
|
|
Pose<double> bodyB_pose_t1 = Pose<double>::Zero(dim);
|
|
|
|
double expected_toi = -1;
|
|
bool is_impact_expected = true;
|
|
SECTION("Translation")
|
|
{
|
|
double posx = GENERATE(-2 - 1e-8, -2, -1, -0.5);
|
|
double sign = GENERATE(-1, 1);
|
|
bodyA_vertices.row(0).x() = sign * posx;
|
|
bodyA_vertices.row(1).x() = -sign * posx;
|
|
double y_t1 = GENERATE(-10, -0.5, -1e-12, 0.0, 1e-12, 0.5, 10);
|
|
expected_toi =
|
|
bodyA_pose_t0.position.y() / (-y_t1 + bodyA_pose_t0.position.y());
|
|
bodyA_pose_t1.position = Eigen::Vector3d(0, y_t1, 0).head(pos_ndof);
|
|
|
|
is_impact_expected =
|
|
expected_toi >= 0 && expected_toi <= 1 && posx >= -2;
|
|
}
|
|
SECTION("Rotation")
|
|
{
|
|
double theta = igl::PI
|
|
* GENERATE(-2, -7.0 / 6.0, -1, 0, 1.0 / 12.0, 1.0 / 6.0 - 1e-12,
|
|
1.0 / 6.0, 1.0 / 6.0 + 1e-12, 0.5, 1, 2, 100);
|
|
expected_toi = (theta < 0 ? -7.0 : 1.0) / 6.0 * igl::PI / theta;
|
|
is_impact_expected =
|
|
theta >= igl::PI / 6.0 || theta <= -igl::PI * 7.0 / 6.0;
|
|
bodyA_pose_t1.position = bodyA_pose_t0.position;
|
|
bodyA_pose_t1.rotation = Eigen::Vector3d(0, 0, theta).tail(rot_ndof);
|
|
}
|
|
|
|
RigidBody bodyA = create_body(bodyA_vertices, bodyA_edges);
|
|
RigidBody bodyB = create_body(bodyB_vertices, bodyB_edges);
|
|
|
|
double toi;
|
|
bool is_impacting = compute_edge_vertex_time_of_impact(
|
|
bodyA, bodyA_pose_t0, bodyA_pose_t1, /*vertex_id=*/0, //
|
|
bodyB, bodyB_pose_t0, bodyB_pose_t1, /*edge_id=*/0, //
|
|
toi, /*earliest_toi=*/1, /*toi_tolerance=*/TESTING_TOI_TOLERANCE);
|
|
CAPTURE(toi, expected_toi);
|
|
CHECK(is_impacting == is_impact_expected);
|
|
if (is_impacting) {
|
|
// clang-format off
|
|
CHECK(toi == Approx(expected_toi).margin(TESTING_TOI_TOLERANCE));
|
|
// clang-format on
|
|
CHECK(toi <= expected_toi);
|
|
}
|
|
}
|
|
|
|
TEST_CASE("Rigid edge-edge time of impact", "[ccd][rigid_toi][edge_edge]")
|
|
{
|
|
int dim = 3;
|
|
int ndof = Pose<double>::dim_to_ndof(dim);
|
|
int pos_ndof = Pose<double>::dim_to_pos_ndof(dim);
|
|
int rot_ndof = Pose<double>::dim_to_rot_ndof(dim);
|
|
|
|
Eigen::MatrixXd bodyA_vertices(2, dim);
|
|
bodyA_vertices.row(0) << -1, 0, 0;
|
|
bodyA_vertices.row(1) << 1, 0, 0;
|
|
Eigen::MatrixXd bodyB_vertices(2, dim);
|
|
bodyB_vertices.row(0) << 0, 0, -1;
|
|
bodyB_vertices.row(1) << 0, 0, 1;
|
|
|
|
Eigen::MatrixXi bodyA_edges(1, 2);
|
|
bodyA_edges.row(0) << 0, 1;
|
|
Eigen::MatrixXi bodyB_edges(1, 2);
|
|
bodyB_edges.row(0) << 0, 1;
|
|
|
|
RigidBody bodyA = create_body(bodyA_vertices, bodyA_edges);
|
|
RigidBody bodyB = create_body(bodyB_vertices, bodyB_edges);
|
|
|
|
Pose<double> bodyA_pose_t0 = Pose<double>::Zero(dim);
|
|
double z = GENERATE(1000.0, 10.0, 1.0 + 1e-8, 1.0, 1.0 - 1e-8, 0.5, 0.0);
|
|
double sign = GENERATE(-1, 1);
|
|
double z_t0 = sign * z;
|
|
bodyA_pose_t0.position.y() = 1.0;
|
|
bodyA_pose_t0.position.z() = z_t0;
|
|
Pose<double> bodyB_pose_t0 = Pose<double>::Zero(dim);
|
|
Pose<double> bodyA_pose_t1 = Pose<double>::Zero(dim);
|
|
bodyA_pose_t1.position.z() = z_t0;
|
|
Pose<double> bodyB_pose_t1 = Pose<double>::Zero(dim);
|
|
|
|
double expected_toi = -1;
|
|
bool is_impact_expected = false;
|
|
SECTION("Translation")
|
|
{
|
|
double y_t1 =
|
|
GENERATE(2.0, 1.0, 0.1, 1e-6, 0.0, -1e-6, -1, -2, -10, -1000);
|
|
expected_toi = 1 / (1 - y_t1);
|
|
is_impact_expected = y_t1 <= 0.0 && z_t0 >= -1 && z_t0 <= 1;
|
|
bodyA_pose_t1.position.y() = y_t1;
|
|
}
|
|
|
|
double toi;
|
|
bool is_impacting = compute_edge_edge_time_of_impact(
|
|
bodyA, bodyA_pose_t0, bodyA_pose_t1, /*edgeA_id=*/0, //
|
|
bodyB, bodyB_pose_t0, bodyB_pose_t1, /*edgeA_id=*/0, //
|
|
toi, /*earliest_toi=*/1, /*toi_tolerance=*/TESTING_TOI_TOLERANCE);
|
|
CAPTURE(
|
|
bodyA_pose_t0.position.transpose(), bodyA_pose_t1.position.transpose(),
|
|
bodyA.world_vertex(bodyA_pose_t0, 0).transpose(),
|
|
bodyA.world_vertex(bodyA_pose_t1, 0).transpose(), toi, expected_toi);
|
|
CHECK(is_impacting == is_impact_expected);
|
|
if (is_impacting) {
|
|
// clang-format off
|
|
CHECK(toi == Approx(expected_toi).margin(
|
|
Constants::RIGID_CCD_LENGTH_TOL));
|
|
// clang-format on
|
|
CHECK(toi <= expected_toi);
|
|
}
|
|
}
|
|
|
|
TEST_CASE("Rigid face-vertex time of impact", "[ccd][rigid_toi][face_vertex]")
|
|
{
|
|
int dim = 3;
|
|
int ndof = Pose<double>::dim_to_ndof(dim);
|
|
int pos_ndof = Pose<double>::dim_to_pos_ndof(dim);
|
|
int rot_ndof = Pose<double>::dim_to_rot_ndof(dim);
|
|
|
|
Eigen::MatrixXd bodyA_vertices(3, dim);
|
|
bodyA_vertices.row(0) << -1, 0, 0;
|
|
bodyA_vertices.row(1) << 1, 0, 0;
|
|
bodyA_vertices.row(2) << 0, 1, 0;
|
|
double y = GENERATE(1.0 + 1e-8, 1.0, 1.0 - 1e-8, 0.5, 1e-8, 0.0, 1e-8);
|
|
Eigen::MatrixXd bodyB_vertices(3, dim);
|
|
bodyB_vertices.row(0) << 0, y, 1;
|
|
bodyB_vertices.row(1) << 1, y, 1.5;
|
|
bodyB_vertices.row(2) << -1, y, 1.5;
|
|
|
|
Eigen::MatrixXi bodyA_faces(1, 3);
|
|
bodyA_faces.row(0) << 0, 1, 2;
|
|
Eigen::MatrixXi bodyB_faces(1, 3);
|
|
bodyB_faces.row(0) << 0, 1, 2;
|
|
|
|
Eigen::MatrixXi bodyA_edges, bodyB_edges;
|
|
igl::edges(bodyA_faces, bodyA_edges);
|
|
igl::edges(bodyB_faces, bodyB_edges);
|
|
|
|
RigidBody bodyA = create_body(bodyA_vertices, bodyA_edges, bodyA_faces);
|
|
RigidBody bodyB = create_body(bodyB_vertices, bodyB_edges, bodyB_faces);
|
|
|
|
Pose<double> bodyA_pose_t0 = bodyA.pose;
|
|
Pose<double> bodyB_pose_t0 = bodyB.pose;
|
|
|
|
Pose<double> bodyA_pose_t1 = bodyA.pose;
|
|
Pose<double> bodyB_pose_t1 = bodyB.pose;
|
|
|
|
double expected_toi = -1;
|
|
bool is_impact_expected = false;
|
|
SECTION("Translation")
|
|
{
|
|
double z_t1 = GENERATE(1, 1e-8, 0.0, -1e-8, -2.0, -10);
|
|
expected_toi = 1 / (-z_t1 + 1);
|
|
is_impact_expected = z_t1 <= 0.0 && y >= 0 && y <= 1.0;
|
|
bodyB_pose_t1.position.z() = z_t1 - bodyB.vertices(0, 2);
|
|
}
|
|
// SECTION("Rotation")
|
|
// {
|
|
// expected_toi = ;
|
|
// is_impact_expected = ;
|
|
// bodyB_velocity.rotation.z() = ;
|
|
// }
|
|
|
|
CAPTURE(
|
|
y, bodyB.vertices.row(0), bodyB_pose_t0.position.transpose(),
|
|
bodyB_pose_t1.position.transpose(),
|
|
bodyB.world_vertex(bodyB_pose_t0, 0).transpose(),
|
|
bodyB.world_vertex(bodyB_pose_t1, 0).transpose());
|
|
double toi;
|
|
bool is_impacting = compute_face_vertex_time_of_impact(
|
|
bodyB, bodyB_pose_t0, bodyB_pose_t1, /*vertex_id=*/0, // Vertex body
|
|
bodyA, bodyA_pose_t0, bodyA_pose_t1, /*face_id=*/0, // Face body
|
|
toi, /*earliest_toi=*/1, /*toi_tolerance=*/TESTING_TOI_TOLERANCE);
|
|
CAPTURE(toi, expected_toi);
|
|
CHECK(is_impacting == is_impact_expected);
|
|
if (is_impact_expected) {
|
|
// clang-format off
|
|
CHECK(toi == Approx(expected_toi).margin(
|
|
Constants::RIGID_CCD_LENGTH_TOL));
|
|
// clang-format on
|
|
CHECK(toi <= expected_toi);
|
|
}
|
|
}
|
|
|
|
TEST_CASE("Fast EE case", "[!benchmark][ccd][rigid_toi][edge_edge][fast]")
|
|
{
|
|
Eigen::MatrixXd bodyA_vertices = Eigen::MatrixXd::Zero(2, 3);
|
|
Eigen::MatrixXd bodyB_vertices = Eigen::MatrixXd::Zero(2, 3);
|
|
|
|
Eigen::MatrixXi bodyA_edges(1, 2);
|
|
bodyA_edges.row(0) << 0, 1;
|
|
Eigen::MatrixXi bodyB_edges(1, 2);
|
|
bodyB_edges.row(0) << 0, 1;
|
|
|
|
RigidBody bodyA = create_body(bodyA_vertices, bodyA_edges);
|
|
RigidBody bodyB = create_body(bodyB_vertices, bodyB_edges);
|
|
|
|
// clang-format off
|
|
bodyA.vertices.row(0) << 2.66473512640082, 0.622074238426736, 0.0506824409538513;
|
|
bodyA.vertices.row(1) << 2.11663114018755, 0.24694623070118, 0.689392835886464;
|
|
|
|
bodyB.vertices.row(0) << -8.23540431483827, -2.00204356583054, -0.0850398676470792;
|
|
bodyB.vertices.row(1) << -7.59698634143846, -2.65778542381018, 0.220598272033643;
|
|
|
|
Pose<double> bodyA_pose_t0(
|
|
Eigen::Vector3d(-0.0743518262648221, 2.0045466941596, -7.30362621222097e-05),
|
|
Eigen::Vector3d(1.56635729875648, 0.00484560212950007, -0.00477941835507233)
|
|
);
|
|
Pose<double> bodyA_pose_t1(
|
|
Eigen::Vector3d(-0.0743518262648221, 2.0045466941596, -7.30362621222097e-05),
|
|
Eigen::Vector3d(1.56635729875648, 0.00484560212950007, -0.00477941835507233)
|
|
);
|
|
Pose<double> bodyB_pose_t0(
|
|
Eigen::Vector3d(-0.0625913211582116, 9.56420995832003, -0.0276191274028154),
|
|
Eigen::Vector3d(-0.0330556101141157, -0.0327790458706025, 1.56913520538901)
|
|
);
|
|
Pose<double> bodyB_pose_t1(
|
|
Eigen::Vector3d(-0.0625913211582116, 9.56371945832003, -0.0276191274028154),
|
|
Eigen::Vector3d(-0.0467509876415133, -0.0464937186371367, 1.56888122389168)
|
|
);
|
|
// clang-format on
|
|
|
|
BENCHMARK("Fast EE case")
|
|
{
|
|
double toi;
|
|
bool is_impacting = compute_edge_edge_time_of_impact(
|
|
bodyA, bodyA_pose_t0, bodyA_pose_t1, /*edgeA_id=*/0, //
|
|
bodyB, bodyB_pose_t0, bodyB_pose_t1, /*edgeA_id=*/0, //
|
|
toi);
|
|
};
|
|
}
|
|
|
|
TEST_CASE("Slow EE case", "[!benchmark][ccd][rigid_toi][edge_edge][slow]")
|
|
{
|
|
Eigen::MatrixXd bodyA_vertices = Eigen::MatrixXd::Zero(2, 3);
|
|
Eigen::MatrixXd bodyB_vertices = Eigen::MatrixXd::Zero(2, 3);
|
|
|
|
Eigen::MatrixXi bodyA_edges(1, 2);
|
|
bodyA_edges.row(0) << 0, 1;
|
|
Eigen::MatrixXi bodyB_edges(1, 2);
|
|
bodyB_edges.row(0) << 0, 1;
|
|
|
|
RigidBody bodyA = create_body(bodyA_vertices, bodyA_edges);
|
|
RigidBody bodyB = create_body(bodyB_vertices, bodyB_edges);
|
|
|
|
// clang-format off
|
|
bodyA.vertices.row(0) << -1.45054325721069, 2.29538642849017, 0.461193969193908;
|
|
bodyA.vertices.row(1) << -1.12537372801783, 1.78188213954136, 1.12303701209507;
|
|
|
|
bodyB.vertices.row(0) << -8.63773122773594, 0.523601125992661, 1.91725528075351;
|
|
bodyB.vertices.row(1) << -8.00096703934998, 1.01814387645424, 2.44728456523133;
|
|
|
|
Pose<double> bodyA_pose_t0(
|
|
Eigen::Vector3d(-0.0743518262648221, 2.0045466941596, -7.30362621222097e-05),
|
|
Eigen::Vector3d(1.56635729875648, 0.00484560212950006, -0.00477941835507233)
|
|
);
|
|
Pose<double> bodyA_pose_t1(
|
|
Eigen::Vector3d(-0.0743518262648221, 2.0045466941596, -7.30362621222097e-05),
|
|
Eigen::Vector3d(1.56635729875648, 0.00484560212950006, -0.00477941835507233)
|
|
);
|
|
Pose<double> bodyB_pose_t0(
|
|
Eigen::Vector3d(-0.0625913211582116, 9.53281795832003, -0.0276191274028154),
|
|
Eigen::Vector3d(-0.142589758121075, -0.142468016151593, 1.56466991422561)
|
|
);
|
|
Pose<double> bodyB_pose_t1(
|
|
Eigen::Vector3d(-0.0625913211582116, 9.52447945832003, -0.0276191274028154),
|
|
Eigen::Vector3d(-0.156274624583429, -0.156172306264317, 1.56372038452934)
|
|
);
|
|
// clang-format on
|
|
|
|
BENCHMARK("Slow EE Case")
|
|
{
|
|
double toi;
|
|
bool is_impacting = compute_edge_edge_time_of_impact(
|
|
bodyA, bodyA_pose_t0, bodyA_pose_t1, /*edgeA_id=*/0, //
|
|
bodyB, bodyB_pose_t0, bodyB_pose_t1, /*edgeA_id=*/0, //
|
|
toi);
|
|
};
|
|
}
|
|
|
|
TEST_CASE("Actual EE Collision", "[!benchmark][ccd][rigid_toi][edge_edge]")
|
|
{
|
|
Eigen::MatrixXd bodyA_vertices = Eigen::MatrixXd::Zero(2, 3);
|
|
Eigen::MatrixXd bodyB_vertices = Eigen::MatrixXd::Zero(2, 3);
|
|
|
|
Eigen::MatrixXi bodyA_edges(1, 2);
|
|
bodyA_edges.row(0) << 0, 1;
|
|
Eigen::MatrixXi bodyB_edges(1, 2);
|
|
bodyB_edges.row(0) << 0, 1;
|
|
|
|
RigidBody bodyA = create_body(bodyA_vertices, bodyA_edges);
|
|
RigidBody bodyB = create_body(bodyB_vertices, bodyB_edges);
|
|
|
|
// clang-format off
|
|
bodyA.vertices.row(0) << -1, 0, 0;
|
|
bodyA.vertices.row(1) << 1, 0, 0;
|
|
|
|
bodyB.vertices.row(0) << 0, 0, -1;
|
|
bodyB.vertices.row(1) << 0, 0, 1;
|
|
|
|
Pose<double> bodyA_pose_t0(
|
|
Eigen::Vector3d(0, 0.5, 0),
|
|
Eigen::Vector3d(0, 0, 0)
|
|
);
|
|
Pose<double> bodyA_pose_t1(
|
|
Eigen::Vector3d(0, 0.5, 0),
|
|
Eigen::Vector3d(0, 0, 0)
|
|
);
|
|
Pose<double> bodyB_pose_t0(
|
|
Eigen::Vector3d(0, 1, 0),
|
|
Eigen::Vector3d(0, 0, 0)
|
|
);
|
|
Pose<double> bodyB_pose_t1(
|
|
Eigen::Vector3d(0, 1, 0),
|
|
Eigen::Vector3d(2 * 3.14, 0, 0)
|
|
);
|
|
// clang-format on
|
|
|
|
BENCHMARK("Actually EE Collision")
|
|
{
|
|
double toi;
|
|
bool is_impacting = compute_edge_edge_time_of_impact(
|
|
bodyA, bodyA_pose_t0, bodyA_pose_t1, /*edgeA_id=*/0, //
|
|
bodyB, bodyB_pose_t0, bodyB_pose_t1, /*edgeA_id=*/0, //
|
|
toi);
|
|
};
|
|
}
|
|
|
|
TEST_CASE("Actual VF Collision", "[!benchmark][ccd][rigid_toi][face_vertex]")
|
|
{
|
|
Eigen::MatrixXd bodyA_vertices = Eigen::MatrixXd::Zero(3, 3);
|
|
Eigen::MatrixXd bodyB_vertices = Eigen::MatrixXd::Zero(3, 3);
|
|
|
|
Eigen::MatrixXi bodyA_faces(1, 3);
|
|
bodyA_faces.row(0) << 0, 1, 2;
|
|
Eigen::MatrixXi bodyB_faces(1, 3);
|
|
bodyB_faces.row(0) << 0, 1, 2;
|
|
|
|
Eigen::MatrixXi bodyA_edges, bodyB_edges;
|
|
igl::edges(bodyA_faces, bodyA_edges);
|
|
igl::edges(bodyB_faces, bodyB_edges);
|
|
|
|
RigidBody bodyA = create_body(bodyA_vertices, bodyA_edges, bodyA_faces);
|
|
RigidBody bodyB = create_body(bodyB_vertices, bodyB_edges, bodyB_faces);
|
|
|
|
// clang-format off
|
|
bodyA.vertices.row(0) << 0.063161123153442, -0.00975209722618602, 0.0246948915619087;
|
|
|
|
bodyB.vertices.row(0) << -0.00733894260009082, 0.0199670606490534, 0.000727755816038143;
|
|
bodyB.vertices.row(1) << -0.0122514761614292, 0.0244249832266042, -0.00566776185395443;
|
|
bodyB.vertices.row(2) << -0.00945035723923828, 0.025642170175986, -0.00591155842365654;
|
|
|
|
Pose<double> bodyA_pose_t0(
|
|
Eigen::Vector3d(-0.000591328227883731, 0.0888868556875028, -0.000277685809028307),
|
|
Eigen::Vector3d(1.20966059163976, -1.2107299033619, -1.20913719904179)
|
|
);
|
|
Pose<double> bodyA_pose_t1(
|
|
Eigen::Vector3d(-0.000591328227883731, 0.0879058556875029, -0.000277685809028307),
|
|
Eigen::Vector3d(1.20966059163976, -1.2107299033619, -1.20913719904179)
|
|
);
|
|
Pose<double> bodyB_pose_t0(
|
|
Eigen::Vector3d(-0.00052287436757153, 0.020051622753965, -4.06061775370095e-06),
|
|
Eigen::Vector3d(1.21919966781284, -1.19787487380512, 1.19585545343065)
|
|
);
|
|
Pose<double> bodyB_pose_t1(
|
|
Eigen::Vector3d(-0.00052287436757153, 0.020051622753965, -4.06061775370095e-06),
|
|
Eigen::Vector3d(1.21919966781284, -1.19787487380512, 1.19585545343065)
|
|
);
|
|
// clang-format on
|
|
|
|
BENCHMARK("Actually VF Collision")
|
|
{
|
|
double toi;
|
|
bool is_impacting = compute_face_vertex_time_of_impact(
|
|
bodyA, bodyA_pose_t0, bodyA_pose_t1, /*vertex_id=*/0, //
|
|
bodyB, bodyB_pose_t0, bodyB_pose_t1, /*face_id=*/0, //
|
|
toi);
|
|
// std::cout << toi << std::endl;
|
|
};
|
|
}
|
|
|
|
TEST_CASE(
|
|
"Extremly Slow EE Case",
|
|
"[!benchmark][ccd][rigid_toi][edge_edge][extremly_slow]")
|
|
{
|
|
Eigen::MatrixXd bodyA_vertices = Eigen::MatrixXd::Zero(2, 3);
|
|
Eigen::MatrixXd bodyB_vertices = Eigen::MatrixXd::Zero(2, 3);
|
|
|
|
Eigen::MatrixXi bodyA_edges(1, 2);
|
|
bodyA_edges.row(0) << 0, 1;
|
|
Eigen::MatrixXi bodyB_edges(1, 2);
|
|
bodyB_edges.row(0) << 0, 1;
|
|
|
|
RigidBody bodyA = create_body(bodyA_vertices, bodyA_edges);
|
|
RigidBody bodyB = create_body(bodyB_vertices, bodyB_edges);
|
|
|
|
Pose<double> bodyA_pose_t0, bodyA_pose_t1, bodyB_pose_t0, bodyB_pose_t1;
|
|
|
|
double earliest_toi;
|
|
|
|
SECTION("0")
|
|
{
|
|
// clang-format off
|
|
bodyA.vertices.row(0) << 1.25, 0.625, -1.11022302462516e-16;
|
|
bodyA.vertices.row(1) << 1.25, -0.625, -1.11022302462516e-16;
|
|
|
|
bodyB.vertices.row(0) << 1.25, 0.625, 1.11022302462516e-16;
|
|
bodyB.vertices.row(1) << 1.25, -0.625, 1.11022302462516e-16;
|
|
|
|
bodyA_pose_t0 = Pose<double>(
|
|
Eigen::Vector3d(-0.749789935368566, 1.00585262304029, 1.37760763963751e-05),
|
|
Eigen::Vector3d(1.44479640128067, 0.727816581920491, 0.729072550558035)
|
|
);
|
|
bodyA_pose_t1 = Pose<double>(
|
|
Eigen::Vector3d(-0.749767679498726, 0.999291290743525, 1.56515114692218e-05),
|
|
Eigen::Vector3d(1.44714648553188, 0.720936564079057, 0.722362732765182)
|
|
);
|
|
bodyB_pose_t0 = Pose<double>(
|
|
Eigen::Vector3d(0.749821260870553, 1.00573285487415, -1.25619191880717e-05),
|
|
Eigen::Vector3d(0.836943953227619, 1.66096593034921, 1.66114035080701)
|
|
);
|
|
bodyB_pose_t1 = Pose<double>(
|
|
Eigen::Vector3d(0.749804770233396, 0.999147283739376, -1.37411290439571e-05),
|
|
Eigen::Vector3d(0.830693602998813, 1.6669129896604, 1.66713787700267)
|
|
);
|
|
// clang-format on
|
|
earliest_toi = 0.739807;
|
|
}
|
|
SECTION("1")
|
|
{
|
|
// clang-format off
|
|
bodyA.vertices.row(0) << 1.25, 0.625, -1.11022302462516e-16;
|
|
bodyA.vertices.row(1) << 1.25, -0.625, -1.11022302462516e-16;
|
|
|
|
bodyB.vertices.row(0) << 1.25, 0.625, 1.11022302462516e-16;
|
|
bodyB.vertices.row(1) << 1.25, -0.625, 1.11022302462516e-16;
|
|
|
|
bodyA_pose_t0 = Pose<double>(
|
|
Eigen::Vector3d(-0.749781303981602, 1.00328869329824, 1.45053758187115e-05),
|
|
Eigen::Vector3d(1.44571477658472, 0.725130939760901,
|
|
0.726452486140648)
|
|
);
|
|
bodyA_pose_t1 = Pose<double>(
|
|
Eigen::Vector3d(-0.749768505996024,
|
|
0.999271155189446, 1.56109303515142e-05),
|
|
Eigen::Vector3d(1.44714132170885, 0.720946434493765,
|
|
0.722364003050481)
|
|
);
|
|
bodyB_pose_t0 = Pose<double>(
|
|
Eigen::Vector3d(0.749814828894052, 1.0031128414029,
|
|
-1.30219790400155e-05),
|
|
Eigen::Vector3d(0.834519686991294, 1.66327347194605, 1.6634661858807)
|
|
);
|
|
bodyB_pose_t1 = Pose<double>(
|
|
Eigen::Vector3d(0.749754900679728, 0.999118556601472,
|
|
-1.68864775474044e-05),
|
|
Eigen::Vector3d(0.830720747893929, 1.66690721127267, 1.66711165819808)
|
|
);
|
|
// clang-format on
|
|
earliest_toi = 0.57421;
|
|
}
|
|
|
|
// print_EE_obj(
|
|
// bodyA, bodyA_pose_t0, bodyA_pose_t1, /*edgeA_id=*/0, //
|
|
// bodyB, bodyB_pose_t0, bodyB_pose_t1, /*edgeB_id=*/0);
|
|
|
|
BENCHMARK("Extremly Slow EE CCD")
|
|
{
|
|
double toi;
|
|
bool is_impacting = compute_edge_edge_time_of_impact(
|
|
bodyA, bodyA_pose_t0, bodyA_pose_t1, /*edgeA_id=*/0, //
|
|
bodyB, bodyB_pose_t0, bodyB_pose_t1, /*edgeB_id=*/0, //
|
|
toi);
|
|
};
|
|
}
|
|
|
|
TEST_CASE("Failing earliest tois", "[ccd][rigid_toi][failing_toi]")
|
|
{
|
|
int id = GENERATE(range(0, 385));
|
|
std::string filename =
|
|
fmt::format("wrecking-ball/ccd-test-{:03d}.json", id);
|
|
fs::path data_path =
|
|
fs::path(__FILE__).parent_path().parent_path() / "data" / filename;
|
|
|
|
std::ifstream input(data_path.string());
|
|
nlohmann::json json = nlohmann::json::parse(input);
|
|
|
|
Eigen::MatrixXd bodyA_vertices, bodyB_vertices;
|
|
Eigen::MatrixXi bodyA_edges, bodyB_edges, bodyA_faces, bodyB_faces;
|
|
Pose<double> bodyA_pose_t0, bodyA_pose_t1, bodyB_pose_t0, bodyB_pose_t1;
|
|
|
|
std::string ccd_type = json["type"];
|
|
|
|
nlohmann::json poseA_t0_json, poseA_t1_json, poseB_t0_json, poseB_t1_json;
|
|
|
|
if (ccd_type == "ee") {
|
|
Eigen::VectorXd tmp;
|
|
|
|
bodyA_vertices.resize(2, 3);
|
|
from_json(json["edge0"]["vertex0"], tmp);
|
|
bodyA_vertices.row(0) = tmp;
|
|
from_json(json["edge0"]["vertex1"], tmp);
|
|
bodyA_vertices.row(1) = tmp;
|
|
|
|
bodyB_vertices.resize(2, 3);
|
|
from_json(json["edge1"]["vertex0"], tmp);
|
|
bodyB_vertices.row(0) = tmp;
|
|
from_json(json["edge1"]["vertex1"], tmp);
|
|
bodyB_vertices.row(1) = tmp;
|
|
|
|
bodyA_edges.resize(1, 2);
|
|
bodyA_edges.row(0) << 0, 1;
|
|
bodyB_edges.resize(1, 2);
|
|
bodyB_edges.row(0) << 0, 1;
|
|
|
|
poseA_t0_json = json["edge0"]["pose_t0"];
|
|
poseA_t1_json = json["edge0"]["pose_t1"];
|
|
poseB_t0_json = json["edge1"]["pose_t0"];
|
|
poseB_t1_json = json["edge1"]["pose_t1"];
|
|
} else if (ccd_type == "fv") {
|
|
Eigen::VectorXd tmp;
|
|
|
|
bodyA_vertices.resize(3, 3);
|
|
from_json(json["face"]["vertex0"], tmp);
|
|
bodyA_vertices.row(0) = tmp;
|
|
from_json(json["face"]["vertex1"], tmp);
|
|
bodyA_vertices.row(1) = tmp;
|
|
from_json(json["face"]["vertex2"], tmp);
|
|
bodyA_vertices.row(2) = tmp;
|
|
|
|
bodyB_vertices.resize(1, 3);
|
|
from_json(json["vertex"]["vertex"], tmp);
|
|
bodyB_vertices.row(0) = tmp;
|
|
|
|
bodyA_faces.resize(1, 3);
|
|
bodyA_faces.row(0) << 0, 1, 2;
|
|
bodyA_edges.resize(3, 2);
|
|
bodyA_edges.row(0) << 0, 1;
|
|
bodyA_edges.row(1) << 1, 2;
|
|
bodyA_edges.row(2) << 2, 0;
|
|
|
|
poseA_t0_json = json["face"]["pose_t0"];
|
|
poseA_t1_json = json["face"]["pose_t1"];
|
|
poseB_t0_json = json["vertex"]["pose_t0"];
|
|
poseB_t1_json = json["vertex"]["pose_t1"];
|
|
} else if (ccd_type == "ev") {
|
|
// TODO
|
|
return;
|
|
}
|
|
|
|
from_json(poseA_t0_json["position"], bodyA_pose_t0.position);
|
|
from_json(poseA_t0_json["rotation"], bodyA_pose_t0.rotation);
|
|
from_json(poseA_t1_json["position"], bodyA_pose_t1.position);
|
|
from_json(poseA_t1_json["rotation"], bodyA_pose_t1.rotation);
|
|
from_json(poseB_t0_json["position"], bodyB_pose_t0.position);
|
|
from_json(poseB_t0_json["rotation"], bodyB_pose_t0.rotation);
|
|
from_json(poseB_t1_json["position"], bodyB_pose_t1.position);
|
|
from_json(poseB_t1_json["rotation"], bodyB_pose_t1.rotation);
|
|
|
|
RigidBody bodyA = create_body(bodyA_vertices, bodyA_edges, bodyA_faces);
|
|
RigidBody bodyB = create_body(bodyB_vertices, bodyB_edges, bodyB_faces);
|
|
|
|
double toi = 1;
|
|
bool is_impacting = false;
|
|
|
|
if (ccd_type == "ee") {
|
|
is_impacting = compute_piecewise_linear_edge_edge_time_of_impact(
|
|
bodyA, bodyA_pose_t0, bodyA_pose_t1, /*edgeA_id=*/0, //
|
|
bodyB, bodyB_pose_t0, bodyB_pose_t1, /*edgeB_id=*/0, //
|
|
toi);
|
|
} else if (ccd_type == "fv") {
|
|
is_impacting = compute_piecewise_linear_face_vertex_time_of_impact(
|
|
bodyB, bodyB_pose_t0, bodyB_pose_t1, /*vertex_id=*/0, // Vertex body
|
|
bodyA, bodyA_pose_t0, bodyA_pose_t1, /*face_id=*/0, // Face body
|
|
toi);
|
|
}
|
|
|
|
CAPTURE(ccd_type);
|
|
if (!is_impacting) {
|
|
return;
|
|
}
|
|
|
|
// toi *= 0.99;
|
|
double toi2 = 1;
|
|
|
|
Pose<double> bodyA_pose_toi =
|
|
Pose<double>::interpolate(bodyA_pose_t0, bodyA_pose_t1, toi);
|
|
Pose<double> bodyB_pose_toi =
|
|
Pose<double>::interpolate(bodyB_pose_t0, bodyB_pose_t1, toi);
|
|
|
|
if (ccd_type == "ee") {
|
|
is_impacting = compute_edge_edge_time_of_impact(
|
|
bodyA, bodyA_pose_t0, bodyA_pose_toi, /*edgeA_id=*/0, //
|
|
bodyB, bodyB_pose_t0, bodyB_pose_toi, /*edgeB_id=*/0, //
|
|
toi2);
|
|
} else if (ccd_type == "fv") {
|
|
is_impacting = compute_face_vertex_time_of_impact(
|
|
bodyB, bodyB_pose_t0, bodyB_pose_t1, /*vertex_id=*/0, // Vertex
|
|
bodyA, bodyA_pose_t0, bodyA_pose_t1, /*face_id=*/0, // Face
|
|
toi2);
|
|
}
|
|
CAPTURE(toi, toi2, toi * toi2);
|
|
CHECK(!is_impacting);
|
|
}
|
|
|
|
TEST_CASE("toi=0", "[ccd][rigid_toi][thisone]")
|
|
{
|
|
int id = GENERATE(range(0, 13));
|
|
std::string filename = fmt::format("kinematic/ccd-test-{:03d}.json", id);
|
|
fs::path data_path =
|
|
fs::path(__FILE__).parent_path().parent_path() / "data" / filename;
|
|
|
|
std::ifstream input(data_path.string());
|
|
nlohmann::json json = nlohmann::json::parse(input);
|
|
|
|
Eigen::MatrixXd bodyA_vertices, bodyB_vertices;
|
|
Eigen::MatrixXi bodyA_edges, bodyB_edges, bodyA_faces, bodyB_faces;
|
|
Pose<double> bodyA_pose_t0, bodyA_pose_t1, bodyB_pose_t0, bodyB_pose_t1;
|
|
|
|
std::string ccd_type = json["type"];
|
|
|
|
nlohmann::json poseA_t0_json, poseA_t1_json, poseB_t0_json, poseB_t1_json;
|
|
|
|
if (ccd_type == "ee") {
|
|
Eigen::VectorXd tmp;
|
|
|
|
bodyA_vertices.resize(2, 3);
|
|
from_json(json["edge0"]["vertex0"], tmp);
|
|
bodyA_vertices.row(0) = tmp;
|
|
from_json(json["edge0"]["vertex1"], tmp);
|
|
bodyA_vertices.row(1) = tmp;
|
|
|
|
bodyB_vertices.resize(2, 3);
|
|
from_json(json["edge1"]["vertex0"], tmp);
|
|
bodyB_vertices.row(0) = tmp;
|
|
from_json(json["edge1"]["vertex1"], tmp);
|
|
bodyB_vertices.row(1) = tmp;
|
|
|
|
bodyA_edges.resize(1, 2);
|
|
bodyA_edges.row(0) << 0, 1;
|
|
bodyB_edges.resize(1, 2);
|
|
bodyB_edges.row(0) << 0, 1;
|
|
|
|
poseA_t0_json = json["edge0"]["pose_t0"];
|
|
poseA_t1_json = json["edge0"]["pose_t1"];
|
|
poseB_t0_json = json["edge1"]["pose_t0"];
|
|
poseB_t1_json = json["edge1"]["pose_t1"];
|
|
} else if (ccd_type == "fv") {
|
|
Eigen::VectorXd tmp;
|
|
|
|
bodyA_vertices.resize(3, 3);
|
|
from_json(json["face"]["vertex0"], tmp);
|
|
bodyA_vertices.row(0) = tmp;
|
|
from_json(json["face"]["vertex1"], tmp);
|
|
bodyA_vertices.row(1) = tmp;
|
|
from_json(json["face"]["vertex2"], tmp);
|
|
bodyA_vertices.row(2) = tmp;
|
|
|
|
bodyB_vertices.resize(1, 3);
|
|
from_json(json["vertex"]["vertex"], tmp);
|
|
bodyB_vertices.row(0) = tmp;
|
|
|
|
bodyA_faces.resize(1, 3);
|
|
bodyA_faces.row(0) << 0, 1, 2;
|
|
bodyA_edges.resize(3, 2);
|
|
bodyA_edges.row(0) << 0, 1;
|
|
bodyA_edges.row(1) << 1, 2;
|
|
bodyA_edges.row(2) << 2, 0;
|
|
|
|
poseA_t0_json = json["face"]["pose_t0"];
|
|
poseA_t1_json = json["face"]["pose_t1"];
|
|
poseB_t0_json = json["vertex"]["pose_t0"];
|
|
poseB_t1_json = json["vertex"]["pose_t1"];
|
|
} else if (ccd_type == "ev") {
|
|
// TODO
|
|
return;
|
|
}
|
|
|
|
from_json(poseA_t0_json["position"], bodyA_pose_t0.position);
|
|
from_json(poseA_t0_json["rotation"], bodyA_pose_t0.rotation);
|
|
from_json(poseA_t1_json["position"], bodyA_pose_t1.position);
|
|
from_json(poseA_t1_json["rotation"], bodyA_pose_t1.rotation);
|
|
from_json(poseB_t0_json["position"], bodyB_pose_t0.position);
|
|
from_json(poseB_t0_json["rotation"], bodyB_pose_t0.rotation);
|
|
from_json(poseB_t1_json["position"], bodyB_pose_t1.position);
|
|
from_json(poseB_t1_json["rotation"], bodyB_pose_t1.rotation);
|
|
|
|
RigidBody bodyA = create_body(bodyA_vertices, bodyA_edges, bodyA_faces);
|
|
RigidBody bodyB = create_body(bodyB_vertices, bodyB_edges, bodyB_faces);
|
|
|
|
double toi = 1;
|
|
bool is_impacting = false;
|
|
|
|
if (ccd_type == "ee") {
|
|
is_impacting = compute_piecewise_linear_edge_edge_time_of_impact(
|
|
bodyA, bodyA_pose_t0, bodyA_pose_t1, /*edgeA_id=*/0, //
|
|
bodyB, bodyB_pose_t0, bodyB_pose_t1, /*edgeB_id=*/0, //
|
|
toi);
|
|
} else if (ccd_type == "fv") {
|
|
is_impacting = compute_piecewise_linear_face_vertex_time_of_impact(
|
|
bodyB, bodyB_pose_t0, bodyB_pose_t1, /*vertex_id=*/0, // Vertex body
|
|
bodyA, bodyA_pose_t0, bodyA_pose_t1, /*face_id=*/0, // Face body
|
|
toi);
|
|
}
|
|
|
|
Eigen::MatrixXd VA_t0 = bodyA.world_vertices(bodyA_pose_t0);
|
|
Eigen::MatrixXd VB_t0 = bodyB.world_vertices(bodyB_pose_t0);
|
|
|
|
if (ccd_type == "ee") {
|
|
double distance = ipc::edge_edge_distance(
|
|
VA_t0.row(0), VA_t0.row(1), VB_t0.row(0), VB_t0.row(1));
|
|
// std::cout << "distance_t0=" << distance << std::endl;
|
|
}
|
|
|
|
Pose<double> bodyA_pose_toi =
|
|
Pose<double>::interpolate(bodyA_pose_t0, bodyA_pose_t1, toi);
|
|
Pose<double> bodyB_pose_toi =
|
|
Pose<double>::interpolate(bodyB_pose_t0, bodyB_pose_t1, toi);
|
|
Eigen::MatrixXd VA_toi = bodyA.world_vertices(bodyA_pose_toi);
|
|
Eigen::MatrixXd VB_toi = bodyB.world_vertices(bodyB_pose_toi);
|
|
|
|
if (ccd_type == "ee") {
|
|
double distance = ipc::edge_edge_distance(
|
|
VA_toi.row(0), VA_toi.row(1), VB_toi.row(0), VB_toi.row(1));
|
|
// std::cout << "distance_toi=" << distance << std::endl;
|
|
CHECK(distance == Approx(0).margin(1e-10));
|
|
}
|
|
|
|
CAPTURE(ccd_type);
|
|
if (is_impacting) {
|
|
CHECK(toi > 0);
|
|
}
|
|
}
|
|
|