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

Refactor nativeccd interface.

Browse Source 
- Update mj_ccd interface for multi-contacts,
- Support GJK cutoff distance, and
- Remove mjc_fixNormal from nativeccd (causes bug with cylindar box collisions).

PiperOrigin-RevId: 696185362
Change-Id: I4828b7ee1bde078268220172a4937cd553f6187e
This commit is contained in:
Kyle Bayes
2024-11-13 10:27:54 -08:00
committed by Copybara-Service
parent c68ee8055e
commit 0e8cca93e1
6 changed files with 162 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_convex.c
+45 -34
View File
@@ -30,7 +30,7 @@
#include "engine/engine_util_misc.h"
#include "engine/engine_util_spatial.h"
// call LibCCD or GJK to recover penetration info
// call libccd or nativeccd to recover penetration info
static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2,
const ccd_t* ccd, ccd_real_t* depth, ccd_vec3_t* dir, ccd_vec3_t* pos) {
if (mjENABLED(mjENBL_NATIVECCD)) {
@@ -40,8 +40,8 @@ static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2,
// set config
config.max_iterations = ccd->max_iterations,
config.tolerance = ccd->mpr_tolerance,
config.contacts = 1;
config.distances = 0; // no geom distances needed
config.max_contacts = 1;
config.dist_cutoff = 0; // no geom distances needed
mjtNum dist = mjc_ccd(&config, &status, obj1, obj2);
if (dist < 0) {
@@ -68,6 +68,7 @@ static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2,
}
// ccd center function
void mjccd_center(const void *obj, ccd_vec3_t *center) {
mjc_center(center->v, (const mjCCDObj*) obj);
@@ -744,12 +745,46 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) {
// find single convex-convex collision
static int mjc_CCDIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd,
const mjModel* m, const mjData* d,
static int mjc_CCDIteration(const mjModel* m, const mjData* d, mjCCDObj* obj1, mjCCDObj* obj2,
mjContact* con, mjtNum margin) {
if (mjENABLED(mjENBL_NATIVECCD)) {
mjCCDConfig config;
mjCCDStatus status;
// set config
config.max_iterations = m->opt.ccd_iterations;
config.tolerance = m->opt.ccd_tolerance;
config.max_contacts = 1;
config.dist_cutoff = 0; // no geom distances needed
mjtNum dist = mjc_ccd(&config, &status, obj1, obj2);
if (dist < 0) {
con->dist = margin + dist;
mju_sub3(con->frame, status.x1, status.x2);
mju_normalize3(con->frame);
con->pos[0] = 0.5 * (status.x1[0] + status.x2[0]);
con->pos[1] = 0.5 * (status.x1[1] + status.x2[1]);
con->pos[2] = 0.5 * (status.x1[2] + status.x2[2]);
mju_zero3(con->frame+3);
return 1;
}
return 0;
}
// init libccd structure
ccd_t ccd;
mjc_initCCD(&ccd, m);
ccd.first_dir = ccdFirstDirDefault;
ccd.center1 = mjccd_center;
ccd.center2 = mjccd_center;
ccd.support1 = mjccd_support;
ccd.support2 = mjccd_support;
ccd_vec3_t dir, pos;
ccd_real_t depth;
if (mjc_penetration(m, obj1, obj2, ccd, &depth, &dir, &pos) == 0) {
// call MPR from libccd
if (ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos) == 0) {
// contact is found but normal is undefined
if (ccdVec3Eq(&dir, ccd_vec3_origin)) {
return 0;
@@ -768,11 +803,7 @@ static int mjc_CCDIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd,
return 1;
}
// no contact found
else {
return 0;
}
return 0;
}
@@ -819,17 +850,8 @@ int mjc_Convex(const mjModel* m, const mjData* d,
mjc_initCCDObj(&obj1, m, d, g1, margin);
mjc_initCCDObj(&obj2, m, d, g2, margin);
// init libccd structure
ccd_t ccd;
mjc_initCCD(&ccd, m);
ccd.first_dir = ccdFirstDirDefault;
ccd.center1 = mjccd_center;
ccd.center2 = mjccd_center;
ccd.support1 = mjccd_support;
ccd.support2 = mjccd_support;
// find initial contact
int ncon = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con, margin);
int ncon = mjc_CCDIteration(m, d, &obj1, &obj2, con, margin);
// look for additional contacts
if (ncon && mjENABLED(mjENBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?)
@@ -878,7 +900,7 @@ int mjc_Convex(const mjModel* m, const mjData* d,
mju_rotateFrame(con[0].pos, invrot, d->geom_xmat+9*g2, d->geom_xpos+3*g2);
// search for new contact
int new_contact = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con+ncon, margin);
int new_contact = mjc_CCDIteration(m, d, &obj1, &obj2, con+ncon, margin);
// check new contact
if (new_contact && mjc_isDistinctContact(con, ncon + 1, tolerance)) {
@@ -1525,19 +1547,8 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
mjc_setCCDObjFlex(&obj1, f1, e1, v1);
mjc_setCCDObjFlex(&obj2, f2, e2, -1);
// init libccd structure
ccd_t ccd;
mjc_initCCD(&ccd, m);
ccd.first_dir = ccdFirstDirDefault;
ccd.center1 = mjccd_center;
ccd.center2 = mjccd_center;
ccd.support1 = mjccd_support;
ccd.support2 = mjccd_support;
// find contacts
int ncon = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con, margin);
return ncon;
return mjc_CCDIteration(m, d, &obj1, &obj2, con, margin);
}