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

Add height fields to list of discrete objects in NativeCCD.

Browse Source 
PiperOrigin-RevId: 676452215
Change-Id: I5d6a0d1e26ee852c774b44f550c1d0531b7a269b
This commit is contained in:
Kyle Bayes
2024-09-19 09:47:40 -07:00
committed by Copybara-Service
parent 4998e7b392
commit 7a92423cf2
4 changed files with 31 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_convex.c
+18 -10
View File
@@ -163,7 +163,7 @@ void mjc_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
mju_mulMatTVec3(local_dir, d->geom_xmat+9*g, dir);
// compute result according to geom type
switch ((mjtGeom) m->geom_type[g]) {
switch ((mjtGeom) obj->geom_type) {
case mjGEOM_SPHERE:
mju_scl3(res, local_dir, size[0]);
break;
@@ -396,8 +396,10 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
int mjc_Convex(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
ccd_t ccd;
mjCCDObj obj1 = {m, d, g1, -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}, mjc_center, mjc_support};
mjCCDObj obj1 = {m, d, g1, m->geom_type[g1], -1, -1, -1, -1, margin, {1, 0, 0, 0},
mjc_center, mjc_support};
mjCCDObj obj2 = {m, d, g2, m->geom_type[g2], -1, -1, -1, -1, margin, {1, 0, 0, 0},
mjc_center, mjc_support};
// init ccd structure
mjc_initCCD(&ccd, m);
@@ -525,7 +527,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
mjGETINFO
mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]};
ccd_vec3_t dir, vec;
mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}};
mjCCDObj obj = {m, d, g2, m->geom_type[g2], -1, -1, -1, -1, 0, {1, 0, 0, 0}};
// get support point in -normal direction
ccdVec3Set(&dir, -mat1[2], -mat1[5], -mat1[8]);
@@ -700,12 +702,14 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
int ncol = m->hfield_ncol[hid];
int dr[2], cnt, rmin, rmax, cmin, cmax;
const float* data = m->hfield_data + m->hfield_adr[hid];
mjCCDObj obj1 = {m, d, -1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_prism_center, mjc_prism_support};
mjCCDObj obj1 = {m, d, -1, mjGEOM_HFIELD, -1, -1, -1, -1, 0, {1, 0, 0, 0},
mjc_prism_center, mjc_prism_support};
// ccd-related
ccd_vec3_t dirccd, vecccd;
ccd_real_t depth;
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center, mjc_support};
mjCCDObj obj2 = {m, d, g2, m->geom_type[g2], -1, -1, -1, -1, 0, {1, 0, 0, 0},
mjc_center, mjc_support};
ccd_t ccd;
// point size1 to hfield size instead of geom1 size
@@ -1139,8 +1143,11 @@ 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 g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) {
ccd_t ccd;
mjCCDObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}, mjc_center, mjc_support};
mjCCDObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}, mjc_center, mjc_support};
int geom_type = (g1 >= 0) ? m->geom_type[g1] : mjGEOM_FLEX;
mjCCDObj obj1 = {m, d, g1, geom_type, -1, f1, e1, v1, margin, {1, 0, 0, 0},
mjc_center, mjc_support};
mjCCDObj obj2 = {m, d, -1, mjGEOM_FLEX, -1, f2, e2, -1, margin, {1, 0, 0, 0},
mjc_center, mjc_support};
// init ccd structure
mjc_initCCD(&ccd, m);
@@ -1158,7 +1165,7 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
// test a heighfield geom and a flex flex element for collision
// test a height field and a flex element for collision
int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
int g, int f, int e, mjtNum margin) {
mjtNum vec[3], dx, dy;
@@ -1189,7 +1196,8 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
// ccd-related
ccd_vec3_t dirccd, vecccd;
ccd_real_t depth;
mjCCDObj obj2 = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}, mjc_center, mjc_support};
mjCCDObj obj2 = {m, d, -1, mjGEOM_FLEX, -1, f, e, -1, margin, {1, 0, 0, 0},
mjc_center, mjc_support};
ccd_t ccd;
//------------------------------------- AABB computation, box-box test
src/engine/engine_collision_convex.h
+1
View File
@@ -45,6 +45,7 @@ struct _mjCCDObj {
const mjModel* model;
const mjData* data;
int geom;
int geom_type;
int meshindex;
int flex;
int elem;
src/engine/engine_collision_gjk.c
+4 -7
View File
@@ -92,13 +92,10 @@ static int discreteGeoms(mjCCDObj* obj1, mjCCDObj* obj2) {
// non-zero margin makes geoms smooth
if (obj1->margin != 0 || obj2->margin != 0) return 0;
// negative geom indices correspond to flex objects, return
if (obj1->geom < 0 || obj2->geom < 0) return 0;
int g1 = obj1->model->geom_type[obj1->geom];
int g2 = obj2->model->geom_type[obj2->geom];
return (g1 == mjGEOM_MESH || g1 == mjGEOM_BOX) &&
(g2 == mjGEOM_MESH || g2 == mjGEOM_BOX);
int g1 = obj1->geom_type;
int g2 = obj2->geom_type;
return (g1 == mjGEOM_MESH || g1 == mjGEOM_BOX || g1 == mjGEOM_HFIELD) &&
(g2 == mjGEOM_MESH || g2 == mjGEOM_BOX || g2 == mjGEOM_HFIELD);
}
test/engine/engine_collision_gjk_test.cc
+8 -8
View File
@@ -58,10 +58,10 @@ mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
config.contacts = 0; // no geom contacts needed
config.distances = 1;
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};
mjCCDObj obj1 = {m, d, g1, m->geom_type[g1], -1, -1, -1, -1, 0, {1, 0, 0, 0},
mjc_center, mjc_support};
mjCCDObj obj2 = {m, d, g2, m->geom_type[g2], -1, -1, -1, -1, 0, {1, 0, 0, 0},
mjc_center, mjc_support};
mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2);
if (x1 != nullptr) mju_copy3(x1, status.x1);
if (x2 != nullptr) mju_copy3(x2, status.x2);
@@ -103,10 +103,10 @@ int PenetrationWrapper(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd,
mjtNum Penetration(mjModel* m, mjData* d, int g1, int g2,
mjtNum dir[3] = nullptr, mjtNum pos[3] = nullptr,
mjtNum margin = 0) {
mjCCDObj obj1 = {m, d, g1, -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}, mjc_center,
mjc_support};
mjCCDObj obj1 = {m, d, g1, m->geom_type[g1], -1, -1, -1, -1, margin,
{1, 0, 0, 0}, mjc_center, mjc_support};
mjCCDObj obj2 = {m, d, g2, m->geom_type[g2], -1, -1, -1, -1, margin,
{1, 0, 0, 0}, mjc_center, mjc_support};
ccd_t ccd;
// CCD_INIT(&ccd); // uncomment to run ccdMPRPenetration
ccd.mpr_tolerance = kTolerance;