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

Reorganize GJK support logic.

Browse Source 
PiperOrigin-RevId: 649361144
Change-Id: I9868377cf36cccdcf66de3bc27afb872a164da8d
This commit is contained in:
Kyle Bayes
2024-07-04 03:47:26 -07:00
committed by Copybara-Service
parent 4b733b14d7
commit d831153401
1 changed files with 9 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+9 -10
View File
@@ -39,7 +39,7 @@ static void S1D(mjtNum lambda[2], const mjtNum simplex[6]);
// helper function to compute the support point in the Minkowski difference // helper function to compute the support point in the Minkowski difference
static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
const mjtNum dir[3]); const mjtNum x_k[3]);
// linear algebra utility functions // linear algebra utility functions
static mjtNum det3(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]); static mjtNum det3(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]);
@@ -64,11 +64,8 @@ mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum s_k[3]; // the kth support point of Minkowski difference mjtNum s_k[3]; // the kth support point of Minkowski difference
mjtNum lambda[4]; // barycentric coordinates for x_k mjtNum lambda[4]; // barycentric coordinates for x_k
// compute the kth support point in the direction of -(x_k) // compute the kth support point
mjtNum x_k_neg[3]; support(s1, s2, obj1, obj2, x_k);
mju_scl3(x_k_neg, x_k, -1);
mju_normalize3(x_k_neg);
support(s1, s2, obj1, obj2, x_k_neg);
mju_sub3(s_k, s1, s2); mju_sub3(s_k, s1, s2);
// the stopping criteria relies on the Frank-Wolfe duality gap given by // the stopping criteria relies on the Frank-Wolfe duality gap given by
@@ -121,11 +118,13 @@ mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) {
// helper function to compute the support point in the Minkowski difference // computes the support points in obj1 and obj2 for the kth approximation point
static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
const mjtNum dir[3]) { const mjtNum x_k[3]) {
mjtNum dir_neg[3]; mjtNum dir[3], dir_neg[3];
mju_scl3(dir_neg, dir, -1); mju_copy3(dir_neg, x_k);
mju_normalize3(dir_neg); // mjc_support assumes a normalized direction
mju_scl3(dir, dir_neg, -1);
// compute S_{A-B}(dir) = S_A(dir) - S_B(-dir) // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
mjc_support(s1, obj1, dir); mjc_support(s1, obj1, dir);