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

Use squared distance for sphere comparison in collision filtering.

Browse Source 
PiperOrigin-RevId: 476165394
Change-Id: Id53493fc7cf4c10a22f5f885baeec0ed4b1d8574
This commit is contained in:
Kyle Bayes
2022-09-22 12:17:42 -07:00
committed by Copybara-Service
parent c13979cd17
commit 36b47e56d0
1 changed files with 11 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_driver.c
+11 -4
View File
@@ -491,6 +491,12 @@ static mjtNum plane_geom(const mjModel* m, mjData* d, int g1, int g2) {
return mju_dot3(dif, norm);
}
// squared Euclidean distance between 3D vectors
static inline mjtNum squaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]};
return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2];
}
// test two geoms for collision, apply filters, add to contact list
// flg_user disables filters and uses usermargin
@@ -576,10 +582,11 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
}
// bounding sphere filter
if (m->geom_rbound[g1]>0 && m->geom_rbound[g2]>0 &&
(mju_dist3(d->geom_xpos+3*g1, d->geom_xpos+3*g2) >
m->geom_rbound[g1] + m->geom_rbound[g2] + margin)) {
return;
if (m->geom_rbound[g1]>0 && m->geom_rbound[g2]>0) {
mjtNum bound = m->geom_rbound[g1] + m->geom_rbound[g2] + margin;
if (squaredDist3(d->geom_xpos+3*g1, d->geom_xpos+3*g2) > bound*bound) {
return;
}
}
// plane : bounding sphere filter