Added initial works on sconvcol - Simple Convex Collision manifold calculator
Martin Felis
parent
d01417e8c9
commit
d2eca6f19b
|
|
@ -29,7 +29,7 @@ target_include_directories(
|
|||
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/rbdl/include>
|
||||
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/3rdparty/rbdl/include>)
|
||||
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 $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src>
|
||||
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})
|
||||
|
|
|
|||
|
|
@ -0,0 +1,3 @@
|
|||
#define SCONVCOL_IMPLEMENTATION
|
||||
|
||||
#include "sconvcol.h"
|
||||
|
|
@ -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 */
|
||||
|
|
@ -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));
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue