diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index 0e85da07..4d40040f 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -957,9 +957,11 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co // exclude body using bounding sphere test if (m->body_bvhadr[b] != -1) { mjtNum* pos = m->bvh_aabb + 6*m->body_bvhadr[b]; + mjtNum center[3]; mjtNum* size = pos + 3; mjtNum ssz = size[0]*size[0] + size[1]*size[1] + size[2]*size[2]; - if (ray_sphere(pos, NULL, ssz, pnt, vec)<0) { + mju_add3(center, pos, d->xipos+3*b); + if (ray_sphere(center, NULL, ssz, pnt, vec)<0) { continue; } } diff --git a/test/engine/engine_ray_test.cc b/test/engine/engine_ray_test.cc index c6d8d3f5..abf82a69 100644 --- a/test/engine/engine_ray_test.cc +++ b/test/engine/engine_ray_test.cc @@ -29,7 +29,9 @@ namespace { static constexpr char kSingleGeomModel[] = R"( - + + + )"; @@ -200,7 +202,7 @@ TEST_F(RayTest, EdgeCases) { int rgeomid; // pnt contained in bounding box - mjtNum pnt1[] = {0, 0, 0}; + mjtNum pnt1[] = {-1, 0, 0}; mju_multiRayPrepare(m, d, pnt1, NULL, NULL, 1, -1, geom_ba, NULL); EXPECT_FLOAT_EQ(geom_ba[0], -mjPI); EXPECT_FLOAT_EQ(geom_ba[1], 0); @@ -211,13 +213,13 @@ TEST_F(RayTest, EdgeCases) { EXPECT_FLOAT_EQ(dist, 0.1); // pnt at phi = Pi, -Pi - mjtNum pnt2[] = {1, 0, 0}; + mjtNum pnt2[] = {-.5, 0, 0}; mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, geom_ba, NULL); EXPECT_FLOAT_EQ(geom_ba[0], -mjPI); // atan(y<0, x<0) EXPECT_FLOAT_EQ(geom_ba[2], mjPI); // atan(y>0, x<0) mjtNum vec2[] = {-1, 0, 0}; mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, 1); - EXPECT_FLOAT_EQ(dist, 0.9); + EXPECT_FLOAT_EQ(dist, 0.4); // pnt on the boundary of the box mjtNum pnt3[] = {.1, .1, .05}; @@ -229,7 +231,7 @@ TEST_F(RayTest, EdgeCases) { EXPECT_FLOAT_EQ(dist, -1); // size 0 geom - mjtNum pnt4[] = {-1, 0, 0}; + mjtNum pnt4[] = {-2, 0, 0}; m->geom_aabb[0] = m->geom_aabb[1] = m->geom_aabb[2] = 0; m->geom_aabb[3] = m->geom_aabb[4] = m->geom_aabb[5] = 0; mju_multiRayPrepare(m, d, pnt4, NULL, NULL, 1, -1, geom_ba, NULL);