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

Add nativeccd flag. When this flag is enabled, general convex collision detection is handled natively, as opposed to using libccd.

Browse Source 
PiperOrigin-RevId: 663859009
Change-Id: Ife18bb1de62f2154e4725fdf764484a18586b836
This commit is contained in:
Kyle Bayes
2024-08-16 14:07:53 -07:00
committed by Copybara-Service
parent 390bce2352
commit 0bffd744f9
13 changed files with 62 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_convex.c
+16 -8
View File
@@ -23,14 +23,24 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include "engine/engine_collision_gjk.h"
#include "engine/engine_collision_primitive.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_spatial.h"
// the LibCCD penetration function we use (ccdMPRPenetration or ccdGJKPenetration)
#define _mjCCDPENETRATION ccdMPRPenetration
// call LibCCD or GJK to recover penetration info
static int mjc_penetration(const mjModel* m, const void *obj1, const void *obj2, const ccd_t *ccd,
ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos) {
if (mjENABLED(mjENBL_NATIVECCD)) {
return mj_gjkPenetration(obj1, obj2, ccd, depth, dir, pos);
} else {
return ccdMPRPenetration(obj1, obj2, ccd, depth, dir, pos);
}
}
// ccd center function
void mjccd_center(const void *obj, ccd_vec3_t *center) {
@@ -294,7 +304,7 @@ static int mjc_MPRIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd,
mjContact* con, mjtNum margin) {
ccd_vec3_t dir, pos;
ccd_real_t depth;
if (_mjCCDPENETRATION(obj1, obj2, ccd, &depth, &dir, &pos) == 0) {
if (mjc_penetration(m, obj1, obj2, ccd, &depth, &dir, &pos) == 0) {
// contact is found but normal is undefined
if (ccdVec3Eq(&dir, ccd_vec3_origin)) {
return 0;
@@ -806,8 +816,8 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
continue;
}
// run MPR, save contact
if (_mjCCDPENETRATION(&obj1, &obj2, &ccd, &depth, &dirccd, &vecccd) == 0
// run penetration function, save contact
if (mjc_penetration(m, &obj1, &obj2, &ccd, &depth, &dirccd, &vecccd) == 0
&& !ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
// fill in contact data, transform to global coordinates
con[cnt].dist = -depth;
@@ -1257,7 +1267,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
}
// run MPR, save contact
if (_mjCCDPENETRATION(&obj1, &obj2, &ccd, &depth, &dirccd, &vecccd) == 0) {
if (mjc_penetration(m, &obj1, &obj2, &ccd, &depth, &dirccd, &vecccd) == 0) {
if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
// fill in contact data, transform to global coordinates
con[cnt].dist = -depth;
@@ -1289,5 +1299,3 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
return cnt;
}
#undef _mjCCDPENETRATION
src/engine/engine_support.c
+2 -1
View File
@@ -69,7 +69,8 @@ const char* mjENABLESTRING[mjNENABLE] = {
"Fwdinv",
"InvDiscrete",
"MultiCCD",
"Island"
"Island",
"NativeCCD"
};