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

Remove libccd compatibility layer from native ccd.

Browse Source 
PiperOrigin-RevId: 671740643
Change-Id: If3c1097df5c505f70c2dc0c090a7f5a93a43c683
This commit is contained in:
Kyle Bayes
2024-09-06 06:55:01 -07:00
committed by Copybara-Service
parent fa156ed22f
commit 4fc6fb179b
5 changed files with 218 additions and 242 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_convex.c
+44 -23
View File
@@ -32,13 +32,40 @@
// call LibCCD or GJK to recover penetration info // 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, static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2,
ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos) { const ccd_t* ccd, ccd_real_t* depth, ccd_vec3_t* dir, ccd_vec3_t* pos) {
if (mjENABLED(mjENBL_NATIVECCD)) { if (mjENABLED(mjENBL_NATIVECCD)) {
return mj_gjkPenetration(obj1, obj2, ccd, depth, dir, pos); mjCCDConfig config;
} else { mjCCDStatus status;
return ccdMPRPenetration(obj1, obj2, ccd, depth, dir, pos);
// set config
config.max_iterations = ccd->max_iterations,
config.tolerance = ccd->mpr_tolerance,
config.contacts = 1;
config.distances = 0; // no geom distances needed
mjtNum dist = mjc_ccd(&config, &status, obj1, obj2);
if (dist < 0) {
if (depth) *depth = -dist;
if (dir) {
mju_sub3(dir->v, status.x1, status.x2);
mju_normalize3(dir->v);
}
if (pos) {
pos->v[0] = 0.5 * (status.x1[0] + status.x2[0]);
pos->v[1] = 0.5 * (status.x1[1] + status.x2[1]);
pos->v[2] = 0.5 * (status.x1[2] + status.x2[2]);
}
return 0;
}
if (depth) *depth = 0;
if (dir) mju_zero3(dir->v);
if (pos) mju_zero3(dir->v);
return 1;
} }
// fallback to MPR
return ccdMPRPenetration(obj1, obj2, ccd, depth, dir, pos);
} }
@@ -298,8 +325,8 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) {
// find single convex-convex collision, using libccd // find single convex-convex collision
static int mjc_MPRIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, static int mjc_CCDIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd,
const mjModel* m, const mjData* d, const mjModel* m, const mjData* d,
mjContact* con, mjtNum margin) { mjContact* con, mjtNum margin) {
ccd_vec3_t dir, pos; ccd_vec3_t dir, pos;
@@ -370,10 +397,8 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
int mjc_Convex(const mjModel* m, const mjData* d, int mjc_Convex(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) { mjContact* con, int g1, int g2, mjtNum margin) {
ccd_t ccd; ccd_t ccd;
mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}, mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}, mjc_center, mjc_support};
mjc_center, mjc_support}; mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}, mjc_center, mjc_support};
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0},
mjc_center, mjc_support};
// init ccd structure // init ccd structure
mjc_initCCD(&ccd, m); mjc_initCCD(&ccd, m);
@@ -384,7 +409,7 @@ int mjc_Convex(const mjModel* m, const mjData* d,
ccd.support2 = mjccd_support; ccd.support2 = mjccd_support;
// find initial contact // find initial contact
int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, margin); int ncon = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con, margin);
// look for additional contacts // look for additional contacts
if (ncon && mjENABLED(mjENBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?) if (ncon && mjENABLED(mjENBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?)
@@ -433,7 +458,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); mju_rotateFrame(con[0].pos, invrot, d->geom_xmat+9*g2, d->geom_xpos+3*g2);
// search for new contact // search for new contact
int new_contact = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con+ncon, margin); int new_contact = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con+ncon, margin);
// check new contact // check new contact
if (new_contact && mjc_isDistinctContact(con, ncon + 1, tolerance)) { if (new_contact && mjc_isDistinctContact(con, ncon + 1, tolerance)) {
@@ -501,7 +526,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
mjGETINFO mjGETINFO
mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]}; mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]};
ccd_vec3_t dir, vec; ccd_vec3_t dir, vec;
mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}};
// get support point in -normal direction // get support point in -normal direction
ccdVec3Set(&dir, -mat1[2], -mat1[5], -mat1[8]); ccdVec3Set(&dir, -mat1[2], -mat1[5], -mat1[8]);
@@ -683,8 +708,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
// ccd-related // ccd-related
ccd_vec3_t dirccd, vecccd; ccd_vec3_t dirccd, vecccd;
ccd_real_t depth; ccd_real_t depth;
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center, mjc_support};
mjc_center, mjc_support};
ccd_t ccd; ccd_t ccd;
// point size1 to hfield size instead of geom1 size // point size1 to hfield size instead of geom1 size
@@ -1118,10 +1142,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con, int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) { int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) {
ccd_t ccd; ccd_t ccd;
mjCCDObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}, {0, 0, 0}, mjCCDObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}, mjc_center, mjc_support};
mjc_center, mjc_support}; mjCCDObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}, mjc_center, mjc_support};
mjCCDObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}, {0, 0, 0},
mjc_center, mjc_support};
// init ccd structure // init ccd structure
mjc_initCCD(&ccd, m); mjc_initCCD(&ccd, m);
@@ -1132,7 +1154,7 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
ccd.support2 = mjccd_support; ccd.support2 = mjccd_support;
// find contacts // find contacts
int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, margin); int ncon = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con, margin);
return ncon; return ncon;
} }
@@ -1170,8 +1192,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
// ccd-related // ccd-related
ccd_vec3_t dirccd, vecccd; ccd_vec3_t dirccd, vecccd;
ccd_real_t depth; ccd_real_t depth;
mjCCDObj obj2 = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}, {0, 0, 0}, mjCCDObj obj2 = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}, mjc_center, mjc_support};
mjc_center, mjc_support};
ccd_t ccd; ccd_t ccd;
//------------------------------------- AABB computation, box-box test //------------------------------------- AABB computation, box-box test
src/engine/engine_collision_convex.h
-1
View File
@@ -51,7 +51,6 @@ struct _mjCCDObj {
int vert; int vert;
mjtNum margin; mjtNum margin;
mjtNum rotate[4]; mjtNum rotate[4];
mjtNum x0[3]; // initial guess of the witness point
void (*center)(mjtNum res[3], const struct _mjCCDObj* obj); void (*center)(mjtNum res[3], const struct _mjCCDObj* obj);
void (*support)(mjtNum res[3], struct _mjCCDObj* obj, const mjtNum dir[3]); void (*support)(mjtNum res[3], struct _mjCCDObj* obj, const mjtNum dir[3]);
mjtNum prism[6][3]; // for hfield mjtNum prism[6][3]; // for hfield
src/engine/engine_collision_gjk.c
+73 -130
View File
@@ -25,9 +25,6 @@
#include "engine/engine_util_errmem.h" #include "engine/engine_util_errmem.h"
#include "engine/engine_util_spatial.h" #include "engine/engine_util_spatial.h"
#include <ccd/ccd.h>
#include <ccd/vec3.h>
// Computes the shortest distance between the origin and an n-simplex (n <= 3) and returns the // Computes the shortest distance between the origin and an n-simplex (n <= 3) and returns the
// barycentric coordinates of the closest point in the simplex. This is the so called distance // barycentric coordinates of the closest point in the simplex. This is the so called distance
// sub-algorithm of the original 1988 GJK algorithm. // sub-algorithm of the original 1988 GJK algorithm.
@@ -48,8 +45,8 @@ static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
// support function tweaked for GJK by taking kth iteration point as input and setting both // support function tweaked for GJK by taking kth iteration point as input and setting both
// support points to recover witness points // support points to recover witness points
static void gjk_support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, static void gjkSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
const mjtNum x_k[3]); const mjtNum x_k[3]);
// linear algebra utility functions // linear algebra utility functions
static mjtNum det3(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]); static mjtNum det3(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]);
@@ -81,11 +78,9 @@ typedef struct {
// generates a polytope from a 1-simplex, 2-simplex, or 3-simplex respectively // generates a polytope from a 1-simplex, 2-simplex, or 3-simplex respectively
// returns true if the polytope can be generated, false otherwise // returns true if the polytope can be generated, false otherwise
static int polytope2(Polytope* pt, const mjtNum simplex1[6], const mjtNum simplex2[6], static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2);
mjCCDObj* obj1, mjCCDObj* obj2); static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2);
static int polytope3(Polytope* pt, const mjtNum simplex1[9], const mjtNum simplex2[9], static int polytope4(Polytope* pt, const mjCCDStatus* status);
mjCCDObj* obj1, mjCCDObj* obj2);
static int polytope4(Polytope* pt, const mjtNum simplex1[12], const mjtNum simplex2[12]);
// initializes the polytope (faces and vertices must be freed by caller) // initializes the polytope (faces and vertices must be freed by caller)
static void initPolytope(Polytope* pt); static void initPolytope(Polytope* pt);
@@ -98,37 +93,31 @@ static void attachFace(Polytope* pt, int v1, int v2, int v3);
// returns the penetration depth (negative distance) of the convex objects // returns the penetration depth (negative distance) of the convex objects
// witness points are stored in x1 and x2 // witness points are stored in x1 and x2
static mjtNum epa(const mjCCDConfig* config, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2, static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2);
mjtNum dir[3]);
// internal data structure for the returning simplex from GJK
typedef struct {
mjtNum verts[12];
int nverts;
} Simplex;
// internal GJK with returned data for EPA
static mjtNum _gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2, // GJK algorithm
Simplex* ret1, Simplex* ret2) { static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum simplex[12]; // our current simplex with max 4 vertices due to only 3 dimensions mjtNum simplex[12]; // our current simplex with max 4 vertices due to only 3 dimensions
int n = 0; // number of vertices in the simplex int n = 0; // number of vertices in the simplex
mjtNum x_k[3]; // the kth approximation point with initial value x_0 mjtNum x_k[3]; // the kth approximation point with initial value x_0
// segregated simplices and points for the two objects to recover witness points // segregated simplices and points for the two objects to recover witness points
mjtNum simplex1[12], simplex2[12]; mjtNum *simplex1 = status->simplex1, *simplex2 = status->simplex2;
mjtNum* x1_k = obj1->x0; mjtNum* x1_k = status->x1;
mjtNum* x2_k = obj2->x0; mjtNum* x2_k = status->x2;
mju_sub3(x_k, x1_k, x2_k); mju_sub3(x_k, x1_k, x2_k);
mjtNum epsilon = config->tolerance * config->tolerance; mjtNum epsilon = status->tolerance * status->tolerance;
int N = config->max_iterations; int k = 0, N = status->max_iterations;
for (size_t k = 0; k < N; k++) { for (; k < N; k++) {
mjtNum s1[3], s2[3]; // the support points in obj1 and obj2 mjtNum s1[3], s2[3]; // the support points in obj1 and obj2
mjtNum s_k[3]; // the kth support point of Minkowski difference mjtNum s_k[3]; // the kth support point of Minkowski difference
mjtNum lambda[4]; // barycentric coordinates for x_k mjtNum lambda[4]; // barycentric coordinates for x_k
// compute the kth support point // compute the kth support point
gjk_support(s1, s2, obj1, obj2, x_k); gjkSupport(s1, s2, obj1, obj2, x_k);
mju_sub3(s_k, s1, s2); mju_sub3(s_k, s1, s2);
// the stopping criteria relies on the Frank-Wolfe duality gap given by // the stopping criteria relies on the Frank-Wolfe duality gap given by
@@ -181,30 +170,17 @@ static mjtNum _gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2,
break; break;
} }
} }
if (ret1 && ret2) {
ret1->nverts = n; status->gjk_iterations = k;
ret2->nverts = n; status->nsimplex = n;
for (int i = 0; i < n; i++) {
mju_copy3(ret1->verts + 3*i, simplex1 + 3*i);
mju_copy3(ret2->verts + 3*i, simplex2 + 3*i);
}
}
return mju_norm3(x_k); return mju_norm3(x_k);
} }
// returns the distance between the two objects. The witness points are
// recoverable from the x_0 field in obj1 and obj2.
mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) {
return _gjk(config, obj1, obj2, NULL, NULL);
}
// computes the support points in obj1 and obj2 for the kth approximation point // computes the support points in obj1 and obj2 for the kth approximation point
static void gjk_support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, static void gjkSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
const mjtNum x_k[3]) { const mjtNum x_k[3]) {
mjtNum dir[3], dir_neg[3]; mjtNum dir[3], dir_neg[3];
mju_copy3(dir_neg, x_k); mju_copy3(dir_neg, x_k);
mju_normalize3(dir_neg); // mjc_support assumes a normalized direction mju_normalize3(dir_neg); // mjc_support assumes a normalized direction
@@ -648,11 +624,10 @@ static void rotmat(mjtNum R[9], const mjtNum axis[3]) {
// creates a polytope from a 1-simplex (2 points i.e. line segment) // creates a polytope from a 1-simplex (2 points i.e. line segment)
static int polytope2(Polytope* pt, const mjtNum simplex1[6], const mjtNum simplex2[6], static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum v1[3], v2[3]; mjtNum v1[3], v2[3];
mju_sub3(v1, simplex1 + 0, simplex2 + 0); mju_sub3(v1, status->simplex1 + 0, status->simplex2 + 0);
mju_sub3(v2, simplex1 + 3, simplex2 + 3); mju_sub3(v2, status->simplex1 + 3, status->simplex2 + 3);
mjtNum diff[3]; mjtNum diff[3];
mju_sub3(diff, v2, v1); mju_sub3(diff, v2, v1);
@@ -701,8 +676,8 @@ static int polytope2(Polytope* pt, const mjtNum simplex1[6], const mjtNum simple
} }
// save vertices and get indices for each one // save vertices and get indices for each one
int v1i = newVertex(pt, simplex1 + 0, simplex2 + 0); int v1i = newVertex(pt, status->simplex1 + 0, status->simplex2 + 0);
int v2i = newVertex(pt, simplex1 + 3, simplex2 + 3); int v2i = newVertex(pt, status->simplex1 + 3, status->simplex2 + 3);
int v3i = newVertex(pt, v3a, v3b); int v3i = newVertex(pt, v3a, v3b);
int v4i = newVertex(pt, v4a, v4b); int v4i = newVertex(pt, v4a, v4b);
int v5i = newVertex(pt, v5a, v5b); int v5i = newVertex(pt, v5a, v5b);
@@ -795,13 +770,12 @@ static int triPointIntersect(const mjtNum v1[3], const mjtNum v2[3], const mjtNu
// creates a polytope from a 2-simplex (3 points i.e. triangle) // creates a polytope from a 2-simplex (3 points i.e. triangle)
static int polytope3(Polytope* pt, const mjtNum simplex1[9], const mjtNum simplex2[9], static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
mjCCDObj* obj1, mjCCDObj* obj2) {
// get vertices of simplex from GJK // get vertices of simplex from GJK
mjtNum v1[3], v2[3], v3[3]; mjtNum v1[3], v2[3], v3[3];
mju_sub3(v1, simplex1 + 0, simplex2 + 0); mju_sub3(v1, status->simplex1 + 0, status->simplex2 + 0);
mju_sub3(v2, simplex1 + 3, simplex2 + 3); mju_sub3(v2, status->simplex1 + 3, status->simplex2 + 3);
mju_sub3(v3, simplex1 + 6, simplex2 + 6); mju_sub3(v3, status->simplex1 + 6, status->simplex2 + 6);
// get normals in both directions // get normals in both directions
mjtNum diff1[3], diff2[3], n[3], nn[3]; mjtNum diff1[3], diff2[3], n[3], nn[3];
@@ -842,15 +816,15 @@ static int polytope3(Polytope* pt, const mjtNum simplex1[9], const mjtNum simple
// it but within tolerance from it. In that case the hexahedron could possibly be constructed // it but within tolerance from it. In that case the hexahedron could possibly be constructed
// that doesn't contain the origin, but nonetheless there is penetration depth. // that doesn't contain the origin, but nonetheless there is penetration depth.
mjtNum dir[3]; mjtNum dir[3];
mju_sub3(dir, obj1->x0, obj2->x0); mju_sub3(dir, status->x1, status->x2);
if (mju_norm3(dir) > mjMINVAL && !testTetra(v1, v2, v3, v4) && !testTetra(v1, v2, v3, v5)) { if (mju_norm3(dir) > mjMINVAL && !testTetra(v1, v2, v3, v4) && !testTetra(v1, v2, v3, v5)) {
return 0; return 0;
} }
// save vertices and get indices for each one // save vertices and get indices for each one
int v1i = newVertex(pt, simplex1 + 0, simplex2 + 0); int v1i = newVertex(pt, status->simplex1 + 0, status->simplex2 + 0);
int v2i = newVertex(pt, simplex1 + 3, simplex2 + 3); int v2i = newVertex(pt, status->simplex1 + 3, status->simplex2 + 3);
int v3i = newVertex(pt, simplex1 + 6, simplex2 + 6); int v3i = newVertex(pt, status->simplex1 + 6, status->simplex2 + 6);
int v5i = newVertex(pt, v5a, v5b); int v5i = newVertex(pt, v5a, v5b);
int v4i = newVertex(pt, v4a, v4b); int v4i = newVertex(pt, v4a, v4b);
@@ -875,11 +849,11 @@ static int polytope3(Polytope* pt, const mjtNum simplex1[9], const mjtNum simple
// creates a polytope from a 3-simplex (4 points i.e. tetrahedron) // creates a polytope from a 3-simplex (4 points i.e. tetrahedron)
static int polytope4(Polytope* pt, const mjtNum simplex1[12], const mjtNum simplex2[12]) { static int polytope4(Polytope* pt, const mjCCDStatus* status) {
int v1 = newVertex(pt, simplex1 + 0, simplex2 + 0); int v1 = newVertex(pt, status->simplex1 + 0, status->simplex2 + 0);
int v2 = newVertex(pt, simplex1 + 3, simplex2 + 3); int v2 = newVertex(pt, status->simplex1 + 3, status->simplex2 + 3);
int v3 = newVertex(pt, simplex1 + 6, simplex2 + 6); int v3 = newVertex(pt, status->simplex1 + 6, status->simplex2 + 6);
int v4 = newVertex(pt, simplex1 + 9, simplex2 + 9); int v4 = newVertex(pt, status->simplex1 + 9, status->simplex2 + 9);
attachFace(pt, v1, v2, v3); attachFace(pt, v1, v2, v3);
attachFace(pt, v1, v2, v4); attachFace(pt, v1, v2, v4);
@@ -1035,16 +1009,15 @@ static void epaWitness(const Polytope* pt, int index, mjtNum x1[3], mjtNum x2[3]
// returns the penetration depth (negative distance) of the convex objects // returns the penetration depth (negative distance) of the convex objects
static mjtNum epa(const mjCCDConfig* config, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2, static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum dir[3]) {
mjtNum dist = mjMAXVAL; mjtNum dist = mjMAXVAL;
int index; int index;
Horizon h; Horizon h;
initHorizon(&h); initHorizon(&h);
int N = config->max_iterations; mjtNum tolerance = status->tolerance;
mjtNum tolerance = config->tolerance;
for (int j = 0; j < N; j++) { int k = 0, N = status->max_iterations;
for (; k < N; k++) {
// find the closest face to the origin // find the closest face to the origin
dist = mjMAXVAL; dist = mjMAXVAL;
index = -1; index = -1;
@@ -1100,79 +1073,49 @@ static mjtNum epa(const mjCCDConfig* config, Polytope* pt, mjCCDObj* obj1, mjCCD
h.n = 0; // clear horizon h.n = 0; // clear horizon
} }
free(h.edges); free(h.edges);
Face face = pt->faces[index]; epaWitness(pt, index, status->x1, status->x2);
mju_copy3(dir, face.v); status->epa_iterations = k;
epaWitness(pt, index, obj1->x0, obj2->x0);
return dist; return dist;
} }
// runs both GJK and EPA (if needed) // run general convex collision detection
static mjtNum _gjk_epa(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2, Polytope* pt, mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum dir[3]) { // set up
Simplex simplex1, simplex2; obj1->center(status->x1, obj1);
mjtNum dist = _gjk(config, obj1, obj2, &simplex1, &simplex2); obj2->center(status->x2, obj2);
status->epa_iterations = -1;
status->tolerance = config->tolerance;
status->max_iterations = config->max_iterations;
if (dist <= config->tolerance && simplex1.nverts > 1) { mjtNum dist = gjk(status, obj1, obj2);
// penetration recovery for contacts not needed
if (!config->contacts) {
return dist;
}
if (dist <= config->tolerance && status->nsimplex > 1) {
Polytope pt;
initPolytope(&pt);
int ret; int ret;
if (simplex1.nverts == 2) { if (status->nsimplex == 2) {
ret = polytope2(pt, simplex1.verts, simplex2.verts, obj1, obj2); ret = polytope2(&pt, status, obj1, obj2);
} else if (simplex1.nverts == 3) { } else if (status->nsimplex == 3) {
ret = polytope3(pt, simplex1.verts, simplex2.verts, obj1, obj2); ret = polytope3(&pt, status, obj1, obj2);
} else { } else {
ret = polytope4(pt, simplex1.verts, simplex2.verts); ret = polytope4(&pt, status);
} }
// simplex not on boundary (objects are penetrating) // simplex not on boundary (objects are penetrating)
if (ret) { if (ret) {
dist = epa(config, pt, obj1, obj2, dir); dist = -epa(status, &pt, obj1, obj2);
return -dist; } else {
dist = 0;
} }
return 0; mju_free(pt.faces);
mju_free(pt.verts);
} }
return dist; return dist;
} }
// --------------------------- LibCCD Compatibility Layer -----------------------------------------
// Penetration function with same signature as LibCCD's ccdMPRPenetration and ccdGJKPenetration
int mj_gjkPenetration(const void *obj1, const void *obj2, const ccd_t *ccd,
ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos) {
Polytope pt;
initPolytope(&pt);
mjCCDConfig config;
mjCCDObj* o1 = (mjCCDObj*) obj1;
mjtNum* x1 = o1->x0;
mjCCDObj* o2 = (mjCCDObj*) obj2;
mjtNum* x2 = o2->x0;
o1->center(x1, o1);
o2->center(x2, o2);
config.max_iterations = ccd->max_iterations;
config.tolerance = ccd->mpr_tolerance;
mjtNum d[3];
mjtNum dist = _gjk_epa(&config, o1, o2, &pt, d);
if (dist < 0) {
if (depth) *depth = -dist;
if (dir) {
mju_normalize3(d);
mju_copy3(dir->v, d);
}
if (pos) {
mju_scl3(x1, x1, 0.5);
mju_scl3(x2, x2, 0.5);
mju_add3(pos->v, x1, x2);
}
} else {
if (depth) *depth = 0;
if (dir) mju_zero3(dir->v);
if (pos) mju_zero3(dir->v);
}
free(pt.faces);
free(pt.verts);
return dist >= 0;
}
src/engine/engine_collision_gjk.h
+24 -12
View File
@@ -19,27 +19,39 @@
#include <mujoco/mjtnum.h> #include <mujoco/mjtnum.h>
#include "engine/engine_collision_convex.h" #include "engine/engine_collision_convex.h"
#include <ccd/ccd.h>
#include <ccd/vec3.h>
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif
// internal configuration for convex collision detection // configuration for convex collision detection
struct _mjCCDConfig { struct _mjCCDConfig {
int max_iterations; int max_iterations; // the maximum number of iterations for GJK and EPA
mjtNum tolerance; mjtNum tolerance; // tolerance used by GJK and EPA
int contacts; // set to true to recover contact (pendetration) info
int distances; // set to true to recover distance info
}; };
typedef struct _mjCCDConfig mjCCDConfig; typedef struct _mjCCDConfig mjCCDConfig;
// Returns the distance between the two objects. The witness points are // data produced from running GJK and EPA
// recoverable from x_0 in obj1 and obj2. struct _mjCCDStatus {
MJAPI mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2); mjtNum x1[3]; // witness point for geom 1
mjtNum x2[3]; // witness point for geom 2
// Penetration function with same signature as LibCCD's ccdMPRPenetration and ccdGJKPenetration // configurations used
MJAPI int mj_gjkPenetration(const void *obj1, const void *obj2, const ccd_t *ccd, int max_iterations; // the maximum number of iterations for GJK and EPA
ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos); mjtNum tolerance; // tolerance used by GJK and EPA
// statistics for debugging purposes
int gjk_iterations; // number of iterations that GJK ran
int epa_iterations; // number of iterations that EPA ran (negative if EPA did not run)
mjtNum simplex1[12]; // the simplex that GJK returned for obj1
mjtNum simplex2[12]; // the simplex that GJK returned for obj2
int nsimplex; // size of simplex 1 & 2
};
typedef struct _mjCCDStatus mjCCDStatus;
// run general convex collision detection, returns positive for distance, negative for penetration
MJAPI mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif
test/engine/engine_collision_gjk_test.cc
+77 -76
View File
@@ -47,28 +47,58 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
mjc_support(vec->v, (mjCCDObj*) obj, _dir->v); mjc_support(vec->v, (mjCCDObj*) obj, _dir->v);
} }
mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
mjtNum x2[3]) { mjtNum x2[3]) {
mjCCDConfig config = {kMaxIterations, kTolerance}; mjCCDConfig config = {kMaxIterations, kTolerance};
mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center,
mjc_center, mjc_support}; mjc_support};
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center,
mjc_center, mjc_support}; mjc_support};
mjc_center(obj1.x0, &obj1); mjCCDStatus status;
mjc_center(obj2.x0, &obj2); mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2);
mjtNum dist = mj_gjk(&config, &obj1, &obj2); if (x1 != nullptr) mju_copy3(x1, status.x1);
if (x1 != nullptr) mju_copy3(x1, obj1.x0); if (x2 != nullptr) mju_copy3(x2, status.x2);
if (x2 != nullptr) mju_copy3(x2, obj2.x0);
return dist; return dist;
} }
// drop in replacement for ccdMPRPenetration taken from mjc_penetration
int PenetrationWrapper(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd,
ccd_real_t* depth, ccd_vec3_t* dir, ccd_vec3_t* pos) {
mjCCDConfig config;
mjCCDStatus status;
mjtNum run_gjkPenetration(mjModel* m, mjData* d, int g1, int g2, // set config
mjtNum dir[3] = nullptr, mjtNum pos[3] = nullptr) { config.max_iterations = ccd->max_iterations,
mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, config.tolerance = ccd->mpr_tolerance,
mjc_center, mjc_support}; config.contacts = 1;
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, config.distances = 0; // no geom distances needed
mjc_center, mjc_support};
mjtNum dist = mjc_ccd(&config, &status, obj1, obj2);
if (dist < 0) {
if (depth) *depth = -dist;
if (dir) {
mju_sub3(dir->v, status.x1, status.x2);
mju_normalize3(dir->v);
}
if (pos) {
pos->v[0] = 0.5 * (status.x1[0] + status.x2[0]);
pos->v[1] = 0.5 * (status.x1[1] + status.x2[1]);
pos->v[2] = 0.5 * (status.x1[2] + status.x2[2]);
}
return 0;
}
if (depth) *depth = 0;
if (dir) mju_zero3(dir->v);
if (pos) mju_zero3(dir->v);
return 1;
}
mjtNum Penetration(mjModel* m, mjData* d, int g1, int g2,
mjtNum dir[3] = nullptr, mjtNum pos[3] = nullptr) {
mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center,
mjc_support};
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center,
mjc_support};
ccd_t ccd; ccd_t ccd;
// CCD_INIT(&ccd); // uncomment to run ccdMPRPenetration // CCD_INIT(&ccd); // uncomment to run ccdMPRPenetration
ccd.mpr_tolerance = kTolerance; ccd.mpr_tolerance = kTolerance;
@@ -82,7 +112,7 @@ mjtNum run_gjkPenetration(mjModel* m, mjData* d, int g1, int g2,
ccd_real_t depth; ccd_real_t depth;
ccd_vec3_t ccd_dir, ccd_pos; ccd_vec3_t ccd_dir, ccd_pos;
int ret = mj_gjkPenetration(&obj1, &obj2, &ccd, &depth, &ccd_dir, &ccd_pos); int ret = PenetrationWrapper(&obj1, &obj2, &ccd, &depth, &ccd_dir, &ccd_pos);
if (ret) return 0; // objects not colliding if (ret) return 0; // objects not colliding
if (dir) mju_copy3(dir, ccd_dir.v); if (dir) mju_copy3(dir, ccd_dir.v);
if (pos) mju_copy3(pos, ccd_pos.v); if (pos) mju_copy3(pos, ccd_pos.v);
@@ -116,7 +146,7 @@ TEST_F(MjGjkTest, SphereSphere) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum x1[3], x2[3]; mjtNum x1[3], x2[3];
mjtNum dist = run_gjk(model, data, geom1, geom2, x1, x2); mjtNum dist = GeomDist(model, data, geom1, geom2, x1, x2);
EXPECT_EQ(dist, 1); EXPECT_EQ(dist, 1);
EXPECT_THAT(x1, ElementsAre(-.5, 0, 0)); EXPECT_THAT(x1, ElementsAre(-.5, 0, 0));
@@ -125,60 +155,6 @@ TEST_F(MjGjkTest, SphereSphere) {
mj_deleteModel(model); mj_deleteModel(model);
} }
TEST_F(MjGjkTest, BoxBox) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="-1.5 .5 0" size="1 1 1"/>
<geom name="geom2" type="box" pos="1.5 0 0" size="1 1 1"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
mj_forward(model, data);
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr);
EXPECT_EQ(dist, 1);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxBoxIntersect) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="-1 0 0" size="2.5 2.5 2.5"/>
<geom name="geom2" type="box" pos="1.5 0 0" size="1 1 1"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
mj_forward(model, data);
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dir[3], pos[3];
mjtNum dist = run_gjkPenetration(model, data, geom1, geom2, dir, pos);
EXPECT_NEAR(dist, -1, kTolerance);
EXPECT_NEAR(dir[0], 1, kTolerance);
EXPECT_NEAR(dir[1], 0, kTolerance);
EXPECT_NEAR(dir[2], 0, kTolerance);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, EllipsoidEllipsoid) { TEST_F(MjGjkTest, EllipsoidEllipsoid) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -197,13 +173,38 @@ TEST_F(MjGjkTest, EllipsoidEllipsoid) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr); mjtNum dist = GeomDist(model, data, geom1, geom2, nullptr, nullptr);
EXPECT_NEAR(dist, 0.7542, .0001); EXPECT_NEAR(dist, 0.7542, .0001);
mj_deleteData(data); mj_deleteData(data);
mj_deleteModel(model); mj_deleteModel(model);
} }
TEST_F(MjGjkTest, BoxBox) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="-1.5 .5 0" size="1 1 1"/>
<geom name="geom2" type="box" pos="1.5 0 0" size="1 1 1"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
mj_forward(model, data);
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dist = GeomDist(model, data, geom1, geom2, nullptr, nullptr);
EXPECT_EQ(dist, 1);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) { TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -222,7 +223,7 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr); mjtNum dist = GeomDist(model, data, geom1, geom2, nullptr, nullptr);
EXPECT_NEAR(dist, 0, kTolerance); EXPECT_NEAR(dist, 0, kTolerance);
mj_deleteData(data); mj_deleteData(data);
@@ -247,7 +248,7 @@ TEST_F(MjGjkTest, CapsuleCapsule) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr); mjtNum dist = GeomDist(model, data, geom1, geom2, nullptr, nullptr);
EXPECT_NEAR(dist, 0.4711, .0001); EXPECT_NEAR(dist, 0.4711, .0001);
mj_deleteData(data); mj_deleteData(data);