Added initial works on sconvcol - Simple Convex Collision manifold calculator

master
Martin Felis 2020-12-12 23:22:57 +01:00
parent d01417e8c9
commit d2eca6f19b
4 changed files with 212 additions and 4 deletions

View File

@ -29,7 +29,7 @@ target_include_directories(
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/rbdl/include> PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/rbdl/include>
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/3rdparty/rbdl/include>) PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/3rdparty/rbdl/include>)
target_link_libraries(${PROJECT_NAME} rbdl ccd) 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 # Simulator Executable
add_executable(runsim) add_executable(runsim)
@ -57,7 +57,6 @@ target_include_directories(
target_link_libraries(vissim ${PROJECT_NAME} glfw) target_link_libraries(vissim ${PROJECT_NAME} glfw)
target_sources(vissim PRIVATE target_sources(vissim PRIVATE
src/utils.cc
src/srender.c src/srender.c
3rdparty/glfw/deps/glad_gl.c 3rdparty/glfw/deps/glad_gl.c
3rdparty/imgui/imgui.cpp 3rdparty/imgui/imgui.cpp
@ -74,12 +73,15 @@ target_sources(vissim PRIVATE
# Tests # Tests
add_executable(runtests) 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( target_include_directories(
runtests runtests
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src> PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src>
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/rbdl/include> PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/rbdl/include>
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/3rdparty/rbdl/include>) PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/3rdparty/rbdl/include>
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/vectorial/include>
)
target_link_libraries(runtests ${PROJECT_NAME}) target_link_libraries(runtests ${PROJECT_NAME})

3
src/sconvcol.c Normal file
View File

@ -0,0 +1,3 @@
#define SCONVCOL_IMPLEMENTATION
#include "sconvcol.h"

133
src/sconvcol.h Normal file
View File

@ -0,0 +1,133 @@
#ifndef SCONVCOL_H
#define SCONVCOL_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <assert.h>
#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 */

70
tests/sconvcolTests.cc Normal file
View File

@ -0,0 +1,70 @@
#include <iostream>
#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));
}