rbdlsim/tests/sconvcolTests.cc

642 lines
20 KiB
C++

#include <iostream>
#include "catch.hpp"
#include "sconvcol.h"
using namespace std;
TEST_CASE("simd4x4f_trans_inf", "[sconvcol]") {
simd4x4f trans;
simd4x4f_lookat(&trans, simd4f_create (1.f, 0.f, 1.f, 1.f), simd4f_create (0.f, 0.f, -1.f, 1.f), simd4f_create (0.f, 1.f, 0.f, 1.f));
simd4x4f inv;
sch_simd4x4f_transform_inv(&trans, &inv);
simd4x4f mul_res;
simd4x4f_matrix_mul(&inv, &trans, &mul_res);
simd4x4f identity;
simd4x4f_identity(&identity);
REQUIRE(sch_simd4f_close(identity.x, mul_res.x, 1.0e-5));
REQUIRE(sch_simd4f_close(identity.y, mul_res.y, 1.0e-5));
REQUIRE(sch_simd4f_close(identity.z, mul_res.z, 1.0e-5));
REQUIRE(sch_simd4f_close(identity.w, mul_res.w, 1.0e-5));
}
TEST_CASE("Plane Distance", "[sconvcol]") {
sch_plane plane = {
simd4f_create(0.f, 0.f, 0.f, 0.f),
simd4f_create(0.f, 1.f, 0.f, 0.f)};
simd4f point = simd4f_create(0.f, 0.23f, 0.f, 0.f);
float dist = sch_plane_distance(&plane, &point);
REQUIRE(dist == 0.23f);
}
TEST_CASE("Edge Get Direction", "[sconvcol]") {
simd4f vertices[4] = {
simd4f_create(-1.f, -1.f, 0.f, 0.f),
simd4f_create(1.f, -1.f, 0.f, 0.f),
simd4f_create(1.f, 1.f, 0.f, 0.f),
simd4f_create(-1.f, 1.f, 0.f, 0.f)};
sch_face face = {nullptr};
sch_create_face(4, vertices, &face);
simd4f dir;
sch_edge_get_dir(face.edge, &dir);
simd4f dir_ref = simd4f_create(1.f, 0.f, 0.f, 0.f);
REQUIRE(1.f == simd4f_dot3_scalar(dir, dir_ref));
}
TEST_CASE("HullBuilder", "[sconvcol]") {
sch_hull_builder builder;
builder.num_vertices = 3;
GIVEN("A builder") {
WHEN("is reset") {
sch_builder_reset(&builder);
THEN("number of vertices and faces is set to 0") {
REQUIRE(builder.num_vertices == 0);
REQUIRE(builder.num_faces == 0);
}
WHEN("adding a vertex") {
int num_vertices = builder.num_vertices;
simd4f vertex = simd4f_create(1.f, 0.f, 0.f, 1.f);
sch_builder_face_begin(&builder);
sch_builder_face_vertex(&builder, vertex);
THEN("number of vertices increased") {
REQUIRE(builder.num_vertices > num_vertices);
}
}
}
}
GIVEN("An empty builder") {
sch_builder_reset(&builder);
simd4f vertex0 = simd4f_create(0.f, 0.f, 0.f, 1.f);
simd4f vertex1 = simd4f_create(1.f, 0.f, 0.f, 1.f);
simd4f vertex2 = simd4f_create(0.5f, 1.f, 0.f, 1.f);
WHEN("adding two vertices ") {
sch_builder_face_begin(&builder);
sch_builder_face_vertex(&builder, vertex0);
sch_builder_face_vertex(&builder, vertex1);
WHEN("ending the face") {
sch_builder_face_end(&builder);
THEN("face idx range goes from 0 to 1") {
REQUIRE(builder.num_faces == 1);
REQUIRE(builder.num_vertices == 2);
REQUIRE(builder.face_vert_idx_start[0] == 0);
REQUIRE(builder.face_vert_idx_end[0] == 1);
}
}
WHEN("creating a triangle") {
sch_builder_face_vertex(&builder, vertex2);
sch_builder_face_end(&builder);
THEN("face idx range goes from 0 to 2") {
REQUIRE(builder.num_faces == 1);
REQUIRE(builder.num_vertices == 3);
REQUIRE(builder.face_vert_idx_start[0] == 0);
REQUIRE(builder.face_vert_idx_end[0] == 2);
}
WHEN("creating a hull") {
sch_hull hull;
SchHullResult result = sch_builder_create_hull(&builder, &hull);
THEN("verify triangle hull") {
REQUIRE(result == SchHullResultOpenHull);
REQUIRE(hull.num_faces == 1);
sch_face* face = &hull.faces[0];
sch_vert* hull_vert0 = face->edge->vert;
sch_vert* hull_vert1 = face->edge->next->vert;
sch_vert* hull_vert2 = face->edge->next->next->vert;
REQUIRE(sch_simd4f_equal(hull_vert0->p, vertex0));
REQUIRE(sch_simd4f_equal(hull_vert1->p, vertex1));
REQUIRE(sch_simd4f_equal(hull_vert2->p, vertex2));
REQUIRE(face->edge->next->next->next == face->edge);
}
sch_hull_free_memory(&hull);
}
WHEN("adding concave face") {
sch_hull hull;
SchHullResult result = sch_builder_create_hull(&builder, &hull);
simd4f vertex3 = simd4f_create(0.5f, 0.25f, 0.1337f, 1.f);
sch_builder_face_begin(&builder);
sch_builder_face_vertex(&builder, vertex0);
sch_builder_face_vertex(&builder, vertex1);
sch_builder_face_vertex(&builder, vertex3);
sch_builder_face_end(&builder);
THEN("check face and vertex definitions") {
REQUIRE(builder.num_faces == 2);
REQUIRE(builder.num_vertices == 4);
REQUIRE(builder.face_vert_idx_start[0] == 0);
REQUIRE(builder.face_vert_idx_end[0] == 2);
REQUIRE(builder.face_vert_idx_start[1] == 3);
REQUIRE(builder.face_vert_idx_end[1] == 5);
}
THEN("hull creation fails") {
sch_hull concave_hull;
SchHullResult result = sch_builder_create_hull(&builder, &concave_hull);
REQUIRE(result == SchHullResultConcaveVertex);
sch_hull_free_memory(&concave_hull);
}
sch_hull_free_memory(&hull);
}
WHEN("adding adjacent clockwise winded face") {
simd4f vertex4 = simd4f_create(0.5f, -0.25f, 0, 1.f);
sch_builder_face_begin(&builder);
sch_builder_face_vertex(&builder, vertex0);
sch_builder_face_vertex(&builder, vertex1);
sch_builder_face_vertex(&builder, vertex4);
sch_builder_face_end(&builder);
THEN("hull creation fails") {
sch_hull hull;
SchHullResult result = sch_builder_create_hull(&builder, &hull);
REQUIRE(result == SchHullResultWrongWinding);
sch_hull_free_memory(&hull);
}
}
WHEN("adding adjacent counter clockwise winded face") {
simd4f vertex4 = simd4f_create(0.5f, -0.25f, 0, 1.f);
sch_builder_face_begin(&builder);
sch_builder_face_vertex(&builder, vertex0);
sch_builder_face_vertex(&builder, vertex4);
sch_builder_face_vertex(&builder, vertex1);
sch_builder_face_end(&builder);
THEN("edge v0-v1 has twin edge v1-v0") {
sch_hull hull;
SchHullResult result = sch_builder_create_hull(&builder, &hull);
REQUIRE(result == SchHullResultOpenHull);
sch_edge* edge_v0_v1 = sch_hull_find_edge(&hull, vertex0, vertex1);
sch_edge* edge_v1_v0 = sch_hull_find_edge(&hull, vertex1, vertex0);
REQUIRE (edge_v0_v1 != NULL);
REQUIRE (edge_v1_v0->twin == edge_v0_v1);
sch_hull_free_memory(&hull);
}
}
}
}
WHEN("creating a tetrahedron ") {
simd4f vertex3 = simd4f_create (0.5f, 0.f, -0.5f, 1.f);
// +z facing triangle
sch_builder_face_begin(&builder);
sch_builder_face_vertex(&builder, vertex0);
sch_builder_face_vertex(&builder, vertex1);
sch_builder_face_vertex(&builder, vertex2);
sch_builder_face_end(&builder);
// +x,-z facing triangle
sch_builder_face_begin(&builder);
sch_builder_face_vertex(&builder, vertex1);
sch_builder_face_vertex(&builder, vertex3);
sch_builder_face_vertex(&builder, vertex2);
sch_builder_face_end(&builder);
// -x,-z facing triangle
sch_builder_face_begin(&builder);
sch_builder_face_vertex(&builder, vertex0);
sch_builder_face_vertex(&builder, vertex2);
sch_builder_face_vertex(&builder, vertex3);
sch_builder_face_end(&builder);
// -y facing triangle
sch_builder_face_begin(&builder);
sch_builder_face_vertex(&builder, vertex0);
sch_builder_face_vertex(&builder, vertex3);
sch_builder_face_vertex(&builder, vertex1);
sch_builder_face_end(&builder);
THEN("results in a closed hull") {
sch_hull hull;
SchHullResult result = sch_builder_create_hull(&builder, &hull);
REQUIRE (result == SchHullResultOK);
sch_hull_free_memory(&hull);
}
}
}
}
TEST_CASE("UnitCubeSupport", "[sconvcol]") {
GIVEN("A unit cube hull") {
sch_hull hull;
sch_create_unitbox(&hull);
WHEN("Querying support for 2, 2, 2") {
simd4f support;
simd4f normal = simd4f_create(2.f, 2.f, 2.f, 1.f);
sch_hull_get_support(&hull, normal, &support);
THEN ("support is 0.5, 0.5, 0.5") {
simd4f reference = simd4f_create (0.5f, 0.5f, 0.5f, 1.f);
REQUIRE (sch_simd4f_equal(reference, support));
}
}
WHEN("Querying support for -2, -2, -2") {
simd4f support;
simd4f normal = simd4f_create(-2.f, -2.f, -2.f, 1.f);
sch_hull_get_support(&hull, normal, &support);
THEN ("support is -0.5, -0.5, -0.5") {
simd4f reference = simd4f_create (-0.5f, -0.5f, -0.5f, 1.f);
REQUIRE (sch_simd4f_equal(reference, support));
}
}
WHEN("Querying support for 0, 1, 0") {
simd4f support;
simd4f normal = simd4f_create(0.f, 1.f, 0.f, 1.f);
sch_hull_get_support(&hull, normal, &support);
THEN ("y component of support is 0.5") {
REQUIRE (simd4f_get_y(support) == 0.5);
}
}
sch_hull_free_memory(&hull);
}
}
TEST_CASE("UnitCubeTransform", "[sconvcol]") {
GIVEN("A unit cube hull") {
sch_hull hull;
sch_create_unitbox(&hull);
WHEN ("not transforming cube") {
THEN ("center is at 0,0,0") {
REQUIRE (sch_simd4f_equal(simd4f_create(0.f, 0.f, 0.f, 1.f), hull.center));
}
}
WHEN ("translating by 1, 2, 3") {
sch_hull_translate(&hull, 1.f, 2.f, 3.f);
THEN ("center is at 1, 2, 3") {
REQUIRE (sch_simd4f_equal(simd4f_create(1.f, 2.f, 3.f, 1.f), hull.center));
}
THEN ("is equivalent to transform by translation matrix") {
sch_hull hull_trans_mat;
sch_create_unitbox(&hull_trans_mat);
simd4x4f trans_mat;
simd4x4f_translation(&trans_mat, 1.f, 2.f, 3.f);
sch_hull_transform(&hull_trans_mat, trans_mat);
REQUIRE (sch_simd4f_equal(hull.center, hull_trans_mat.center));
sch_hull_free_memory(&hull_trans_mat);
}
}
WHEN ("rotating around y by 90 degrees") {
simd4f vert0 = simd4f_create (0.5f, -0.5f, 0.5f, 1.f);
REQUIRE (sch_simd4f_equal(vert0, hull.vertices[0].p));
simd4f vert0_rot = simd4f_create (0.5f, -0.5f, -0.5f, 1.f);
sch_hull_rotate(&hull, M_PI * 0.5, simd4f_create (1.f, 0.f, 0.f, 1.f));
THEN ("center is at 0, 0, 0") {
REQUIRE (sch_simd4f_equal(simd4f_create(0.f, 0.f, 0.f, 1.f), hull.center));
REQUIRE (sch_simd4f_close(vert0_rot, hull.vertices[0].p, 1.0e-5f));
}
}
sch_hull_free_memory(&hull);
}
}
TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
sch_hull hull_A;
sch_create_unitbox(&hull_A);
sch_hull hull_B;
sch_create_unitbox(&hull_B);
sch_manifold manifold;
sch_manifold_alloc(&manifold, SCH_BUILDER_MAX_NUM_FACE_VERTICES);
GIVEN ("Two unit boxes separated by 1.1 along x axis") {
simd4x4f translation;
simd4x4f_translation(
&translation,
0.5f + 0.4f * sqrtf(2.),
0.f,
0.f);
THEN("Boxes separated") {
sch_hull_transform(&hull_B, translation);
simd4x4f identity;
simd4x4f_identity(&identity);
bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
REQUIRE(separated);
}
THEN("Boxes separated when supplying trans_B") {
simd4x4f identity;
simd4x4f_identity(&identity);
bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &translation, &manifold);
REQUIRE(separated);
}
THEN("Boxes separated when supplying trans_A") {
simd4f r = simd4f_create(-0.5f -0.51f * sqrtf(2.), 0.f, 0.f, 1.f);
simd4x4f_translation(
&translation,
simd4f_get_x(r),
simd4f_get_y(r),
simd4f_get_z(r));
simd4x4f identity;
simd4x4f_identity(&identity);
bool separated = sch_hull_sat(&hull_A, &translation, &hull_B, &identity, &manifold);
REQUIRE(separated);
}
THEN("Boxes separated when supplying rot_trans to B") {
simd4x4f identity;
simd4x4f_identity(&identity);
simd4x4f rot;
simd4x4f_axis_rotation(&rot, 0.4f, simd4f_create (0.f, 1.f, 0.f, 1.f));
simd4x4f translation;
simd4x4f_translation(&translation, 0.5f + 0.51f * sqrtf(2.), 0.0f, 0.f);
simd4x4f rot_trans;
simd4x4f_matrix_mul(&translation, &rot, &rot_trans);
bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &rot_trans, &manifold);
REQUIRE(separated);
}
THEN("Boxes separated when supplying rot_trans to A") {
simd4x4f identity;
simd4x4f_identity(&identity);
simd4x4f rot;
simd4x4f_axis_rotation(&rot, 0.4f, simd4f_create (0.f, 1.f, 0.f, 1.f));
simd4x4f translation;
simd4x4f_translation(&translation, -0.5f - 0.51f * sqrtf(2.), 0.0f, 0.f);
simd4x4f rot_trans;
simd4x4f_matrix_mul(&translation, &rot, &rot_trans);
bool separated = sch_hull_sat(&hull_A, &rot_trans, &hull_B, &identity, &manifold);
REQUIRE(separated);
}
}
GIVEN ("Box B in edge-edge contact with A") {
simd4x4f rot_y;
simd4x4f_axis_rotation(&rot_y, 0.25f * M_PI, simd4f_create (0.f, 1.f, 0.f, 1.f));
simd4x4f rot_z;
simd4x4f_axis_rotation(&rot_z, 0.25f * M_PI, simd4f_create (0.f, 0.f, 1.f, 1.f));
simd4x4f trans_rot;
simd4x4f_matrix_mul(&rot_y, &rot_z, &trans_rot);
simd4x4f translation;
simd4x4f_translation(&translation, 0.98f, 0.f, -0.98f);
simd4x4f identity;
simd4x4f_identity(&identity);
bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &trans_rot, &manifold);
REQUIRE(!separated);
}
GIVEN("Box B rotated by 45 degrees around y and translated along x by 1.1") {
sch_hull_rotate(&hull_B, M_PI / 180.0f * 45.f, simd4f_create(0.f, 1.f, 0.f, 1.f));
sch_hull_translate(&hull_B, 1.1f, 0.f, 0.f);
THEN("Boxes overlap") {
simd4x4f identity;
simd4x4f_identity(&identity);
bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
REQUIRE(!separated);
}
}
GIVEN("Box A rotated by 45 deg around z, Box B rotated by 45 degrees around y and translated along x by sqrt(2)") {
sch_hull_rotate(&hull_A, M_PI / 180.0f * 45.f, simd4f_create(0.f, 0.f, 1.f, 1.f));
sch_hull_rotate(&hull_B, M_PI / 180.0f * 45.f, simd4f_create(0.f, 1.f, 0.f, 1.f));
sch_hull_translate(&hull_B, sqrt(2.f), 0.f, 0.f);
THEN("Boxes overlap") {
simd4x4f identity;
simd4x4f_identity(&identity);
bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
REQUIRE(!separated);
}
WHEN("Shifted by another 0.001 along x") {
sch_hull_translate(&hull_B, 0.001f, 0.f, 0.f);
THEN("Boxes are separated") {
simd4x4f identity;
simd4x4f_identity(&identity);
bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
REQUIRE(separated);
}
}
}
GIVEN("Box B translated by 1z and rotated 45 degrees around z") {
sch_hull_rotate(&hull_B, M_PI / 180.0f * 45.f, simd4f_create(0.f, 0.f, 1.f, 1.f));
sch_hull_translate(&hull_B, 0., 0.f, 0.97f);
// THEN("Boxes overlap") {
// simd4x4f identity;
// simd4x4f_identity(&identity);
// bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
// REQUIRE(!separated);
// REQUIRE(manifold.num_points == 8);
// }
}
GIVEN("Box B translated by 1x, 1.001z") {
sch_hull_translate(&hull_B, 1., 0.f, 1.00f);
// THEN("Boxes overlap") {
// simd4x4f identity;
// simd4x4f_identity(&identity);
// bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
// REQUIRE(!separated);
// REQUIRE(manifold.num_points == 8);
// }
}
sch_manifold_free_memory(&manifold);
sch_hull_free_memory(&hull_A);
sch_hull_free_memory(&hull_B);
}
TEST_CASE("PlaneTests", "[sconvcol]") {
GIVEN("Z=0 Plane") {
sch_plane plane;
plane.n = simd4f_create(0.f, 0.f, 1.f, 1.f);
plane.p = simd4f_create(1.f, 1.2354f, 0.f, 1.f);
WHEN("Querying point with z = -0.1") {
simd4f p = simd4f_create (1231.3f, 552.2f, -0.1f, 1.f);
float dist = sch_plane_distance (&plane, &p);
THEN ("distance is -0.1") {
REQUIRE (dist == -0.1f);
}
}
WHEN ("Intersecting with line (5., 3., 2.) - (12., 332., SCH_EPS") {
simd4f p0 = simd4f_create (5.f, 3.f, 1.f, 1.f);
simd4f p1 = simd4f_create (12.f, 332.f, SCH_EPS, 1.f);
simd4f result;
bool is_intersecting = sch_plane_intersection(&plane, &p0, &p1, &result);
THEN ("have intersection") {
REQUIRE (is_intersecting);
}
WHEN ("Reversing the query") {
simd4f result;
bool is_intersecting = sch_plane_intersection(&plane, &p1, &p0, &result);
THEN ("have intersection") {
REQUIRE (is_intersecting);
}
}
}
WHEN ("Intersecting with line (5., 3., 2.) - (12., 332., SCH_EPS * 2") {
simd4f p0 = simd4f_create (5.f, 3.f, 1.f, 1.f);
simd4f p1 = simd4f_create (12.f, 332.f, SCH_EPS * 2.f, 1.f);
simd4f result;
bool is_intersecting = sch_plane_intersection(&plane, &p0, &p1, &result);
THEN ("have no intersection") {
REQUIRE (is_intersecting);
}
WHEN ("Reversing the query") {
simd4f result;
bool is_intersecting = sch_plane_intersection(&plane, &p1, &p0, &result);
THEN ("have no intersection") {
REQUIRE (is_intersecting);
}
}
}
WHEN("Projecting point (123.4, 332., 212.) onto plane") {
simd4f p = simd4f_create (123.4f, 332.f, 212.f, 1.f);
sch_plane_point_project(&plane, &p);
THEN ("z component is 0.") {
REQUIRE (simd4f_get_z(p) == 0.f);
}
WHEN("Using z = -212.0") {
p = simd4f_create(123.4f, 332.f, -212.f, 1.f);
sch_plane_point_project(&plane, &p);
THEN("projected z component is 0.") {
REQUIRE(simd4f_get_z(p) == 0.f);
}
}
}
}
}
TEST_CASE ("ClipFaces", "[sconvcol]") {
sch_hull_builder builder;
sch_hull triangle_hull;
sch_hull quad_hull;
// Build triangle
sch_builder_reset(&builder);
// top
sch_builder_face_begin(&builder);
sch_builder_face_vertex(&builder, simd4f_create (0.5f, -0.5f, 0.f, 1.f));
sch_builder_face_vertex(&builder, simd4f_create ( 0.0f, 0.5f, 0.f, 1.f));
sch_builder_face_vertex(&builder, simd4f_create ( -0.5f, -0.5f, 0.f, 1.f));
sch_builder_face_end(&builder);
// bottom
sch_builder_face_begin(&builder);
sch_builder_face_vertex(&builder, simd4f_create (0.5f, -0.5f, 0.f, 1.f));
sch_builder_face_vertex(&builder, simd4f_create ( -0.5f, -0.5f, 0.f, 1.f));
sch_builder_face_vertex(&builder, simd4f_create ( 0.0f, 0.5f, 0.f, 1.f));
sch_builder_face_end(&builder);
int hull_result = sch_builder_create_hull(&builder, &triangle_hull);
triangle_hull.center = simd4f_create(0.f, 0.f, 0.f, 1.f);
REQUIRE (hull_result == SchHullResultOK);
// Build quad
sch_builder_reset(&builder);
// top
sch_builder_face_begin(&builder);
sch_builder_face_vertex(&builder, simd4f_create (-0.5f, -0.5f, 0.f, 1.f));
sch_builder_face_vertex(&builder, simd4f_create ( 0.5f, -0.5f, 0.f, 1.f));
sch_builder_face_vertex(&builder, simd4f_create ( 0.5f, 0.5f, 0.f, 1.f));
sch_builder_face_vertex(&builder, simd4f_create (-0.5f, 0.5f, 0.f, 1.f));
sch_builder_face_end(&builder);
// bottom
sch_builder_face_begin(&builder);
sch_builder_face_vertex(&builder, simd4f_create(0.5f, -0.5f, 0.f, 1.f));
sch_builder_face_vertex(&builder, simd4f_create(-0.5f, -0.5f, 0.f, 1.f));
sch_builder_face_vertex(&builder, simd4f_create(-0.5f, 0.5f, 0.f, 1.f));
sch_builder_face_vertex(&builder, simd4f_create(0.5f, 0.5f, 0.f, 1.f));
sch_builder_face_end(&builder);
hull_result = sch_builder_create_hull(&builder, &quad_hull);
quad_hull.center = simd4f_create(0.f, 0.f, 0.f, 1.f);
REQUIRE (hull_result == SchHullResultOK);
GIVEN ("A Triangle and a Quad") {
WHEN ("Translating the Quad along +Y") {
sch_hull_translate(&quad_hull, 0.f, 0.5f, 0.f);
sch_manifold manifold;
sch_manifold_alloc(&manifold, SCH_BUILDER_MAX_NUM_FACE_VERTICES);
sch_manifold_reset(&manifold);
sch_clip_faces(&triangle_hull.faces[0], &quad_hull.faces[1], &manifold);
REQUIRE (manifold.num_points == 3);
sch_manifold_free_memory(&manifold);
}
}
}