Added initial works on sconvcol - Simple Convex Collision manifold calculator

CMakeLists.txt

@@ -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})

src/sconvcol.c

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

src/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 */

tests/sconvcolTests.cc

@@ -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));
+}
+