From d2eca6f19b746039a429a22a0b9c0bd59f5c760d Mon Sep 17 00:00:00 2001 From: Martin Felis Date: Sat, 12 Dec 2020 23:22:57 +0100 Subject: [PATCH] Added initial works on sconvcol - Simple Convex Collision manifold calculator --- CMakeLists.txt | 10 ++-- src/sconvcol.c | 3 + src/sconvcol.h | 133 +++++++++++++++++++++++++++++++++++++++++ tests/sconvcolTests.cc | 70 ++++++++++++++++++++++ 4 files changed, 212 insertions(+), 4 deletions(-) create mode 100644 src/sconvcol.c create mode 100644 src/sconvcol.h create mode 100644 tests/sconvcolTests.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 7be5820..9943cef 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,7 @@ target_include_directories( PUBLIC $ PUBLIC $) target_link_libraries(${PROJECT_NAME} rbdl ccd) -target_sources(${PROJECT_NAME} PRIVATE src/rbdlsim.cc) +target_sources(${PROJECT_NAME} PRIVATE src/rbdlsim.cc src/utils.cc) # Simulator Executable add_executable(runsim) @@ -57,7 +57,6 @@ target_include_directories( target_link_libraries(vissim ${PROJECT_NAME} glfw) target_sources(vissim PRIVATE - src/utils.cc src/srender.c 3rdparty/glfw/deps/glad_gl.c 3rdparty/imgui/imgui.cpp @@ -74,12 +73,15 @@ target_sources(vissim PRIVATE # Tests add_executable(runtests) -target_sources(runtests PRIVATE tests/runtests.cc tests/CollisionTests.cc src/utils.cc) +target_sources(runtests PRIVATE tests/runtests.cc tests/CollisionTests.cc + src/utils.cc src/sconvcol.c tests/sconvcolTests.cc) target_include_directories( runtests PUBLIC $ PUBLIC $ PUBLIC $ - PUBLIC $) + PUBLIC $ + PUBLIC $ + ) target_link_libraries(runtests ${PROJECT_NAME}) diff --git a/src/sconvcol.c b/src/sconvcol.c new file mode 100644 index 0000000..c155383 --- /dev/null +++ b/src/sconvcol.c @@ -0,0 +1,3 @@ +#define SCONVCOL_IMPLEMENTATION + +#include "sconvcol.h" diff --git a/src/sconvcol.h b/src/sconvcol.h new file mode 100644 index 0000000..2dadde3 --- /dev/null +++ b/src/sconvcol.h @@ -0,0 +1,133 @@ +#ifndef SCONVCOL_H +#define SCONVCOL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +#include "vectorial/simd4x4f.h" + +typedef struct sch_edge sch_edge; +typedef struct sch_vert sch_vert; +typedef struct sch_face sch_face; +typedef struct sch_hull sch_hull; +typedef struct sch_plane sch_plane; + +struct sch_edge { + sch_vert* vert; + sch_edge* twin; + sch_face* face; + sch_edge* next; +}; + +struct sch_vert { + simd4f p; + sch_edge* edge; +}; + +struct sch_face { + sch_edge* edge; +}; + +struct sch_hull { + int num_faces; + sch_face* faces; +}; + +struct sch_plane { + simd4f p; + simd4f n; +}; + +float sch_plane_distance (sch_plane* plane, simd4f* v); + +void sch_edge_get_dir (sch_edge* edge, simd4f* dir); + +void sch_hull_get_plane (sch_hull* hull, int index, sch_plane *out_plane); + +void sch_hull_get_support (sch_hull* hull, simd4f n, simd4f* out_vert); + +void sch_create_face (int num_vert, simd4f* vertices, sch_face* out_face); + +void sch_create_hull (int num_faces, sch_face* faces, + sch_hull* out_hull); + +// +// srender Implementation +// + +#ifdef SCONVCOL_IMPLEMENTATION + +float sch_plane_distance (sch_plane* plane, simd4f* v) { + return simd4f_dot3_scalar(simd4f_sub (*v, plane->p), plane->n); +} + +void sch_create_face (int num_vert, simd4f* vertices, sch_face* out_face) { + assert(out_face != NULL); + assert(out_face->edge == NULL); + + int i = 0; + sch_edge* f_edges = malloc(sizeof(sch_edge) * num_vert); + sch_vert* f_verts = malloc(sizeof(sch_vert) * num_vert); + + while (i < num_vert) { + sch_vert* vert = &f_verts[i]; + sch_edge* edge = &f_edges[i]; + + edge->twin = NULL; + edge->vert = vert; + edge->face = out_face; + edge->next = &f_edges[(i + 1) % num_vert]; + + vert->edge = edge; + vert->p = vertices[i]; + + i++; + } + + out_face->edge = &f_edges[0]; +} + +void sch_edge_get_dir (sch_edge* edge, simd4f* out_dir) { + *out_dir = simd4f_sub (edge->next->vert->p, edge->vert->p); + float recip_len = 1. / sqrt(simd4f_dot3_scalar (*out_dir, *out_dir)); + *out_dir = simd4f_mul (*out_dir, simd4f_splat(recip_len)); +} + +void sch_create_hull (int num_faces, sch_face* faces, + sch_hull* out_hull) { + assert (out_hull != NULL); + assert (out_hull->num_faces == NULL); + + out_hull->num_faces = num_faces; + out_hull->faces = faces; +} + +void sch_hull_get_plane (sch_hull* hull, int index, sch_plane* out_plane) { + assert (hull != NULL); + assert (index >= 0 && index < hull->num_faces); + assert (out_plane != NULL); + + // TODO move plane calculation to create hull? + sch_face* face = &hull->faces[index]; + sch_edge* edge0 = face->edge; + sch_edge* edge1 = edge0->next; + simd4f dir0; + simd4f dir1; + sch_edge_get_dir(edge0, &dir0); + sch_edge_get_dir(edge1, &dir1); + + out_plane->p = edge0->vert->p; + out_plane->n = simd4f_cross3 (dir0, dir1); +} + +#endif /* SCONVCOL_IMPLEMENTATION */ + +#ifdef __cplusplus +} +#endif + +#endif /* SCONVCOL_H */ diff --git a/tests/sconvcolTests.cc b/tests/sconvcolTests.cc new file mode 100644 index 0000000..115138f --- /dev/null +++ b/tests/sconvcolTests.cc @@ -0,0 +1,70 @@ +#include + +#include "catch.hpp" +#include "sconvcol.h" + +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 = {0}; + 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("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 = {0}; + 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 = {0}; + 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)); +} +