chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Introduce mjPreContact, a minimal struct passed into the collision functions.

Browse Source 
PiperOrigin-RevId: 918533795
Change-Id: I2b5af05c1479b25d5c2cfdc690321a26fce6ede6
This commit is contained in:
Kyle Bayes
2026-05-20 10:47:11 -07:00
committed by Copybara-Service
parent 53b3137a12
commit 7174d33f08
25 changed files with 709 additions and 406 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/APItypes.rst
+10 -1
View File
@@ -936,6 +936,15 @@ MJCF element :ref:`statistic <statistic>`. One instance of it is embedded in mjM
.. mujoco-include:: mjStatistic .. 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:
mjContact mjContact
@@ -1780,7 +1789,7 @@ mjfCollision
.. code-block:: C .. code-block:: C
typedef int (*mjfCollision)(const mjModel* m, const mjData* d, 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`. This is the function type of the callbacks in the collision table :ref:`mjCOLLISIONFUNC`.
doc/changelog.rst
+3
View File
@@ -70,6 +70,9 @@ General
Negative ``margin`` values are now permitted (corresponding to ``gap > margin`` under the old semantics). The 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. 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 - MJX: Removed the deprecated ``nconmax`` argument from ``mjx.make_data`` and ``mjx.put_data`` in favor of
``naconmax``. ``naconmax``.
doc/includes/references.h
+8 -1
View File
@@ -101,8 +101,15 @@ typedef enum mjtSleepState_ { // sleep state of an object
mjS_ASLEEP = 0, // object is asleep mjS_ASLEEP = 0, // object is asleep
mjS_AWAKE = 1 // object is awake mjS_AWAKE = 1 // object is awake
} mjtSleepState; } 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 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 dist; // distance between nearest points; neg: penetration
mjtNum pos[3]; // position of contact point: midpoint between geoms 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] mjtNum frame[9]; // normal is in [0-2], points from geom[0] to geom[1]
include/mujoco/mjdata.h
+11 -3
View File
@@ -121,10 +121,18 @@ typedef enum mjtSleepState_ { // sleep state of an object
} mjtSleepState; } 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 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 dist; // distance between nearest points; neg: penetration
mjtNum pos[3]; // position of contact point: midpoint between geoms 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] 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); typedef mjtNum (*mjfAct)(const mjModel* m, const mjData* d, int id);
// collision detection // 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); mjtNum margin);
#endif // MUJOCO_MJDATA_H_ #endif // MUJOCO_MJDATA_H_
python/mujoco/introspect/structs.py
+36
View File
@@ -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', ('mjContact',
StructDecl( StructDecl(
name='mjContact', name='mjContact',
python/mujoco/raw.h
+1
View File
@@ -25,6 +25,7 @@
// to them under the "raw" namespace. // to them under the "raw" namespace.
namespace mujoco::raw { namespace mujoco::raw {
using MjPreContact = ::mjPreContact;
using MjContact = ::mjContact; using MjContact = ::mjContact;
using MjData = ::mjData; using MjData = ::mjData;
using MjLROpt = ::mjLROpt; using MjLROpt = ::mjLROpt;
python/mujoco/structs.cc
+26
View File
@@ -615,6 +615,32 @@ This is useful for example when the MJB is not available as a file on disk.)"));
X(int, nupdate); X(int, nupdate);
#undef X #undef X
// ==================== MJPRECONTACT ==========================================
py::class_<MjPreContactWrapper> 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 ============================================ // ==================== MJCONTACT ============================================
py::class_<MjContactWrapper> mjContact(m, "MjContact"); py::class_<MjContactWrapper> mjContact(m, "MjContact");
mjContact.def(py::init<>()); mjContact.def(py::init<>());
python/mujoco/structs.h
+22
View File
@@ -547,6 +547,27 @@ template <>
struct enable_if_mj_struct<raw::MjModel> { using type = void; }; struct enable_if_mj_struct<raw::MjModel> { using type = void; };
// ==================== MJCONTACT ============================================== // ==================== MJCONTACT ==============================================
template <>
class MjWrapper<raw::MjPreContact> : public WrapperBase<raw::MjPreContact> {
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<decltype(raw::MjPreContact::var)>> \
var
X(pos);
X(normal);
X(tangent);
#undef X
};
using MjPreContactWrapper = MjWrapper<raw::MjPreContact>;
template <> template <>
class MjWrapper<raw::MjContact> : public WrapperBase<raw::MjContact> { class MjWrapper<raw::MjContact> : public WrapperBase<raw::MjContact> {
public: public:
@@ -961,6 +982,7 @@ using _impl::MjSolverStatWrapper;
using _impl::MjModelWrapper; using _impl::MjModelWrapper;
using _impl::MjDataWrapper; using _impl::MjDataWrapper;
using _impl::MjContactWrapper; using _impl::MjContactWrapper;
using _impl::MjPreContactWrapper;
using _impl::MjvPerturbWrapper; using _impl::MjvPerturbWrapper;
using _impl::MjvCameraWrapper; using _impl::MjvCameraWrapper;
using _impl::MjvGLCameraWrapper; using _impl::MjvGLCameraWrapper;
python/mujoco/structs_wrappers.cc
+20
View File
@@ -535,6 +535,26 @@ std::unique_ptr<MjModelWrapper> MjModelWrapper::Deserialize(
return std::unique_ptr<MjModelWrapper>(new MjModelWrapper(model)); return std::unique_ptr<MjModelWrapper>(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 ============================================== // ==================== MJCONTACT ==============================================
#define X(var) var(InitPyArray(ptr_->var, owner_)) #define X(var) var(InitPyArray(ptr_->var, owner_))
MjContactWrapper::MjWrapper() MjContactWrapper::MjWrapper()
src/engine/engine_collision_box.c
+70 -66
View File
@@ -20,24 +20,18 @@
#include "engine/engine_util_misc.h" #include "engine/engine_util_misc.h"
// hard-clamp vector to range [-limit(i), +limit(i)] // hard-clamp vector to range [-limit(i), +limit(i)]
static void mju_clampVec(mjtNum* vec, const mjtNum* limit, int n) static void mju_clampVec(mjtNum* vec, const mjtNum* limit, int n) {
{ for (int i = 0; i < n; i++) {
int i; // loop over active limits
// loop over active limits
for (i = 0; i < n; i++) {
if (limit[i] > 0) { if (limit[i] > 0) {
if (vec[i] < -limit[i]) vec[i] = mju_clip(vec[i], -limit[i], limit[i]);
vec[i] = -limit[i];
else if (vec[i] > limit[i])
vec[i] = limit[i];
} }
} }
} }
// raw sphere : box // 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* 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) {
int i, k; int i, k;
@@ -74,29 +68,32 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin,
mji_copy3(pos, center); mji_copy3(pos, center);
mji_addToScl3(pos, nearest, (size1[0] - closest) / 2); mji_addToScl3(pos, nearest, (size1[0] - closest) / 2);
mji_mulMatVec3(con[0].frame, mat2, nearest); mji_mulMatVec3(con[0].normal, mat2, nearest);
dist = -closest; dist = -closest;
} else { } else {
mji_addToScl3(deepest, tmp, size1[0]); mji_addToScl3(deepest, tmp, size1[0]);
mju_zero3(pos); mju_zero3(pos);
mji_addToScl3(pos, clamped, 0.5); mji_addToScl3(pos, clamped, 0.5);
mji_addToScl3(pos, deepest, 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_mulMatVec3(tmp, mat2, pos);
mji_add3(con[0].pos, tmp, pos2); mji_add3(con[0].pos, tmp, pos2);
con[0].dist = dist - size1[0]; con[0].dist = dist - size1[0];
mju_zero3(con[0].frame + 3); mji_zero3(con[0].tangent);
return 1; return 1;
} }
// sphere : box // sphere : box
int mjc_SphereBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { int mjc_SphereBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) {
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;
return mjraw_SphereBox(con, margin, pos1, mat1, size1, pos2, mat2, size2); 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 // 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* 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 tmp1[3], tmp2[3], tmp3[3], halfaxis[3], axis[3], dif[3];
mjtNum pos[3]; // position of capsule in box-local frame 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) 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_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[0] = mat1[2]; // capsule's axis
tmp1[1] = mat1[5]; tmp1[1] = mat1[5];
@@ -591,21 +589,25 @@ skip:
// capsule : box // capsule : box
int mjc_CapsuleBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { int mjc_CapsuleBox(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;
return mjraw_CapsuleBox(con, margin, pos1, mat1, size1, pos2, mat2, size2); return mjraw_CapsuleBox(con, margin, pos1, mat1, size1, pos2, mat2, size2);
} }
// internal box : box // internal box : box
static inline 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* 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* pos2 = D->geom_xpos + 3 * g2;
const mjtNum* mat1 = D->geom_xmat + 9 * g1;
const mjtNum* mat2 = D->geom_xmat + 9 * g2; const mjtNum* mat2 = D->geom_xmat + 9 * g2;
const mjtNum* size1 = M->geom_size + 3 * g1;
const mjtNum* size2 = M->geom_size + 3 * g2; 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], 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]) if (mju_abs(c2) > ss[1 - q])
continue; continue;
mji_copy3(points[n], lines[i]); if (n < mjMAXCONPAIR) {
mji_addToScl3(points[n++], lines[i] + 3, c1); 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) if (u <= 0 || v <= 0 || u >= 1 || v >= 1)
continue; continue;
points[n][0] = llx; if (n < mjMAXCONPAIR) {
points[n][1] = lly; points[n][0] = llx;
points[n][2] = (pts[0][2] + u * pts[1][2] + v * pts[2][2]); points[n][1] = lly;
n++; 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) if (tmp1[1] <= -ly || tmp1[1] >= ly)
continue; 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[1] = (q2 ? -1 : 1) * r[5];
tmp2[2] = (q2 ? -1 : 1) * r[8]; tmp2[2] = (q2 ? -1 : 1) * r[8];
mji_copy3(con[0].frame, tmp2); mji_copy3(con[0].normal, tmp2);
mju_zero3(con[0].frame + 3); mji_zero3(con[0].tangent);
for (i = 0; i < n; i++) for (i = 0; i < n; i++) {
{
con[i].dist = 2 * points[i][2]; con[i].dist = 2 * points[i][2];
points[i][2] += hz; points[i][2] += hz;
mji_mulMatVec3(tmp2, r, points[i]); mji_mulMatVec3(tmp2, r, points[i]);
mji_add3(con[i].pos, tmp2, p); mji_add3(con[i].pos, tmp2, p);
if (i) if (i) {
mji_copy6(con[i].frame, con[0].frame); mji_copy3(con[i].normal, con[0].normal);
mji_zero3(con[i].tangent);
}
} }
@@ -1315,8 +1324,8 @@ edgeedge:
mji_mulMatVec3(tmp1, r, rnorm); mji_mulMatVec3(tmp1, r, rnorm);
mji_scl3(con[0].frame, tmp1, in ? -1 : 1); mji_scl3(con[0].normal, tmp1, in ? -1 : 1);
mju_zero3(con[0].frame + 3); mji_zero3(con[0].tangent);
for (i = 0; i < n; i++) { for (i = 0; i < n; i++) {
@@ -1327,7 +1336,8 @@ edgeedge:
mji_add3(con[i].pos, tmp2, pos1); 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; return n;
@@ -1338,13 +1348,13 @@ edgeedge:
// box : box // box : box
int mjc_BoxBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { int mjc_BoxBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) {
int num = _boxbox(m, d, con, g1, g2, 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 // get box info
const mjtNum* pos1 = d->geom_xpos + 3 * g1; 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; static mjtNum kRemoveRatio = 1.01;
// is the contact outside: 1, inside: -1, within the removal width: 0 // is the contact outside: 1, inside: -1, within the removal width: 0
int out1 = mju_outsideBox(con[i].pos, pos1, mat1, sz1, kRemoveRatio); int out1 = mju_outsideBox(tmp[i].pos, pos1, mat1, sz1, kRemoveRatio);
int out2 = mju_outsideBox(con[i].pos, pos2, mat2, sz2, 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 // mark as bad if outside one box and not inside the other box
if ((out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1)) { if ((out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1)) {
con[i].dim = -1; dupe[i] = -1;
} }
} }
// find duplicates // find duplicates
for (int i=0; i < num-1; i++) { for (int i=0; i < num-1; i++) {
if (con[i].dim == -1) { if (dupe[i] == -1) {
continue; // already marked bad: skip continue; // already marked bad: skip
} }
for (int j=i+1; j < num; j++) { for (int j=i+1; j < num; j++) {
if (con[j].dim == -1) { if (dupe[j] == -1) {
continue; // already marked bad: skip continue; // already marked bad: skip
} }
if (con[i].pos[0] == con[j].pos[0] && if (tmp[i].pos[0] == tmp[j].pos[0] &&
con[i].pos[1] == con[j].pos[1] && tmp[i].pos[1] == tmp[j].pos[1] &&
con[i].pos[2] == con[j].pos[2]) { tmp[i].pos[2] == tmp[j].pos[2]) {
con[i].dim = -1; dupe[i] = -1;
break; break;
} }
} }
@@ -1394,14 +1404,8 @@ int mjc_BoxBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN
// consolidate good // consolidate good
int i = 0; int i = 0;
for (int j=0; j < num; j++) { for (int j=0; j < num; j++) {
// good: maybe copy if (dupe[j] == 0) {
if (con[j].dim == 0) { con[i] = tmp[j];
// different: copy
if (i < j) {
con[i] = con[j];
}
// advance either way
i++; i++;
} }
} }
src/engine/engine_collision_convex.c
+117 -107
View File
@@ -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 // wrapper around libccd; returns number of collisions found
static inline int _libccd_wrapper(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2, static inline int _libccd_wrapper(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2,
mjContact* con, mjtNum margin) { mjPreContact* con, mjtNum margin) {
ccd_t ccd; ccd_t ccd;
CCD_INIT(&ccd); CCD_INIT(&ccd);
ccd.mpr_tolerance = m->opt.ccd_tolerance; 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)) { if (ccdVec3Eq(&ccd_dir, ccd_vec3_origin)) {
return 0; return 0;
} }
con->dist = margin - ccd_depth; con[0].dist = margin - ccd_depth;
mji_copy3(con->frame, ccd_dir.v); mji_copy3(con[0].normal, ccd_dir.v);
mji_copy3(con->pos, ccd_pos.v); mji_copy3(con[0].pos, ccd_pos.v);
mji_zero3(con->frame + 3); mji_zero3(con[0].tangent);
return 1; return 1;
} }
return 0; 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 // find penetration info between two geoms; returns number of collisions found
static int mjc_penetration(const mjModel* m, mjData* d, mjCCDObj* obj1, mjCCDObj* obj2, 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)) { if (mjDISABLED(mjDSBL_NATIVECCD)) {
return _libccd_wrapper(m, obj1, obj2, con, margin); 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) { if ((dist = mjc_ccd(&config, &status, obj1, obj2)) < 0) {
mj_freeStack(d); mj_freeStack(d);
int nwitness = status.nx; int nwitness = status.nx;
for (int i = 0; i < nwitness; i++, con++) { for (int i = 0; i < nwitness; i++) {
con->dist = margin + dist; con[i].dist = margin + dist;
con->pos[0] = 0.5*(status.x1[3*i + 0] + status.x2[3*i + 0]); con[i].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[i].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]); con[i].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); mji_sub3(con[i].normal, status.x1 + 3*i, status.x2 + 3*i);
mju_normalize3(con->frame); mju_normalize3(con[i].normal);
mji_zero3(con->frame + 3); mji_zero3(con[i].tangent);
} }
return nwitness; 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 // 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++) { 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; 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 // 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 // init ccd objects
mjCCDObj obj1, obj2; mjCCDObj obj1, obj2;
mjc_initCCDObj(&obj1, m, d, g1, margin); 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 // 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[g1] != mjGEOM_ELLIPSOID && m->geom_type[g1] != mjGEOM_SPHERE
&& m->geom_type[g2] != mjGEOM_ELLIPSOID && m->geom_type[g2] != mjGEOM_SPHERE) { && m->geom_type[g2] != mjGEOM_ELLIPSOID && m->geom_type[g2] != mjGEOM_SPHERE) {
// multiCCD parameters // 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 // complete frame of initial contact
mjtNum frame[9]; mjtNum frame[9];
mji_copy9(frame, con[0].frame); mji_copy3(frame, con[0].normal);
mju_zero(frame+3, 6);
mju_makeFrame(frame); mju_makeFrame(frame);
// tolerance for determining if newly found contacts are distinct // tolerance for determining if newly found contacts are distinct
@@ -926,7 +928,7 @@ const int maxplanemesh = 3;
const mjtNum tolplanemesh = 0.3; const mjtNum tolplanemesh = 0.3;
// add one plane-mesh contact // 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 pos1[3], const mjtNum normal1[3],
const mjtNum pos2[3], const mjtNum mat2[9], const mjtNum pos2[3], const mjtNum mat2[9],
const mjtNum first[3], mjtNum rbound) { const mjtNum first[3], mjtNum rbound) {
@@ -945,44 +947,47 @@ static int addplanemesh(mjContact* con, const float vertex[3],
mji_sub3(dif, pnt, pos1); mji_sub3(dif, pnt, pos1);
// set distance // set distance
con->dist = mju_dot3(normal1, dif); con[0].dist = mju_dot3(normal1, dif);
// set position // set position
mji_copy3(con->pos, pnt); mji_copy3(con[0].pos, pnt);
mji_addToScl3(con->pos, normal1, -0.5*con->dist); mji_addToScl3(con[0].pos, normal1, -0.5*con[0].dist);
// set frame // set frame
mji_copy3(con->frame, normal1); mji_copy3(con[0].normal, normal1);
mju_zero3(con->frame+3); mji_zero3(con[0].tangent);
return 1; return 1;
} }
// plane-convex collision, using libccd // plane-convex collision, using libccd
int mjc_PlaneConvex(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) {
mjGETINFO const mjtNum* pos1 = d->geom_xpos + 3*g1;
mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]}; const mjtNum* mat1 = d->geom_xmat + 9*g1;
ccd_vec3_t dir, vec; 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; mjCCDObj obj;
mjc_initCCDObj(&obj, m, d, g2, 0); mjc_initCCDObj(&obj, m, d, g2, 0);
// get support point in -normal direction // get support point in -normal direction
ccdVec3Set(&dir, -mat1[2], -mat1[5], -mat1[8]); ccdVec3Set(&ccd_dir, -mat1[2], -mat1[5], -mat1[8]);
mjccd_support(&obj, &dir, &vec); mjccd_support(&obj, &ccd_dir, &ccd_vec);
// compute normal distance, return if too far // compute normal distance, return if too far
mji_sub3(dif, vec.v, pos1); mji_sub3(dif, ccd_vec.v, pos1);
dist = mju_dot3(normal, dif); con[0].dist = mju_dot3(normal, dif);
if (dist > margin) { if (con[0].dist > margin) {
return 0; return 0;
} }
// fill in contact data // fill in contact data
con->dist = dist; mji_copy3(con[0].pos, ccd_vec.v);
mji_copy3(con->pos, vec.v); mji_addToScl3(con[0].pos, normal, -0.5*con[0].dist);
mji_addToScl3(con->pos, normal, -0.5*dist); mji_copy3(con[0].normal, normal);
mji_copy3(con->frame, normal); mji_zero3(con[0].tangent);
mju_zero3(con->frame+3);
//--------------- add all/connected vertices below margin //--------------- add all/connected vertices below margin
float* vertdata; 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 // express dir in geom local frame
mjtNum locdir[3]; 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 // inclusion threshold along locdir, relative to geom2 center
mji_sub3(dif, pos2, pos1); 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 // 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 // hfield frame
const mjtNum* pos1 = d->geom_xpos + 3*g1; const mjtNum* pos1 = d->geom_xpos + 3*g1;
const mjtNum* mat1 = d->geom_xmat + 9*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 // try early return using box-sphere test
// express geom2 pos in hfield frame // express geom2 pos in hfield frame
mjtNum pos[3] = {pos2[0] - pos1[0], pos2[1] - pos1[1], pos2[2] - pos1[2]}; mjtNum local_pos[3] = {pos2[0] - pos1[0], pos2[1] - pos1[1], pos2[2] - pos1[2]};
mju_mulMatTVec3(pos, mat1, pos); mju_mulMatTVec3(local_pos, mat1, local_pos);
// sphere radius is geom2 rbound + margin // sphere radius is geom2 rbound + margin
mjtNum radius = m->geom_rbound[g2] + margin; mjtNum radius = m->geom_rbound[g2] + margin;
// box-sphere test // box-sphere test
if ((size0 < pos[0] - radius) || (-size0 > pos[0] + radius) || if ((size0 < local_pos[0] - radius) || (-size0 > local_pos[0] + radius) ||
(size1 < pos[1] - radius) || (-size1 > pos[1] + radius) || (size1 < local_pos[1] - radius) || (-size1 > local_pos[1] + radius) ||
(size2 < pos[2] - radius) || (-size3 > pos[2] + radius)) { (size2 < local_pos[2] - radius) || (-size3 > local_pos[2] + radius)) {
return 0; 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_mulMatTMat3(mat, mat1, mat2);
mji_copy9(obj2.mat, mat); 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 // get support point in +X
dir[0] = 1; local_dir[0] = 1;
obj2.support(res, &obj2, dir); obj2.support(res, &obj2, local_dir);
mjtNum xmax = res[0]; mjtNum xmax = res[0];
// get support point in -X // get support point in -X
dir[0] = -1; local_dir[0] = -1;
obj2.support(res, &obj2, dir); obj2.support(res, &obj2, local_dir);
mjtNum xmin = res[0]; mjtNum xmin = res[0];
dir[0] = 0; local_dir[0] = 0;
// get support point in +Y // get support point in +Y
dir[1] = 1; local_dir[1] = 1;
obj2.support(res, &obj2, dir); obj2.support(res, &obj2, local_dir);
mjtNum ymax = res[1]; mjtNum ymax = res[1];
// get support point in -Y // get support point in -Y
dir[1] = -1; local_dir[1] = -1;
obj2.support(res, &obj2, dir); obj2.support(res, &obj2, local_dir);
mjtNum ymin = res[1]; mjtNum ymin = res[1];
dir[1] = 0; local_dir[1] = 0;
// get support point in +Z // get support point in +Z
dir[2] = 1; local_dir[2] = 1;
obj2.support(res, &obj2, dir); obj2.support(res, &obj2, local_dir);
mjtNum zmax = res[2]; mjtNum zmax = res[2];
// get support point in -Z // get support point in -Z
dir[2] = -1; local_dir[2] = -1;
obj2.support(res, &obj2, dir); obj2.support(res, &obj2, local_dir);
mjtNum zmin = res[2]; mjtNum zmin = res[2];
// AABB box-box test // 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 // run penetration function, save contact
if (mjc_penetration(m, d, &obj1, &obj2, con + ncon, 1, 0.0)) { if (mjc_penetration(m, d, &obj1, &obj2, con + ncon, 1, 0.0)) {
// transform to global coordinates // transform to global coordinates
mji_copy3(dir, con[ncon].frame); mji_copy3(local_dir, con[ncon].normal);
mji_copy3(pos, con[ncon].pos); mji_copy3(local_pos, con[ncon].pos);
mji_mulMatVec3(con[ncon].frame, mat1, dir); mji_mulMatVec3(con[ncon].normal, mat1, local_dir);
mji_mulMatVec3(con[ncon].pos, mat1, pos); mji_mulMatVec3(con[ncon].pos, mat1, local_pos);
mji_addTo3(con[ncon].pos, pos1); mji_addTo3(con[ncon].pos, pos1);
// force out of all loops if max contacts reached // 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)) { if (mjDISABLED(mjDSBL_NATIVECCD)) {
// fix contact normals // fix contact normals
for (int i=0; i < ncon; i++) { 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 // 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; mjtNum dst1, dst2;
// get geom ids and types // 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 // init normals
mjtNum normal[2][3] = { mjtNum normal[2][3] = {
{con->frame[0], con->frame[1], con->frame[2]}, {con->normal[0], con->normal[1], con->normal[2]},
{-con->frame[0], -con->frame[1], -con->frame[2]} {-con->normal[0], -con->normal[1], -con->normal[2]}
}; };
// process geoms in type range // 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]; mjtNum* size = m->geom_size + 3*gid[i];
// map contact point and normal to local frame // 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_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]); mju_mulMatTVec3(nrm, mat, normal[i]);
// process according to type // process according to type
switch (type[i]) { switch (type[i]) {
case mjGEOM_SPHERE: case mjGEOM_SPHERE:
mji_copy3(nrm, pos); mji_copy3(nrm, pos1);
processed[i] = 1; processed[i] = 1;
break; break;
case mjGEOM_CAPSULE: case mjGEOM_CAPSULE:
// Z: bottom cap // Z: bottom cap
if (pos[2] < -size[1]) { if (pos1[2] < -size[1]) {
nrm[2] = pos[2]+size[1]; nrm[2] = pos1[2]+size[1];
} }
// Z: top cap // Z: top cap
else if (pos[2] > size[1]) { else if (pos1[2] > size[1]) {
nrm[2] = pos[2]-size[1]; nrm[2] = pos1[2]-size[1];
} }
// Z: cylinder // Z: cylinder
@@ -1431,8 +1437,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
} }
// copy XY // copy XY
nrm[0] = pos[0]; nrm[0] = pos1[0];
nrm[1] = pos[1]; nrm[1] = pos1[1];
processed[i] = 1; processed[i] = 1;
break; break;
@@ -1443,27 +1449,27 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
} }
// compute elliptic distance^2 // compute elliptic distance^2
dst1 = pos[0]*pos[0]/(size[0]*size[0]) + dst1 = pos1[0]*pos1[0]/(size[0]*size[0]) +
pos[1]*pos[1]/(size[1]*size[1]) + pos1[1]*pos1[1]/(size[1]*size[1]) +
pos[2]*pos[2]/(size[2]*size[2]); pos1[2]*pos1[2]/(size[2]*size[2]);
// dispatch to inside or outside solver // dispatch to inside or outside solver
if (dst1 <= 1) { if (dst1 <= 1) {
processed[i] = mjc_ellipsoidInside(nrm, pos, size); processed[i] = mjc_ellipsoidInside(nrm, pos1, size);
} else { } else {
processed[i] = mjc_ellipsoidOutside(nrm, pos, size); processed[i] = mjc_ellipsoidOutside(nrm, pos1, size);
} }
break; break;
case mjGEOM_CYLINDER: case mjGEOM_CYLINDER:
// skip if within 5% length of flat wall // 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; break;
} }
// compute distances to flat and round wall // compute distances to flat and round wall
dst1 = mju_abs(size[1]-mju_abs(pos[2])); dst1 = mju_abs(size[1]-mju_abs(pos1[2]));
dst2 = mju_abs(size[0]-mju_norm(pos, 2)); dst2 = mju_abs(size[0]-mju_norm(pos1, 2));
// require 4x closer to round than flat wall // require 4x closer to round than flat wall
if (dst1 < 0.25*dst2) { 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 // set normal for round wall
nrm[0] = pos[0]; nrm[0] = pos1[0];
nrm[1] = pos[1]; nrm[1] = pos1[1];
nrm[2] = 0; nrm[2] = 0;
processed[i] = 1; processed[i] = 1;
break; break;
@@ -1492,23 +1498,18 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
// both processed: average // both processed: average
if (processed[0] && processed[1]) { if (processed[0] && processed[1]) {
mji_sub3(con->frame, normal[0], normal[1]); mji_sub3(con->normal, normal[0], normal[1]);
mju_normalize3(con->frame); mju_normalize3(con->normal);
} }
// first processed: copy // first processed: copy
else if (processed[0]) { else if (processed[0]) {
mji_copy3(con->frame, normal[0]); mji_copy3(con->normal, normal[0]);
} }
// second processed: copy reverse // second processed: copy reverse
else if (processed[1]) { else if (processed[1]) {
mji_scl3(con->frame, normal[1], -1); mji_scl3(con->normal, normal[1], -1);
}
// clear second frame axis if processed, just in case
if (processed[0] || processed[1]) {
mju_zero3(con->frame+3);
} }
} }
@@ -1525,7 +1526,8 @@ int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1,
mjc_setCCDObjFlex(&obj2, f2, e2, -1); mjc_setCCDObjFlex(&obj2, f2, e2, -1);
// find contacts // 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 // fix normals for 2D flex
if (ncon && !mjDISABLED(mjDSBL_NATIVECCD)) { 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) { if (isflex2d) {
for (int i = 0; i < ncon; i++) { 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; 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 // 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 // transform to global coordinates
mjtNum dir[3], pos[3]; mji_zero3(ccon->frame + 3);
mji_copy3(dir, con[cnt].frame); mji_mulMatVec3(ccon->frame, hmat, precon.normal);
mji_copy3(pos, con[cnt].pos); mji_mulMatVec3(ccon->pos, hmat, precon.pos);
mji_mulMatVec3(con[cnt].frame, hmat, dir); mji_addTo3(ccon->pos, hpos);
mji_mulMatVec3(con[cnt].pos, hmat, pos);
mji_addTo3(con[cnt].pos, hpos);
// count, stop if max number reached // count, stop if max number reached
cnt++; cnt++;
src/engine/engine_collision_convex.h
+4 -4
View File
@@ -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]); void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]);
// pairwise geom collision functions using ccd // pairwise geom collision functions using ccd
int mjc_PlaneConvex(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, mjContact* 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, mjContact* 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 // 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, 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); mjtNum margin);
// fix contact frame normal // 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 #ifdef __cplusplus
} }
src/engine/engine_collision_driver.c
+52 -29
View File
@@ -1745,20 +1745,11 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) {
return; 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 // call collision detector to generate contacts
num = collisionFunc(m, d, con, g1, g2, margin + gap); mjPreContact precon[mjMAXCONPAIR];
if (!(num = collisionFunc(m, d, precon, g1, g2, margin + gap))) {
// check contacts
if (!num) {
resetArena(d);
return; 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"); 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 // set condim, solref, solimp, friction: dynamic
if (ipair < 0) { if (ipair < 0) {
mj_contactParam(m, &condim, solref, solimp, friction, g1, g2, -1, -1); 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 // add contacts returned by collision detector
for (int i=0; i < num; i++) { 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[0] = g1;
con[i].geom[1] = g2; con[i].geom[1] = g2;
con[i].flex[0] = -1; 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); pos, mat, size);
// call raw primitive for corresponding geom type // call raw primitive for corresponding geom type
if (type == mjGEOM_SPHERE) { mjPreContact precon[2];
num = mjraw_SphereCapsule(con, margin + gap, switch (type) {
d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, case mjGEOM_SPHERE:
pos, mat, size); 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, for (int i=0; i < num; i++) {
d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, con[i].dist = precon[i].dist;
pos, mat, size); mju_copy3(con[i].pos, precon[i].pos);
} mju_copy3(con[i].frame + 0, precon[i].normal);
else { mju_copy3(con[i].frame + 3, precon[i].tangent);
num = mjraw_CapsuleBox(con, margin + gap,
pos, mat, size,
d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g);
// reverse contact normals, since box geom is second // 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); 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); pos2, mat2, size2);
// raw primitive // 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 // 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 pos[3], mat[9], size[2];
mjtNum I[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; mjtNum I[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
mj_makeCapsule(m, d, f, edata, pos, mat, size); 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 // sphere : triangle
src/engine/engine_collision_primitive.c
+106 -57
View File
@@ -25,70 +25,87 @@
//--------------------------- plane collisions ----------------------------------------------------- //--------------------------- plane collisions -----------------------------------------------------
// raw plane : sphere // 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* 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) {
// set normal // set normal
con[0].frame[0] = mat1[2]; con[0].normal[0] = mat1[2];
con[0].frame[1] = mat1[5]; con[0].normal[1] = mat1[5];
con[0].frame[2] = mat1[8]; con[0].normal[2] = mat1[8];
// compute distance, return if too large // compute distance, return if too large
mjtNum tmp[3] = {pos2[0] - pos1[0], pos2[1] - pos1[1], pos2[2] - pos1[2]}; 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]) { if (cdist > margin + size2[0]) {
return 0; return 0;
} }
// depth and position // depth and position
con[0].dist = cdist - size2[0]; 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); mji_add3(con[0].pos, pos2, tmp);
mji_zero3(con[0].tangent);
mju_zero3(con[0].frame+3);
return 1; return 1;
} }
// plane : sphere // plane : sphere
int mjc_PlaneSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { int mjc_PlaneSphere(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
mjGETINFO 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); return mjraw_PlaneSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
} }
// plane : capsule // plane : capsule
int mjc_PlaneCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { int mjc_PlaneCapsule(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
mjGETINFO 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 // get capsule axis, segment = scaled axis
mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]}; mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]};
mjtNum segment[3] = {size2[1]*axis[0], size2[1]*axis[1], size2[1]*axis[2]}; mjtNum segment[3] = {size2[1]*axis[0], size2[1]*axis[1], size2[1]*axis[2]};
// get point 1, do sphere-plane test // get point 1, do sphere-plane test
mjtNum pos[3]; mjtNum endpoint[3];
mju_add3(pos, pos2, segment); mju_add3(endpoint, pos2, segment);
int n1 = mjraw_PlaneSphere(con, margin, pos1, mat1, size1, pos, mat2, size2); int n1 = mjraw_PlaneSphere(con, margin, pos1, mat1, size1, endpoint, mat2, size2);
// get point 2, do sphere-plane test // get point 2, do sphere-plane test
mju_sub3(pos, pos2, segment); mju_sub3(endpoint, pos2, segment);
int n2 = mjraw_PlaneSphere(con+n1, margin, pos1, mat1, size1, pos, mat2, size2); int n2 = mjraw_PlaneSphere(con + n1, margin, pos1, mat1, size1, endpoint, mat2, size2);
// align contact frames with capsule axis // align contact frames with capsule axis
if (n1) { if (n1) {
mji_copy3(con->frame + 3, axis); mji_copy3(con[0].tangent, axis);
} }
if (n2) { if (n2) {
mji_copy3((con + n1)->frame + 3, axis); mji_copy3(con[n1].tangent, axis);
} }
return n1+n2; return n1 + n2;
} }
// plane : cylinder // plane : cylinder
int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
mjGETINFO 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 normal[3] = {mat1[2], mat1[5], mat1[8]};
mjtNum axis[3] = {mat2[2], mat2[5], mat2[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_add3(con[cnt].pos, pos2, vec);
mji_addTo3(con[cnt].pos, axis); mji_addTo3(con[cnt].pos, axis);
mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5); mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5);
mji_copy3(con[cnt].frame, normal); mji_copy3(con[cnt].normal, normal);
mju_zero3(con[cnt].frame+3); mji_zero3(con[cnt].tangent);
cnt++; cnt++;
} else { } else {
return 0; // nearest point is above margin: no contacts 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_add3(con[cnt].pos, pos2, vec);
mji_subFrom3(con[cnt].pos, axis); mji_subFrom3(con[cnt].pos, axis);
mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5); mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5);
mji_copy3(con[cnt].frame, normal); mji_copy3(con[cnt].normal, normal);
mju_zero3(con[cnt].frame+3); mji_zero3(con[cnt].tangent);
cnt++; 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_addTo3(con[cnt].pos, axis);
mji_addToScl3(con[cnt].pos, vec, -0.5); mji_addToScl3(con[cnt].pos, vec, -0.5);
mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5); mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5);
mji_copy3(con[cnt].frame, normal); mji_copy3(con[cnt].normal, normal);
mju_zero3(con[cnt].frame+3); mji_zero3(con[cnt].tangent);
cnt++; cnt++;
// add point B // 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_addTo3(con[cnt].pos, axis);
mji_addToScl3(con[cnt].pos, vec, -0.5); mji_addToScl3(con[cnt].pos, vec, -0.5);
mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5); mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5);
mji_copy3(con[cnt].frame, normal); mji_copy3(con[cnt].normal, normal);
mju_zero3(con[cnt].frame+3); mji_zero3(con[cnt].tangent);
cnt++; cnt++;
} }
@@ -190,8 +207,12 @@ int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g
// plane : box // plane : box
int mjc_PlaneBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { int mjc_PlaneBox(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* size2 = m->geom_size + 3*g2;
// get normal, difference between centers, normal distance // get normal, difference between centers, normal distance
mjtNum norm[3] = {mat1[2], mat1[5], mat1[8]}; 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 // construct contact
con[cnt].dist = dist + ldist; con[cnt].dist = dist + ldist;
mji_copy3(con[cnt].frame, norm); mji_copy3(con[cnt].normal, norm);
mju_zero3(con[cnt].frame+3);
mji_addTo3(corner, pos2); mji_addTo3(corner, pos2);
mji_scl3(vec, norm, -con[cnt].dist / 2); mji_scl3(vec, norm, -con[cnt].dist / 2);
mji_add3(con[cnt].pos, corner, vec); mji_add3(con[cnt].pos, corner, vec);
mji_zero3(con[cnt].tangent);
// count; max is 4 // count; max is 4
if (++cnt >= 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 and capsule collisions ----------------------------------------
// sphere : sphere (actual implementation, can be called with modified parameters) // 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* 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) {
// check bounding spheres (this is called from other functions) // check bounding spheres (this is called from other functions)
@@ -251,36 +272,45 @@ static int mjraw_SphereSphere(mjContact* con, mjtNum margin,
// depth and normal // depth and normal
con[0].dist = mju_sqrt(cdist_sqr) - size1[0] - size2[0]; con[0].dist = mju_sqrt(cdist_sqr) - size1[0] - size2[0];
mji_sub3(con[0].frame, pos2, pos1); mji_sub3(con[0].normal, pos2, pos1);
mjtNum len = mju_normalize3(con[0].frame); mjtNum len = mju_normalize3(con[0].normal);
// if centers are the same, norm = cross-product of z axes // if centers are the same, norm = cross-product of z axes
// if z axes are parallel, norm = [1;0;0] // if z axes are parallel, norm = [1;0;0]
if (len < mjMINVAL) { if (len < mjMINVAL) {
mjtNum axis1[3] = {mat1[2], mat1[5], mat1[8]}; mjtNum axis1[3] = {mat1[2], mat1[5], mat1[8]};
mjtNum axis2[3] = {mat2[2], mat2[5], mat2[8]}; mjtNum axis2[3] = {mat2[2], mat2[5], mat2[8]};
mji_cross(con[0].frame, axis1, axis2); mji_cross(con[0].normal, axis1, axis2);
mju_normalize3(con[0].frame); mju_normalize3(con[0].normal);
} }
// position // 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); mji_addTo3(con[0].pos, pos1);
mju_zero3(con[0].frame+3); // axis
mji_zero3(con[0].tangent);
return 1; return 1;
} }
// sphere : sphere // sphere : sphere
int mjc_SphereSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { int mjc_SphereSphere(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
mjGETINFO 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); return mjraw_SphereSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
} }
// raw sphere : capsule // 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* 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) {
// get capsule length and axis // get capsule length and axis
@@ -299,16 +329,27 @@ int mjraw_SphereCapsule(mjContact* con, mjtNum margin,
// sphere : capsule // sphere : capsule
int mjc_SphereCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { int mjc_SphereCapsule(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
mjGETINFO 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); return mjraw_SphereCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2);
} }
// sphere : cylinder // 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) { 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 // get cylinder sizes and axis
mjtNum radius = size2[0]; mjtNum radius = size2[0];
@@ -359,9 +400,10 @@ int mjc_SphereCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int
mat_cap = flipmat; mat_cap = flipmat;
} }
int ncon = mjraw_PlaneSphere(con, margin, pos_cap, mat_cap, size2, pos1, mat1, size1); int ncon = mjraw_PlaneSphere(con, margin, pos_cap, mat_cap, size2, pos1, mat1, size1);
if (ncon) { if (ncon) {
// flip frame normal (because mjGEOM_PLANE < mjGEOM_SPHERE < mjGEOM_CYLINDER) // flip direction normal (because mjGEOM_PLANE < mjGEOM_SPHERE < mjGEOM_CYLINDER)
mju_scl3(con->frame, con->frame, -1); mju_scl3(con[0].normal, con[0].normal, -1);
} }
return ncon; return ncon;
} }
@@ -380,7 +422,7 @@ int mjc_SphereCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int
// raw capsule : capsule // 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* 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) {
// get capsule axes (scaled) and center difference // 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); x2 = mju_clip((v + mb) / mc, -1, 1);
mji_scl3(vec2, axis2, x2); mji_scl3(vec2, axis2, x2);
mji_addTo3(vec2, pos2); 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 // return if two contacts already found
if (n1+n2 >= 2) { if (n1+n2 >= 2) {
@@ -456,7 +498,7 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
mjtNum x1 = mju_clip((u - mb) / ma, -1, 1); mjtNum x1 = mju_clip((u - mb) / ma, -1, 1);
mji_scl3(vec1, axis1, x1); mji_scl3(vec1, axis1, x1);
mji_addTo3(vec1, pos1); 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 // return if two contacts already found
if (n1+n2+n3 >= 2) { if (n1+n2+n3 >= 2) {
@@ -468,7 +510,7 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
x1 = mju_clip((u + mb) / ma, -1, 1); x1 = mju_clip((u + mb) / ma, -1, 1);
mji_scl3(vec1, axis1, x1); mji_scl3(vec1, axis1, x1);
mji_addTo3(vec1, pos1); 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; return n1+n2+n3+n4;
} }
@@ -476,9 +518,14 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
// capsule : capsule // capsule : capsule
int mjc_CapsuleCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, int mjc_CapsuleCapsule(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
mjtNum margin) { 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;
return mjraw_CapsuleCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2); return mjraw_CapsuleCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2);
} }
@@ -601,6 +648,7 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin,
return 1; return 1;
} }
// box : triangle with radius // box : triangle with radius
int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos,
const mjtNum* mat, const mjtNum* size, const mjtNum* t1, 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; return cnt;
} }
// capsule : triangle with radius // capsule : triangle with radius
int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos,
const mjtNum* mat, const mjtNum* size, const mjtNum* mat, const mjtNum* size,
src/engine/engine_collision_primitive.h
+28 -39
View File
@@ -18,68 +18,57 @@
#include <mujoco/mjdata.h> #include <mujoco/mjdata.h>
#include <mujoco/mjexport.h> #include <mujoco/mjexport.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
// 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
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif
// raw collision functions (called by mjc_XXX) // raw collision functions (called by mjc_XXX)
int mjraw_SphereCapsule (mjContact* con, mjtNum margin, int mjraw_SphereCapsule(mjPreContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, const mjtNum* size1, const mjtNum* pos2, const mjtNum* mat2,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2); const mjtNum* size2);
int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin, int mjraw_CapsuleCapsule(mjPreContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, const mjtNum* size1, const mjtNum* pos2, const mjtNum* mat2,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2); const mjtNum* size2);
int mjraw_CapsuleBox (mjContact* con, mjtNum margin, int mjraw_CapsuleBox(mjPreContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, const mjtNum* size1, const mjtNum* pos2, const mjtNum* mat2,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2); const mjtNum* size2);
int mjraw_SphereTriangle(mjContact* con, mjtNum margin, int mjraw_SphereTriangle(mjContact* con, mjtNum margin, const mjtNum* s, mjtNum rs,
const mjtNum* s, mjtNum rs,
const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt); const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt);
int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, const mjtNum* mat,
const mjtNum* mat, const mjtNum* size, const mjtNum* t1, const mjtNum* size, const mjtNum* t1, const mjtNum* t2, const mjtNum* t3,
const mjtNum* t2, const mjtNum* t3, mjtNum rt); mjtNum rt);
int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, const mjtNum* mat,
const mjtNum* mat, const mjtNum* size, const mjtNum* size, const mjtNum* t1, const mjtNum* t2, const mjtNum* t3,
const mjtNum* t1, const mjtNum* t2, const mjtNum* t3,
mjtNum rt); mjtNum rt);
// plane collisions // 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); 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); 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); 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); mjtNum margin);
// sphere and capsule collisions // sphere and capsule collisions
MJAPI int mjc_SphereSphere(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_SphereCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
mjtNum margin); 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); 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); mjtNum margin);
// box collisions: from engine_collision_box.c // 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); 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); 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); mjtNum margin);
#ifdef __cplusplus #ifdef __cplusplus
src/engine/engine_collision_sdf.c
+55 -31
View File
@@ -21,7 +21,6 @@
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mjsan.h> // IWYU pragma: keep #include <mujoco/mjsan.h> // IWYU pragma: keep
#include <mujoco/mjtnum.h> #include <mujoco/mjtnum.h>
#include "engine/engine_collision_primitive.h"
#include "engine/engine_plugin.h" #include "engine/engine_plugin.h"
#include "engine/engine_util_blas.h" #include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.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 // adds candidate point to result
// flipNormal: 0 = normal points INTO SDF (for mesh-SDF where SDF is g2) // 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) // 1 = normal points OUT of SDF (for flex-SDF where SDF is g1)
static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3], static int addPreContact(mjtNum* points, mjPreContact* con, const mjtNum x[3],
const mjtNum pos2[3], const mjtNum quat2[4], mjtNum dist, const mjtNum pos2[3], const mjtNum quat2[4], mjtNum dist,
int cnt, const mjModel* m, const mjSDF* s, const mjData* d, int cnt, const mjModel* m, const mjSDF* s, const mjData* d,
int flipNormal) { int flipNormal) {
// check if there is a collision // check if there is a collision
if (dist > 0 || isknown(points, x, cnt)) { if (dist > 0 || isknown(points, x, cnt)) {
return cnt; return cnt;
@@ -570,18 +569,37 @@ static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3],
} }
// construct contact // construct contact
con[cnt].dist = dist; con->dist = dist;
mju_rotVecQuat(con[cnt].frame, norm, quat2); mju_rotVecQuat(con->normal, norm, quat2);
mju_zero3(con[cnt].frame+3); mju_scl3(vec, con->normal, - 0.5 * dist);
mju_makeFrame(con[cnt].frame); mju_rotVecQuat(con->pos, x, quat2);
mju_scl3(vec, con[cnt].frame, -con[cnt].dist/2); mju_zero3(con->tangent);
mju_rotVecQuat(con[cnt].pos, x, quat2); mju_addTo3(con->pos, pos2);
mju_addTo3(con[cnt].pos, pos2); mju_addTo3(con->pos, vec);
mju_addTo3(con[cnt].pos, vec);
return cnt+1; 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 // finds minimum of Frank-Wolfe objective
static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners, static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners,
const mjModel* m, const mjSDF* sdf, const mjData* d) { 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 functions -----------------------------------------------
// collision between a height field and a signed distance field // 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!"); mju_warning("HField vs SDF collision not yet supported!");
return 0; return 0;
} }
// collision between a mesh and a signed distance field // 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) {
mjGETINFO; 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 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; int cnt = 0, ncandidate = 0;
// get sdf plugin // 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.faceadr = m->mesh_faceadr[m->geom_dataid[g1]];
ctx.nstartpts = mju_max(1, m->opt.sdf_initpoints); ctx.nstartpts = mju_max(1, m->opt.sdf_initpoints);
ctx.candidate = candidate; ctx.candidate = candidate;
ctx.dist = dist; ctx.dist = dist2;
ctx.ncandidate = &ncandidate; ctx.ncandidate = &ncandidate;
// BVH traversal for mesh faces // 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 few candidates, add them all directly
if (ncandidate <= mjMAXCONPAIR) { if (ncandidate <= mjMAXCONPAIR) {
for (int i = 0; i < ncandidate; i++) { for (int i = 0; i < ncandidate; i++) {
cnt = addContact(points, con, candidate + 3*i, pos2, sdf_quat, cnt = addPreContact(points, con + cnt, candidate + 3*i, pos2, sdf_quat,
dist[i], cnt, m, &sdf, d, 0); dist2[i], cnt, m, &sdf, d, 0);
} }
return cnt; return cnt;
} }
// farthest point sampling (FPS) for spatial coverage // farthest point sampling (FPS) for spatial coverage
int selected_indices[mjMAXCONPAIR]; 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 // add selected contacts
for (int i = 0; i < nselected; i++) { for (int i = 0; i < nselected; i++) {
int idx = selected_indices[i]; int idx = selected_indices[i];
cnt = addContact(points, con, candidate + 3*idx, pos2, sdf_quat, cnt = addPreContact(points, con + cnt, candidate + 3*idx, pos2, sdf_quat,
dist[idx], cnt, m, &sdf, d, 0); dist2[idx], cnt, m, &sdf, d, 0);
} }
return cnt; return cnt;
} }
// collision between two SDFs // collision between two SDFs
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) {
mjGETINFO; const mjtNum* pos1 = d->geom_xpos + 3*g1;
size1 = m->geom_aabb + 6*g1; const mjtNum* mat1 = d->geom_xmat + 9*g1;
size2 = m->geom_aabb + 6*g2; 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; 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 aabb1[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
mjtNum aabb2[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}; 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 // gradient descent - we use a special function of the two SDF as objective
sdf.type = mjSDFTYPE_COLLISION; 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 // inexact SDFs can yield spurious collisions, filter them by projecting on the midsurface
sdf.type = mjSDFTYPE_INTERSECTION; 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 // contact point and normal - we use the midsurface where SDF1=SDF2 as zero level set
sdf.type = mjSDFTYPE_MIDSURFACE; 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 // SHOULD NOT OCCUR
if (cnt > mjMAXCONPAIR) { if (cnt > mjMAXCONPAIR) {
src/engine/engine_collision_sdf.h
+3 -3
View File
@@ -36,13 +36,13 @@ MJAPI void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s, mjtNu
const mjtNum x[3]); const mjtNum x[3]);
// collision between a height field and a signed distance field // 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 // 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 // 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) // collision between entire flex and SDF geom (batched processing)
int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con, int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con,
src/engine/engine_support.c
+3 -3
View File
@@ -553,7 +553,7 @@ static mjtNum mj_geomDistanceCCD(const mjModel* m, mjData* d, int g1, int g2,
// returns the smallest distance between two geoms // returns the smallest distance between two geoms
mjtNum mj_geomDistance(const mjModel* m, mjData* d, int geom1, int geom2, mjtNum distmax, mjtNum mj_geomDistance(const mjModel* m, mjData* d, int geom1, int geom2, mjtNum distmax,
mjtNum fromto[6]) { mjtNum fromto[6]) {
mjContact con[mjMAXCONPAIR]; mjPreContact con[mjMAXCONPAIR];
mjtNum dist = distmax; mjtNum dist = distmax;
if (fromto) mju_zero(fromto, 6); 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 // write fromto if given and a collision has been found
if (fromto && smallest >= 0) { if (fromto && smallest >= 0) {
mjtNum sign = flip ? -1 : 1; mjtNum sign = flip ? -1 : 1;
mju_addScl3(fromto+0, 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].frame, 0.5*sign*dist); mju_addScl3(fromto+3, con[smallest].pos, con[smallest].normal, 0.5*sign*dist);
} }
return dist; return dist;
src/engine/engine_util_spatial.c
+1 -1
View File
@@ -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]) { void mju_makeFrame(mjtNum frame[9]) {
mjtNum tmp[3]; mjtNum tmp[3];
test/engine/engine_collision_box_test.cc
+35 -38
View File
@@ -15,6 +15,7 @@
// Tests for engine/engine_collision_box.c. // Tests for engine/engine_collision_box.c.
#include <string> #include <string>
#include <vector>
#include <gmock/gmock.h> #include <gmock/gmock.h>
#include <gtest/gtest.h> #include <gtest/gtest.h>
@@ -45,11 +46,9 @@ TEST_F(MjCollisionBoxTest, BadContacts) {
mj_forward(model, data); mj_forward(model, data);
// allocate contact array and matching arrays // allocate contact array and matching arrays
mj_markStack(data); std::vector<mjPreContact> precon(mjMAXCONPAIR);
mjContact* con_raw = (mjContact*) mj_stackAllocByte( std::vector<int> match_raw(mjMAXCONPAIR);
data, mjMAXCONPAIR * sizeof(mjContact), alignof(mjContact)); std::vector<int> match(data->ncon);
int* match_raw = mj_stackAllocInt(data, mjMAXCONPAIR);
int* match = mj_stackAllocInt(data, data->ncon);
int g1 = -1; int g1 = -1;
int g2 = -1; int g2 = -1;
@@ -73,20 +72,21 @@ TEST_F(MjCollisionBoxTest, BadContacts) {
g2 = g2new; g2 = g2new;
// call low-level box-box collider // 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 // allocate and clear arrays marking already matched contacts
mju_zeroInt(match_raw, num); mju_zeroInt(match_raw.data(), num);
mju_zeroInt(match, data->ncon); mju_zeroInt(match.data(), data->ncon);
// loop over raw contacts, match with contact array using pos // loop over raw contacts, match with contact array using pos
int nmatched = 0; int nmatched = 0;
for (int i = 0; i < num; i++) { for (int i = 0; i < num; i++) {
for (int j = 0; j < data->ncon; j++) { for (int j = 0; j < data->ncon; j++) {
if (!match[j] && if (!match[j] &&
con_raw[i].pos[0] == data->contact[j].pos[0] && precon[i].pos[0] == data->contact[j].pos[0] &&
con_raw[i].pos[1] == data->contact[j].pos[1] && precon[i].pos[1] == data->contact[j].pos[1] &&
con_raw[i].pos[2] == data->contact[j].pos[2]) { precon[i].pos[2] == data->contact[j].pos[2]) {
match_raw[i] = match[j] = 1; match_raw[i] = match[j] = 1;
nmatched++; nmatched++;
} }
@@ -97,11 +97,11 @@ TEST_F(MjCollisionBoxTest, BadContacts) {
EXPECT_EQ(nmatched, num) << local_path; EXPECT_EQ(nmatched, num) << local_path;
// get box info // get box info
const mjtNum* pos1 = data->geom_xpos + 3 * g1; const mjtNum* pos1 = data->geom_xpos + 3 * g1;
const mjtNum* mat1 = data->geom_xmat + 9 * g1; const mjtNum* mat1 = data->geom_xmat + 9 * g1;
const mjtNum* size1 = model->geom_size + 3 * g1; const mjtNum* size1 = model->geom_size + 3 * g1;
const mjtNum* pos2 = data->geom_xpos + 3 * g2; const mjtNum* pos2 = data->geom_xpos + 3 * g2;
const mjtNum* mat2 = data->geom_xmat + 9 * g2; const mjtNum* mat2 = data->geom_xmat + 9 * g2;
const mjtNum* size2 = model->geom_size + 3 * g2; const mjtNum* size2 = model->geom_size + 3 * g2;
mjtNum margin = mju_max(model->geom_margin[g1], model->geom_margin[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; static mjtNum kRatio = 1.01;
// is the contact outside: 1, inside: -1, within the removal width: 0 // 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 out1 = mju_outsideBox(precon[i].pos, pos1, mat1, sz1,
int out2 = mju_outsideBox(con_raw[i].pos, pos2, mat2, sz2, kRatio); 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 // mark as bad if outside one box and not inside the other box
bool outside = (out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1); 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_deleteData(data);
mj_deleteModel(model); mj_deleteModel(model);
} }
@@ -146,11 +147,9 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) {
mj_forward(model, data); mj_forward(model, data);
// allocate contact array and matching arrays // allocate contact array and matching arrays
mj_markStack(data); std::vector<mjPreContact> precon(mjMAXCONPAIR);
mjContact* con_raw = (mjContact*) mj_stackAllocByte( std::vector<int> match_raw(mjMAXCONPAIR);
data, mjMAXCONPAIR * sizeof(mjContact), alignof(mjContact)); std::vector<int> match(data->ncon);
int* match_raw = mj_stackAllocInt(data, mjMAXCONPAIR);
int* match = mj_stackAllocInt(data, data->ncon);
int g1 = -1; int g1 = -1;
@@ -175,20 +174,21 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) {
g2 = g2new; g2 = g2new;
// call low-level box-box collider // 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 // allocate and clear arrays marking already matched contacts
mju_zeroInt(match_raw, num); mju_zeroInt(match_raw.data(), num);
mju_zeroInt(match, data->ncon); mju_zeroInt(match.data(), data->ncon);
// loop over raw contacts, match with contact array using pos // loop over raw contacts, match with contact array using pos
int nmatched = 0; int nmatched = 0;
for (int i = 0; i < num; i++) { for (int i = 0; i < num; i++) {
for (int j = 0; j < data->ncon; j++) { for (int j = 0; j < data->ncon; j++) {
if (!match[j] && if (!match[j] &&
con_raw[i].pos[0] == data->contact[j].pos[0] && precon[i].pos[0] == data->contact[j].pos[0] &&
con_raw[i].pos[1] == data->contact[j].pos[1] && precon[i].pos[1] == data->contact[j].pos[1] &&
con_raw[i].pos[2] == data->contact[j].pos[2]) { precon[i].pos[2] == data->contact[j].pos[2]) {
match_raw[i] = match[j] = 1; match_raw[i] = match[j] = 1;
nmatched++; nmatched++;
} }
@@ -207,9 +207,9 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) {
if (duplicate || i == j) { if (duplicate || i == j) {
continue; continue;
} }
if (con_raw[i].pos[0] == con_raw[j].pos[0] && if (precon[i].pos[0] == precon[j].pos[0] &&
con_raw[i].pos[1] == con_raw[j].pos[1] && precon[i].pos[1] == precon[j].pos[1] &&
con_raw[i].pos[2] == con_raw[j].pos[2]) { precon[i].pos[2] == precon[j].pos[2]) {
duplicate = true; duplicate = true;
} }
} }
@@ -220,12 +220,10 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) {
} }
} }
mj_freeStack(data);
mj_deleteData(data); mj_deleteData(data);
mj_deleteModel(model); mj_deleteModel(model);
} }
static const char* const kDeepFilePath = static const char* const kDeepFilePath =
"engine/testdata/collision_box/boxbox_deep.xml"; "engine/testdata/collision_box/boxbox_deep.xml";
@@ -273,7 +271,6 @@ TEST_F(MjCollisionBoxTest, BoxSphere) {
mj_deleteModel(model); mj_deleteModel(model);
} }
TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) { TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) {
constexpr char xml[] = R"( constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -289,12 +286,12 @@ TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) {
mjData* data = mj_makeData(model); mjData* data = mj_makeData(model);
mj_kinematics(model, data); mj_kinematics(model, data);
mjContact contact[9]; mjPreContact precon[mjMAXCONPAIR];
for (mjfCollision collision : {mjc_BoxBox, mjc_Convex}) { 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++) { 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));
} }
} }
unity/Runtime/Bindings/MjBindings.cs
+8
View File
@@ -1520,6 +1520,14 @@ public unsafe struct mjModel_ {
public UInt64 signature; 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)] [StructLayout(LayoutKind.Sequential)]
public unsafe struct mjContact_ { public unsafe struct mjContact_ {
public double dist; public double dist;
wasm/codegen/generated/bindings.cc
+87 -21
View File
@@ -766,6 +766,36 @@ struct MjOption {
bool owned_ = false; bool owned_ = false;
}; };
struct MjPreContact {
~MjPreContact();
MjPreContact();
explicit MjPreContact(mjPreContact *ptr);
MjPreContact(const MjPreContact &);
MjPreContact &operator=(const MjPreContact &);
std::unique_ptr<MjPreContact> 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 { struct MjSolverStat {
~MjSolverStat(); ~MjSolverStat();
MjSolverStat(); MjSolverStat();
@@ -7139,7 +7169,7 @@ MjContact::~MjContact() {
delete ptr_; delete ptr_;
} }
} }
MjContact::MjContact() : ptr_(new mjContact) { MjContact::MjContact() : ptr_(new mjContact()) {
owned_ = true; owned_ = true;
} }
MjContact::MjContact(const MjContact &other) : MjContact() { MjContact::MjContact(const MjContact &other) : MjContact() {
@@ -7168,7 +7198,7 @@ MjLROpt::~MjLROpt() {
delete ptr_; delete ptr_;
} }
} }
MjLROpt::MjLROpt() : ptr_(new mjLROpt) { MjLROpt::MjLROpt() : ptr_(new mjLROpt()) {
owned_ = true; owned_ = true;
mj_defaultLROpt(ptr_); mj_defaultLROpt(ptr_);
} }
@@ -7198,7 +7228,7 @@ MjOption::~MjOption() {
delete ptr_; delete ptr_;
} }
} }
MjOption::MjOption() : ptr_(new mjOption) { MjOption::MjOption() : ptr_(new mjOption()) {
owned_ = true; owned_ = true;
mj_defaultOption(ptr_); mj_defaultOption(ptr_);
} }
@@ -7222,13 +7252,42 @@ void MjOption::set(mjOption* ptr) {
ptr_ = 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> MjPreContact::copy() {
return std::make_unique<MjPreContact>(*this);
}
mjPreContact* MjPreContact::get() const {
return ptr_;
}
void MjPreContact::set(mjPreContact* ptr) {
ptr_ = ptr;
}
MjSolverStat::MjSolverStat(mjSolverStat *ptr) : ptr_(ptr) {} MjSolverStat::MjSolverStat(mjSolverStat *ptr) : ptr_(ptr) {}
MjSolverStat::~MjSolverStat() { MjSolverStat::~MjSolverStat() {
if (owned_ && ptr_) { if (owned_ && ptr_) {
delete ptr_; delete ptr_;
} }
} }
MjSolverStat::MjSolverStat() : ptr_(new mjSolverStat) { MjSolverStat::MjSolverStat() : ptr_(new mjSolverStat()) {
owned_ = true; owned_ = true;
} }
MjSolverStat::MjSolverStat(const MjSolverStat &other) : MjSolverStat() { MjSolverStat::MjSolverStat(const MjSolverStat &other) : MjSolverStat() {
@@ -7257,7 +7316,7 @@ MjStatistic::~MjStatistic() {
delete ptr_; delete ptr_;
} }
} }
MjStatistic::MjStatistic() : ptr_(new mjStatistic) { MjStatistic::MjStatistic() : ptr_(new mjStatistic()) {
owned_ = true; owned_ = true;
} }
MjStatistic::MjStatistic(const MjStatistic &other) : MjStatistic() { MjStatistic::MjStatistic(const MjStatistic &other) : MjStatistic() {
@@ -7286,7 +7345,7 @@ MjTimerStat::~MjTimerStat() {
delete ptr_; delete ptr_;
} }
} }
MjTimerStat::MjTimerStat() : ptr_(new mjTimerStat) { MjTimerStat::MjTimerStat() : ptr_(new mjTimerStat()) {
owned_ = true; owned_ = true;
} }
MjTimerStat::MjTimerStat(const MjTimerStat &other) : MjTimerStat() { MjTimerStat::MjTimerStat(const MjTimerStat &other) : MjTimerStat() {
@@ -7315,7 +7374,7 @@ MjVisualGlobal::~MjVisualGlobal() {
delete ptr_; delete ptr_;
} }
} }
MjVisualGlobal::MjVisualGlobal() : ptr_(new mjVisualGlobal) { MjVisualGlobal::MjVisualGlobal() : ptr_(new mjVisualGlobal()) {
owned_ = true; owned_ = true;
} }
MjVisualGlobal::MjVisualGlobal(const MjVisualGlobal &other) : MjVisualGlobal() { MjVisualGlobal::MjVisualGlobal(const MjVisualGlobal &other) : MjVisualGlobal() {
@@ -7344,7 +7403,7 @@ MjVisualHeadlight::~MjVisualHeadlight() {
delete ptr_; delete ptr_;
} }
} }
MjVisualHeadlight::MjVisualHeadlight() : ptr_(new mjVisualHeadlight) { MjVisualHeadlight::MjVisualHeadlight() : ptr_(new mjVisualHeadlight()) {
owned_ = true; owned_ = true;
} }
MjVisualHeadlight::MjVisualHeadlight(const MjVisualHeadlight &other) : MjVisualHeadlight() { MjVisualHeadlight::MjVisualHeadlight(const MjVisualHeadlight &other) : MjVisualHeadlight() {
@@ -7373,7 +7432,7 @@ MjVisualMap::~MjVisualMap() {
delete ptr_; delete ptr_;
} }
} }
MjVisualMap::MjVisualMap() : ptr_(new mjVisualMap) { MjVisualMap::MjVisualMap() : ptr_(new mjVisualMap()) {
owned_ = true; owned_ = true;
} }
MjVisualMap::MjVisualMap(const MjVisualMap &other) : MjVisualMap() { MjVisualMap::MjVisualMap(const MjVisualMap &other) : MjVisualMap() {
@@ -7402,7 +7461,7 @@ MjVisualQuality::~MjVisualQuality() {
delete ptr_; delete ptr_;
} }
} }
MjVisualQuality::MjVisualQuality() : ptr_(new mjVisualQuality) { MjVisualQuality::MjVisualQuality() : ptr_(new mjVisualQuality()) {
owned_ = true; owned_ = true;
} }
MjVisualQuality::MjVisualQuality(const MjVisualQuality &other) : MjVisualQuality() { MjVisualQuality::MjVisualQuality(const MjVisualQuality &other) : MjVisualQuality() {
@@ -7431,7 +7490,7 @@ MjVisualRgba::~MjVisualRgba() {
delete ptr_; delete ptr_;
} }
} }
MjVisualRgba::MjVisualRgba() : ptr_(new mjVisualRgba) { MjVisualRgba::MjVisualRgba() : ptr_(new mjVisualRgba()) {
owned_ = true; owned_ = true;
} }
MjVisualRgba::MjVisualRgba(const MjVisualRgba &other) : MjVisualRgba() { MjVisualRgba::MjVisualRgba(const MjVisualRgba &other) : MjVisualRgba() {
@@ -7460,7 +7519,7 @@ MjVisualScale::~MjVisualScale() {
delete ptr_; delete ptr_;
} }
} }
MjVisualScale::MjVisualScale() : ptr_(new mjVisualScale) { MjVisualScale::MjVisualScale() : ptr_(new mjVisualScale()) {
owned_ = true; owned_ = true;
} }
MjVisualScale::MjVisualScale(const MjVisualScale &other) : MjVisualScale() { MjVisualScale::MjVisualScale(const MjVisualScale &other) : MjVisualScale() {
@@ -7489,7 +7548,7 @@ MjWarningStat::~MjWarningStat() {
delete ptr_; delete ptr_;
} }
} }
MjWarningStat::MjWarningStat() : ptr_(new mjWarningStat) { MjWarningStat::MjWarningStat() : ptr_(new mjWarningStat()) {
owned_ = true; owned_ = true;
} }
MjWarningStat::MjWarningStat(const MjWarningStat &other) : MjWarningStat() { MjWarningStat::MjWarningStat(const MjWarningStat &other) : MjWarningStat() {
@@ -7534,7 +7593,7 @@ MjvCamera::~MjvCamera() {
delete ptr_; delete ptr_;
} }
} }
MjvCamera::MjvCamera() : ptr_(new mjvCamera) { MjvCamera::MjvCamera() : ptr_(new mjvCamera()) {
owned_ = true; owned_ = true;
mjv_defaultCamera(ptr_); mjv_defaultCamera(ptr_);
} }
@@ -7564,7 +7623,7 @@ MjvFigure::~MjvFigure() {
delete ptr_; delete ptr_;
} }
} }
MjvFigure::MjvFigure() : ptr_(new mjvFigure) { MjvFigure::MjvFigure() : ptr_(new mjvFigure()) {
owned_ = true; owned_ = true;
mjv_defaultFigure(ptr_); mjv_defaultFigure(ptr_);
} }
@@ -7594,7 +7653,7 @@ MjvGLCamera::~MjvGLCamera() {
delete ptr_; delete ptr_;
} }
} }
MjvGLCamera::MjvGLCamera() : ptr_(new mjvGLCamera) { MjvGLCamera::MjvGLCamera() : ptr_(new mjvGLCamera()) {
owned_ = true; owned_ = true;
} }
MjvGLCamera::MjvGLCamera(const MjvGLCamera &other) : MjvGLCamera() { MjvGLCamera::MjvGLCamera(const MjvGLCamera &other) : MjvGLCamera() {
@@ -7623,7 +7682,7 @@ MjvGeom::~MjvGeom() {
delete ptr_; delete ptr_;
} }
} }
MjvGeom::MjvGeom() : ptr_(new mjvGeom) { MjvGeom::MjvGeom() : ptr_(new mjvGeom()) {
owned_ = true; owned_ = true;
mjv_initGeom(ptr_, mjGEOM_NONE, nullptr, nullptr, nullptr, nullptr); mjv_initGeom(ptr_, mjGEOM_NONE, nullptr, nullptr, nullptr, nullptr);
} }
@@ -7653,7 +7712,7 @@ MjvLight::~MjvLight() {
delete ptr_; delete ptr_;
} }
} }
MjvLight::MjvLight() : ptr_(new mjvLight) { MjvLight::MjvLight() : ptr_(new mjvLight()) {
owned_ = true; owned_ = true;
} }
MjvLight::MjvLight(const MjvLight &other) : MjvLight() { MjvLight::MjvLight(const MjvLight &other) : MjvLight() {
@@ -7682,7 +7741,7 @@ MjvOption::~MjvOption() {
delete ptr_; delete ptr_;
} }
} }
MjvOption::MjvOption() : ptr_(new mjvOption) { MjvOption::MjvOption() : ptr_(new mjvOption()) {
owned_ = true; owned_ = true;
mjv_defaultOption(ptr_); mjv_defaultOption(ptr_);
} }
@@ -7712,7 +7771,7 @@ MjvPerturb::~MjvPerturb() {
delete ptr_; delete ptr_;
} }
} }
MjvPerturb::MjvPerturb() : ptr_(new mjvPerturb) { MjvPerturb::MjvPerturb() : ptr_(new mjvPerturb()) {
owned_ = true; owned_ = true;
mjv_defaultPerturb(ptr_); mjv_defaultPerturb(ptr_);
} }
@@ -7750,7 +7809,7 @@ MjVisual::~MjVisual() {
delete ptr_; 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; owned_ = true;
mj_defaultVisual(ptr_); mj_defaultVisual(ptr_);
} }
@@ -12375,6 +12434,13 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("tolerance", &MjOption::tolerance, &MjOption::set_tolerance, reference()) .property("tolerance", &MjOption::tolerance, &MjOption::set_tolerance, reference())
.property("viscosity", &MjOption::viscosity, &MjOption::set_viscosity, reference()) .property("viscosity", &MjOption::viscosity, &MjOption::set_viscosity, reference())
.property("wind", &MjOption::wind); .property("wind", &MjOption::wind);
emscripten::class_<MjPreContact>("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>("MjSolverStat") emscripten::class_<MjSolverStat>("MjSolverStat")
.constructor<>() .constructor<>()
.function("copy", &MjSolverStat::copy, take_ownership()) .function("copy", &MjSolverStat::copy, take_ownership())
wasm/codegen/generators/constants.py
+1
View File
@@ -292,6 +292,7 @@ STRUCTS_TO_BIND: list[str] = list(
NO_DEFAULT_CONSTRUCTORS: tuple[str, ...] = ( NO_DEFAULT_CONSTRUCTORS: tuple[str, ...] = (
# go/keep-sorted start # go/keep-sorted start
"mjContact", "mjContact",
"mjPreContact",
"mjSolverStat", "mjSolverStat",
"mjStatistic", "mjStatistic",
"mjTimerStat", "mjTimerStat",
wasm/codegen/generators/structs.py
+1 -1
View File
@@ -493,7 +493,7 @@ def build_struct_source(
if not is_mjs: if not is_mjs:
# default constructor # 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;") builder.line("owned_ = true;")
default_func = _default_function_statement(s) default_func = _default_function_statement(s)
if default_func: if default_func:
wasm/codegen/tests/generators_test.py
+1 -1
View File
@@ -266,7 +266,7 @@ MjvLight::~MjvLight() {
delete ptr_; delete ptr_;
} }
} }
MjvLight::MjvLight() : ptr_(new mjvLight) { MjvLight::MjvLight() : ptr_(new mjvLight()) {
owned_ = true; owned_ = true;
} }
MjvLight::MjvLight(const MjvLight &other) : MjvLight() { MjvLight::MjvLight(const MjvLight &other) : MjvLight() {