rbdlsim/tests/sconvcolTests.cc

161 lines
4.5 KiB
C++

#include <iostream>
#include "catch.hpp"
#include "sconvcol.h"
bool simd4f_equal(simd4f a, simd4f b) {
return (simd4f_get_x(simd4f_length4_squared(simd4f_sub(a, b))) == 0.f);
}
using namespace std;
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("ManualCubeHull", "[sconvcol]") {}
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);
WHEN("adding two vertices ") {
simd4f vertex0 = simd4f_create(1.f, 0.f, 0.f, 1.f);
sch_builder_face_begin(&builder);
sch_builder_face_vertex(&builder, vertex0);
simd4f vertex1 = simd4f_create(0.f, 0.f, 0.f, 1.f);
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") {
simd4f vertex2 = simd4f_create(0.5f, 1.f, 0.f, 1.f);
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;
sch_builder_create_hull(&builder, &hull);
THEN("verify triangle hull") {
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(simd4f_equal(hull_vert0->p, vertex0));
REQUIRE(simd4f_equal(hull_vert1->p, vertex1));
REQUIRE(simd4f_equal(hull_vert2->p, vertex2));
REQUIRE(face->edge->next->next->next == face->edge);
}
}
}
}
}
}
TEST_CASE("Hull Get Plane", "[sconvcol]") {
simd4f vertices_pos_z[4] = {
simd4f_create(-1.f, -1.f, 1.f, 0.f),
simd4f_create(1.f, -1.f, 1.f, 0.f),
simd4f_create(1.f, 1.f, 1.f, 0.f),
simd4f_create(-1.f, 1.f, 1.f, 0.f)};
sch_face face_pos_z = {nullptr};
sch_create_face(4, vertices_pos_z, &face_pos_z);
simd4f vertices_pos_x[4] = {
simd4f_create(1.f, -1.f, 1.f, 0.f),
simd4f_create(1.f, -1.f, -1.f, 0.f),
simd4f_create(1.f, 1.f, -1.f, 0.f),
simd4f_create(1.f, 1.f, 1.f, 0.f)};
sch_face face_pos_x = {nullptr};
sch_create_face(4, vertices_pos_x, &face_pos_x);
sch_hull hull = {0};
sch_face faces[2] = {face_pos_z, face_pos_x};
sch_create_hull(2, faces, &hull);
sch_plane plane_pos_z;
sch_hull_get_plane(&hull, 0, &plane_pos_z);
simd4f point = simd4f_create(0.f, 0.f, 1.23f, 0.f);
float dist = sch_plane_distance(&plane_pos_z, &point);
REQUIRE_THAT(dist, Catch::Matchers::WithinRel(0.23, 1.0e-3));
}
TEST_CASE("Hull Get Support", "[sconvcol]") {}