diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index 6fe1c6c5..dd34f411 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -936,6 +936,15 @@ MJCF element :ref:`statistic `. One instance of it is embedded in mjM .. mujoco-include:: mjStatistic +.. _mjPreContact: + +mjPreContact +~~~~~~~~~~~~ + +This is the data structure holding information about one contact filled out by a narrowphase collision detector. + +.. mujoco-include:: mjPreContact + .. _mjContact: mjContact @@ -1780,7 +1789,7 @@ mjfCollision .. code-block:: C typedef int (*mjfCollision)(const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); + mjPreContact* con, int g1, int g2, mjtNum margin); This is the function type of the callbacks in the collision table :ref:`mjCOLLISIONFUNC`. diff --git a/doc/changelog.rst b/doc/changelog.rst index ea44b45a..29b713cc 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -70,6 +70,9 @@ General Negative ``margin`` values are now permitted (corresponding to ``gap > margin`` under the old semantics). The constraint ``margin + gap >= 0`` should be maintained to ensure valid collision detection. + - The :ref:`mjfCollision` functions now populate the :ref:`mjPreContact` struct instead of the :ref:`mjContact` + struct. The :ref:`mjPreContact` only contains the necessary fields needed for the narrowphase collision detection. + - MJX: Removed the deprecated ``nconmax`` argument from ``mjx.make_data`` and ``mjx.put_data`` in favor of ``naconmax``. diff --git a/doc/includes/references.h b/doc/includes/references.h index cc3d6b49..7d929ee3 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -101,8 +101,15 @@ typedef enum mjtSleepState_ { // sleep state of an object mjS_ASLEEP = 0, // object is asleep mjS_AWAKE = 1 // object is awake } mjtSleepState; +struct mjPreContact_ { // contact parameters set by narrowphase collision functions + mjtNum dist; + mjtNum pos[3]; + mjtNum normal[3]; // contact normal of the collision + mjtNum tangent[3]; // first tangent direction +}; +typedef struct mjPreContact_ mjPreContact; struct mjContact_ { // result of collision detection functions - // contact parameters set by near-phase collision function + // contact parameters set by narrowphase collision function mjtNum dist; // distance between nearest points; neg: penetration mjtNum pos[3]; // position of contact point: midpoint between geoms mjtNum frame[9]; // normal is in [0-2], points from geom[0] to geom[1] diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 3709896a..8a22911c 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -121,10 +121,18 @@ typedef enum mjtSleepState_ { // sleep state of an object } mjtSleepState; -//---------------------------------- mjContact ----------------------------------------------------- +//------------------------------------- Contact ---------------------------------------------------- + +struct mjPreContact_ { // contact parameters set by narrowphase collision functions + mjtNum dist; + mjtNum pos[3]; + mjtNum normal[3]; // contact normal of the collision + mjtNum tangent[3]; // first tangent direction +}; +typedef struct mjPreContact_ mjPreContact; struct mjContact_ { // result of collision detection functions - // contact parameters set by near-phase collision function + // contact parameters set by narrowphase collision function mjtNum dist; // distance between nearest points; neg: penetration mjtNum pos[3]; // position of contact point: midpoint between geoms mjtNum frame[9]; // normal is in [0-2], points from geom[0] to geom[1] @@ -533,7 +541,7 @@ typedef mjtNum (*mjfTime)(void); typedef mjtNum (*mjfAct)(const mjModel* m, const mjData* d, int id); // collision detection -typedef int (*mjfCollision)(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, +typedef int (*mjfCollision)(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); #endif // MUJOCO_MJDATA_H_ diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 0b924bf7..443b9f12 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -5140,6 +5140,42 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), ), )), + ('mjPreContact', + StructDecl( + name='mjPreContact', + declname='struct mjPreContact_', + fields=( + StructFieldDecl( + name='dist', + type=ValueType(name='mjtNum'), + doc='', + ), + StructFieldDecl( + name='pos', + type=ArrayType( + inner_type=ValueType(name='mjtNum'), + extents=(3,), + ), + doc='', + ), + StructFieldDecl( + name='normal', + type=ArrayType( + inner_type=ValueType(name='mjtNum'), + extents=(3,), + ), + doc='contact normal of the collision', + ), + StructFieldDecl( + name='tangent', + type=ArrayType( + inner_type=ValueType(name='mjtNum'), + extents=(3,), + ), + doc='first tangent direction', + ), + ), + )), ('mjContact', StructDecl( name='mjContact', diff --git a/python/mujoco/raw.h b/python/mujoco/raw.h index 7776e88c..542f0292 100644 --- a/python/mujoco/raw.h +++ b/python/mujoco/raw.h @@ -25,6 +25,7 @@ // to them under the "raw" namespace. namespace mujoco::raw { +using MjPreContact = ::mjPreContact; using MjContact = ::mjContact; using MjData = ::mjData; using MjLROpt = ::mjLROpt; diff --git a/python/mujoco/structs.cc b/python/mujoco/structs.cc index 79840742..a07f89b1 100644 --- a/python/mujoco/structs.cc +++ b/python/mujoco/structs.cc @@ -615,6 +615,32 @@ This is useful for example when the MJB is not available as a file on disk.)")); X(int, nupdate); #undef X + // ==================== MJPRECONTACT ========================================== + py::class_ mjPreContact(m, "MjPreContact"); + mjPreContact.def(py::init<>()); + mjPreContact.def("__copy__", [](const MjPreContactWrapper& self) { + return MjPreContactWrapper(self); + }); + mjPreContact.def("__deepcopy__", [](const MjPreContactWrapper& self, py::dict) { + return MjPreContactWrapper(self); + }); + DefineStructFunctions(mjPreContact); + +#define X(var) \ + mjPreContact.def_property( \ + #var, [](const MjPreContactWrapper& c) { return c.get()->var; }, \ + [](MjPreContactWrapper& c, decltype(raw::MjPreContact::var) rhs) { \ + c.get()->var = rhs; \ + }) + X(dist); +#undef X + +#define X(var) DefinePyArray(mjPreContact, #var, &MjPreContactWrapper::var) + X(pos); + X(normal); + X(tangent); +#undef X + // ==================== MJCONTACT ============================================ py::class_ mjContact(m, "MjContact"); mjContact.def(py::init<>()); diff --git a/python/mujoco/structs.h b/python/mujoco/structs.h index 03080618..60bcf558 100644 --- a/python/mujoco/structs.h +++ b/python/mujoco/structs.h @@ -547,6 +547,27 @@ template <> struct enable_if_mj_struct { using type = void; }; // ==================== MJCONTACT ============================================== +template <> +class MjWrapper : public WrapperBase { + public: + MjWrapper(); + MjWrapper(const MjWrapper&); + MjWrapper(MjWrapper&&) = default; + MjWrapper(raw::MjPreContact* ptr, pybind11::handle owner); + ~MjWrapper() = default; + + #define X(var) \ + py_array_or_tuple_t< \ + std::remove_all_extents_t> \ + var + X(pos); + X(normal); + X(tangent); + #undef X +}; + +using MjPreContactWrapper = MjWrapper; + template <> class MjWrapper : public WrapperBase { public: @@ -961,6 +982,7 @@ using _impl::MjSolverStatWrapper; using _impl::MjModelWrapper; using _impl::MjDataWrapper; using _impl::MjContactWrapper; +using _impl::MjPreContactWrapper; using _impl::MjvPerturbWrapper; using _impl::MjvCameraWrapper; using _impl::MjvGLCameraWrapper; diff --git a/python/mujoco/structs_wrappers.cc b/python/mujoco/structs_wrappers.cc index 1a35c0dc..c5c8b6ca 100644 --- a/python/mujoco/structs_wrappers.cc +++ b/python/mujoco/structs_wrappers.cc @@ -535,6 +535,26 @@ std::unique_ptr MjModelWrapper::Deserialize( return std::unique_ptr(new MjModelWrapper(model)); } +// ==================== MJPRECONTACT ========================================== +#define X(var) var(InitPyArray(ptr_->var, owner_)) +MjPreContactWrapper::MjWrapper() + : WrapperBase(new raw::MjPreContact{}), + X(pos), + X(normal), + X(tangent) {} + +MjPreContactWrapper::MjWrapper(raw::MjPreContact* ptr, py::handle owner) + : WrapperBase(ptr, owner), + X(pos), + X(normal), + X(tangent) {} +#undef X + +MjPreContactWrapper::MjWrapper(const MjPreContactWrapper& other) + : MjPreContactWrapper() { + *this->ptr_ = *other.ptr_; +} + // ==================== MJCONTACT ============================================== #define X(var) var(InitPyArray(ptr_->var, owner_)) MjContactWrapper::MjWrapper() diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c index e30b7e16..6cf09568 100644 --- a/src/engine/engine_collision_box.c +++ b/src/engine/engine_collision_box.c @@ -20,24 +20,18 @@ #include "engine/engine_util_misc.h" // hard-clamp vector to range [-limit(i), +limit(i)] -static void mju_clampVec(mjtNum* vec, const mjtNum* limit, int n) -{ - int i; - - // loop over active limits - for (i = 0; i < n; i++) { +static void mju_clampVec(mjtNum* vec, const mjtNum* limit, int n) { + for (int i = 0; i < n; i++) { + // loop over active limits if (limit[i] > 0) { - if (vec[i] < -limit[i]) - vec[i] = -limit[i]; - else if (vec[i] > limit[i]) - vec[i] = limit[i]; + vec[i] = mju_clip(vec[i], -limit[i], limit[i]); } } } // raw sphere : box -int mjraw_SphereBox(mjContact* con, mjtNum margin, +int mjraw_SphereBox(mjPreContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) { int i, k; @@ -74,29 +68,32 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin, mji_copy3(pos, center); mji_addToScl3(pos, nearest, (size1[0] - closest) / 2); - mji_mulMatVec3(con[0].frame, mat2, nearest); + mji_mulMatVec3(con[0].normal, mat2, nearest); dist = -closest; } else { mji_addToScl3(deepest, tmp, size1[0]); mju_zero3(pos); mji_addToScl3(pos, clamped, 0.5); mji_addToScl3(pos, deepest, 0.5); - mji_mulMatVec3(con[0].frame, mat2, tmp); + mji_mulMatVec3(con[0].normal, mat2, tmp); } mji_mulMatVec3(tmp, mat2, pos); mji_add3(con[0].pos, tmp, pos2); con[0].dist = dist - size1[0]; - mju_zero3(con[0].frame + 3); - + mji_zero3(con[0].tangent); return 1; } // sphere : box -int mjc_SphereBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { - mjGETINFO; - +int mjc_SphereBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) { + const mjtNum* pos1 = d->geom_xpos + 3*g1; + const mjtNum* mat1 = d->geom_xmat + 9*g1; + const mjtNum* size1 = m->geom_size + 3*g1; + const mjtNum* pos2 = d->geom_xpos + 3*g2; + const mjtNum* mat2 = d->geom_xmat + 9*g2; + const mjtNum* size2 = m->geom_size + 3*g2; return mjraw_SphereBox(con, margin, pos1, mat1, size1, pos2, mat2, size2); } @@ -115,9 +112,10 @@ int mjc_SphereBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, m */ // raw capsule : box -int mjraw_CapsuleBox(mjContact* con, mjtNum margin, +int mjraw_CapsuleBox(mjPreContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, - const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) { + const mjtNum* pos2, const mjtNum* mat2, + const mjtNum* size2) { mjtNum tmp1[3], tmp2[3], tmp3[3], halfaxis[3], axis[3], dif[3]; mjtNum pos[3]; // position of capsule in box-local frame @@ -148,7 +146,7 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin, secondpos = -4; // initialize to no 2nd contact (valid values are between -1 and 1) mji_sub3(tmp1, pos1, pos2); // bring capsule to box-local frame (center's box is at (0,0,0)) - mji_mulMatTVec3(pos, mat2, tmp1); // and axis parralel to world + mji_mulMatTVec3(pos, mat2, tmp1); // and axis parallel to world tmp1[0] = mat1[2]; // capsule's axis tmp1[1] = mat1[5]; @@ -591,21 +589,25 @@ skip: // capsule : box -int mjc_CapsuleBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { - mjGETINFO +int mjc_CapsuleBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) { + const mjtNum* pos1 = d->geom_xpos + 3*g1; + const mjtNum* pos2 = d->geom_xpos + 3*g2; + const mjtNum* mat1 = d->geom_xmat + 9*g1; + const mjtNum* mat2 = d->geom_xmat + 9*g2; + const mjtNum* size1 = m->geom_size + 3*g1; + const mjtNum* size2 = m->geom_size + 3*g2; return mjraw_CapsuleBox(con, margin, pos1, mat1, size1, pos2, mat2, size2); } // internal box : box static inline -int _boxbox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2, mjtNum margin) -{ +int _boxbox(const mjModel* M, const mjData* D, mjPreContact* con, int g1, int g2, mjtNum margin) { const mjtNum* pos1 = D->geom_xpos + 3 * g1; - const mjtNum* mat1 = D->geom_xmat + 9 * g1; - const mjtNum* size1 = M->geom_size + 3 * g1; const mjtNum* pos2 = D->geom_xpos + 3 * g2; + const mjtNum* mat1 = D->geom_xmat + 9 * g1; const mjtNum* mat2 = D->geom_xmat + 9 * g2; + const mjtNum* size1 = M->geom_size + 3 * g1; const mjtNum* size2 = M->geom_size + 3 * g2; mjtNum pos12[3], pos21[3], rot[9], rott[9], rotabs[9], rottabs[9], tmp1[3], tmp2[3], plen1[3], @@ -883,8 +885,10 @@ int _boxbox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2, m if (mju_abs(c2) > ss[1 - q]) continue; - mji_copy3(points[n], lines[i]); - mji_addToScl3(points[n++], lines[i] + 3, c1); + if (n < mjMAXCONPAIR) { + mji_copy3(points[n], lines[i]); + mji_addToScl3(points[n++], lines[i] + 3, c1); + } } } } @@ -911,10 +915,12 @@ int _boxbox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2, m if (u <= 0 || v <= 0 || u >= 1 || v >= 1) continue; - points[n][0] = llx; - points[n][1] = lly; - points[n][2] = (pts[0][2] + u * pts[1][2] + v * pts[2][2]); - n++; + if (n < mjMAXCONPAIR) { + points[n][0] = llx; + points[n][1] = lly; + points[n][2] = (pts[0][2] + u * pts[1][2] + v * pts[2][2]); + n++; + } } } @@ -929,7 +935,9 @@ int _boxbox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2, m if (tmp1[1] <= -ly || tmp1[1] >= ly) continue; - mji_copy3(points[n++], tmp1); + if (n < mjMAXCONPAIR) { + mji_copy3(points[n++], tmp1); + } } @@ -955,22 +963,23 @@ int _boxbox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2, m tmp2[1] = (q2 ? -1 : 1) * r[5]; tmp2[2] = (q2 ? -1 : 1) * r[8]; - mji_copy3(con[0].frame, tmp2); - mju_zero3(con[0].frame + 3); + mji_copy3(con[0].normal, tmp2); + mji_zero3(con[0].tangent); - for (i = 0; i < n; i++) - { + for (i = 0; i < n; i++) { con[i].dist = 2 * points[i][2]; points[i][2] += hz; mji_mulMatVec3(tmp2, r, points[i]); mji_add3(con[i].pos, tmp2, p); - if (i) - mji_copy6(con[i].frame, con[0].frame); + if (i) { + mji_copy3(con[i].normal, con[0].normal); + mji_zero3(con[i].tangent); + } } @@ -1315,8 +1324,8 @@ edgeedge: mji_mulMatVec3(tmp1, r, rnorm); - mji_scl3(con[0].frame, tmp1, in ? -1 : 1); - mju_zero3(con[0].frame + 3); + mji_scl3(con[0].normal, tmp1, in ? -1 : 1); + mji_zero3(con[0].tangent); for (i = 0; i < n; i++) { @@ -1327,7 +1336,8 @@ edgeedge: mji_add3(con[i].pos, tmp2, pos1); - mji_copy6(con[i].frame, con[0].frame); + mji_copy3(con[i].normal, con[0].normal); + mji_zero3(con[i].tangent); } return n; @@ -1338,13 +1348,13 @@ edgeedge: // box : box -int mjc_BoxBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { - int num = _boxbox(m, d, con, g1, g2, margin); +int mjc_BoxBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) { + mjPreContact tmp[mjMAXCONPAIR]; + int num = _boxbox(m, d, tmp, g1, g2, margin); + + // -1: bad, 0: good + int dupe[mjMAXCONPAIR] = {0}; - // use dim field to mark: -1: bad, 0: good - for (int i=0; i < num; i++) { - con[i].dim = 0; - } // get box info const mjtNum* pos1 = d->geom_xpos + 3 * g1; @@ -1364,28 +1374,28 @@ int mjc_BoxBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN static mjtNum kRemoveRatio = 1.01; // is the contact outside: 1, inside: -1, within the removal width: 0 - int out1 = mju_outsideBox(con[i].pos, pos1, mat1, sz1, kRemoveRatio); - int out2 = mju_outsideBox(con[i].pos, pos2, mat2, sz2, kRemoveRatio); + int out1 = mju_outsideBox(tmp[i].pos, pos1, mat1, sz1, kRemoveRatio); + int out2 = mju_outsideBox(tmp[i].pos, pos2, mat2, sz2, kRemoveRatio); // mark as bad if outside one box and not inside the other box if ((out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1)) { - con[i].dim = -1; + dupe[i] = -1; } } // find duplicates for (int i=0; i < num-1; i++) { - if (con[i].dim == -1) { + if (dupe[i] == -1) { continue; // already marked bad: skip } for (int j=i+1; j < num; j++) { - if (con[j].dim == -1) { + if (dupe[j] == -1) { continue; // already marked bad: skip } - if (con[i].pos[0] == con[j].pos[0] && - con[i].pos[1] == con[j].pos[1] && - con[i].pos[2] == con[j].pos[2]) { - con[i].dim = -1; + if (tmp[i].pos[0] == tmp[j].pos[0] && + tmp[i].pos[1] == tmp[j].pos[1] && + tmp[i].pos[2] == tmp[j].pos[2]) { + dupe[i] = -1; break; } } @@ -1394,14 +1404,8 @@ int mjc_BoxBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN // consolidate good int i = 0; for (int j=0; j < num; j++) { - // good: maybe copy - if (con[j].dim == 0) { - // different: copy - if (i < j) { - con[i] = con[j]; - } - - // advance either way + if (dupe[j] == 0) { + con[i] = tmp[j]; i++; } } diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 46f4cf09..2c52a789 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -41,7 +41,7 @@ static void prism_firstdir(const void* o1, const void* o2, ccd_vec3_t *vec) { // wrapper around libccd; returns number of collisions found static inline int _libccd_wrapper(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2, - mjContact* con, mjtNum margin) { + mjPreContact* con, mjtNum margin) { ccd_t ccd; CCD_INIT(&ccd); ccd.mpr_tolerance = m->opt.ccd_tolerance; @@ -64,10 +64,10 @@ static inline int _libccd_wrapper(const mjModel* m, mjCCDObj* obj1, mjCCDObj* ob if (ccdVec3Eq(&ccd_dir, ccd_vec3_origin)) { return 0; } - con->dist = margin - ccd_depth; - mji_copy3(con->frame, ccd_dir.v); - mji_copy3(con->pos, ccd_pos.v); - mji_zero3(con->frame + 3); + con[0].dist = margin - ccd_depth; + mji_copy3(con[0].normal, ccd_dir.v); + mji_copy3(con[0].pos, ccd_pos.v); + mji_zero3(con[0].tangent); return 1; } return 0; @@ -76,7 +76,7 @@ static inline int _libccd_wrapper(const mjModel* m, mjCCDObj* obj1, mjCCDObj* ob // find penetration info between two geoms; returns number of collisions found static int mjc_penetration(const mjModel* m, mjData* d, mjCCDObj* obj1, mjCCDObj* obj2, - mjContact* con, int ncon, mjtNum margin) { + mjPreContact* con, int ncon, mjtNum margin) { if (mjDISABLED(mjDSBL_NATIVECCD)) { return _libccd_wrapper(m, obj1, obj2, con, margin); } @@ -97,14 +97,14 @@ static int mjc_penetration(const mjModel* m, mjData* d, mjCCDObj* obj1, mjCCDObj if ((dist = mjc_ccd(&config, &status, obj1, obj2)) < 0) { mj_freeStack(d); int nwitness = status.nx; - for (int i = 0; i < nwitness; i++, con++) { - con->dist = margin + dist; - con->pos[0] = 0.5*(status.x1[3*i + 0] + status.x2[3*i + 0]); - con->pos[1] = 0.5*(status.x1[3*i + 1] + status.x2[3*i + 1]); - con->pos[2] = 0.5*(status.x1[3*i + 2] + status.x2[3*i + 2]); - mji_sub3(con->frame, status.x1 + 3*i, status.x2 + 3*i); - mju_normalize3(con->frame); - mji_zero3(con->frame + 3); + for (int i = 0; i < nwitness; i++) { + con[i].dist = margin + dist; + con[i].pos[0] = 0.5*(status.x1[3*i + 0] + status.x2[3*i + 0]); + con[i].pos[1] = 0.5*(status.x1[3*i + 1] + status.x2[3*i + 1]); + con[i].pos[2] = 0.5*(status.x1[3*i + 2] + status.x2[3*i + 2]); + mji_sub3(con[i].normal, status.x1 + 3*i, status.x2 + 3*i); + mju_normalize3(con[i].normal); + mji_zero3(con[i].tangent); } return nwitness; } @@ -780,9 +780,10 @@ static void mjc_setCCDObjFlex(mjCCDObj* obj, int flex, int elem, int vert) { // compare new contact to previous contacts, return 1 if it is far from all of them -static int mjc_isDistinctContact(mjContact* con, int ncon, mjtNum tolerance) { +static int mjc_isDistinctContact(const mjPreContact* con, int ncon, mjtNum tolerance) { + const mjtNum* last_pos = con[ncon - 1].pos; for (int i=0; i < ncon-1; i++) { - if (mju_dist3(con[i].pos, con[ncon - 1].pos) <= tolerance) { + if (mju_dist3(con[i].pos, last_pos) <= tolerance) { return 0; } } @@ -838,7 +839,7 @@ static int maxContacts(const mjModel* m, const mjCCDObj* obj1, const mjCCDObj* o // multi-point convex-convex collision, using libccd -int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { +int mjc_Convex(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) { // init ccd objects mjCCDObj obj1, obj2; mjc_initCCDObj(&obj1, m, d, g1, margin); @@ -857,7 +858,7 @@ int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN } // look for additional contacts - if (ncon == 1 && !mjDISABLED(mjDSBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?) + if (ncon == 1 && !mjDISABLED(mjDSBL_MULTICCD) && m->geom_type[g1] != mjGEOM_ELLIPSOID && m->geom_type[g1] != mjGEOM_SPHERE && m->geom_type[g2] != mjGEOM_ELLIPSOID && m->geom_type[g2] != mjGEOM_SPHERE) { // multiCCD parameters @@ -866,7 +867,8 @@ int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN // complete frame of initial contact mjtNum frame[9]; - mji_copy9(frame, con[0].frame); + mji_copy3(frame, con[0].normal); + mju_zero(frame+3, 6); mju_makeFrame(frame); // tolerance for determining if newly found contacts are distinct @@ -926,7 +928,7 @@ const int maxplanemesh = 3; const mjtNum tolplanemesh = 0.3; // add one plane-mesh contact -static int addplanemesh(mjContact* con, const float vertex[3], +static int addplanemesh(mjPreContact* con, const float vertex[3], const mjtNum pos1[3], const mjtNum normal1[3], const mjtNum pos2[3], const mjtNum mat2[9], const mjtNum first[3], mjtNum rbound) { @@ -945,44 +947,47 @@ static int addplanemesh(mjContact* con, const float vertex[3], mji_sub3(dif, pnt, pos1); // set distance - con->dist = mju_dot3(normal1, dif); + con[0].dist = mju_dot3(normal1, dif); // set position - mji_copy3(con->pos, pnt); - mji_addToScl3(con->pos, normal1, -0.5*con->dist); + mji_copy3(con[0].pos, pnt); + mji_addToScl3(con[0].pos, normal1, -0.5*con[0].dist); // set frame - mji_copy3(con->frame, normal1); - mju_zero3(con->frame+3); + mji_copy3(con[0].normal, normal1); + mji_zero3(con[0].tangent); return 1; } // plane-convex collision, using libccd -int mjc_PlaneConvex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { - mjGETINFO - mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]}; - ccd_vec3_t dir, vec; +int mjc_PlaneConvex(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) { + const mjtNum* pos1 = d->geom_xpos + 3*g1; + const mjtNum* mat1 = d->geom_xmat + 9*g1; + const mjtNum* pos2 = d->geom_xpos + 3*g2; + const mjtNum* mat2 = d->geom_xmat + 9*g2; + + mjtNum dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]}; + ccd_vec3_t ccd_dir, ccd_vec; mjCCDObj obj; mjc_initCCDObj(&obj, m, d, g2, 0); // get support point in -normal direction - ccdVec3Set(&dir, -mat1[2], -mat1[5], -mat1[8]); - mjccd_support(&obj, &dir, &vec); + ccdVec3Set(&ccd_dir, -mat1[2], -mat1[5], -mat1[8]); + mjccd_support(&obj, &ccd_dir, &ccd_vec); // compute normal distance, return if too far - mji_sub3(dif, vec.v, pos1); - dist = mju_dot3(normal, dif); - if (dist > margin) { + mji_sub3(dif, ccd_vec.v, pos1); + con[0].dist = mju_dot3(normal, dif); + if (con[0].dist > margin) { return 0; } // fill in contact data - con->dist = dist; - mji_copy3(con->pos, vec.v); - mji_addToScl3(con->pos, normal, -0.5*dist); - mji_copy3(con->frame, normal); - mju_zero3(con->frame+3); + mji_copy3(con[0].pos, ccd_vec.v); + mji_addToScl3(con[0].pos, normal, -0.5*con[0].dist); + mji_copy3(con[0].normal, normal); + mji_zero3(con[0].tangent); //--------------- add all/connected vertices below margin float* vertdata; @@ -1001,7 +1006,7 @@ int mjc_PlaneConvex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, // express dir in geom local frame mjtNum locdir[3]; - mju_mulMatTVec3(locdir, d->geom_xmat+9*g, dir.v); + mju_mulMatTVec3(locdir, d->geom_xmat+9*g, ccd_dir.v); // inclusion threshold along locdir, relative to geom2 center mji_sub3(dif, pos2, pos1); @@ -1097,7 +1102,8 @@ static inline void addPrismVert(mjCCDObj* obj, int r, int c, int i, mjtNum dx, m // entry point for heightfield collisions -int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { +int mjc_ConvexHField(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, + mjtNum margin) { // hfield frame const mjtNum* pos1 = d->geom_xpos + 3*g1; const mjtNum* mat1 = d->geom_xmat + 9*g1; @@ -1116,16 +1122,16 @@ int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2 // try early return using box-sphere test // express geom2 pos in hfield frame - mjtNum pos[3] = {pos2[0] - pos1[0], pos2[1] - pos1[1], pos2[2] - pos1[2]}; - mju_mulMatTVec3(pos, mat1, pos); + mjtNum local_pos[3] = {pos2[0] - pos1[0], pos2[1] - pos1[1], pos2[2] - pos1[2]}; + mju_mulMatTVec3(local_pos, mat1, local_pos); // sphere radius is geom2 rbound + margin mjtNum radius = m->geom_rbound[g2] + margin; // box-sphere test - if ((size0 < pos[0] - radius) || (-size0 > pos[0] + radius) || - (size1 < pos[1] - radius) || (-size1 > pos[1] + radius) || - (size2 < pos[2] - radius) || (-size3 > pos[2] + radius)) { + if ((size0 < local_pos[0] - radius) || (-size0 > local_pos[0] + radius) || + (size1 < local_pos[1] - radius) || (-size1 > local_pos[1] + radius) || + (size2 < local_pos[2] - radius) || (-size3 > local_pos[2] + radius)) { return 0; } @@ -1142,42 +1148,42 @@ int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2 mji_mulMatTMat3(mat, mat1, mat2); mji_copy9(obj2.mat, mat); - mji_copy3(obj2.pos, pos); + mji_copy3(obj2.pos, local_pos); - mjtNum dir[3] = {0, 0, 0}, res[3]; + mjtNum local_dir[3] = {0, 0, 0}, res[3]; // get support point in +X - dir[0] = 1; - obj2.support(res, &obj2, dir); + local_dir[0] = 1; + obj2.support(res, &obj2, local_dir); mjtNum xmax = res[0]; // get support point in -X - dir[0] = -1; - obj2.support(res, &obj2, dir); + local_dir[0] = -1; + obj2.support(res, &obj2, local_dir); mjtNum xmin = res[0]; - dir[0] = 0; + local_dir[0] = 0; // get support point in +Y - dir[1] = 1; - obj2.support(res, &obj2, dir); + local_dir[1] = 1; + obj2.support(res, &obj2, local_dir); mjtNum ymax = res[1]; // get support point in -Y - dir[1] = -1; - obj2.support(res, &obj2, dir); + local_dir[1] = -1; + obj2.support(res, &obj2, local_dir); mjtNum ymin = res[1]; - dir[1] = 0; + local_dir[1] = 0; // get support point in +Z - dir[2] = 1; - obj2.support(res, &obj2, dir); + local_dir[2] = 1; + obj2.support(res, &obj2, local_dir); mjtNum zmax = res[2]; // get support point in -Z - dir[2] = -1; - obj2.support(res, &obj2, dir); + local_dir[2] = -1; + obj2.support(res, &obj2, local_dir); mjtNum zmin = res[2]; // AABB box-box test @@ -1226,10 +1232,10 @@ int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2 // run penetration function, save contact if (mjc_penetration(m, d, &obj1, &obj2, con + ncon, 1, 0.0)) { // transform to global coordinates - mji_copy3(dir, con[ncon].frame); - mji_copy3(pos, con[ncon].pos); - mji_mulMatVec3(con[ncon].frame, mat1, dir); - mji_mulMatVec3(con[ncon].pos, mat1, pos); + mji_copy3(local_dir, con[ncon].normal); + mji_copy3(local_pos, con[ncon].pos); + mji_mulMatVec3(con[ncon].normal, mat1, local_dir); + mji_mulMatVec3(con[ncon].pos, mat1, local_pos); mji_addTo3(con[ncon].pos, pos1); // force out of all loops if max contacts reached @@ -1247,7 +1253,7 @@ int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2 if (mjDISABLED(mjDSBL_NATIVECCD)) { // fix contact normals for (int i=0; i < ncon; i++) { - mjc_fixNormal(m, d, con+i, g1, g2); + mjc_fixNormal(m, d, con + i, g1, g2); } } @@ -1362,7 +1368,7 @@ static int mjc_ellipsoidOutside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum // fix normals if required -void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2) { +void mjc_fixNormal(const mjModel* m, const mjData* d, mjPreContact* con, int g1, int g2) { mjtNum dst1, dst2; // get geom ids and types @@ -1389,8 +1395,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in // init normals mjtNum normal[2][3] = { - {con->frame[0], con->frame[1], con->frame[2]}, - {-con->frame[0], -con->frame[1], -con->frame[2]} + {con->normal[0], con->normal[1], con->normal[2]}, + {-con->normal[0], -con->normal[1], -con->normal[2]} }; // process geoms in type range @@ -1402,27 +1408,27 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in mjtNum* size = m->geom_size + 3*gid[i]; // map contact point and normal to local frame - mjtNum dif[3], pos[3], nrm[3]; + mjtNum dif[3], pos1[3], nrm[3]; mju_sub3(dif, con->pos, d->geom_xpos+3*gid[i]); - mju_mulMatTVec3(pos, mat, dif); + mju_mulMatTVec3(pos1, mat, dif); mju_mulMatTVec3(nrm, mat, normal[i]); // process according to type switch (type[i]) { case mjGEOM_SPHERE: - mji_copy3(nrm, pos); + mji_copy3(nrm, pos1); processed[i] = 1; break; case mjGEOM_CAPSULE: // Z: bottom cap - if (pos[2] < -size[1]) { - nrm[2] = pos[2]+size[1]; + if (pos1[2] < -size[1]) { + nrm[2] = pos1[2]+size[1]; } // Z: top cap - else if (pos[2] > size[1]) { - nrm[2] = pos[2]-size[1]; + else if (pos1[2] > size[1]) { + nrm[2] = pos1[2]-size[1]; } // Z: cylinder @@ -1431,8 +1437,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in } // copy XY - nrm[0] = pos[0]; - nrm[1] = pos[1]; + nrm[0] = pos1[0]; + nrm[1] = pos1[1]; processed[i] = 1; break; @@ -1443,27 +1449,27 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in } // compute elliptic distance^2 - dst1 = pos[0]*pos[0]/(size[0]*size[0]) + - pos[1]*pos[1]/(size[1]*size[1]) + - pos[2]*pos[2]/(size[2]*size[2]); + dst1 = pos1[0]*pos1[0]/(size[0]*size[0]) + + pos1[1]*pos1[1]/(size[1]*size[1]) + + pos1[2]*pos1[2]/(size[2]*size[2]); // dispatch to inside or outside solver if (dst1 <= 1) { - processed[i] = mjc_ellipsoidInside(nrm, pos, size); + processed[i] = mjc_ellipsoidInside(nrm, pos1, size); } else { - processed[i] = mjc_ellipsoidOutside(nrm, pos, size); + processed[i] = mjc_ellipsoidOutside(nrm, pos1, size); } break; case mjGEOM_CYLINDER: // skip if within 5% length of flat wall - if (mju_abs(pos[2]) > 0.95*size[1]) { + if (mju_abs(pos1[2]) > 0.95*size[1]) { break; } // compute distances to flat and round wall - dst1 = mju_abs(size[1]-mju_abs(pos[2])); - dst2 = mju_abs(size[0]-mju_norm(pos, 2)); + dst1 = mju_abs(size[1]-mju_abs(pos1[2])); + dst2 = mju_abs(size[0]-mju_norm(pos1, 2)); // require 4x closer to round than flat wall if (dst1 < 0.25*dst2) { @@ -1471,8 +1477,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in } // set normal for round wall - nrm[0] = pos[0]; - nrm[1] = pos[1]; + nrm[0] = pos1[0]; + nrm[1] = pos1[1]; nrm[2] = 0; processed[i] = 1; break; @@ -1492,23 +1498,18 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in // both processed: average if (processed[0] && processed[1]) { - mji_sub3(con->frame, normal[0], normal[1]); - mju_normalize3(con->frame); + mji_sub3(con->normal, normal[0], normal[1]); + mju_normalize3(con->normal); } // first processed: copy else if (processed[0]) { - mji_copy3(con->frame, normal[0]); + mji_copy3(con->normal, normal[0]); } // second processed: copy reverse else if (processed[1]) { - mji_scl3(con->frame, normal[1], -1); - } - - // clear second frame axis if processed, just in case - if (processed[0] || processed[1]) { - mju_zero3(con->frame+3); + mji_scl3(con->normal, normal[1], -1); } } @@ -1525,7 +1526,8 @@ int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1, mjc_setCCDObjFlex(&obj2, f2, e2, -1); // find contacts - int ncon = mjc_penetration(m, d, &obj1, &obj2, con, 1, margin); + mjPreContact precon[8]; + int ncon = mjc_penetration(m, d, &obj1, &obj2, precon, 1, margin); // fix normals for 2D flex if (ncon && !mjDISABLED(mjDSBL_NATIVECCD)) { @@ -1536,11 +1538,19 @@ int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1, if (isflex2d) { for (int i = 0; i < ncon; i++) { - mjc_fixNormal(m, d, con + i, g1, -1); + mjc_fixNormal(m, d, precon + i, g1, -1); } } } + // copy to con + for (int i = 0; i < ncon; i++) { + con[i].dist = precon[i].dist; + mji_copy3(con[i].pos, precon[i].pos); + mji_copy3(con[i].frame, precon[i].normal); + mju_zero3(con[i].frame + 3); + } + return ncon; } @@ -1659,14 +1669,14 @@ int mjc_HFieldElem(const mjModel* m, mjData* d, mjContact* con, int g, int f, in } // run ccd, save contact - if (mjc_penetration(m, d, &obj1, &obj2, con + cnt, 1, 0.0)) { + mjContact* ccon = con + cnt; + mjPreContact precon; + if (mjc_penetration(m, d, &obj1, &obj2, &precon, 1, 0.0)) { // transform to global coordinates - mjtNum dir[3], pos[3]; - mji_copy3(dir, con[cnt].frame); - mji_copy3(pos, con[cnt].pos); - mji_mulMatVec3(con[cnt].frame, hmat, dir); - mji_mulMatVec3(con[cnt].pos, hmat, pos); - mji_addTo3(con[cnt].pos, hpos); + mji_zero3(ccon->frame + 3); + mji_mulMatVec3(ccon->frame, hmat, precon.normal); + mji_mulMatVec3(ccon->pos, hmat, precon.pos); + mji_addTo3(ccon->pos, hpos); // count, stop if max number reached cnt++; diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index d9212fbd..88fda2ea 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -109,9 +109,9 @@ void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]); void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]); // pairwise geom collision functions using ccd -int mjc_PlaneConvex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin); -int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin); -MJAPI int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin); +int mjc_PlaneConvex(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); +int mjc_ConvexHField(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_Convex(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); // geom-elem or elem-elem or vert-elem collision function using ccd int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1, int e1, int v1, @@ -122,7 +122,7 @@ int mjc_HFieldElem(const mjModel* m, mjData* d, mjContact* con, int g, int f, in mjtNum margin); // fix contact frame normal -void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2); +void mjc_fixNormal(const mjModel* m, const mjData* d, mjPreContact* con, int g1, int g2); #ifdef __cplusplus } diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 5c31b16c..a598602f 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -1745,20 +1745,11 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) { return; } - // allocate mjContact[mjMAXCONPAIR] on the arena - mjContact* con = - (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact)); - if (!con) { - mj_warning(d, mjWARN_CONTACTFULL, d->ncon); - return; - } + // call collision detector to generate contacts - num = collisionFunc(m, d, con, g1, g2, margin + gap); - - // check contacts - if (!num) { - resetArena(d); + mjPreContact precon[mjMAXCONPAIR]; + if (!(num = collisionFunc(m, d, precon, g1, g2, margin + gap))) { return; } @@ -1767,6 +1758,12 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) { mjERROR("too many contacts returned by collision function"); } + mjContact* con = (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * num, _Alignof(mjContact)); + if (!con) { + mj_warning(d, mjWARN_CONTACTFULL, d->ncon); + return; + } + // set condim, solref, solimp, friction: dynamic if (ipair < 0) { mj_contactParam(m, &condim, solref, solimp, friction, g1, g2, -1, -1); @@ -1787,7 +1784,12 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) { // add contacts returned by collision detector for (int i=0; i < num; i++) { - // set contact ids + // set contact parameters + con[i].dist = precon[i].dist; + mji_copy3(con[i].pos, precon[i].pos); + mji_copy3(con[i].frame + 0, precon[i].normal); + mji_copy3(con[i].frame + 3, precon[i].tangent); + con[i].geom[0] = g1; con[i].geom[1] = g2; con[i].flex[0] = -1; @@ -2109,23 +2111,32 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { pos, mat, size); // call raw primitive for corresponding geom type - if (type == mjGEOM_SPHERE) { - num = mjraw_SphereCapsule(con, margin + gap, - d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, - pos, mat, size); + mjPreContact precon[2]; + switch (type) { + case mjGEOM_SPHERE: + num = mjraw_SphereCapsule(precon, margin + gap, d->geom_xpos+3*g, d->geom_xmat+9*g, + m->geom_size+3*g, pos, mat, size); + break; + case mjGEOM_CAPSULE: + num = mjraw_CapsuleCapsule(precon, margin + gap, d->geom_xpos+3*g, d->geom_xmat+9*g, + m->geom_size+3*g, pos, mat, size); + break; + case mjGEOM_BOX: + num = mjraw_CapsuleBox(precon, margin + gap, pos, mat, size, d->geom_xpos+3*g, + d->geom_xmat+9*g, m->geom_size+3*g); + break; + default: + num = 0; } - else if (type == mjGEOM_CAPSULE) { - num = mjraw_CapsuleCapsule(con, margin + gap, - d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, - pos, mat, size); - } - else { - num = mjraw_CapsuleBox(con, margin + gap, - pos, mat, size, - d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g); + + for (int i=0; i < num; i++) { + con[i].dist = precon[i].dist; + mju_copy3(con[i].pos, precon[i].pos); + mju_copy3(con[i].frame + 0, precon[i].normal); + mju_copy3(con[i].frame + 3, precon[i].tangent); // reverse contact normals, since box geom is second - for (int i=0; i < num; i++) { + if (type == mjGEOM_BOX) { mju_scl3(con[i].frame, con[i].frame, -1); } } @@ -2256,7 +2267,14 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2 pos2, mat2, size2); // raw primitive - num = mjraw_CapsuleCapsule(con, margin + gap, pos1, mat1, size1, pos2, mat2, size2); + mjPreContact precon[mjMAXCONPAIR]; + num = mjraw_CapsuleCapsule(precon, margin + gap, pos1, mat1, size1, pos2, mat2, size2); + for (int i=0; i < num; i++) { + con[i].dist = precon[i].dist; + mju_copy3(con[i].pos, precon[i].pos); + mju_copy3(con[i].frame + 0, precon[i].normal); + mju_copy3(con[i].frame + 3, precon[i].tangent); + } } // general convex collision @@ -2332,7 +2350,12 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) { mjtNum pos[3], mat[9], size[2]; mjtNum I[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; mj_makeCapsule(m, d, f, edata, pos, mat, size); - num = mjraw_SphereCapsule(con, 0, vert, I, &radius, pos, mat, size); + mjPreContact precon; + num = mjraw_SphereCapsule(&precon, 0, vert, I, &radius, pos, mat, size); + con->dist = precon.dist; + mju_copy3(con->pos, precon.pos); + mju_copy3(con->frame + 0, precon.normal); + mju_copy3(con->frame + 3, precon.tangent); } // sphere : triangle diff --git a/src/engine/engine_collision_primitive.c b/src/engine/engine_collision_primitive.c index 145dfc50..a8823008 100644 --- a/src/engine/engine_collision_primitive.c +++ b/src/engine/engine_collision_primitive.c @@ -25,70 +25,87 @@ //--------------------------- plane collisions ----------------------------------------------------- // raw plane : sphere -static int mjraw_PlaneSphere(mjContact* con, mjtNum margin, +static int mjraw_PlaneSphere(mjPreContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) { // set normal - con[0].frame[0] = mat1[2]; - con[0].frame[1] = mat1[5]; - con[0].frame[2] = mat1[8]; + con[0].normal[0] = mat1[2]; + con[0].normal[1] = mat1[5]; + con[0].normal[2] = mat1[8]; // compute distance, return if too large mjtNum tmp[3] = {pos2[0] - pos1[0], pos2[1] - pos1[1], pos2[2] - pos1[2]}; - mjtNum cdist = mju_dot3(tmp, con[0].frame); + mjtNum cdist = mju_dot3(tmp, con[0].normal); if (cdist > margin + size2[0]) { return 0; } // depth and position con[0].dist = cdist - size2[0]; - mji_scl3(tmp, con[0].frame, -con[0].dist / 2 - size2[0]); + mji_scl3(tmp, con[0].normal, -con[0].dist / 2 - size2[0]); mji_add3(con[0].pos, pos2, tmp); - - mju_zero3(con[0].frame+3); + mji_zero3(con[0].tangent); return 1; } // plane : sphere -int mjc_PlaneSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { - mjGETINFO +int mjc_PlaneSphere(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, + mjtNum margin) { + const mjtNum* pos1 = d->geom_xpos + 3*g1; + const mjtNum* pos2 = d->geom_xpos + 3*g2; + const mjtNum* mat1 = d->geom_xmat + 9*g1; + const mjtNum* mat2 = d->geom_xmat + 9*g2; + const mjtNum* size1 = m->geom_size + 3*g1; + const mjtNum* size2 = m->geom_size + 3*g2; return mjraw_PlaneSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2); } // plane : capsule -int mjc_PlaneCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { - mjGETINFO +int mjc_PlaneCapsule(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, + mjtNum margin) { + const mjtNum* pos1 = d->geom_xpos + 3*g1; + const mjtNum* pos2 = d->geom_xpos + 3*g2; + const mjtNum* mat1 = d->geom_xmat + 9*g1; + const mjtNum* mat2 = d->geom_xmat + 9*g2; + const mjtNum* size1 = m->geom_size + 3*g1; + const mjtNum* size2 = m->geom_size + 3*g2; // get capsule axis, segment = scaled axis mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]}; mjtNum segment[3] = {size2[1]*axis[0], size2[1]*axis[1], size2[1]*axis[2]}; // get point 1, do sphere-plane test - mjtNum pos[3]; - mju_add3(pos, pos2, segment); - int n1 = mjraw_PlaneSphere(con, margin, pos1, mat1, size1, pos, mat2, size2); + mjtNum endpoint[3]; + mju_add3(endpoint, pos2, segment); + int n1 = mjraw_PlaneSphere(con, margin, pos1, mat1, size1, endpoint, mat2, size2); // get point 2, do sphere-plane test - mju_sub3(pos, pos2, segment); - int n2 = mjraw_PlaneSphere(con+n1, margin, pos1, mat1, size1, pos, mat2, size2); + mju_sub3(endpoint, pos2, segment); + int n2 = mjraw_PlaneSphere(con + n1, margin, pos1, mat1, size1, endpoint, mat2, size2); // align contact frames with capsule axis if (n1) { - mji_copy3(con->frame + 3, axis); + mji_copy3(con[0].tangent, axis); } if (n2) { - mji_copy3((con + n1)->frame + 3, axis); + mji_copy3(con[n1].tangent, axis); } - return n1+n2; + return n1 + n2; } // plane : cylinder -int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { - mjGETINFO +int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, + mjtNum margin) { + const mjtNum* pos1 = d->geom_xpos + 3*g1; + const mjtNum* pos2 = d->geom_xpos + 3*g2; + const mjtNum* mat1 = d->geom_xmat + 9*g1; + const mjtNum* mat2 = d->geom_xmat + 9*g2; + const mjtNum* size2 = m->geom_size + 3*g2; + mjtNum normal[3] = {mat1[2], mat1[5], mat1[8]}; mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]}; @@ -137,8 +154,8 @@ int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g mji_add3(con[cnt].pos, pos2, vec); mji_addTo3(con[cnt].pos, axis); mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5); - mji_copy3(con[cnt].frame, normal); - mju_zero3(con[cnt].frame+3); + mji_copy3(con[cnt].normal, normal); + mji_zero3(con[cnt].tangent); cnt++; } else { return 0; // nearest point is above margin: no contacts @@ -150,8 +167,8 @@ int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g mji_add3(con[cnt].pos, pos2, vec); mji_subFrom3(con[cnt].pos, axis); mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5); - mji_copy3(con[cnt].frame, normal); - mju_zero3(con[cnt].frame+3); + mji_copy3(con[cnt].normal, normal); + mji_zero3(con[cnt].tangent); cnt++; } @@ -170,8 +187,8 @@ int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g mji_addTo3(con[cnt].pos, axis); mji_addToScl3(con[cnt].pos, vec, -0.5); mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5); - mji_copy3(con[cnt].frame, normal); - mju_zero3(con[cnt].frame+3); + mji_copy3(con[cnt].normal, normal); + mji_zero3(con[cnt].tangent); cnt++; // add point B @@ -180,8 +197,8 @@ int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g mji_addTo3(con[cnt].pos, axis); mji_addToScl3(con[cnt].pos, vec, -0.5); mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5); - mji_copy3(con[cnt].frame, normal); - mju_zero3(con[cnt].frame+3); + mji_copy3(con[cnt].normal, normal); + mji_zero3(con[cnt].tangent); cnt++; } @@ -190,8 +207,12 @@ int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g // plane : box -int mjc_PlaneBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { - mjGETINFO +int mjc_PlaneBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) { + const mjtNum* pos1 = d->geom_xpos + 3*g1; + const mjtNum* pos2 = d->geom_xpos + 3*g2; + const mjtNum* mat1 = d->geom_xmat + 9*g1; + const mjtNum* mat2 = d->geom_xmat + 9*g2; + const mjtNum* size2 = m->geom_size + 3*g2; // get normal, difference between centers, normal distance mjtNum norm[3] = {mat1[2], mat1[5], mat1[8]}; @@ -219,11 +240,11 @@ int mjc_PlaneBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mj // construct contact con[cnt].dist = dist + ldist; - mji_copy3(con[cnt].frame, norm); - mju_zero3(con[cnt].frame+3); + mji_copy3(con[cnt].normal, norm); mji_addTo3(corner, pos2); mji_scl3(vec, norm, -con[cnt].dist / 2); mji_add3(con[cnt].pos, corner, vec); + mji_zero3(con[cnt].tangent); // count; max is 4 if (++cnt >= 4) { @@ -238,7 +259,7 @@ int mjc_PlaneBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mj //--------------------------- sphere and capsule collisions ---------------------------------------- // sphere : sphere (actual implementation, can be called with modified parameters) -static int mjraw_SphereSphere(mjContact* con, mjtNum margin, +static int mjraw_SphereSphere(mjPreContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) { // check bounding spheres (this is called from other functions) @@ -251,36 +272,45 @@ static int mjraw_SphereSphere(mjContact* con, mjtNum margin, // depth and normal con[0].dist = mju_sqrt(cdist_sqr) - size1[0] - size2[0]; - mji_sub3(con[0].frame, pos2, pos1); - mjtNum len = mju_normalize3(con[0].frame); + mji_sub3(con[0].normal, pos2, pos1); + mjtNum len = mju_normalize3(con[0].normal); // if centers are the same, norm = cross-product of z axes // if z axes are parallel, norm = [1;0;0] if (len < mjMINVAL) { mjtNum axis1[3] = {mat1[2], mat1[5], mat1[8]}; mjtNum axis2[3] = {mat2[2], mat2[5], mat2[8]}; - mji_cross(con[0].frame, axis1, axis2); - mju_normalize3(con[0].frame); + mji_cross(con[0].normal, axis1, axis2); + mju_normalize3(con[0].normal); } // position - mji_scl3(con[0].pos, con[0].frame, size1[0] + con[0].dist / 2); + mji_scl3(con[0].pos, con[0].normal, size1[0] + con[0].dist / 2); mji_addTo3(con[0].pos, pos1); - mju_zero3(con[0].frame+3); + // axis + mji_zero3(con[0].tangent); + return 1; } // sphere : sphere -int mjc_SphereSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { - mjGETINFO +int mjc_SphereSphere(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, + mjtNum margin) { + const mjtNum* pos1 = d->geom_xpos + 3*g1; + const mjtNum* mat1 = d->geom_xmat + 9*g1; + const mjtNum* size1 = m->geom_size + 3*g1; + const mjtNum* pos2 = d->geom_xpos + 3*g2; + const mjtNum* mat2 = d->geom_xmat + 9*g2; + const mjtNum* size2 = m->geom_size + 3*g2; return mjraw_SphereSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2); } + // raw sphere : capsule -int mjraw_SphereCapsule(mjContact* con, mjtNum margin, +int mjraw_SphereCapsule(mjPreContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) { // get capsule length and axis @@ -299,16 +329,27 @@ int mjraw_SphereCapsule(mjContact* con, mjtNum margin, // sphere : capsule -int mjc_SphereCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { - mjGETINFO +int mjc_SphereCapsule(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, + mjtNum margin) { + const mjtNum* pos1 = d->geom_xpos + 3*g1; + const mjtNum* pos2 = d->geom_xpos + 3*g2; + const mjtNum* mat1 = d->geom_xmat + 9*g1; + const mjtNum* mat2 = d->geom_xmat + 9*g2; + const mjtNum* size1 = m->geom_size + 3*g1; + const mjtNum* size2 = m->geom_size + 3*g2; return mjraw_SphereCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2); } // sphere : cylinder -int mjc_SphereCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, +int mjc_SphereCylinder(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) { - mjGETINFO + const mjtNum* pos1 = d->geom_xpos + 3*g1; + const mjtNum* pos2 = d->geom_xpos + 3*g2; + const mjtNum* mat1 = d->geom_xmat + 9*g1; + const mjtNum* mat2 = d->geom_xmat + 9*g2; + const mjtNum* size1 = m->geom_size + 3*g1; + const mjtNum* size2 = m->geom_size + 3*g2; // get cylinder sizes and axis mjtNum radius = size2[0]; @@ -359,9 +400,10 @@ int mjc_SphereCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int mat_cap = flipmat; } int ncon = mjraw_PlaneSphere(con, margin, pos_cap, mat_cap, size2, pos1, mat1, size1); + if (ncon) { - // flip frame normal (because mjGEOM_PLANE < mjGEOM_SPHERE < mjGEOM_CYLINDER) - mju_scl3(con->frame, con->frame, -1); + // flip direction normal (because mjGEOM_PLANE < mjGEOM_SPHERE < mjGEOM_CYLINDER) + mju_scl3(con[0].normal, con[0].normal, -1); } return ncon; } @@ -380,7 +422,7 @@ int mjc_SphereCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int // raw capsule : capsule -int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin, +int mjraw_CapsuleCapsule(mjPreContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) { // get capsule axes (scaled) and center difference @@ -444,7 +486,7 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin, x2 = mju_clip((v + mb) / mc, -1, 1); mji_scl3(vec2, axis2, x2); mji_addTo3(vec2, pos2); - int n2 = mjraw_SphereSphere(con+n1, margin, vec1, mat1, size1, vec2, mat2, size2); + int n2 = mjraw_SphereSphere(con + n1, margin, vec1, mat1, size1, vec2, mat2, size2); // return if two contacts already found if (n1+n2 >= 2) { @@ -456,7 +498,7 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin, mjtNum x1 = mju_clip((u - mb) / ma, -1, 1); mji_scl3(vec1, axis1, x1); mji_addTo3(vec1, pos1); - int n3 = mjraw_SphereSphere(con+n1+n2, margin, vec1, mat1, size1, vec2, mat2, size2); + int n3 = mjraw_SphereSphere(con + n1 + n2, margin, vec1, mat1, size1, vec2, mat2, size2); // return if two contacts already found if (n1+n2+n3 >= 2) { @@ -468,7 +510,7 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin, x1 = mju_clip((u + mb) / ma, -1, 1); mji_scl3(vec1, axis1, x1); mji_addTo3(vec1, pos1); - int n4 = mjraw_SphereSphere(con+n1+n2+n3, margin, vec1, mat1, size1, vec2, mat2, size2); + int n4 = mjraw_SphereSphere(con + n1 + n2 + n3, margin, vec1, mat1, size1, vec2, mat2, size2); return n1+n2+n3+n4; } @@ -476,9 +518,14 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin, // capsule : capsule -int mjc_CapsuleCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, - mjtNum margin) { - mjGETINFO +int mjc_CapsuleCapsule(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, + mjtNum margin) { + const mjtNum* pos1 = d->geom_xpos + 3*g1; + const mjtNum* pos2 = d->geom_xpos + 3*g2; + const mjtNum* mat1 = d->geom_xmat + 9*g1; + const mjtNum* mat2 = d->geom_xmat + 9*g2; + const mjtNum* size1 = m->geom_size + 3*g1; + const mjtNum* size2 = m->geom_size + 3*g2; return mjraw_CapsuleCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2); } @@ -601,6 +648,7 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin, return 1; } + // box : triangle with radius int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, const mjtNum* mat, const mjtNum* size, const mjtNum* t1, @@ -693,6 +741,7 @@ int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, return cnt; } + // capsule : triangle with radius int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, const mjtNum* mat, const mjtNum* size, diff --git a/src/engine/engine_collision_primitive.h b/src/engine/engine_collision_primitive.h index 806e8742..b598c0e4 100644 --- a/src/engine/engine_collision_primitive.h +++ b/src/engine/engine_collision_primitive.h @@ -18,68 +18,57 @@ #include #include #include - -// define and extract geom info -#define mjGETINFO \ - const mjtNum* pos1 = d->geom_xpos + 3*g1; \ - const mjtNum* mat1 = d->geom_xmat + 9*g1; \ - const mjtNum* size1 = m->geom_size + 3*g1; \ - const mjtNum* pos2 = d->geom_xpos + 3*g2; \ - const mjtNum* mat2 = d->geom_xmat + 9*g2; \ - const mjtNum* size2 = m->geom_size + 3*g2; \ - (void) size1; (void) size2; // size1 and size2 are sometimes unused +#include #ifdef __cplusplus extern "C" { #endif // raw collision functions (called by mjc_XXX) -int mjraw_SphereCapsule (mjContact* con, mjtNum margin, - const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, - const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2); -int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin, - const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, - const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2); -int mjraw_CapsuleBox (mjContact* con, mjtNum margin, - const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, - const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2); -int mjraw_SphereTriangle(mjContact* con, mjtNum margin, - const mjtNum* s, mjtNum rs, +int mjraw_SphereCapsule(mjPreContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1, + const mjtNum* size1, const mjtNum* pos2, const mjtNum* mat2, + const mjtNum* size2); +int mjraw_CapsuleCapsule(mjPreContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1, + const mjtNum* size1, const mjtNum* pos2, const mjtNum* mat2, + const mjtNum* size2); +int mjraw_CapsuleBox(mjPreContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1, + const mjtNum* size1, const mjtNum* pos2, const mjtNum* mat2, + const mjtNum* size2); +int mjraw_SphereTriangle(mjContact* con, mjtNum margin, const mjtNum* s, mjtNum rs, const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt); -int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, - const mjtNum* mat, const mjtNum* size, const mjtNum* t1, - const mjtNum* t2, const mjtNum* t3, mjtNum rt); -int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, - const mjtNum* mat, const mjtNum* size, - const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, +int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, const mjtNum* mat, + const mjtNum* size, const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, + mjtNum rt); +int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, const mjtNum* mat, + const mjtNum* size, const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt); // plane collisions -MJAPI int mjc_PlaneSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, +MJAPI int mjc_PlaneSphere(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); -MJAPI int mjc_PlaneCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, +MJAPI int mjc_PlaneCapsule(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); -MJAPI int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, +MJAPI int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); -MJAPI int mjc_PlaneBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, +MJAPI int mjc_PlaneBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); // sphere and capsule collisions -MJAPI int mjc_SphereSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, - mjtNum margin); -MJAPI int mjc_SphereCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, +MJAPI int mjc_SphereSphere(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); -MJAPI int mjc_SphereCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, +MJAPI int mjc_SphereCapsule(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, + mjtNum margin); +MJAPI int mjc_SphereCylinder(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); -MJAPI int mjc_CapsuleCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, +MJAPI int mjc_CapsuleCapsule(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); // box collisions: from engine_collision_box.c -MJAPI int mjc_CapsuleBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, +MJAPI int mjc_CapsuleBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); -MJAPI int mjc_SphereBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, +MJAPI int mjc_SphereBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); -MJAPI int mjc_BoxBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, +MJAPI int mjc_BoxBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); #ifdef __cplusplus diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index cd074841..c5d093c0 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -21,7 +21,6 @@ #include #include // IWYU pragma: keep #include -#include "engine/engine_collision_primitive.h" #include "engine/engine_plugin.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" @@ -543,10 +542,10 @@ static int isknown(const mjtNum* points, const mjtNum x[3], int cnt) { // adds candidate point to result // flipNormal: 0 = normal points INTO SDF (for mesh-SDF where SDF is g2) // 1 = normal points OUT of SDF (for flex-SDF where SDF is g1) -static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3], - const mjtNum pos2[3], const mjtNum quat2[4], mjtNum dist, - int cnt, const mjModel* m, const mjSDF* s, const mjData* d, - int flipNormal) { +static int addPreContact(mjtNum* points, mjPreContact* con, const mjtNum x[3], + const mjtNum pos2[3], const mjtNum quat2[4], mjtNum dist, + int cnt, const mjModel* m, const mjSDF* s, const mjData* d, + int flipNormal) { // check if there is a collision if (dist > 0 || isknown(points, x, cnt)) { return cnt; @@ -570,18 +569,37 @@ static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3], } // construct contact - con[cnt].dist = dist; - mju_rotVecQuat(con[cnt].frame, norm, quat2); - mju_zero3(con[cnt].frame+3); - mju_makeFrame(con[cnt].frame); - mju_scl3(vec, con[cnt].frame, -con[cnt].dist/2); - mju_rotVecQuat(con[cnt].pos, x, quat2); - mju_addTo3(con[cnt].pos, pos2); - mju_addTo3(con[cnt].pos, vec); + con->dist = dist; + mju_rotVecQuat(con->normal, norm, quat2); + mju_scl3(vec, con->normal, - 0.5 * dist); + mju_rotVecQuat(con->pos, x, quat2); + mju_zero3(con->tangent); + mju_addTo3(con->pos, pos2); + mju_addTo3(con->pos, vec); return cnt+1; } + +// adds candidate point to result +// flipNormal: 0 = normal points INTO SDF (for mesh-SDF where SDF is g2) +// 1 = normal points OUT of SDF (for flex-SDF where SDF is g1) +static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3], + const mjtNum pos2[3], const mjtNum quat2[4], mjtNum dist, + int cnt, const mjModel* m, const mjSDF* s, const mjData* d, + int flipNormal) { + mjPreContact precon; + int ncon = addPreContact(points, &precon, x, pos2, quat2, dist, cnt, m, s, d, + flipNormal); + if (ncon > cnt) { + con[cnt].dist = precon.dist; + mju_copy3(con[cnt].pos, precon.pos); + mju_copy3(con[cnt].frame, precon.normal); + mju_zero3(con[cnt].frame + 3); + } + return ncon; +} + // finds minimum of Frank-Wolfe objective static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners, const mjModel* m, const mjSDF* sdf, const mjData* d) { @@ -952,18 +970,21 @@ static int meshFaceCallback(int face_id, int node, void* ctx) { //------------------------------ collision functions ----------------------------------------------- // collision between a height field and a signed distance field -int mjc_HFieldSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { +int mjc_HFieldSDF(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) { mju_warning("HField vs SDF collision not yet supported!"); return 0; } // collision between a mesh and a signed distance field -int mjc_MeshSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { - mjGETINFO; +int mjc_MeshSDF(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) { + const mjtNum* pos1 = d->geom_xpos + 3*g1; + const mjtNum* mat1 = d->geom_xmat + 9*g1; + const mjtNum* pos2 = d->geom_xpos + 3*g2; + const mjtNum* mat2 = d->geom_xmat + 9*g2; mjtNum offset[3], rotation[9]; - mjtNum points[3*mjMAXCONPAIR], dist[mjMAXCONPAIR], candidate[3*mjMAXCONPAIR]; + mjtNum points[3*mjMAXCONPAIR], dist2[mjMAXCONPAIR], candidate[3*mjMAXCONPAIR]; int cnt = 0, ncandidate = 0; // get sdf plugin @@ -996,7 +1017,7 @@ int mjc_MeshSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjt ctx.faceadr = m->mesh_faceadr[m->geom_dataid[g1]]; ctx.nstartpts = mju_max(1, m->opt.sdf_initpoints); ctx.candidate = candidate; - ctx.dist = dist; + ctx.dist = dist2; ctx.ncandidate = &ncandidate; // BVH traversal for mesh faces @@ -1014,34 +1035,37 @@ int mjc_MeshSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjt // if few candidates, add them all directly if (ncandidate <= mjMAXCONPAIR) { for (int i = 0; i < ncandidate; i++) { - cnt = addContact(points, con, candidate + 3*i, pos2, sdf_quat, - dist[i], cnt, m, &sdf, d, 0); + cnt = addPreContact(points, con + cnt, candidate + 3*i, pos2, sdf_quat, + dist2[i], cnt, m, &sdf, d, 0); } return cnt; } // farthest point sampling (FPS) for spatial coverage int selected_indices[mjMAXCONPAIR]; - int nselected = selectFPS(candidate, dist, ncandidate, selected_indices, mjMAXCONPAIR); + int nselected = selectFPS(candidate, dist2, ncandidate, selected_indices, mjMAXCONPAIR); // add selected contacts for (int i = 0; i < nselected; i++) { int idx = selected_indices[i]; - cnt = addContact(points, con, candidate + 3*idx, pos2, sdf_quat, - dist[idx], cnt, m, &sdf, d, 0); + cnt = addPreContact(points, con + cnt, candidate + 3*idx, pos2, sdf_quat, + dist2[idx], cnt, m, &sdf, d, 0); } return cnt; } // collision between two SDFs -int mjc_SDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { - mjGETINFO; - size1 = m->geom_aabb + 6*g1; - size2 = m->geom_aabb + 6*g2; +int mjc_SDF(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) { + const mjtNum* pos1 = d->geom_xpos + 3*g1; + const mjtNum* mat1 = d->geom_xmat + 9*g1; + const mjtNum* pos2 = d->geom_xpos + 3*g2; + const mjtNum* mat2 = d->geom_xmat + 9*g2; + const mjtNum* size1 = m->geom_aabb + 6*g1; + const mjtNum* size2 = m->geom_aabb + 6*g2; int cnt = 0; - mjtNum x[3], y[3], dist, vec1[3], vec2[3]; + mjtNum x[3], y[3], dist2, vec1[3], vec2[3]; mjtNum aabb1[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; mjtNum aabb2[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; mjtNum aabb[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; @@ -1153,15 +1177,15 @@ int mjc_SDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum // gradient descent - we use a special function of the two SDF as objective sdf.type = mjSDFTYPE_COLLISION; - dist = stepGradient(x, m, &sdf, d, m->opt.sdf_iterations); + dist2 = stepGradient(x, m, &sdf, d, m->opt.sdf_iterations); // inexact SDFs can yield spurious collisions, filter them by projecting on the midsurface sdf.type = mjSDFTYPE_INTERSECTION; - dist = stepGradient(x, m, &sdf, d, 1); + dist2 = stepGradient(x, m, &sdf, d, 1); // contact point and normal - we use the midsurface where SDF1=SDF2 as zero level set sdf.type = mjSDFTYPE_MIDSURFACE; - cnt = addContact(contacts, con, x, pos2, quat2, dist, cnt, m, &sdf, d, 0); + cnt = addPreContact(contacts, con + cnt, x, pos2, quat2, dist2, cnt, m, &sdf, d, 0); // SHOULD NOT OCCUR if (cnt > mjMAXCONPAIR) { diff --git a/src/engine/engine_collision_sdf.h b/src/engine/engine_collision_sdf.h index b50f3082..64f9b298 100644 --- a/src/engine/engine_collision_sdf.h +++ b/src/engine/engine_collision_sdf.h @@ -36,13 +36,13 @@ MJAPI void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s, mjtNu const mjtNum x[3]); // collision between a height field and a signed distance field -int mjc_HFieldSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin); +int mjc_HFieldSDF(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); // collision between a mesh and a signed distance field -int mjc_MeshSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin); +int mjc_MeshSDF(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); // collision between two signed distance fields -int mjc_SDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin); +int mjc_SDF(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); // collision between entire flex and SDF geom (batched processing) int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con, diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 6a247f09..eaca8ba8 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -553,7 +553,7 @@ static mjtNum mj_geomDistanceCCD(const mjModel* m, mjData* d, int g1, int g2, // returns the smallest distance between two geoms mjtNum mj_geomDistance(const mjModel* m, mjData* d, int geom1, int geom2, mjtNum distmax, mjtNum fromto[6]) { - mjContact con[mjMAXCONPAIR]; + mjPreContact con[mjMAXCONPAIR]; mjtNum dist = distmax; if (fromto) mju_zero(fromto, 6); @@ -594,8 +594,8 @@ mjtNum mj_geomDistance(const mjModel* m, mjData* d, int geom1, int geom2, mjtNum // write fromto if given and a collision has been found if (fromto && smallest >= 0) { mjtNum sign = flip ? -1 : 1; - mju_addScl3(fromto+0, con[smallest].pos, con[smallest].frame, -0.5*sign*dist); - mju_addScl3(fromto+3, con[smallest].pos, con[smallest].frame, 0.5*sign*dist); + mju_addScl3(fromto+0, con[smallest].pos, con[smallest].normal, -0.5*sign*dist); + mju_addScl3(fromto+3, con[smallest].pos, con[smallest].normal, 0.5*sign*dist); } return dist; diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c index bfff1a35..fb7ec3fb 100644 --- a/src/engine/engine_util_spatial.c +++ b/src/engine/engine_util_spatial.c @@ -504,7 +504,7 @@ void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_force, } -// make 3D frame given X axis (normal) and possibly Y axis (tangent 1) +// make 3D frame given x-axis (normal) and possibly y-axis (tangent 1) void mju_makeFrame(mjtNum frame[9]) { mjtNum tmp[3]; diff --git a/test/engine/engine_collision_box_test.cc b/test/engine/engine_collision_box_test.cc index e75a6c83..0ac0ae62 100644 --- a/test/engine/engine_collision_box_test.cc +++ b/test/engine/engine_collision_box_test.cc @@ -15,6 +15,7 @@ // Tests for engine/engine_collision_box.c. #include +#include #include #include @@ -45,11 +46,9 @@ TEST_F(MjCollisionBoxTest, BadContacts) { mj_forward(model, data); // allocate contact array and matching arrays - mj_markStack(data); - mjContact* con_raw = (mjContact*) mj_stackAllocByte( - data, mjMAXCONPAIR * sizeof(mjContact), alignof(mjContact)); - int* match_raw = mj_stackAllocInt(data, mjMAXCONPAIR); - int* match = mj_stackAllocInt(data, data->ncon); + std::vector precon(mjMAXCONPAIR); + std::vector match_raw(mjMAXCONPAIR); + std::vector match(data->ncon); int g1 = -1; int g2 = -1; @@ -73,20 +72,21 @@ TEST_F(MjCollisionBoxTest, BadContacts) { g2 = g2new; // call low-level box-box collider - int num = mjc_BoxBox(model, data, con_raw, g1, g2, con->includemargin); + int num = mjc_BoxBox(model, data, precon.data(), g1, g2, + con->includemargin); // allocate and clear arrays marking already matched contacts - mju_zeroInt(match_raw, num); - mju_zeroInt(match, data->ncon); + mju_zeroInt(match_raw.data(), num); + mju_zeroInt(match.data(), data->ncon); // loop over raw contacts, match with contact array using pos int nmatched = 0; for (int i = 0; i < num; i++) { for (int j = 0; j < data->ncon; j++) { if (!match[j] && - con_raw[i].pos[0] == data->contact[j].pos[0] && - con_raw[i].pos[1] == data->contact[j].pos[1] && - con_raw[i].pos[2] == data->contact[j].pos[2]) { + precon[i].pos[0] == data->contact[j].pos[0] && + precon[i].pos[1] == data->contact[j].pos[1] && + precon[i].pos[2] == data->contact[j].pos[2]) { match_raw[i] = match[j] = 1; nmatched++; } @@ -97,11 +97,11 @@ TEST_F(MjCollisionBoxTest, BadContacts) { EXPECT_EQ(nmatched, num) << local_path; // get box info - const mjtNum* pos1 = data->geom_xpos + 3 * g1; - const mjtNum* mat1 = data->geom_xmat + 9 * g1; + const mjtNum* pos1 = data->geom_xpos + 3 * g1; + const mjtNum* mat1 = data->geom_xmat + 9 * g1; const mjtNum* size1 = model->geom_size + 3 * g1; - const mjtNum* pos2 = data->geom_xpos + 3 * g2; - const mjtNum* mat2 = data->geom_xmat + 9 * g2; + const mjtNum* pos2 = data->geom_xpos + 3 * g2; + const mjtNum* mat2 = data->geom_xmat + 9 * g2; const mjtNum* size2 = model->geom_size + 3 * g2; mjtNum margin = mju_max(model->geom_margin[g1], model->geom_margin[g2]); @@ -117,8 +117,10 @@ TEST_F(MjCollisionBoxTest, BadContacts) { static mjtNum kRatio = 1.01; // is the contact outside: 1, inside: -1, within the removal width: 0 - int out1 = mju_outsideBox(con_raw[i].pos, pos1, mat1, sz1, kRatio); - int out2 = mju_outsideBox(con_raw[i].pos, pos2, mat2, sz2, kRatio); + int out1 = mju_outsideBox(precon[i].pos, pos1, mat1, sz1, + kRatio); + int out2 = mju_outsideBox(precon[i].pos, pos2, mat2, sz2, + kRatio); // mark as bad if outside one box and not inside the other box bool outside = (out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1); @@ -129,7 +131,6 @@ TEST_F(MjCollisionBoxTest, BadContacts) { } } - mj_freeStack(data); mj_deleteData(data); mj_deleteModel(model); } @@ -146,11 +147,9 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) { mj_forward(model, data); // allocate contact array and matching arrays - mj_markStack(data); - mjContact* con_raw = (mjContact*) mj_stackAllocByte( - data, mjMAXCONPAIR * sizeof(mjContact), alignof(mjContact)); - int* match_raw = mj_stackAllocInt(data, mjMAXCONPAIR); - int* match = mj_stackAllocInt(data, data->ncon); + std::vector precon(mjMAXCONPAIR); + std::vector match_raw(mjMAXCONPAIR); + std::vector match(data->ncon); int g1 = -1; @@ -175,20 +174,21 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) { g2 = g2new; // call low-level box-box collider - int num = mjc_BoxBox(model, data, con_raw, g1, g2, con->includemargin); + int num = mjc_BoxBox(model, data, precon.data(), g1, g2, + con->includemargin); // allocate and clear arrays marking already matched contacts - mju_zeroInt(match_raw, num); - mju_zeroInt(match, data->ncon); + mju_zeroInt(match_raw.data(), num); + mju_zeroInt(match.data(), data->ncon); // loop over raw contacts, match with contact array using pos int nmatched = 0; for (int i = 0; i < num; i++) { for (int j = 0; j < data->ncon; j++) { if (!match[j] && - con_raw[i].pos[0] == data->contact[j].pos[0] && - con_raw[i].pos[1] == data->contact[j].pos[1] && - con_raw[i].pos[2] == data->contact[j].pos[2]) { + precon[i].pos[0] == data->contact[j].pos[0] && + precon[i].pos[1] == data->contact[j].pos[1] && + precon[i].pos[2] == data->contact[j].pos[2]) { match_raw[i] = match[j] = 1; nmatched++; } @@ -207,9 +207,9 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) { if (duplicate || i == j) { continue; } - if (con_raw[i].pos[0] == con_raw[j].pos[0] && - con_raw[i].pos[1] == con_raw[j].pos[1] && - con_raw[i].pos[2] == con_raw[j].pos[2]) { + if (precon[i].pos[0] == precon[j].pos[0] && + precon[i].pos[1] == precon[j].pos[1] && + precon[i].pos[2] == precon[j].pos[2]) { duplicate = true; } } @@ -220,12 +220,10 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) { } } - mj_freeStack(data); mj_deleteData(data); mj_deleteModel(model); } - static const char* const kDeepFilePath = "engine/testdata/collision_box/boxbox_deep.xml"; @@ -273,7 +271,6 @@ TEST_F(MjCollisionBoxTest, BoxSphere) { mj_deleteModel(model); } - TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) { constexpr char xml[] = R"( @@ -289,12 +286,12 @@ TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) { mjData* data = mj_makeData(model); mj_kinematics(model, data); - mjContact contact[9]; + mjPreContact precon[mjMAXCONPAIR]; for (mjfCollision collision : {mjc_BoxBox, mjc_Convex}) { - int n = collision(model, data, contact, 0, 1, 0.0); + int n = collision(model, data, precon, 0, 1, 0.0); for (int i = 0; i < n; i++) { - EXPECT_NEAR(contact[i].dist, -0.5, MjTol(1e-8, 1e-6)); + EXPECT_NEAR(precon[i].dist, -0.5, MjTol(1e-8, 1e-6)); } } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 164edc61..e66b47a3 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -1520,6 +1520,14 @@ public unsafe struct mjModel_ { public UInt64 signature; } +[StructLayout(LayoutKind.Sequential)] +public unsafe struct mjPreContact_ { + public double dist; + public fixed double pos[3]; + public fixed double normal[3]; + public fixed double tangent[3]; +} + [StructLayout(LayoutKind.Sequential)] public unsafe struct mjContact_ { public double dist; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index c82c2de3..8262902d 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -766,6 +766,36 @@ struct MjOption { bool owned_ = false; }; +struct MjPreContact { + ~MjPreContact(); + MjPreContact(); + explicit MjPreContact(mjPreContact *ptr); + MjPreContact(const MjPreContact &); + MjPreContact &operator=(const MjPreContact &); + std::unique_ptr copy(); + mjPreContact* get() const; + void set(mjPreContact* ptr); + mjtNum dist() const { + return ptr_->dist; + } + void set_dist(mjtNum value) { + ptr_->dist = value; + } + emscripten::val pos() const { + return emscripten::val(emscripten::typed_memory_view(3, ptr_->pos)); + } + emscripten::val normal() const { + return emscripten::val(emscripten::typed_memory_view(3, ptr_->normal)); + } + emscripten::val tangent() const { + return emscripten::val(emscripten::typed_memory_view(3, ptr_->tangent)); + } + + private: + mjPreContact* ptr_; + bool owned_ = false; +}; + struct MjSolverStat { ~MjSolverStat(); MjSolverStat(); @@ -7139,7 +7169,7 @@ MjContact::~MjContact() { delete ptr_; } } -MjContact::MjContact() : ptr_(new mjContact) { +MjContact::MjContact() : ptr_(new mjContact()) { owned_ = true; } MjContact::MjContact(const MjContact &other) : MjContact() { @@ -7168,7 +7198,7 @@ MjLROpt::~MjLROpt() { delete ptr_; } } -MjLROpt::MjLROpt() : ptr_(new mjLROpt) { +MjLROpt::MjLROpt() : ptr_(new mjLROpt()) { owned_ = true; mj_defaultLROpt(ptr_); } @@ -7198,7 +7228,7 @@ MjOption::~MjOption() { delete ptr_; } } -MjOption::MjOption() : ptr_(new mjOption) { +MjOption::MjOption() : ptr_(new mjOption()) { owned_ = true; mj_defaultOption(ptr_); } @@ -7222,13 +7252,42 @@ void MjOption::set(mjOption* ptr) { ptr_ = ptr; } +MjPreContact::MjPreContact(mjPreContact *ptr) : ptr_(ptr) {} +MjPreContact::~MjPreContact() { + if (owned_ && ptr_) { + delete ptr_; + } +} +MjPreContact::MjPreContact() : ptr_(new mjPreContact()) { + owned_ = true; +} +MjPreContact::MjPreContact(const MjPreContact &other) : MjPreContact() { + *ptr_ = *other.get(); +} +MjPreContact& MjPreContact::operator=(const MjPreContact &other) { + if (this == &other) { + return *this; + } + *ptr_ = *other.get(); + return *this; +} +std::unique_ptr MjPreContact::copy() { + return std::make_unique(*this); +} +mjPreContact* MjPreContact::get() const { + return ptr_; +} +void MjPreContact::set(mjPreContact* ptr) { + ptr_ = ptr; +} + MjSolverStat::MjSolverStat(mjSolverStat *ptr) : ptr_(ptr) {} MjSolverStat::~MjSolverStat() { if (owned_ && ptr_) { delete ptr_; } } -MjSolverStat::MjSolverStat() : ptr_(new mjSolverStat) { +MjSolverStat::MjSolverStat() : ptr_(new mjSolverStat()) { owned_ = true; } MjSolverStat::MjSolverStat(const MjSolverStat &other) : MjSolverStat() { @@ -7257,7 +7316,7 @@ MjStatistic::~MjStatistic() { delete ptr_; } } -MjStatistic::MjStatistic() : ptr_(new mjStatistic) { +MjStatistic::MjStatistic() : ptr_(new mjStatistic()) { owned_ = true; } MjStatistic::MjStatistic(const MjStatistic &other) : MjStatistic() { @@ -7286,7 +7345,7 @@ MjTimerStat::~MjTimerStat() { delete ptr_; } } -MjTimerStat::MjTimerStat() : ptr_(new mjTimerStat) { +MjTimerStat::MjTimerStat() : ptr_(new mjTimerStat()) { owned_ = true; } MjTimerStat::MjTimerStat(const MjTimerStat &other) : MjTimerStat() { @@ -7315,7 +7374,7 @@ MjVisualGlobal::~MjVisualGlobal() { delete ptr_; } } -MjVisualGlobal::MjVisualGlobal() : ptr_(new mjVisualGlobal) { +MjVisualGlobal::MjVisualGlobal() : ptr_(new mjVisualGlobal()) { owned_ = true; } MjVisualGlobal::MjVisualGlobal(const MjVisualGlobal &other) : MjVisualGlobal() { @@ -7344,7 +7403,7 @@ MjVisualHeadlight::~MjVisualHeadlight() { delete ptr_; } } -MjVisualHeadlight::MjVisualHeadlight() : ptr_(new mjVisualHeadlight) { +MjVisualHeadlight::MjVisualHeadlight() : ptr_(new mjVisualHeadlight()) { owned_ = true; } MjVisualHeadlight::MjVisualHeadlight(const MjVisualHeadlight &other) : MjVisualHeadlight() { @@ -7373,7 +7432,7 @@ MjVisualMap::~MjVisualMap() { delete ptr_; } } -MjVisualMap::MjVisualMap() : ptr_(new mjVisualMap) { +MjVisualMap::MjVisualMap() : ptr_(new mjVisualMap()) { owned_ = true; } MjVisualMap::MjVisualMap(const MjVisualMap &other) : MjVisualMap() { @@ -7402,7 +7461,7 @@ MjVisualQuality::~MjVisualQuality() { delete ptr_; } } -MjVisualQuality::MjVisualQuality() : ptr_(new mjVisualQuality) { +MjVisualQuality::MjVisualQuality() : ptr_(new mjVisualQuality()) { owned_ = true; } MjVisualQuality::MjVisualQuality(const MjVisualQuality &other) : MjVisualQuality() { @@ -7431,7 +7490,7 @@ MjVisualRgba::~MjVisualRgba() { delete ptr_; } } -MjVisualRgba::MjVisualRgba() : ptr_(new mjVisualRgba) { +MjVisualRgba::MjVisualRgba() : ptr_(new mjVisualRgba()) { owned_ = true; } MjVisualRgba::MjVisualRgba(const MjVisualRgba &other) : MjVisualRgba() { @@ -7460,7 +7519,7 @@ MjVisualScale::~MjVisualScale() { delete ptr_; } } -MjVisualScale::MjVisualScale() : ptr_(new mjVisualScale) { +MjVisualScale::MjVisualScale() : ptr_(new mjVisualScale()) { owned_ = true; } MjVisualScale::MjVisualScale(const MjVisualScale &other) : MjVisualScale() { @@ -7489,7 +7548,7 @@ MjWarningStat::~MjWarningStat() { delete ptr_; } } -MjWarningStat::MjWarningStat() : ptr_(new mjWarningStat) { +MjWarningStat::MjWarningStat() : ptr_(new mjWarningStat()) { owned_ = true; } MjWarningStat::MjWarningStat(const MjWarningStat &other) : MjWarningStat() { @@ -7534,7 +7593,7 @@ MjvCamera::~MjvCamera() { delete ptr_; } } -MjvCamera::MjvCamera() : ptr_(new mjvCamera) { +MjvCamera::MjvCamera() : ptr_(new mjvCamera()) { owned_ = true; mjv_defaultCamera(ptr_); } @@ -7564,7 +7623,7 @@ MjvFigure::~MjvFigure() { delete ptr_; } } -MjvFigure::MjvFigure() : ptr_(new mjvFigure) { +MjvFigure::MjvFigure() : ptr_(new mjvFigure()) { owned_ = true; mjv_defaultFigure(ptr_); } @@ -7594,7 +7653,7 @@ MjvGLCamera::~MjvGLCamera() { delete ptr_; } } -MjvGLCamera::MjvGLCamera() : ptr_(new mjvGLCamera) { +MjvGLCamera::MjvGLCamera() : ptr_(new mjvGLCamera()) { owned_ = true; } MjvGLCamera::MjvGLCamera(const MjvGLCamera &other) : MjvGLCamera() { @@ -7623,7 +7682,7 @@ MjvGeom::~MjvGeom() { delete ptr_; } } -MjvGeom::MjvGeom() : ptr_(new mjvGeom) { +MjvGeom::MjvGeom() : ptr_(new mjvGeom()) { owned_ = true; mjv_initGeom(ptr_, mjGEOM_NONE, nullptr, nullptr, nullptr, nullptr); } @@ -7653,7 +7712,7 @@ MjvLight::~MjvLight() { delete ptr_; } } -MjvLight::MjvLight() : ptr_(new mjvLight) { +MjvLight::MjvLight() : ptr_(new mjvLight()) { owned_ = true; } MjvLight::MjvLight(const MjvLight &other) : MjvLight() { @@ -7682,7 +7741,7 @@ MjvOption::~MjvOption() { delete ptr_; } } -MjvOption::MjvOption() : ptr_(new mjvOption) { +MjvOption::MjvOption() : ptr_(new mjvOption()) { owned_ = true; mjv_defaultOption(ptr_); } @@ -7712,7 +7771,7 @@ MjvPerturb::~MjvPerturb() { delete ptr_; } } -MjvPerturb::MjvPerturb() : ptr_(new mjvPerturb) { +MjvPerturb::MjvPerturb() : ptr_(new mjvPerturb()) { owned_ = true; mjv_defaultPerturb(ptr_); } @@ -7750,7 +7809,7 @@ MjVisual::~MjVisual() { delete ptr_; } } -MjVisual::MjVisual() : ptr_(new mjVisual), global(&ptr_->global), quality(&ptr_->quality), headlight(&ptr_->headlight), map(&ptr_->map), scale(&ptr_->scale), rgba(&ptr_->rgba) { +MjVisual::MjVisual() : ptr_(new mjVisual()), global(&ptr_->global), quality(&ptr_->quality), headlight(&ptr_->headlight), map(&ptr_->map), scale(&ptr_->scale), rgba(&ptr_->rgba) { owned_ = true; mj_defaultVisual(ptr_); } @@ -12375,6 +12434,13 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("tolerance", &MjOption::tolerance, &MjOption::set_tolerance, reference()) .property("viscosity", &MjOption::viscosity, &MjOption::set_viscosity, reference()) .property("wind", &MjOption::wind); + emscripten::class_("MjPreContact") + .constructor<>() + .function("copy", &MjPreContact::copy, take_ownership()) + .property("dist", &MjPreContact::dist, &MjPreContact::set_dist, reference()) + .property("normal", &MjPreContact::normal) + .property("pos", &MjPreContact::pos) + .property("tangent", &MjPreContact::tangent); emscripten::class_("MjSolverStat") .constructor<>() .function("copy", &MjSolverStat::copy, take_ownership()) diff --git a/wasm/codegen/generators/constants.py b/wasm/codegen/generators/constants.py index c5a84f8b..2338dff0 100644 --- a/wasm/codegen/generators/constants.py +++ b/wasm/codegen/generators/constants.py @@ -292,6 +292,7 @@ STRUCTS_TO_BIND: list[str] = list( NO_DEFAULT_CONSTRUCTORS: tuple[str, ...] = ( # go/keep-sorted start "mjContact", + "mjPreContact", "mjSolverStat", "mjStatistic", "mjTimerStat", diff --git a/wasm/codegen/generators/structs.py b/wasm/codegen/generators/structs.py index 0bb03066..a2731326 100644 --- a/wasm/codegen/generators/structs.py +++ b/wasm/codegen/generators/structs.py @@ -493,7 +493,7 @@ def build_struct_source( if not is_mjs: # default constructor - with builder.function(f"{w}::{w}() : ptr_(new {s}){fields_init}"): + with builder.function(f"{w}::{w}() : ptr_(new {s}()){fields_init}"): builder.line("owned_ = true;") default_func = _default_function_statement(s) if default_func: diff --git a/wasm/codegen/tests/generators_test.py b/wasm/codegen/tests/generators_test.py index 1d21bc89..3da3eb02 100644 --- a/wasm/codegen/tests/generators_test.py +++ b/wasm/codegen/tests/generators_test.py @@ -266,7 +266,7 @@ MjvLight::~MjvLight() { delete ptr_; } } -MjvLight::MjvLight() : ptr_(new mjvLight) { +MjvLight::MjvLight() : ptr_(new mjvLight()) { owned_ = true; } MjvLight::MjvLight(const MjvLight &other) : MjvLight() {