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

Fix bad contact from passing through polytope2 in nativeccd.

Browse Source 
PiperOrigin-RevId: 742666700
Change-Id: I76034579b4e663f9abfa265ec3e2bc608f631f59
This commit is contained in:
Kyle Bayes
2025-04-01 06:10:30 -07:00
committed by Copybara-Service
parent 0f456486a4
commit 9ecec6f238
3 changed files with 155 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+23 -3
View File
@@ -903,6 +903,26 @@ static void rotmat(mjtNum R[9], const mjtNum axis[3]) {
// return nonzero if the ray v1v2 intersects the triangle v3v4v5
static inline int rayTriangle(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3],
const mjtNum v4[3], const mjtNum v5[3]) {
mjtNum diff12[3], diff13[3], diff14[3], diff15[3];
sub3(diff12, v2, v1);
sub3(diff13, v3, v1);
sub3(diff14, v4, v1);
sub3(diff15, v5, v1);
mjtNum vol1 = det3(diff13, diff14, diff12);
mjtNum vol2 = det3(diff14, diff15, diff12);
mjtNum vol3 = det3(diff15, diff13, diff12);
if (vol1 >= 0 && vol2 >= 0 && vol3 >= 0) return 1;
if (vol1 <= 0 && vol2 <= 0 && vol3 <= 0) return -1;
return 0;
}
// create a polytope from a 1-simplex (returns 0 on success)
static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum *v1 = status->simplex[0].vert, *v2 = status->simplex[1].vert;
@@ -970,9 +990,9 @@ static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
return polytope3(pt, status, obj1, obj2);
}
// check that origin is in the hexahedron
if (status->dist > 10*mjMINVAL && !testTetra(v1, v3, v4, v5) && !testTetra(v2, v3, v4, v5)) {
return mjEPA_P2_MISSING_ORIGIN;
// check hexahedron is convex
if (!rayTriangle(v1, v2, v3, v4, v5)) {
return mjEPA_P2_NONCONVEX;
}
for (int i = 0; i < 6; i++) {
src/engine/engine_collision_gjk.h
+1 -1
View File
@@ -44,7 +44,7 @@ typedef enum {
mjEPA_NOCONTACT = -1,
mjEPA_SUCCESS = 0,
mjEPA_P2_INVALID_FACES,
mjEPA_P2_MISSING_ORIGIN,
mjEPA_P2_NONCONVEX,
mjEPA_P2_ORIGIN_ON_FACE,
mjEPA_P3_BAD_NORMAL,
mjEPA_P3_INVALID_V4,
test/engine/engine_collision_gjk_test.cc
+131 -74
View File
@@ -93,7 +93,7 @@ mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
return dist;
}
int Penetration(mjtNum& depth, std::vector<mjtNum>& dir,
int Penetration(mjCCDStatus& status, mjtNum& depth, std::vector<mjtNum>& dir,
std::vector<mjtNum>& pos, mjModel* model, mjData* data,
int g1, int g2, mjtNum margin = 0, int max_contacts = 1) {
mjCCDObj obj1, obj2;
@@ -128,7 +128,6 @@ int Penetration(mjtNum& depth, std::vector<mjtNum>& dir,
#endif
mjCCDConfig config;
mjCCDStatus status;
// set config
config.max_iterations = kMaxIterations;
@@ -235,9 +234,11 @@ TEST_F(MjGjkTest, SphereSphereNoDist) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2);
int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2);
EXPECT_EQ(ncons, 0);
mj_deleteData(data);
@@ -262,9 +263,11 @@ TEST_F(MjGjkTest, SphereSphereIntersect) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2);
int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2);
EXPECT_EQ(ncons, 1);
@@ -304,9 +307,10 @@ TEST_F(MjGjkTest, BoxBoxDepth) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2);
int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2);
EXPECT_EQ(ncons, 1);
@@ -354,9 +358,11 @@ TEST_F(MjGjkTest, BoxBoxDepth2) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2);
int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2);
if (ncons == 1) {
EXPECT_NEAR(dist, -0.033401579411886845, kTolerance);
@@ -421,9 +427,11 @@ TEST_F(MjGjkTest, BoxBoxDepth3) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2);
int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2);
EXPECT_EQ(ncons, 1);
EXPECT_NEAR(dist, -0.003066, kTolerance);
@@ -453,11 +461,14 @@ TEST_F(MjGjkTest, BoxBoxTouching) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2);
int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2);
EXPECT_EQ(ncons, 0);
EXPECT_GT(status.epa_status, 0);
mj_deleteData(data);
mj_deleteModel(model);
@@ -479,11 +490,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD) {
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");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
EXPECT_EQ(ncons, 4);
EXPECT_NEAR(dist, -.1, kTolerance);
@@ -517,11 +530,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD2) {
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");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
EXPECT_EQ(ncons, 4);
EXPECT_NEAR(dist, -.1, kTolerance);
@@ -573,11 +588,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD3) {
xpos[2] = 1.095456702630382306296041861060;
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
EXPECT_EQ(ncons, 4);
mj_deleteData(data);
@@ -634,11 +651,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD4) {
xpos[1] = -0.023500601273213628239489025873;
xpos[2] = -4.958782854594746325460619118530;
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
EXPECT_EQ(ncons, 8);
EXPECT_NEAR(dist, -0.00060425119242707459, kTolerance);
@@ -701,11 +720,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD5) {
xpos[2] = -4.659108354876987156956147373421;
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
EXPECT_EQ(ncons, 8);
EXPECT_NEAR(dist, -0.0001077858631973211, kTolerance);
@@ -750,11 +771,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD6) {
xpos[1] = 0.190777715293135141649827346555;
xpos[2] = 0.100006658017411736993906856696;
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
EXPECT_EQ(ncons, 5);
EXPECT_NEAR(dist, -0.00009843, kTolerance);
@@ -817,11 +840,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD7) {
xpos[2] = -4.958375812037025376355359185254;
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
EXPECT_EQ(ncons, 8);
mj_deleteData(data);
@@ -878,11 +903,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD8) {
xpos[1] = -0.023505499999999998617106200527;
xpos[2] = -4.958574289672835533338002278470;
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
EXPECT_EQ(ncons, 4);
mj_deleteData(data);
@@ -940,11 +967,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD9) {
xpos[2] = 0.2156259187793853615566774806211469694972;
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
EXPECT_EQ(ncons, 4);
mj_deleteData(data);
@@ -979,11 +1008,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD10) {
xpos[1] = -0.0765140000000000264357424839545274153352;
xpos[2] = 0.1751399999999999623767621415026951581240;
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 8);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8);
EXPECT_EQ(ncons, 4);
@@ -1030,9 +1061,11 @@ TEST_F(MjGjkTest, SmallBoxMesh) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2);
int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2);
EXPECT_EQ(ncons, 1);
EXPECT_NEAR(dist, 0, kTolerance);
@@ -1072,11 +1105,13 @@ TEST_F(MjGjkTest, BoxMesh) {
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");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom2, geom1, 0, 1000);
int ncons = Penetration(status, dist, dir, pos, model, data, g2, g1, 0, 1000);
EXPECT_EQ(ncons, 4);
mj_deleteData(data);
@@ -1105,11 +1140,13 @@ TEST_F(MjGjkTest, BoxMesh2) {
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");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom2, geom1, 0, 1000);
int ncons = Penetration(status, dist, dir, pos, model, data, g2, g1, 0, 1000);
EXPECT_EQ(ncons, 5);
mj_deleteData(data);
@@ -1138,11 +1175,13 @@ TEST_F(MjGjkTest, BoxMeshPrune) {
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");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom2, geom1, 0, 4);
int ncons = Penetration(status, dist, dir, pos, model, data, g2, g1, 0, 4);
EXPECT_EQ(ncons, 4);
mj_deleteData(data);
@@ -1173,11 +1212,13 @@ TEST_F(MjGjkTest, MeshMesh) {
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");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
EXPECT_EQ(ncons, 5);
mj_deleteData(data);
@@ -1208,11 +1249,13 @@ TEST_F(MjGjkTest, MeshMeshPrune) {
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");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4);
EXPECT_EQ(ncons, 4);
mj_deleteData(data);
@@ -1246,11 +1289,13 @@ TEST_F(MjGjkTest, BoxEdge) {
mjData* data = mj_makeData(model);
mj_forward(model, data);
int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3");
int g1 = mj_name2id(model, mjOBJ_GEOM, "box2");
int g2 = mj_name2id(model, mjOBJ_GEOM, "box3");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4);
EXPECT_EQ(ncons, 2);
mj_deleteData(data);
@@ -1319,11 +1364,13 @@ TEST_F(MjGjkTest, BoxEdge2) {
xpos[1] = 0.9828851949225971829093850828940048813820;
xpos[2] = 3.0930077345364814789263618877157568931580;
int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3");
int g1 = mj_name2id(model, mjOBJ_GEOM, "box2");
int g2 = mj_name2id(model, mjOBJ_GEOM, "box3");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4);
EXPECT_EQ(ncons, 2);
mj_deleteData(data);
@@ -1391,11 +1438,13 @@ TEST_F(MjGjkTest, BoxEdgeEdge) {
xpos[1] = -0.0000000000000000008679606505055748997840;
xpos[2] = 2.8141526153588731773425024584867060184479;
int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3");
int g1 = mj_name2id(model, mjOBJ_GEOM, "box2");
int g2 = mj_name2id(model, mjOBJ_GEOM, "box3");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4);
EXPECT_EQ(ncons, 2);
mj_deleteData(data);
@@ -1439,11 +1488,13 @@ TEST_F(MjGjkTest, MeshEdge) {
mjData* data = mj_makeData(model);
mj_forward(model, data);
int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3");
int g1 = mj_name2id(model, mjOBJ_GEOM, "box2");
int g2 = mj_name2id(model, mjOBJ_GEOM, "box3");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4);
EXPECT_EQ(ncons, 2);
mj_deleteData(data);
@@ -1461,9 +1512,11 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2);
int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2);
EXPECT_EQ(ncons, 1);
EXPECT_NEAR(dist, -0.00022548856248122027, kTolerance);
@@ -1542,11 +1595,13 @@ static constexpr char xml[] = R"(
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");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2);
EXPECT_EQ(ncons, 1);
EXPECT_NEAR(dist, -0.01, kTolerance);
@@ -1560,7 +1615,7 @@ static constexpr char xml[] = R"(
EXPECT_NEAR(pos[2], -0.005, kTolerance);
// multicontact
ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000);
ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
EXPECT_EQ(ncons, 4);
mj_deleteData(data);
@@ -1583,11 +1638,13 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) {
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");
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 15);
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 15);
EXPECT_EQ(ncons, 1);
EXPECT_NEAR(dist, -14.245732934582151, kTolerance);