From 024616345d4d374d0e7b7afa6aa78307234488ac Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Mon, 24 Feb 2025 09:47:50 -0800 Subject: [PATCH] Support edge contacts with multiccd in nativeccd. PiperOrigin-RevId: 730489114 Change-Id: I02847d84bbc15a1526391d5e1b7109ab83ea4a59 --- src/engine/engine_collision_gjk.c | 274 +++++++++++++--- src/engine/engine_collision_gjk.h | 7 +- test/engine/engine_collision_gjk_test.cc | 231 ++++++++++++++ .../engine/testdata/collision_convex/dice.xml | 46 +++ test/engine/testdata/collision_convex/die.obj | 296 ++++++++++++++++++ .../testdata/collision_convex/edge_edge.xml | 24 ++ .../sensor/fromto_capsule_ellipsoid.xml | 32 ++ 7 files changed, 861 insertions(+), 49 deletions(-) create mode 100644 test/engine/testdata/collision_convex/dice.xml create mode 100644 test/engine/testdata/collision_convex/die.obj create mode 100644 test/engine/testdata/collision_convex/edge_edge.xml create mode 100644 test/engine/testdata/sensor/fromto_capsule_ellipsoid.xml diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index f7b180ee..1603746c 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -1572,6 +1572,11 @@ static mjtNum planeIntersect(mjtNum res[3], const mjtNum pn[3], mjtNum pd, static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1, const mjtNum* face2, int nface2, const mjtNum n[3], const mjtNum dir[3]) { + // clipping face needs to be at least a triangle + if (nface1 < 3) { + return; + } + // compute plane normal and distance to plane for each vertex mjtNum pn[3 * mjMAX_POLYVERT], pd[mjMAX_POLYVERT]; for (int i = 0; i < nface1 - 1; i++) { @@ -1658,11 +1663,23 @@ static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1, } // no pruning needed - status->nx = npolygon; + int k = 0; for (int i = 0; i < 3*npolygon; i += 3) { - copy3(status->x2 + i, polygon + i); - sub3(status->x1 + i, status->x2 + i, dir); + int skip = 0; + + // find possible duplicate vertices + for (int j = 0; j < k; j += 3) { + if (equal3(status->x2 + j, polygon + i)) { + skip = 1; + break; + } + } + if (skip) continue; + copy3(status->x2 + k, polygon + i); + sub3(status->x1 + k, status->x2 + k, dir); + k += 3; } + status->nx = k/3; } @@ -1768,6 +1785,56 @@ static int meshNormals(mjtNum* res, int resind[3], int dim, mjCCDObj* obj, +// compute normal directional vectors along possible edges given by up to two vertices +static int meshEdgeNormals(mjtNum* res, mjtNum* endverts, int dim, mjCCDObj* obj, + const mjtNum v1[3], const mjtNum v2[3], int v1i, int v2i) { + // mesh data + int g = obj->geom; + const mjtNum* mat = obj->data->geom_xmat + 9*g; + const mjtNum* pos = obj->data->geom_xpos + 3*g; + + // only one edge + if (dim == 2) { + copy3(endverts, v2); + sub3(res, v2, v1); + mju_normalize3(res); + return 1; + } + + if (dim == 1) { + const mjModel* m = obj->model; + int polyadr = m->mesh_polyadr[m->geom_dataid[g]]; + int vertadr = m->mesh_vertadr[m->geom_dataid[g]]; + + int v1_adr = m->mesh_polymapadr[vertadr + v1i]; + int v1_num = m->mesh_polymapnum[vertadr + v1i]; + if (v1_num > mjMAX_POLYVERT) v1_num = mjMAX_POLYVERT; + + // loop through all faces with vertex v1 + for (int i = 0; i < v1_num; i++) { + int idx = m->mesh_polymap[v1_adr + i]; + int adr = m->mesh_polyvertadr[polyadr + idx]; + int nvert = m->mesh_polyvertnum[polyadr + idx]; + // find previous vertex in polygon to form edge + for (int j = 0; j < nvert; j++) { + int v = m->mesh_polyvert[adr + j]; + if (v == v1i) { + float* verts = m->mesh_vert + 3*vertadr; + int k = (j == 0) ? nvert - 1 : j - 1; + float* vert = verts + 3*k; + globalcoord(endverts + 3*i, mat, pos, vert[0], vert[1], vert[2]); + sub3(res + 3*i, endverts + 3*i, v1); + mju_normalize3(res + 3*i); + } + } + } + return 3; + } + return 0; +} + + + // compute possible face normals of a box given up to 3 vertices static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj, int v1, int v2, int v3) { @@ -1825,6 +1892,45 @@ static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj, +// compute possible edge normals for box for edge collisions +static int boxEdgeNormals(mjtNum res[9], mjtNum endverts[9], int dim, mjCCDObj* obj, + const mjtNum v1[3], const mjtNum v2[3], int v1i, int v2i) { + // box data + int g = 3*obj->geom; + const mjtNum* mat = obj->data->geom_xmat + 3*g; + const mjtNum* pos = obj->data->geom_xpos + g; + const mjtNum* size = obj->model->geom_size + g; + + if (dim == 2) { + copy3(endverts, v2); + sub3(res, v2, v1); + mju_normalize3(res); + return 1; + } + + // return 3 adjacent vertices + if (dim == 1) { + mjtNum x = (v1i & 1) ? size[0] : -size[0]; + mjtNum y = (v1i & 2) ? size[1] : -size[1]; + mjtNum z = (v1i & 4) ? size[2] : -size[2]; + + globalcoord(endverts, mat, pos, -x, y, z); + sub3(res, endverts, v1); + mju_normalize3(res); + + globalcoord(endverts + 3, mat, pos, x, -y, z); + sub3(res + 3, endverts + 3, v1); + mju_normalize3(res + 3); + + globalcoord(endverts + 6, mat, pos, x, y, -z); + sub3(res + 6, endverts + 6, v1); + mju_normalize3(res + 6); + return 3; + } + return 0; +} + + // recover face of a box from its index static int boxFace(mjtNum res[12], mjCCDObj* obj, int idx) { // box data @@ -1903,11 +2009,11 @@ static int meshFace(mjtNum* res, mjCCDObj* obj, int idx) { // find two normals that are facing each other within a tolerance, return 1 if found -static inline int alignedNormals(int res[2], const mjtNum* v, int nv, +static inline int alignedFaces(int res[2], const mjtNum* v, int nv, const mjtNum* w, int nw) { for (int i = 0; i < nv; i++) { for (int j = 0; j < nw; j++) { - if (dot3(v + 3*i, w + 3*j) < -mjCOSINE_TOL) { + if (dot3(v + 3*i, w + 3*j) < -mjFACE_TOL) { res[0] = i; res[1] = j; return 1; @@ -1919,16 +2025,33 @@ static inline int alignedNormals(int res[2], const mjtNum* v, int nv, -// return number of dimensions of a feature (1, 2 or 3) -static inline int simplexDim(int* v1, int* v2, int* v3) { - int val1 = *v1; - int val2 = *v2; - int val3 = *v3; +// find two normals that are perpendicular to each other within a tolerance, return 1 if found +static inline int alignedFaceEdge(int res[2], const mjtNum* edge, int nedge, + const mjtNum* face, int nface) { + for (int i = 0; i < nface; i++) { + for (int j = 0; j < nedge; j++) { + if (mju_abs(dot3(edge + 3*j, face + 3*i)) < mjEDGE_TOL) { + res[0] = j; + res[1] = i; + return 1; + } + } + } + return 0; +} + + +// return number (1, 2 or 3) of dimensions of a simplex; reorder vertices if necessary +static inline int simplexDim(int* v1i, int* v2i, int* v3i, mjtNum** v1, mjtNum** v2, mjtNum** v3) { + int val1 = *v1i; + int val2 = *v2i; + int val3 = *v3i; if (val1 != val2) { return (val3 == val1 || val3 == val2) ? 2 : 3; } if (val1 != val3) { + *v2i = *v3i; *v2 = *v3; return 2; } @@ -1940,65 +2063,122 @@ static inline int simplexDim(int* v1, int* v2, int* v3) { // recover multiple contacts from EPA polytope static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { - mjtNum face1[mjMAX_POLYVERT * 3], face2[mjMAX_POLYVERT * 3]; - + mjtNum face1[mjMAX_POLYVERT * 3], face2[mjMAX_POLYVERT * 3], endverts[mjMAX_POLYVERT * 3]; // get vertices of faces from EPA - int v11 = pt->verts[face->verts[0]].index1; - int v12 = pt->verts[face->verts[1]].index1; - int v13 = pt->verts[face->verts[2]].index1; - int v21 = pt->verts[face->verts[0]].index2; - int v22 = pt->verts[face->verts[1]].index2; - int v23 = pt->verts[face->verts[2]].index2; + int v11i = pt->verts[face->verts[0]].index1; + int v12i = pt->verts[face->verts[1]].index1; + int v13i = pt->verts[face->verts[2]].index1; + int v21i = pt->verts[face->verts[0]].index2; + int v22i = pt->verts[face->verts[1]].index2; + int v23i = pt->verts[face->verts[2]].index2; + mjtNum* v11 = pt->verts[face->verts[0]].vert1; + mjtNum* v12 = pt->verts[face->verts[1]].vert1; + mjtNum* v13 = pt->verts[face->verts[2]].vert1; + mjtNum* v21 = pt->verts[face->verts[0]].vert2; + mjtNum* v22 = pt->verts[face->verts[1]].vert2; + mjtNum* v23 = pt->verts[face->verts[2]].vert2; // get dimensions of features of geoms 1 and 2 - int nface1 = simplexDim(&v11, &v12, &v13); - int nface2 = simplexDim(&v21, &v22, &v23); + int nface1 = simplexDim(&v11i, &v12i, &v13i, &v11, &v12, &v13); + int nface2 = simplexDim(&v21i, &v22i, &v23i, &v21, &v22, &v23); int nnorms1 = 0, nnorms2 = 0; mjtNum n1[3 * mjMAX_POLYVERT], n2[3 * mjMAX_POLYVERT]; // normals of possible face collisions int idx1[mjMAX_POLYVERT], idx2[mjMAX_POLYVERT]; // indices of faces // get all possible face normals for each geom if (obj1->geom_type == mjGEOM_BOX) { - nnorms1 = boxNormals(n1, idx1, nface1, obj1, v11, v12, v13); + nnorms1 = boxNormals(n1, idx1, nface1, obj1, v11i, v12i, v13i); } else if (obj1->geom_type == mjGEOM_MESH) { - nnorms1 = meshNormals(n1, idx1, nface1, obj1, v11, v12, v13); + nnorms1 = meshNormals(n1, idx1, nface1, obj1, v11i, v12i, v13i); } if (obj2->geom_type == mjGEOM_BOX) { - nnorms2 = boxNormals(n2, idx2, nface2, obj2, v21, v22, v23); + nnorms2 = boxNormals(n2, idx2, nface2, obj2, v21i, v22i, v23i); } else if (obj2->geom_type == mjGEOM_MESH) { - nnorms2 = meshNormals(n2, idx2, nface2, obj2, v21, v22, v23); + nnorms2 = meshNormals(n2, idx2, nface2, obj2, v21i, v22i, v23i); } - // determine if any two normals match - int res[2]; - if (!alignedNormals(res, n1, nnorms1, n2, nnorms2)) { - return; + // determine if any two face normals match + int res[2], edgecon1 = 0, edgecon2 = 0; + if (!alignedFaces(res, n1, nnorms1, n2, nnorms2)) { + // check if edge-face collision + if (nface1 < 3 && nface1 <= nface2) { + nnorms1 = 0; + if (obj1->geom_type == mjGEOM_BOX) { + nnorms1 = boxEdgeNormals(n1, endverts, nface1, obj1, v11, v12, v11i, v12i); + } else if (obj1->geom_type == mjGEOM_MESH) { + nnorms1 = meshEdgeNormals(n1, endverts, nface1, obj1, v11, v12, v11i, v12i); + } + if (!alignedFaceEdge(res, n1, nnorms1, n2, nnorms2)) return; + edgecon1 = 1; + + // check if face-edge collision + } else if (nface2 < 3) { + nnorms2 = 0; + if (obj2->geom_type == mjGEOM_BOX) { + nnorms2 = boxEdgeNormals(n2, endverts, nface2, obj2, v21, v22, v21i, v22i); + } else if (obj2->geom_type == mjGEOM_MESH) { + nnorms2 = meshEdgeNormals(n2, endverts, nface2, obj2, v21, v22, v21i, v22i); + } + if (!alignedFaceEdge(res, n2, nnorms2, n1, nnorms1)) return; + edgecon2 = 1; + } else { + // no multi-contact + return; + } } int i = res[0], j = res[1]; - // recover matching faces - if (obj1->geom_type == mjGEOM_BOX) { - nface1 = boxFace(face1, obj1, idx1[i]); - } else if (obj1->geom_type == mjGEOM_MESH) { - nface1 = meshFace(face1, obj1, idx1[i]); - } - if (obj2->geom_type == mjGEOM_BOX) { - nface2 = boxFace(face2, obj2, idx2[j]); - } else if (obj2->geom_type == mjGEOM_MESH) { - nface2 = meshFace(face2, obj2, idx2[j]); + // recover geom1 matching edge or face + if (edgecon1) { + copy3(face1, pt->verts[face->verts[0]].vert1); + copy3(face1 + 3, endverts + 3*i); + nface1 = 2; + } else { + if (obj1->geom_type == mjGEOM_BOX) { + int ind = (edgecon2 ? idx1[j] : idx1[i]); + nface1 = boxFace(face1, obj1, ind); + } else if (obj1->geom_type == mjGEOM_MESH) { + int ind = (edgecon2 ? idx1[j] : idx1[i]); + nface1 = meshFace(face1, obj1, ind); + } } - if (nface1 >= 3 && nface2 >= 3) { - // TODO(kylebayes): this approximates the contact direction, by scaling the face normal by the - // single contact direction's magnitude. This is effective, but polygonClip should compute - // this for each contact point. - mjtNum diff[3], approx_dir[3]; - sub3(diff, status->x2, status->x1); + // recover geom2 matching edge or face + if (edgecon2) { + copy3(face2, pt->verts[face->verts[0]].vert2); + copy3(face2 + 3, endverts + 3*i); + nface2 = 2; + } else { + if (obj2->geom_type == mjGEOM_BOX) { + nface2 = boxFace(face2, obj2, idx2[j]); + } else if (obj2->geom_type == mjGEOM_MESH) { + nface2 = meshFace(face2, obj2, idx2[j]); + } + } + + // TODO(kylebayes): this approximates the contact direction, by scaling the face normal by the + // single contact direction's magnitude. This is effective, but polygonClip should compute + // this for each contact point. + mjtNum diff[3], approx_dir[3]; + sub3(diff, status->x2, status->x1); + + // face1 is an edge; clip face1 against face2 + if (edgecon1) { scl3(approx_dir, n2 + 3*j, norm3(diff)); - - // clip the faces and store the results in status - polygonClip(status, face1, nface1, face2, nface2, n1 + 3*i, approx_dir); + polygonClip(status, face2, nface2, face1, nface1, n2 + 3*j, approx_dir); + return; } + + // face2 is an edge; clip face2 against face1 + if (edgecon2) { + scl3(approx_dir, n1 + 3*j, -norm3(diff)); + polygonClip(status, face1, nface1, face2, nface2, n1 + 3*j, approx_dir); + return; + } + + // face-face collision + scl3(approx_dir, n2 + 3*j, norm3(diff)); + polygonClip(status, face1, nface1, face2, nface2, n1 + 3*i, approx_dir); } diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index cca220cb..d4bdf006 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -25,8 +25,11 @@ extern "C" { #endif -// tolerance for considering two normals to be aligned -#define mjCOSINE_TOL 0.99999872 +// tolerance for normal alignment of two faces (cosine of 1.6e-3) +#define mjFACE_TOL 0.99999872 + +// tolerance for edge-face alignment (sine of 1.6e-3) +#define mjEDGE_TOL 0.00159999931 // max number of supported vertices in a polygon face of a mesh #define mjMAX_POLYVERT 150 diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 52c63002..f54e0ca2 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -1167,6 +1167,237 @@ TEST_F(MjGjkTest, MeshMeshPrune) { mj_deleteModel(model); } +TEST_F(MjGjkTest, BoxEdge) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4); + + EXPECT_EQ(ncons, 2); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxEdge2) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + mjtNum* xmat = data->geom_xmat + 9; + mjtNum* xpos = data->geom_xpos + 3; + + xmat[0] = 0.9999979704374094557906005320546682924032; + xmat[1] = -0.0017789363449516469497385662279498319549; + xmat[2] = -0.0009457835609818190025430140188689165370; + xmat[3] = 0.0017817418144636251123996695255868871755; + xmat[4] = 0.9999939910254954655854930933855939656496; + xmat[5] = 0.0029737701675293876438233020564894104609; + xmat[6] = 0.0009404877299599626429629783963548561587; + xmat[7] = -0.0029754492741947335607277658198199787876; + xmat[8] = 0.9999951310803708581786963804916013032198; + + xpos[0] = 0.0005578602979296120537716641152314878127; + xpos[1] = 0.0098645950089783600300830102014515432529; + xpos[2] = 1.1037596929447945903746131079969927668571; + + xmat = data->geom_xmat + 18; + xpos = data->geom_xpos + 6; + + xmat[0] = 0.0006737475542006746490053537002040684456; + xmat[1] = -0.0095603689585630827196816028390458086506; + xmat[2] = 0.9999540716500983084102927023195661604404; + xmat[3] = -0.1095658134726250898527410981841967441142; + xmat[4] = 0.9939334085756179604231874691322445869446; + xmat[5] = 0.0095766296438734854756802405972848646343; + xmat[6] = -0.9939793149670246297233688892447389662266; + xmat[7] = -0.1095672335264066821203243762283818796277; + xmat[8] = -0.0003778293989772788311065632171903416747; + + xpos[0] = -0.0218119359455731035013492657981259981170; + xpos[1] = 0.9828851949225971829093850828940048813820; + xpos[2] = 3.0930077345364814789263618877157568931580; + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4); + + EXPECT_EQ(ncons, 2); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxEdgeEdge) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + mjtNum* xmat = data->geom_xmat + 9; + mjtNum* xpos = data->geom_xpos + 3; + + xmat[0] = 0.9182779243587342321575306414160877466202; + xmat[1] = -0.0000000000000000000364268564068890756444; + xmat[2] = 0.3959364262547898638544552341045346111059; + xmat[3] = -0.0000000000000000000591321577502441383525; + xmat[4] = 1.0000000000000000000000000000000000000000; + xmat[5] = 0.0000000000000000002291443915550544102718; + xmat[6] = -0.3959364262547898638544552341045346111059; + xmat[7] = -0.0000000000000000002338308114719865891040; + xmat[8] = 0.9182779243587342321575306414160877466202; + + xpos[0] = -1.3241298058948087756903078116010874509811; + xpos[1] = 0.0000000000000000007148993364299687318184; + xpos[2] = 2.8141526153588731773425024584867060184479; + + xmat = data->geom_xmat + 18; + xpos = data->geom_xpos + 6; + + xmat[0] = 0.9182779243587342321575306414160877466202; + xmat[1] = 0.0000000000000000000728398144756416399722; + xmat[2] = -0.3959364262547898638544552341045346111059; + xmat[3] = -0.0000000000000000001674060251593158490713; + xmat[4] = 1.0000000000000000000000000000000000000000; + xmat[5] = -0.0000000000000000002042889652712837518138; + xmat[6] = 0.3959364262547898638544552341045346111059; + xmat[7] = 0.0000000000000000002538761903338069406631; + xmat[8] = 0.9182779243587342321575306414160877466202; + + xpos[0] = 1.3241298058948089977349127366323955357075; + xpos[1] = -0.0000000000000000008679606505055748997840; + xpos[2] = 2.8141526153588731773425024584867060184479; + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4); + + EXPECT_EQ(ncons, 2); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, MeshEdge) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4); + + EXPECT_EQ(ncons, 2); + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) { std::array error; mjModel* model = LoadModelFromString(kEllipoid, error.data(), error.size()); diff --git a/test/engine/testdata/collision_convex/dice.xml b/test/engine/testdata/collision_convex/dice.xml new file mode 100644 index 00000000..2a3b6880 --- /dev/null +++ b/test/engine/testdata/collision_convex/dice.xml @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + > + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/collision_convex/die.obj b/test/engine/testdata/collision_convex/die.obj new file mode 100644 index 00000000..91964032 --- /dev/null +++ b/test/engine/testdata/collision_convex/die.obj @@ -0,0 +1,296 @@ +v 0.740000 0.740000 -1.000000 +v 0.740000 1.000000 -0.740000 +v 1.000000 0.740000 -0.740000 +v 0.740000 0.807269 -0.991147 +v 0.740000 0.870021 -0.965154 +v 0.740000 0.923848 -0.923848 +v 0.807269 0.740000 -0.991147 +v 0.809387 0.809958 -0.980606 +v 0.808254 0.870420 -0.954318 +v 0.806858 0.917583 -0.917748 +v 0.870021 0.740000 -0.965154 +v 0.870490 0.808646 -0.954150 +v 0.863847 0.863908 -0.932116 +v 0.858399 0.903670 -0.903688 +v 0.890111 0.890111 -0.890111 +v 0.807269 0.991147 -0.740000 +v 0.870021 0.965154 -0.740000 +v 0.923848 0.923848 -0.740000 +v 0.740000 0.991147 -0.807269 +v 0.809958 0.980606 -0.809387 +v 0.870420 0.954318 -0.808254 +v 0.917583 0.917748 -0.806858 +v 0.740000 0.965154 -0.870021 +v 0.808646 0.954150 -0.870490 +v 0.863908 0.932116 -0.863847 +v 0.903670 0.903688 -0.858399 +v 0.991147 0.740000 -0.807269 +v 0.965154 0.740000 -0.870021 +v 0.923848 0.740000 -0.923848 +v 0.991147 0.807269 -0.740000 +v 0.980606 0.809387 -0.809958 +v 0.954318 0.808254 -0.870420 +v 0.917748 0.806858 -0.917583 +v 0.965154 0.870021 -0.740000 +v 0.954150 0.870490 -0.808646 +v 0.932116 0.863847 -0.863908 +v 0.903688 0.858399 -0.903670 +v 0.740000 -1.000000 -0.740000 +v 0.740000 -0.740000 -1.000000 +v 1.000000 -0.740000 -0.740000 +v 0.740000 -0.991147 -0.807269 +v 0.740000 -0.965154 -0.870021 +v 0.740000 -0.923848 -0.923848 +v 0.807269 -0.991147 -0.740000 +v 0.809387 -0.980606 -0.809958 +v 0.808254 -0.954318 -0.870420 +v 0.806858 -0.917748 -0.917583 +v 0.870021 -0.965154 -0.740000 +v 0.870490 -0.954150 -0.808646 +v 0.863847 -0.932116 -0.863908 +v 0.858399 -0.903688 -0.903670 +v 0.890111 -0.890111 -0.890111 +v 0.807269 -0.740000 -0.991147 +v 0.870021 -0.740000 -0.965154 +v 0.923848 -0.740000 -0.923848 +v 0.740000 -0.807269 -0.991147 +v 0.809958 -0.809387 -0.980606 +v 0.870420 -0.808254 -0.954318 +v 0.917583 -0.806858 -0.917748 +v 0.740000 -0.870021 -0.965154 +v 0.808646 -0.870490 -0.954150 +v 0.863908 -0.863847 -0.932116 +v 0.903670 -0.858399 -0.903688 +v 0.991147 -0.807269 -0.740000 +v 0.965154 -0.870021 -0.740000 +v 0.923848 -0.923848 -0.740000 +v 0.991147 -0.740000 -0.807269 +v 0.980606 -0.809958 -0.809387 +v 0.954318 -0.870420 -0.808254 +v 0.917748 -0.917583 -0.806858 +v 0.965154 -0.740000 -0.870021 +v 0.954150 -0.808646 -0.870490 +v 0.932116 -0.863908 -0.863847 +v 0.903688 -0.903670 -0.858399 +v 1.000000 0.740000 0.740000 +v 0.740000 1.000000 0.740000 +v 0.740000 0.740000 1.000000 +v 0.991147 0.807269 0.740000 +v 0.965154 0.870021 0.740000 +v 0.923848 0.923848 0.740000 +v 0.991147 0.740000 0.807269 +v 0.980606 0.809958 0.809387 +v 0.954318 0.870420 0.808254 +v 0.917748 0.917583 0.806858 +v 0.965154 0.740000 0.870021 +v 0.954150 0.808646 0.870490 +v 0.932116 0.863908 0.863847 +v 0.903688 0.903670 0.858399 +v 0.890111 0.890111 0.890111 +v 0.740000 0.991147 0.807269 +v 0.740000 0.965154 0.870021 +v 0.740000 0.923848 0.923848 +v 0.807269 0.991147 0.740000 +v 0.809387 0.980606 0.809958 +v 0.808254 0.954318 0.870420 +v 0.806858 0.917748 0.917583 +v 0.870021 0.965154 0.740000 +v 0.870490 0.954150 0.808646 +v 0.863847 0.932116 0.863908 +v 0.858399 0.903688 0.903670 +v 0.807269 0.740000 0.991147 +v 0.870021 0.740000 0.965154 +v 0.923848 0.740000 0.923848 +v 0.740000 0.807269 0.991147 +v 0.809958 0.809387 0.980606 +v 0.870420 0.808254 0.954318 +v 0.917583 0.806858 0.917748 +v 0.740000 0.870021 0.965154 +v 0.808646 0.870490 0.954150 +v 0.863908 0.863847 0.932116 +v 0.903670 0.858399 0.903688 +v 1.000000 -0.740000 0.740000 +v 0.740000 -0.740000 1.000000 +v 0.740000 -1.000000 0.740000 +v 0.991147 -0.740000 0.807269 +v 0.965154 -0.740000 0.870021 +v 0.923848 -0.740000 0.923848 +v 0.991147 -0.807269 0.740000 +v 0.980606 -0.809387 0.809958 +v 0.954318 -0.808254 0.870420 +v 0.917748 -0.806858 0.917583 +v 0.965154 -0.870021 0.740000 +v 0.954150 -0.870490 0.808646 +v 0.932116 -0.863847 0.863908 +v 0.903688 -0.858399 0.903670 +v 0.890111 -0.890111 0.890111 +v 0.740000 -0.807269 0.991147 +v 0.740000 -0.870021 0.965154 +v 0.740000 -0.923848 0.923848 +v 0.807269 -0.740000 0.991147 +v 0.809387 -0.809958 0.980606 +v 0.808254 -0.870420 0.954318 +v 0.806858 -0.917583 0.917748 +v 0.870021 -0.740000 0.965154 +v 0.870490 -0.808646 0.954150 +v 0.863847 -0.863908 0.932116 +v 0.858399 -0.903670 0.903688 +v 0.807269 -0.991147 0.740000 +v 0.870021 -0.965154 0.740000 +v 0.923848 -0.923848 0.740000 +v 0.740000 -0.991147 0.807269 +v 0.809958 -0.980606 0.809387 +v 0.870420 -0.954318 0.808254 +v 0.917583 -0.917748 0.806858 +v 0.740000 -0.965154 0.870021 +v 0.808646 -0.954150 0.870490 +v 0.863908 -0.932116 0.863847 +v 0.903670 -0.903688 0.858399 +v -0.740000 0.740000 -1.000000 +v -1.000000 0.740000 -0.740000 +v -0.740000 1.000000 -0.740000 +v -0.807269 0.740000 -0.991147 +v -0.870021 0.740000 -0.965154 +v -0.923848 0.740000 -0.923848 +v -0.740000 0.807269 -0.991147 +v -0.809958 0.809387 -0.980606 +v -0.870420 0.808254 -0.954318 +v -0.917583 0.806858 -0.917748 +v -0.740000 0.870021 -0.965154 +v -0.808646 0.870490 -0.954150 +v -0.863908 0.863847 -0.932116 +v -0.903670 0.858399 -0.903688 +v -0.890111 0.890111 -0.890111 +v -0.991147 0.807269 -0.740000 +v -0.965154 0.870021 -0.740000 +v -0.923848 0.923848 -0.740000 +v -0.991147 0.740000 -0.807269 +v -0.980606 0.809958 -0.809387 +v -0.954318 0.870420 -0.808254 +v -0.917748 0.917583 -0.806858 +v -0.965154 0.740000 -0.870021 +v -0.954150 0.808646 -0.870490 +v -0.932116 0.863908 -0.863847 +v -0.903688 0.903670 -0.858399 +v -0.740000 0.991147 -0.807269 +v -0.740000 0.965154 -0.870021 +v -0.740000 0.923848 -0.923848 +v -0.807269 0.991147 -0.740000 +v -0.809387 0.980606 -0.809958 +v -0.808254 0.954318 -0.870420 +v -0.806858 0.917748 -0.917583 +v -0.870021 0.965154 -0.740000 +v -0.870490 0.954150 -0.808646 +v -0.863847 0.932116 -0.863908 +v -0.858399 0.903688 -0.903670 +v -1.000000 -0.740000 -0.740000 +v -0.740000 -0.740000 -1.000000 +v -0.740000 -1.000000 -0.740000 +v -0.991147 -0.740000 -0.807269 +v -0.965154 -0.740000 -0.870021 +v -0.923848 -0.740000 -0.923848 +v -0.991147 -0.807269 -0.740000 +v -0.980606 -0.809387 -0.809958 +v -0.954318 -0.808254 -0.870420 +v -0.917748 -0.806858 -0.917583 +v -0.965154 -0.870021 -0.740000 +v -0.954150 -0.870490 -0.808646 +v -0.932116 -0.863847 -0.863908 +v -0.903688 -0.858399 -0.903670 +v -0.890111 -0.890111 -0.890111 +v -0.740000 -0.807269 -0.991147 +v -0.740000 -0.870021 -0.965154 +v -0.740000 -0.923848 -0.923848 +v -0.807269 -0.740000 -0.991147 +v -0.809387 -0.809958 -0.980606 +v -0.808254 -0.870420 -0.954318 +v -0.806858 -0.917583 -0.917748 +v -0.870021 -0.740000 -0.965154 +v -0.870490 -0.808646 -0.954150 +v -0.863847 -0.863908 -0.932116 +v -0.858399 -0.903670 -0.903688 +v -0.807269 -0.991147 -0.740000 +v -0.870021 -0.965154 -0.740000 +v -0.923848 -0.923848 -0.740000 +v -0.740000 -0.991147 -0.807269 +v -0.809958 -0.980606 -0.809387 +v -0.870420 -0.954318 -0.808254 +v -0.917583 -0.917748 -0.806858 +v -0.740000 -0.965154 -0.870021 +v -0.808646 -0.954150 -0.870490 +v -0.863908 -0.932116 -0.863847 +v -0.903670 -0.903688 -0.858399 +v -1.000000 0.740000 0.740000 +v -0.740000 0.740000 1.000000 +v -0.740000 1.000000 0.740000 +v -0.991147 0.740000 0.807269 +v -0.965154 0.740000 0.870021 +v -0.923848 0.740000 0.923848 +v -0.991147 0.807269 0.740000 +v -0.980606 0.809387 0.809958 +v -0.954318 0.808254 0.870420 +v -0.917748 0.806858 0.917583 +v -0.965154 0.870021 0.740000 +v -0.954150 0.870490 0.808646 +v -0.932116 0.863847 0.863908 +v -0.903688 0.858399 0.903670 +v -0.890111 0.890111 0.890111 +v -0.740000 0.807269 0.991147 +v -0.740000 0.870021 0.965154 +v -0.740000 0.923848 0.923848 +v -0.807269 0.740000 0.991147 +v -0.809387 0.809958 0.980606 +v -0.808254 0.870420 0.954318 +v -0.806858 0.917583 0.917748 +v -0.870021 0.740000 0.965154 +v -0.870490 0.808646 0.954150 +v -0.863847 0.863908 0.932116 +v -0.858399 0.903670 0.903688 +v -0.807269 0.991147 0.740000 +v -0.870021 0.965154 0.740000 +v -0.923848 0.923848 0.740000 +v -0.740000 0.991147 0.807269 +v -0.809958 0.980606 0.809387 +v -0.870420 0.954318 0.808254 +v -0.917583 0.917748 0.806858 +v -0.740000 0.965154 0.870021 +v -0.808646 0.954150 0.870490 +v -0.863908 0.932116 0.863847 +v -0.903670 0.903688 0.858399 +v -0.740000 -1.000000 0.740000 +v -0.740000 -0.740000 1.000000 +v -1.000000 -0.740000 0.740000 +v -0.740000 -0.991147 0.807269 +v -0.740000 -0.965154 0.870021 +v -0.740000 -0.923848 0.923848 +v -0.807269 -0.991147 0.740000 +v -0.809387 -0.980606 0.809958 +v -0.808254 -0.954318 0.870420 +v -0.806858 -0.917748 0.917583 +v -0.870021 -0.965154 0.740000 +v -0.870490 -0.954150 0.808646 +v -0.863847 -0.932116 0.863908 +v -0.858399 -0.903688 0.903670 +v -0.890111 -0.890111 0.890111 +v -0.807269 -0.740000 0.991147 +v -0.870021 -0.740000 0.965154 +v -0.923848 -0.740000 0.923848 +v -0.740000 -0.807269 0.991147 +v -0.809958 -0.809387 0.980606 +v -0.870420 -0.808254 0.954318 +v -0.917583 -0.806858 0.917748 +v -0.740000 -0.870021 0.965154 +v -0.808646 -0.870490 0.954150 +v -0.863908 -0.863847 0.932116 +v -0.903670 -0.858399 0.903688 +v -0.991147 -0.807269 0.740000 +v -0.965154 -0.870021 0.740000 +v -0.923848 -0.923848 0.740000 +v -0.991147 -0.740000 0.807269 +v -0.980606 -0.809958 0.809387 +v -0.954318 -0.870420 0.808254 +v -0.917748 -0.917583 0.806858 +v -0.965154 -0.740000 0.870021 +v -0.954150 -0.808646 0.870490 +v -0.932116 -0.863908 0.863847 +v -0.903688 -0.903670 0.858399 diff --git a/test/engine/testdata/collision_convex/edge_edge.xml b/test/engine/testdata/collision_convex/edge_edge.xml new file mode 100644 index 00000000..b4815850 --- /dev/null +++ b/test/engine/testdata/collision_convex/edge_edge.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/sensor/fromto_capsule_ellipsoid.xml b/test/engine/testdata/sensor/fromto_capsule_ellipsoid.xml new file mode 100644 index 00000000..f626eb1f --- /dev/null +++ b/test/engine/testdata/sensor/fromto_capsule_ellipsoid.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +