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

WIP: edge-edge collision with supplied transforms.

master
Martin Felis 2021-11-07 21:57:06 +01:00
parent 7830840d81
commit b19a74dcdf
3 changed files with 55 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/sconvcol.h
View File

@ -550,8 +550,10 @@ float sch_query_edge_directions(
simd4f dir_A;
sch_edge_get_dir(edge_A, &dir_A);
simd4f dir_B_B;
sch_edge_get_dir(edge_B, &dir_B_B);
simd4f dir_B;
sch_edge_get_dir(edge_B, &dir_B);
simd4x4f_inv_ortho_matrix_vector3_mul(trans_BtoA, &dir_B_B, &dir_B);
simd4f axis = simd4f_cross3 (dir_A, dir_B);
if (simd4f_dot3_scalar(dir_A, simd4f_sub(edge_A->vert->p, hull_A->center) ) < 0.f) {
@ -572,8 +574,11 @@ float sch_query_edge_directions(
// normal. We therefore use the support point of hull_A along the axis.
sch_hull_get_support(hull_A, plane_A.n, &plane_A.p);
simd4f vert_B_B;
sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane_A.n), &vert_B_B);
simd4f vert_B;
sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane_A.n), &vert_B);
simd4x4f_matrix_point3_mul(trans_BtoA, &vert_B_B, &vert_B);
float distance = sch_plane_distance(&plane_A, &vert_B);
if (distance > result->dist) {

34
src/vissim.cc
View File

@ -598,13 +598,7 @@ struct SconvHullScene {
SceneHull mHullB;
SceneHull* mCurrentHull;
void init() {
sch_create_unitbox(&mHullA.hull);
mHullA.createMesh();
sch_create_unitbox(&mHullB.hull);
mHullB.createMesh();
void setupSceneRotTrans() {
simd4x4f rot;
simd4x4f_axis_rotation(&rot, 0.4f, simd4f_create (0.f, 1.f, 0.f, 1.f));
@ -612,6 +606,32 @@ struct SconvHullScene {
simd4x4f_translation(&translation, -0.5f - 0.51f * sqrtf(2.), 0.0f, 0.f);
simd4x4f_matrix_mul(&translation, &rot, &mHullB.transform);
}
void setupSceneEdgeCollision() {
simd4x4f rot_y;
simd4x4f_axis_rotation(&rot_y, 0.25f * M_PI, simd4f_create (0.f, 1.f, 0.f, 1.f));
simd4x4f rot_z;
simd4x4f_axis_rotation(&rot_z, 0.25f * M_PI, simd4f_create (0.f, 0.f, 1.f, 1.f));
simd4x4f rot;
simd4x4f_matrix_mul(&rot_y, &rot_z, &rot);
simd4x4f translation;
simd4x4f_translation(&translation, 0.98f, 0.f, -0.98f);
simd4x4f_matrix_mul(&translation, &rot, &mHullB.transform);
}
void init() {
sch_create_unitbox(&mHullA.hull);
mHullA.createMesh();
sch_create_unitbox(&mHullB.hull);
mHullB.createMesh();
setupSceneEdgeCollision();
mCurrentHull = &mHullA;
};

21
tests/sconvcolTests.cc
View File

@ -422,6 +422,27 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
}
}
GIVEN ("Box B in edge-edge contact with A") {
simd4x4f rot_y;
simd4x4f_axis_rotation(&rot_y, 0.25f * M_PI, simd4f_create (0.f, 1.f, 0.f, 1.f));
simd4x4f rot_z;
simd4x4f_axis_rotation(&rot_z, 0.25f * M_PI, simd4f_create (0.f, 0.f, 1.f, 1.f));
simd4x4f trans_rot;
simd4x4f_matrix_mul(&rot_y, &rot_z, &trans_rot);
simd4x4f translation;
simd4x4f_translation(&translation, 0.98f, 0.f, -0.98f);
simd4x4f identity;
simd4x4f_identity(&identity);
bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &trans_rot, &manifold);
REQUIRE(!separated);
}
GIVEN("Box B rotated by 45 degrees around y and translated along x by 1.1") {
sch_hull_rotate(&hull_B, M_PI / 180.0f * 45.f, simd4f_create(0.f, 1.f, 0.f, 1.f));
sch_hull_translate(&hull_B, 1.1f, 0.f, 0.f);