diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 67fbc9b8..2027d39a 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -163,7 +163,7 @@ void mjc_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { mju_mulMatTVec3(local_dir, d->geom_xmat+9*g, dir); // compute result according to geom type - switch ((mjtGeom) m->geom_type[g]) { + switch ((mjtGeom) obj->geom_type) { case mjGEOM_SPHERE: mju_scl3(res, local_dir, size[0]); break; @@ -396,8 +396,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}, mjc_center, mjc_support}; - mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}, mjc_center, mjc_support}; + mjCCDObj obj1 = {m, d, g1, m->geom_type[g1], -1, -1, -1, -1, margin, {1, 0, 0, 0}, + mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, g2, m->geom_type[g2], -1, -1, -1, -1, margin, {1, 0, 0, 0}, + mjc_center, mjc_support}; // init ccd structure mjc_initCCD(&ccd, m); @@ -525,7 +527,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d, mjGETINFO mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]}; ccd_vec3_t dir, vec; - mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}}; + mjCCDObj obj = {m, d, g2, m->geom_type[g2], -1, -1, -1, -1, 0, {1, 0, 0, 0}}; // get support point in -normal direction ccdVec3Set(&dir, -mat1[2], -mat1[5], -mat1[8]); @@ -700,12 +702,14 @@ 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]; - mjCCDObj obj1 = {m, d, -1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_prism_center, mjc_prism_support}; + mjCCDObj obj1 = {m, d, -1, mjGEOM_HFIELD, -1, -1, -1, -1, 0, {1, 0, 0, 0}, + mjc_prism_center, mjc_prism_support}; // ccd-related ccd_vec3_t dirccd, vecccd; ccd_real_t depth; - mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, g2, m->geom_type[g2], -1, -1, -1, -1, 0, {1, 0, 0, 0}, + mjc_center, mjc_support}; ccd_t ccd; // point size1 to hfield size instead of geom1 size @@ -1139,8 +1143,11 @@ 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}, mjc_center, mjc_support}; - mjCCDObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}, mjc_center, mjc_support}; + int geom_type = (g1 >= 0) ? m->geom_type[g1] : mjGEOM_FLEX; + mjCCDObj obj1 = {m, d, g1, geom_type, -1, f1, e1, v1, margin, {1, 0, 0, 0}, + mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, -1, mjGEOM_FLEX, -1, f2, e2, -1, margin, {1, 0, 0, 0}, + mjc_center, mjc_support}; // init ccd structure mjc_initCCD(&ccd, m); @@ -1158,7 +1165,7 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con, -// test a heighfield geom and a flex flex element for collision +// test a height field and a flex element for collision int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, int g, int f, int e, mjtNum margin) { mjtNum vec[3], dx, dy; @@ -1189,7 +1196,8 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, // ccd-related ccd_vec3_t dirccd, vecccd; ccd_real_t depth; - mjCCDObj obj2 = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}, mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, -1, mjGEOM_FLEX, -1, f, e, -1, margin, {1, 0, 0, 0}, + mjc_center, mjc_support}; ccd_t ccd; //------------------------------------- AABB computation, box-box test diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index 94cbb1ed..e18c4ad2 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -45,6 +45,7 @@ struct _mjCCDObj { const mjModel* model; const mjData* data; int geom; + int geom_type; int meshindex; int flex; int elem; diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index b1b36e8f..7b7ac310 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -92,13 +92,10 @@ static int discreteGeoms(mjCCDObj* obj1, mjCCDObj* obj2) { // non-zero margin makes geoms smooth if (obj1->margin != 0 || obj2->margin != 0) return 0; - // negative geom indices correspond to flex objects, return - if (obj1->geom < 0 || obj2->geom < 0) return 0; - - int g1 = obj1->model->geom_type[obj1->geom]; - int g2 = obj2->model->geom_type[obj2->geom]; - return (g1 == mjGEOM_MESH || g1 == mjGEOM_BOX) && - (g2 == mjGEOM_MESH || g2 == mjGEOM_BOX); + int g1 = obj1->geom_type; + int g2 = obj2->geom_type; + return (g1 == mjGEOM_MESH || g1 == mjGEOM_BOX || g1 == mjGEOM_HFIELD) && + (g2 == mjGEOM_MESH || g2 == mjGEOM_BOX || g2 == mjGEOM_HFIELD); } diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 7477b58f..8c3267af 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -58,10 +58,10 @@ mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], config.contacts = 0; // no geom contacts needed config.distances = 1; - mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center, - mjc_support}; - mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center, - mjc_support}; + mjCCDObj obj1 = {m, d, g1, m->geom_type[g1], -1, -1, -1, -1, 0, {1, 0, 0, 0}, + mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, g2, m->geom_type[g2], -1, -1, -1, -1, 0, {1, 0, 0, 0}, + mjc_center, mjc_support}; mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2); if (x1 != nullptr) mju_copy3(x1, status.x1); if (x2 != nullptr) mju_copy3(x2, status.x2); @@ -103,10 +103,10 @@ int PenetrationWrapper(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, mjtNum Penetration(mjModel* m, mjData* d, int g1, int g2, mjtNum dir[3] = nullptr, mjtNum pos[3] = nullptr, mjtNum margin = 0) { - mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}, mjc_center, - mjc_support}; - mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}, mjc_center, - mjc_support}; + mjCCDObj obj1 = {m, d, g1, m->geom_type[g1], -1, -1, -1, -1, margin, + {1, 0, 0, 0}, mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, g2, m->geom_type[g2], -1, -1, -1, -1, margin, + {1, 0, 0, 0}, mjc_center, mjc_support}; ccd_t ccd; // CCD_INIT(&ccd); // uncomment to run ccdMPRPenetration ccd.mpr_tolerance = kTolerance;