From 36b47e56d08e04cee90adc207105c2c58271f998 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Thu, 22 Sep 2022 12:17:42 -0700 Subject: [PATCH] Use squared distance for sphere comparison in collision filtering. PiperOrigin-RevId: 476165394 Change-Id: Id53493fc7cf4c10a22f5f885baeec0ed4b1d8574 --- src/engine/engine_collision_driver.c | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 36a78984..6e101d29 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -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