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

Correct projected origin on face when the magnitude of face->v becomes very small in EPA.

Browse Source 
PiperOrigin-RevId: 929768876
Change-Id: I5daf0203e998aa1489d9ce4c78cbf0764a8963e3
This commit is contained in:
Kyle Bayes
2026-06-10 04:47:32 -07:00
committed by Copybara-Service
parent aa494e45d4
commit 2c5b8cae6c
2 changed files with 147 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+25 -2
View File
@@ -93,6 +93,7 @@ typedef struct {
Face* faces; // list of faces that make up the polytope
int nfaces; // number of faces
int maxfaces; // max number of faces that can be stored in polytope
mjtNum center[3]; // center of the polytope
Face** map; // linear map storing faces
int nmap; // number of faces in map
struct Horizon { // polytope boundary edges that can be seen from w
@@ -929,6 +930,10 @@ static inline int rayTriangle(const mjtNum v1[3], const mjtNum v2[3], const mjtN
static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum *v1 = status->simplex[0].vert, *v2 = status->simplex[1].vert;
// set the polytope center
add3(pt->center, v1, v2);
scl3(pt->center, pt->center, 0.5);
mjtNum diff[3];
sub3(diff, v2, v1);
@@ -997,13 +1002,12 @@ static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
return mjEPA_P2_NONCONVEX;
}
// populate face map
for (int i = 0; i < 6; i++) {
pt->map[i] = pt->faces + i;
pt->faces[i].index = i;
}
pt->nmap = 6;
// valid hexahedron for EPA
return 0;
}
@@ -1077,6 +1081,11 @@ static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
*v2 = status->simplex[1].vert,
*v3 = status->simplex[2].vert;
// set the polytope center
add3(pt->center, v1, v2);
add3(pt->center, pt->center, v3);
scl3(pt->center, pt->center, 1.0 / 3.0);
// get normals in both directions
mjtNum diff1[3], diff2[3], n[3], n_neg[3];
sub3(diff1, v2, v1);
@@ -1156,6 +1165,12 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
int v3 = insertVertex(pt, status->simplex + 2);
int v4 = insertVertex(pt, status->simplex + 3);
// set the polytope center
add3(pt->center, pt->verts[v1].vert, pt->verts[v2].vert);
add3(pt->center, pt->center, pt->verts[v3].vert);
add3(pt->center, pt->center, pt->verts[v4].vert);
scl3(pt->center, pt->center, 0.25);
// if the origin is on a face, replace the 3-simplex with a 2-simplex
if (attachFace(pt, v1, v2, v3, 1, 3, 2) < mjMINDIST4) {
replaceSimplex3(pt, status, v1, v2, v3);
@@ -1229,6 +1244,14 @@ static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3,
if (ret) {
return 0;
}
// ensure projection points outward from the polytope
mjtNum outward[3];
sub3(outward, pt->verts[v1].vert, pt->center);
if (dot3(face->v, outward) < 0) {
scl3(face->v, face->v, -1);
}
face->dist2 = dot3(face->v, face->v);
face->index = -1;
test/engine/engine_collision_gjk_test.cc
+122
View File
@@ -470,6 +470,128 @@ TEST_F(MjGjkTest, BoxBoxSize05) {
ASSERT_EQ(ncons, 4);
}
TEST_F(MjGjkTest, BoxBoxSize05b) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" size="0.5 0.5 0.5"/>
<geom name="geom2" type="box" size="0.5 0.5 0.5"/>
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat;
mjtNum* xpos = data->geom_xpos;
xmat[0] = 1.000000000000000;
xmat[1] = -0.000000008764291;
xmat[2] = 0.000000386995168;
xmat[3] = 0.000000008764733;
xmat[4] = 1.000000000000000;
xmat[5] = -0.000001144207772;
xmat[6] = -0.000000386995168;
xmat[7] = 0.000001144207772;
xmat[8] = 1.000000000000000;
xpos[0] = 0.000000962082822;
xpos[1] = -0.000001747370789;
xpos[2] = 3.469238519668579;
xmat = data->geom_xmat + 9;
xpos = data->geom_xpos + 3;
xmat[0] = 1.000000000000000;
xmat[1] = 0.000000003313412;
xmat[2] = -0.000000196321196;
xmat[3] = -0.000000003313673;
xmat[4] = 1.000000000000000;
xmat[5] = -0.000001329654879;
xmat[6] = 0.000000196321196;
xmat[7] = 0.000001329654879;
xmat[8] = 1.000000000000000;
xpos[0] = 0.000002897753802;
xpos[1] = -0.000004625266229;
xpos[2] = 4.435211658477783;
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model.get(), mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4);
ASSERT_EQ(ncons, 4);
EXPECT_NEAR(dir[0], 0, 1e-5);
EXPECT_NEAR(dir[1], 0, 1e-5);
EXPECT_NEAR(dir[2], 1, kTolerance);
}
TEST_F(MjGjkTest, BoxBoxSize05c) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" size="0.5 0.5 0.5"/>
<geom name="geom2" type="box" size="0.5 0.5 0.5"/>
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat;
mjtNum* xpos = data->geom_xpos;
xmat[0] = 1.000000000000000;
xmat[1] = -0.000000000570266;
xmat[2] = 0.000000168314983;
xmat[3] = 0.000000000570290;
xmat[4] = 1.000000000000000;
xmat[5] = -0.000000142656916;
xmat[6] = -0.000000168314983;
xmat[7] = 0.000000142656916;
xmat[8] = 1.000000000000000;
xpos[0] = 0.000000188941314;
xpos[1] = -0.000000195227585;
xpos[2] = 0.497281551361084;
xmat = data->geom_xmat + 9;
xpos = data->geom_xpos + 3;
xmat[0] = 1.000000000000000;
xmat[1] = -0.000000000058608;
xmat[2] = 0.000001607574859;
xmat[3] = 0.000000000060496;
xmat[4] = 1.000000000000000;
xmat[5] = -0.000001174638669;
xmat[6] = -0.000001607574859;
xmat[7] = 0.000001174638669;
xmat[8] = 1.000000000000000;
xpos[0] = 0.000000953407323;
xpos[1] = -0.000000923845278;
xpos[2] = 1.493984460830688;
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model.get(), mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4);
ASSERT_EQ(ncons, 4);
EXPECT_NEAR(dir[0], 0, 1e-5);
EXPECT_NEAR(dir[1], 0, 1e-5);
EXPECT_NEAR(dir[2], 1, kTolerance);
}
TEST_F(MjGjkTest, BoxBoxTouching) {
static constexpr char xml[] = R"(
<mujoco>