martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

WIP: explicit transforms refactor.

master
Martin Felis 2021-11-07 17:00:44 +01:00
parent cf87db975c
commit 1adf0c611f
2 changed files with 111 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
src/sconvcol.h
View File

@ -6,8 +6,8 @@ extern "C" {
#endif
#include <assert.h>
#include <stdbool.h>
#include <float.h>
#include <stdbool.h>
#include "vectorial/simd4x4f.h"
@ -21,6 +21,22 @@ inline bool sch_simd4f_close(simd4f a, simd4f b, float tol) {
return (simd4f_get_x(simd4f_length4_squared(simd4f_sub(a, b))) < tol * tol);
}
inline void sch_simd4x4f_transform_inv(simd4x4f* trans, simd4x4f* out) {
*out = *trans;
simd4f w = simd4f_create(
-simd4f_get_x(trans->w),
-simd4f_get_y(trans->w),
-simd4f_get_z(trans->w),
1.f);
out->w = simd4f_create(0.f, 0.f, 0.f, 1.f);
simd4x4f_transpose_inplace(out);
simd4f out_w;
simd4x4f_matrix_vector_mul(out, &w, &out_w);
out->w = out_w;
}
typedef struct sch_edge sch_edge;
typedef struct sch_vert sch_vert;
typedef struct sch_face sch_face;
@ -154,7 +170,11 @@ sch_edge* sch_hull_find_edge (const sch_hull* hull, const simd4f v0, const simd4
void sch_hull_get_support(const sch_hull* hull, simd4f n, simd4f* out_vert);
float sch_query_face_directions (const sch_hull* hull_A, const sch_hull* hull_B, sch_face_query* result);
float sch_query_face_directions(
const sch_hull* hull_A,
simd4x4f* trans_AtoB,
const sch_hull* hull_B,
sch_face_query* result);
void sch_clip_faces (const sch_face* ref_face, const sch_face* inc_face, sch_manifold* manifold);
@ -184,6 +204,7 @@ void sch_manifold_alloc (sch_manifold* manifold, int num_vertices) {
manifold->vertices = (simd4f*) aligned_alloc(16, sizeof(simd4f) * num_vertices);
assert (manifold->vertices != NULL);
manifold->num_vertices = num_vertices;
sch_manifold_reset(manifold);
}
void sch_manifold_add_point (sch_manifold* manifold, simd4f p) {
@ -488,26 +509,39 @@ void sch_hull_get_support(const sch_hull* hull, simd4f normal, simd4f* out_vert)
*out_vert = edge->vert->p;
}
float sch_query_face_directions (const sch_hull* hull_A, const sch_hull* hull_B, sch_face_query* result) {
float sch_query_face_directions(
const sch_hull* hull_A,
simd4x4f* trans_BtoA,
const sch_hull* hull_B,
sch_face_query* result) {
result->dist = -FLT_MAX;
for (int fi = 0; fi < hull_A->num_faces; fi++) {
sch_plane plane;
sch_hull_calc_plane(hull_A, fi, &plane);
simd4f vert;
sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane.n), &vert);
float distance = sch_plane_distance(&plane, &vert);
simd4f vert_B;
simd4f plane_n_B;
simd4x4f_inv_ortho_matrix_vector3_mul(trans_BtoA, &plane.n, &plane_n_B);
sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane_n_B), &vert_B);
simd4f vert_A;
simd4x4f_inv_ortho_matrix_point3_mul(trans_BtoA, &vert_B, &vert_A);
float distance = sch_plane_distance(&plane, &vert_A);
if (distance > result->dist) {
result->dist = distance;
result->face_idx = fi;
result->plane = plane;
result->vert = vert;
result->vert = vert_A;
}
}
return result->dist;
}
float sch_query_edge_directions (const sch_hull* hull_A, const sch_hull* hull_B, sch_edge_query* result) {
float sch_query_edge_directions(
const sch_hull* hull_A,
simd4x4f *trans_BtoA,
const sch_hull* hull_B,
sch_edge_query* result) {
result->dist = -FLT_MAX;
for (int iAe = 0; iAe < hull_A->num_edges; iAe++) {
for (int iBe = 0; iBe < hull_B->num_edges; iBe++) {
@ -678,21 +712,31 @@ void sch_create_edge_contact (const sch_edge_query* query_edge, const sch_hull*
#endif
}
bool sch_hull_sat(const sch_hull* hull_A, simd4x4f* trans_A, const sch_hull* hull_B, simd4x4f* trans_B, sch_manifold* result) {
bool sch_hull_sat(
const sch_hull* hull_A,
simd4x4f* trans_A,
const sch_hull* hull_B,
simd4x4f* trans_B,
sch_manifold* result) {
simd4x4f trans_Binv;
sch_simd4x4f_transform_inv(trans_B, &trans_Binv);
simd4x4f trans_BtoA;
simd4x4f_matrix_mul(&trans_Binv, trans_A, &trans_BtoA);
sch_face_query query_A_B;
sch_query_face_directions(hull_A, hull_B, &query_A_B);
sch_query_face_directions(hull_A, &trans_BtoA, hull_B, &query_A_B);
if (query_A_B.dist > 0.f) {
return true;
}
sch_face_query query_B_A;
sch_query_face_directions(hull_B, hull_A, &query_B_A);
sch_query_face_directions(hull_B, &trans_BtoA, hull_A, &query_B_A);
if (query_B_A.dist > 0.f) {
return true;
}
sch_edge_query query_edge;
sch_query_edge_directions (hull_A, hull_B, &query_edge);
sch_query_edge_directions(hull_A, &trans_BtoA, hull_B, &query_edge);
if (query_edge.dist > 0.f) {
return true;
}

70
tests/sconvcolTests.cc
View File

@ -5,6 +5,25 @@
using namespace std;
TEST_CASE("simd4x4f_trans_inf", "[sconvcol]") {
simd4x4f trans;
simd4x4f_lookat(&trans, simd4f_create (1.f, 0.f, 1.f, 1.f), simd4f_create (0.f, 0.f, -1.f, 1.f), simd4f_create (0.f, 1.f, 0.f, 1.f));
simd4x4f inv;
sch_simd4x4f_transform_inv(&trans, &inv);
simd4x4f mul_res;
simd4x4f_matrix_mul(&inv, &trans, &mul_res);
simd4x4f identity;
simd4x4f_identity(&identity);
REQUIRE(sch_simd4f_close(identity.x, mul_res.x, 1.0e-5));
REQUIRE(sch_simd4f_close(identity.y, mul_res.y, 1.0e-5));
REQUIRE(sch_simd4f_close(identity.z, mul_res.z, 1.0e-5));
REQUIRE(sch_simd4f_close(identity.w, mul_res.w, 1.0e-5));
}
TEST_CASE("Plane Distance", "[sconvcol]") {
sch_plane plane = {
simd4f_create(0.f, 0.f, 0.f, 0.f),
@ -338,14 +357,35 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
simd4f_get_x(r),
simd4f_get_y(r),
simd4f_get_z(r));
sch_hull_transform(&hull_B, translation);
THEN("Boxes separated") {
sch_hull_transform(&hull_B, translation);
simd4x4f identity;
simd4x4f_identity(&identity);
bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
REQUIRE(separated);
}
THEN("Boxes separated when supplying trans_B") {
simd4x4f identity;
simd4x4f_identity(&identity);
bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &translation, &manifold);
REQUIRE(separated);
}
THEN("Boxes separated when supplying trans_A") {
simd4f r = simd4f_create(-0.5f -0.4f * sqrtf(2.), 0.f, 0.f, 1.f);
simd4x4f_translation(
&translation,
simd4f_get_x(r),
simd4f_get_y(r),
simd4f_get_z(r));
simd4x4f identity;
simd4x4f_identity(&identity);
bool separated = sch_hull_sat(&hull_A, &translation, &hull_B, &identity, &manifold);
REQUIRE(separated);
}
}
GIVEN("Box B rotated by 45 degrees around y and translated along x by 1.1") {
@ -388,25 +428,25 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
sch_hull_rotate(&hull_B, M_PI / 180.0f * 45.f, simd4f_create(0.f, 0.f, 1.f, 1.f));
sch_hull_translate(&hull_B, 0., 0.f, 0.97f);
THEN("Boxes overlap") {
simd4x4f identity;
simd4x4f_identity(&identity);
bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
REQUIRE(!separated);
REQUIRE(manifold.num_points == 8);
}
// THEN("Boxes overlap") {
// simd4x4f identity;
// simd4x4f_identity(&identity);
// bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
// REQUIRE(!separated);
// REQUIRE(manifold.num_points == 8);
// }
}
GIVEN("Box B translated by 1x, 1.001z") {
sch_hull_translate(&hull_B, 1., 0.f, 1.00f);
THEN("Boxes overlap") {
simd4x4f identity;
simd4x4f_identity(&identity);
bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
REQUIRE(!separated);
REQUIRE(manifold.num_points == 8);
}
// THEN("Boxes overlap") {
// simd4x4f identity;
// simd4x4f_identity(&identity);
// bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
// REQUIRE(!separated);
// REQUIRE(manifold.num_points == 8);
// }
}
sch_manifold_free_memory(&manifold);