diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 5ba14c02..942e6172 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -360,8 +360,10 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9], int mjc_Convex(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { ccd_t ccd; - mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}}; - mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; // init ccd structure mjc_initCCD(&ccd, m); @@ -586,46 +588,45 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d, //---------------------------- heightfield collisions --------------------------------------------- -// ccd prism object type -struct _mjtPrism { - mjtNum v[6][3]; -}; - -typedef struct _mjtPrism mjtPrism; - - -// ccd prism support function -static void prism_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec) { +// prism support function +static void mjc_prism_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { int istart, ibest; mjtNum best, tmp; - const mjtPrism* p = (const mjtPrism*)obj; // find best vertex in halfspace determined by dir.z - istart = dir->v[2] < 0 ? 0 : 3; + istart = dir[2] < 0 ? 0 : 3; ibest = istart; - best = mju_dot3(p->v[istart], dir->v); + best = mju_dot3(obj->prism[istart], dir); for (int i=istart+1; i < istart+3; i++) { - if ((tmp = mju_dot3(p->v[i], dir->v)) > best) { + if ((tmp = mju_dot3(obj->prism[i], dir)) > best) { ibest = i; best = tmp; } } // copy best point - mju_copy3(vec->v, p->v[ibest]); + mju_copy3(res, obj->prism[ibest]); +} + +// ccd prism support function +static void mjccd_prism_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec) { + mjc_prism_support(vec->v, (mjCCDObj*) obj, dir->v); } -// ccd prism center function -static void prism_center(const void *obj, ccd_vec3_t *center) { - const mjtPrism* p = (const mjtPrism*)obj; - +// prism center function +static void mjc_prism_center(mjtNum res[3], const mjCCDObj* obj) { // compute mean - mju_zero3(center->v); + mju_zero3(res); for (int i=0; i < 6; i++) { - mju_addTo3(center->v, p->v[i]); + mju_addTo3(res, obj->prism[i]); } - mju_scl3(center->v, center->v, 1.0/6.0); + mju_scl3(res, res, 1.0/6.0); +} + +// ccd prism center function +static void mjccd_prism_center(const void *obj, ccd_vec3_t *center) { + mjc_prism_center(center->v, (const mjCCDObj*) obj); } @@ -636,17 +637,17 @@ static void prism_firstdir(const void* o1, const void* o2, ccd_vec3_t *vec) { // add vertex to prism, count vertices -static void addVert(int* nvert, mjtPrism* prism, mjtNum x, mjtNum y, mjtNum z) { +static void addVert(int* nvert, mjCCDObj* obj, mjtNum x, mjtNum y, mjtNum z) { // move old data - mju_copy3(prism->v[0], prism->v[1]); - mju_copy3(prism->v[1], prism->v[2]); - mju_copy3(prism->v[3], prism->v[4]); - mju_copy3(prism->v[4], prism->v[5]); + mju_copy3(obj->prism[0], obj->prism[1]); + mju_copy3(obj->prism[1], obj->prism[2]); + mju_copy3(obj->prism[3], obj->prism[4]); + mju_copy3(obj->prism[4], obj->prism[5]); // add new vertex at last position - prism->v[2][0] = prism->v[5][0] = x; - prism->v[2][1] = prism->v[5][1] = y; - prism->v[5][2] = z; + obj->prism[2][0] = obj->prism[5][0] = x; + obj->prism[2][1] = obj->prism[5][1] = y; + obj->prism[5][2] = z; // count (*nvert)++; @@ -665,12 +666,15 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, int ncol = m->hfield_ncol[hid]; int dr[2], cnt, rmin, rmax, cmin, cmax; const float* data = m->hfield_data + m->hfield_adr[hid]; - mjtPrism prism; + mjCCDObj obj1; + obj1.center = mjc_prism_center; + obj1.support = mjc_prism_support; // ccd-related ccd_vec3_t dirccd, vecccd; ccd_real_t depth; - mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; ccd_t ccd; // point size1 to hfield size instead of geom1 size @@ -713,32 +717,32 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, // get support point in +X ccdVec3Set(&dirccd, 1, 0, 0); - mjccd_support(&obj, &dirccd, &vecccd); + mjccd_support(&obj2, &dirccd, &vecccd); xmax = vecccd.v[0]; // get support point in -X ccdVec3Set(&dirccd, -1, 0, 0); - mjccd_support(&obj, &dirccd, &vecccd); + mjccd_support(&obj2, &dirccd, &vecccd); xmin = vecccd.v[0]; // get support point in +Y ccdVec3Set(&dirccd, 0, 1, 0); - mjccd_support(&obj, &dirccd, &vecccd); + mjccd_support(&obj2, &dirccd, &vecccd); ymax = vecccd.v[1]; // get support point in -Y ccdVec3Set(&dirccd, 0, -1, 0); - mjccd_support(&obj, &dirccd, &vecccd); + mjccd_support(&obj2, &dirccd, &vecccd); ymin = vecccd.v[1]; // get support point in +Z ccdVec3Set(&dirccd, 0, 0, 1); - mjccd_support(&obj, &dirccd, &vecccd); + mjccd_support(&obj2, &dirccd, &vecccd); zmax = vecccd.v[2]; // get support point in -Z ccdVec3Set(&dirccd, 0, 0, -1); - mjccd_support(&obj, &dirccd, &vecccd); + mjccd_support(&obj2, &dirccd, &vecccd); zmin = vecccd.v[2]; // box-box test @@ -767,13 +771,13 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, // init ccd structure mjc_initCCD(&ccd, m); ccd.first_dir = prism_firstdir; - ccd.center1 = prism_center; + ccd.center1 = mjccd_prism_center; ccd.center2 = mjccd_center; - ccd.support1 = prism_support; + ccd.support1 = mjccd_prism_support; ccd.support2 = mjccd_support; // geom margin needed for actual collision test - obj.margin = margin; + obj2.margin = margin; // compute real-valued grid step, and triangulation direction dx = (2.0*size1[0]) / (ncol-1); @@ -782,7 +786,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, dr[1] = 0; // set zbottom value using base size - prism.v[0][2] = prism.v[1][2] = prism.v[2][2] = -size1[3]; + obj1.prism[0][2] = obj1.prism[1][2] = obj1.prism[2][2] = -size1[3]; // process all prisms in sub-grid cnt = 0; @@ -791,19 +795,20 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, for (int c=cmin; c <= cmax; c++) { for (int i=0; i < 2; i++) { // send vertex to prism constructor - addVert(&nvert, &prism, dx*c-size1[0], dy*(r+dr[i])-size1[1], + addVert(&nvert, &obj1, dx*c-size1[0], dy*(r+dr[i])-size1[1], data[(r+dr[i])*ncol+c]*size1[2]+margin); // check for enough vertices if (nvert > 2) { // prism height test - if (prism.v[3][2] < zmin && prism.v[4][2] < zmin && prism.v[5][2] < zmin) { + if (obj1.prism[3][2] < zmin && obj1.prism[4][2] < zmin + && obj1.prism[5][2] < zmin) { continue; } // run MPR, save contact - if (_mjCCDPENETRATION(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0 && - !ccdVec3Eq(&dirccd, ccd_vec3_origin)) { + if (_mjCCDPENETRATION(&obj1, &obj2, &ccd, &depth, &dirccd, &vecccd) == 0 + && !ccdVec3Eq(&dirccd, ccd_vec3_origin)) { // fill in contact data, transform to global coordinates con[cnt].dist = -depth; mju_mulMatVec3(con[cnt].frame, mat1, dirccd.v); @@ -1103,8 +1108,10 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con, int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) { ccd_t ccd; - mjCCDObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}, {0, 0, 0}}; - mjCCDObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; // init ccd structure mjc_initCCD(&ccd, m); @@ -1128,7 +1135,9 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, mjtNum vec[3], dx, dy; mjtNum xmin, xmax, ymin, ymax, zmin, zmax; int dr[2], cnt, rmin, rmax, cmin, cmax; - mjtPrism prism; + mjCCDObj obj1; + obj1.center = mjc_prism_center; + obj1.support = mjc_prism_support; // get hfield info int hid = m->geom_dataid[g]; @@ -1151,7 +1160,8 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, // ccd-related ccd_vec3_t dirccd, vecccd; ccd_real_t depth; - mjCCDObj obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj2 = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; ccd_t ccd; //------------------------------------- AABB computation, box-box test @@ -1211,9 +1221,9 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, // init ccd structure CCD_INIT(&ccd); ccd.first_dir = prism_firstdir; - ccd.center1 = prism_center; + ccd.center1 = mjccd_prism_center; ccd.center2 = mjccd_center; - ccd.support1 = prism_support; + ccd.support1 = mjccd_prism_support; ccd.support2 = mjccd_support; // set ccd parameters @@ -1227,7 +1237,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, dr[1] = 0; // set zbottom value using base size - prism.v[0][2] = prism.v[1][2] = prism.v[2][2] = -hsize[3]; + obj1.prism[0][2] = obj1.prism[1][2] = obj1.prism[2][2] = -hsize[3]; // process all prisms in sub-grid cnt = 0; @@ -1236,18 +1246,18 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, for (int c=cmin; c <= cmax; c++) { for (int k=0; k < 2; k++) { // send vertex to prism constructor - addVert(&nvert, &prism, dx*c-hsize[0], dy*(r+dr[k])-hsize[1], + addVert(&nvert, &obj1, dx*c-hsize[0], dy*(r+dr[k])-hsize[1], hdata[(r+dr[k])*ncol+c]*hsize[2]+margin); // check for enough vertices if (nvert > 2) { // prism height test - if (prism.v[3][2] < zmin && prism.v[4][2] < zmin && prism.v[5][2] < zmin) { + if (obj1.prism[3][2] < zmin && obj1.prism[4][2] < zmin && obj1.prism[5][2] < zmin) { continue; } // run MPR, save contact - if (_mjCCDPENETRATION(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0) { + if (_mjCCDPENETRATION(&obj1, &obj2, &ccd, &depth, &dirccd, &vecccd) == 0) { if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) { // fill in contact data, transform to global coordinates con[cnt].dist = -depth; diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index 24f38648..9c27623f 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -52,6 +52,9 @@ struct _mjCCDObj { mjtNum margin; mjtNum rotate[4]; mjtNum x0[3]; // initial guess of the witness point + void (*center)(mjtNum res[3], const struct _mjCCDObj* obj); + void (*support)(mjtNum res[3], struct _mjCCDObj* obj, const mjtNum dir[3]); + mjtNum prism[6][3]; // for hfield }; typedef struct _mjCCDObj mjCCDObj; diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 55f202e9..2118d67a 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -203,8 +203,8 @@ static void gjk_support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* ob mju_scl3(dir, dir_neg, -1); // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir) - mjc_support(s1, obj1, dir); - mjc_support(s2, obj2, dir_neg); + obj1->support(s1, obj1, dir); + obj2->support(s2, obj2, dir_neg); } @@ -218,8 +218,8 @@ static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, mju_scl3(dir_neg, dir, -1); // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir) - mjc_support(s1, obj1, dir); - mjc_support(s2, obj2, dir_neg); + obj1->support(s1, obj1, dir); + obj2->support(s2, obj2, dir_neg); } @@ -1007,8 +1007,8 @@ int mj_gjkPenetration(const void *obj1, const void *obj2, const ccd_t *ccd, mjCCDObj* o2 = (mjCCDObj*) obj2; nearest.n[1] = 34; - mjc_center(o1->x0, o1); - mjc_center(o2->x0, o2); + o1->center(o1->x0, o1); + o2->center(o2->x0, o2); config.max_iterations = ccd->max_iterations; config.tolerance = ccd->mpr_tolerance; diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 8ced09d0..f7a30e1b 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -50,8 +50,10 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], mjtNum x2[3]) { mjCCDConfig config = {kMaxIterations, kTolerance}; - mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; - mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; mjc_center(obj1.x0, &obj1); mjc_center(obj2.x0, &obj2); mjtNum dist = mj_gjk(&config, &obj1, &obj2); @@ -63,8 +65,10 @@ mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], mjtNum run_gjkPenetration(mjModel* m, mjData* d, int g1, int g2, mjtNum dir[3] = nullptr, mjtNum pos[3] = nullptr) { - mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; - mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; ccd_t ccd; ccd.mpr_tolerance = kTolerance; ccd.epa_tolerance = kTolerance; @@ -198,6 +202,31 @@ TEST_F(MjGjkTest, EllipsoidEllipsoid) { mj_deleteModel(model); } +TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) { + 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, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr); + + EXPECT_NEAR(dist, 0, kTolerance); + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MjGjkTest, CapsuleCapsule) { static constexpr char xml[] = R"(