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

Separate CCD code from support/center logic.

Browse Source 
PiperOrigin-RevId: 645355865
Change-Id: I71303ffd7bbee37c5e8ac7e770f810768780c2e6
This commit is contained in:
Kyle Bayes
2024-06-21 05:17:04 -07:00
committed by Copybara-Service
parent 73cd15344e
commit b0b934cb5f
2 changed files with 74 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_convex.c
+65 -56
View File
@@ -34,25 +34,31 @@
// ccd center function // ccd center function
void mjccd_center(const void *obj, ccd_vec3_t *center) { void mjccd_center(const void *obj, ccd_vec3_t *center) {
const mjtCCD* ccd = (const mjtCCD*)obj; mjc_center(center->v, (const mjtCCObj*) obj);
int g = ccd->geom; }
int f = ccd->flex;
int e = ccd->elem;
int v = ccd->vert;
// center function for convex collision algorithms
void mjc_center(mjtNum res[3], const mjtCCObj *obj) {
int g = obj->geom;
int f = obj->flex;
int e = obj->elem;
int v = obj->vert;
// return geom position // return geom position
if (g >= 0) { if (g >= 0) {
mju_copy3(center->v, ccd->data->geom_xpos + 3*g); mju_copy3(res, obj->data->geom_xpos + 3*g);
} }
// return flex element position // return flex element position
else if (e >= 0) { else if (e >= 0) {
mju_copy3(center->v, ccd->data->flexelem_aabb + 6*(ccd->model->flex_elemadr[f]+e)); mju_copy3(res, obj->data->flexelem_aabb + 6*(obj->model->flex_elemadr[f]+e));
} }
// return flex vertex position // return flex vertex position
else { else {
mju_copy3(center->v, ccd->data->flexvert_xpos + 3*(ccd->model->flex_vertadr[f]+v)); mju_copy3(res, obj->data->flexvert_xpos + 3*(obj->model->flex_vertadr[f]+v));
} }
} }
@@ -60,21 +66,25 @@ void mjccd_center(const void *obj, ccd_vec3_t *center) {
// ccd support function // ccd support function
void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
const mjtCCD* ccd = (const mjtCCD*)obj; mjc_support(vec->v, (mjtCCObj*) obj, _dir->v);
const mjModel* m = ccd->model; }
const mjData* d = ccd->data;
int g = ccd->geom;
// support function for convex collision algorithms
void mjc_support(mjtNum res[3], mjtCCObj* obj, const mjtNum dir[3]) {
const mjModel* m = obj->model;
const mjData* d = obj->data;
int g = obj->geom;
//-------------------------- flex element or vertex ----------------------------- //-------------------------- flex element or vertex -----------------------------
if (g < 0) { if (g < 0) {
int f = ccd->flex; int f = obj->flex;
int dim = m->flex_dim[f]; int dim = m->flex_dim[f];
mjtNum *res = vec->v;
const mjtNum *dir = _dir->v;
// flex element // flex element
if (ccd->elem >= 0) { if (obj->elem >= 0) {
int e = ccd->elem; int e = obj->elem;
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1); const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f]; const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f];
@@ -92,14 +102,14 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
} }
// add radius and margin/2 // add radius and margin/2
mju_addToScl3(res, dir, m->flex_radius[f] + 0.5*ccd->margin); mju_addToScl3(res, dir, m->flex_radius[f] + 0.5*obj->margin);
return; return;
} }
// flex vertex // flex vertex
else { else {
const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + ccd->vert); const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + obj->vert);
mju_addScl3(res, vert, dir, m->flex_radius[f] + 0.5*ccd->margin); mju_addScl3(res, vert, dir, m->flex_radius[f] + 0.5*obj->margin);
return; return;
} }
} }
@@ -111,30 +121,29 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
mjtNum tmp, vdot; mjtNum tmp, vdot;
const mjtNum* size = m->geom_size+3*g; // geom sizes const mjtNum* size = m->geom_size+3*g; // geom sizes
mjtNum dir[3]; // direction in geom local frame mjtNum local_dir[3]; // direction in geom local frame
mjtNum res[3]; // result in geom local frame
// rotate dir to geom local frame // rotate dir to geom local frame
mju_mulMatTVec3(dir, d->geom_xmat+9*g, _dir->v); mju_mulMatTVec3(local_dir, d->geom_xmat+9*g, dir);
// compute result according to geom type // compute result according to geom type
switch ((mjtGeom) m->geom_type[g]) { switch ((mjtGeom) m->geom_type[g]) {
case mjGEOM_SPHERE: case mjGEOM_SPHERE:
mju_scl3(res, dir, size[0]); mju_scl3(res, local_dir, size[0]);
break; break;
case mjGEOM_CAPSULE: case mjGEOM_CAPSULE:
// start with sphere // start with sphere
mju_scl3(res, dir, size[0]); mju_scl3(res, local_dir, size[0]);
// add cylinder contribution // add cylinder contribution
res[2] += mju_sign(dir[2]) * size[1]; res[2] += mju_sign(local_dir[2]) * size[1];
break; break;
case mjGEOM_ELLIPSOID: case mjGEOM_ELLIPSOID:
// find support point on unit sphere: scale dir by ellipsoid sizes and renormalize // find support point on unit sphere: scale dir by ellipsoid sizes and renormalize
for (int i=0; i < 3; i++) { for (int i=0; i < 3; i++) {
res[i] = dir[i] * size[i]; res[i] = local_dir[i] * size[i];
} }
mju_normalize3(res); mju_normalize3(res);
@@ -146,21 +155,21 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
case mjGEOM_CYLINDER: case mjGEOM_CYLINDER:
// set result in XY plane: support on circle // set result in XY plane: support on circle
tmp = mju_sqrt(dir[0]*dir[0] + dir[1]*dir[1]); tmp = mju_sqrt(local_dir[0]*local_dir[0] + local_dir[1]*local_dir[1]);
if (tmp > mjMINVAL) { if (tmp > mjMINVAL) {
res[0] = dir[0]/tmp*size[0]; res[0] = local_dir[0]/tmp*size[0];
res[1] = dir[1]/tmp*size[0]; res[1] = local_dir[1]/tmp*size[0];
} else { } else {
res[0] = res[1] = 0; res[0] = res[1] = 0;
} }
// set result in Z direction // set result in Z direction
res[2] = mju_sign(dir[2]) * size[1]; res[2] = mju_sign(local_dir[2]) * size[1];
break; break;
case mjGEOM_BOX: case mjGEOM_BOX:
for (int i=0; i < 3; i++) { for (int i=0; i < 3; i++) {
res[i] = mju_sign(dir[i]) * size[i]; res[i] = mju_sign(local_dir[i]) * size[i];
} }
break; break;
@@ -176,9 +185,9 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
// search all vertices, find best // search all vertices, find best
for (int i=0; i < m->mesh_vertnum[m->geom_dataid[g]]; i++) { for (int i=0; i < m->mesh_vertnum[m->geom_dataid[g]]; i++) {
// vdot = dot(vertex, dir) // vdot = dot(vertex, dir)
vdot = dir[0] * (mjtNum)vertdata[3*i] + vdot = local_dir[0] * (mjtNum)vertdata[3*i] +
dir[1] * (mjtNum)vertdata[3*i+1] + local_dir[1] * (mjtNum)vertdata[3*i+1] +
dir[2] * (mjtNum)vertdata[3*i+2]; local_dir[2] * (mjtNum)vertdata[3*i+2];
// update best // update best
if (vdot > tmp) { if (vdot > tmp) {
@@ -188,7 +197,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
} }
// record best vertex index, in globalid format // record best vertex index, in globalid format
((mjtCCD*)ccd)->meshindex = ibest; obj->meshindex = ibest;
} }
// hill-climb using graph data // hill-climb using graph data
@@ -202,9 +211,9 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
// init with first vertex in convex hull // init with first vertex in convex hull
ibest = 0; ibest = 0;
tmp = dir[0] * (mjtNum)vertdata[3*vert_globalid[0]] + tmp = local_dir[0] * (mjtNum)vertdata[3*vert_globalid[0]] +
dir[1] * (mjtNum)vertdata[3*vert_globalid[0]+1] + local_dir[1] * (mjtNum)vertdata[3*vert_globalid[0]+1] +
dir[2] * (mjtNum)vertdata[3*vert_globalid[0]+2]; local_dir[2] * (mjtNum)vertdata[3*vert_globalid[0]+2];
// hill-climb until no change // hill-climb until no change
change = 1; change = 1;
@@ -213,10 +222,10 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
change = 0; change = 0;
int i = vert_edgeadr[ibest]; int i = vert_edgeadr[ibest];
while ((locid=edge_localid[i]) >= 0) { while ((locid=edge_localid[i]) >= 0) {
// vdot = dot(vertex, dir) // vdot = dot(vertex, local_dir)
vdot = dir[0] * (mjtNum)vertdata[3*vert_globalid[locid]] + vdot = local_dir[0] * (mjtNum)vertdata[3*vert_globalid[locid]] +
dir[1] * (mjtNum)vertdata[3*vert_globalid[locid]+1] + local_dir[1] * (mjtNum)vertdata[3*vert_globalid[locid]+1] +
dir[2] * (mjtNum)vertdata[3*vert_globalid[locid]+2]; local_dir[2] * (mjtNum)vertdata[3*vert_globalid[locid]+2];
// update best // update best
if (vdot > tmp) { if (vdot > tmp) {
@@ -231,7 +240,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
} }
// record best vertex index, in locid format // record best vertex index, in locid format
((mjtCCD*)ccd)->meshindex = ibest; obj->meshindex = ibest;
// map best index to globalid // map best index to globalid
ibest = vert_globalid[ibest]; ibest = vert_globalid[ibest];
@@ -255,16 +264,16 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
mjERROR("ccd support function is undefined for geom type %d", m->geom_type[g]); mjERROR("ccd support function is undefined for geom type %d", m->geom_type[g]);
} }
// add dir*margin/2 to result // add local_dir*margin/2 to result
for (int i=0; i < 3; i++) { for (int i=0; i < 3; i++) {
res[i] += dir[i] * ccd->margin/2; res[i] += local_dir[i] * obj->margin/2;
} }
// rotate result to global frame // rotate result to global frame
mju_mulMatVec3(vec->v, d->geom_xmat+9*g, res); mju_mulMatVec3(res, d->geom_xmat+9*g, res);
// add geom position // add geom position
mju_addTo3(vec->v, d->geom_xpos+3*g); mju_addTo3(res, d->geom_xpos+3*g);
} }
@@ -280,7 +289,7 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) {
// find single convex-convex collision, using libccd // find single convex-convex collision, using libccd
static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd, static int mjc_MPRIteration(mjtCCObj* obj1, mjtCCObj* 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;
@@ -351,8 +360,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;
mjtCCD obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}}; mjtCCObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}};
mjtCCD obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}}; mjtCCObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}};
// init ccd structure // init ccd structure
mjc_initCCD(&ccd, m); mjc_initCCD(&ccd, m);
@@ -480,7 +489,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;
mjtCCD obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}}; mjtCCObj 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]);
@@ -661,7 +670,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;
mjtCCD obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}}; mjtCCObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}};
ccd_t ccd; ccd_t ccd;
// point size1 to hfield size instead of geom1 size // point size1 to hfield size instead of geom1 size
@@ -1094,8 +1103,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;
mjtCCD obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}}; mjtCCObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}};
mjtCCD obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}}; mjtCCObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}};
// init ccd structure // init ccd structure
mjc_initCCD(&ccd, m); mjc_initCCD(&ccd, m);
@@ -1142,7 +1151,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;
mjtCCD obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}}; mjtCCObj obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}};
ccd_t ccd; ccd_t ccd;
//------------------------------------- AABB computation, box-box test //------------------------------------- AABB computation, box-box test
src/engine/engine_collision_convex.h
+9 -4
View File
@@ -25,6 +25,7 @@
#include <mujoco/mjdata.h> #include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#define mjGETINFO_HFIELD \ #define mjGETINFO_HFIELD \
const mjtNum* pos1 = d->geom_xpos + 3*g1; \ const mjtNum* pos1 = d->geom_xpos + 3*g1; \
@@ -38,8 +39,8 @@
extern "C" { extern "C" {
#endif #endif
// ccd general object type // internal object type for convex collision algorithms
struct _mjtCCD { struct _mjtCCObj {
const mjModel* model; const mjModel* model;
const mjData* data; const mjData* data;
int geom; int geom;
@@ -50,13 +51,17 @@ struct _mjtCCD {
mjtNum margin; mjtNum margin;
mjtNum rotate[4]; mjtNum rotate[4];
}; };
typedef struct _mjtCCD mjtCCD; typedef struct _mjtCCObj mjtCCObj;
// support function for convex collision algorithms
void mjc_support(mjtNum res[3], mjtCCObj* obj, const mjtNum dir[3]);
// center function for convex collision algorithms
void mjc_center(mjtNum res[3], const mjtCCObj *obj);
// ccd support function // ccd support function
void mjccd_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec); void mjccd_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec);
// pairwise geom collision functions using ccd // pairwise geom collision functions using ccd
int mjc_PlaneConvex (const mjModel* m, const mjData* d, int mjc_PlaneConvex (const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin); mjContact* con, int g1, int g2, mjtNum margin);