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

Minor GJK + EPA improvements:

Browse Source 
1. Correct stopping criteria for GJK.
2. Store witness points from EPA in mjCCDObj,
3. Get dir directly from EPA instead of computing dir from witness points.

PiperOrigin-RevId: 666791852
Change-Id: I6a168f635b57c17692d31da6f6076b48c32ff2e3
This commit is contained in:
Kyle Bayes
2024-08-23 07:43:43 -07:00
committed by Copybara-Service
parent 505d01a1c1
commit d360ea1ce6
2 changed files with 29 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_collision_gjk_test.cc
+4 -3
View File
@@ -82,10 +82,11 @@ mjtNum run_gjkPenetration(mjModel* m, mjData* d, int g1, int g2,
ccd_real_t depth;
ccd_vec3_t ccd_dir, ccd_pos;
mj_gjkPenetration(&obj1, &obj2, &ccd, &depth, &ccd_dir, &ccd_pos);
int ret = mj_gjkPenetration(&obj1, &obj2, &ccd, &depth, &ccd_dir, &ccd_pos);
if (ret) return 0; // objects not colliding
if (dir) mju_copy3(dir, ccd_dir.v);
if (pos) mju_copy3(pos, ccd_pos.v);
return depth;
return -depth;
}
using MjGjkTest = MujocoTest;
@@ -170,7 +171,7 @@ TEST_F(MjGjkTest, BoxBoxIntersect) {
mjtNum dir[3], pos[3];
mjtNum dist = run_gjkPenetration(model, data, geom1, geom2, dir, pos);
EXPECT_NEAR(dist, 1, kTolerance);
EXPECT_NEAR(dist, -1, kTolerance);
EXPECT_NEAR(dir[0], 1, kTolerance);
EXPECT_NEAR(dir[1], 0, kTolerance);
EXPECT_NEAR(dir[2], 0, kTolerance);