diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index 04ed3408..0e85da07 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -56,11 +56,7 @@ static mjtNum longitude(const mjtNum vec[3]) { // map to elevation angle in spherical coordinates static mjtNum latitude(const mjtNum vec[3]) { - if (vec[2]>0) { - return mju_atan2(mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1]), vec[2]); - } else { - return mju_atan2(mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1]), vec[2]) - mjPI; - } + return mju_atan2(mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1]), vec[2]); } @@ -892,9 +888,9 @@ void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3], if (point_in_box(aabb, xpos, xmat, pnt)) { (geom_ba+4*g)[0] = -mjPI; - (geom_ba+4*g)[1] = -mjPI/2; + (geom_ba+4*g)[1] = 0; (geom_ba+4*g)[2] = mjPI; - (geom_ba+4*g)[3] = mjPI/2; + (geom_ba+4*g)[3] = mjPI; continue; } @@ -923,9 +919,9 @@ void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3], if (AABB[2]-AABB[0] > mjPI) { AABB[0] = -mjPI; - AABB[1] = -mjPI/2; + AABB[1] = 0; AABB[2] = mjPI; - AABB[3] = mjPI/2; + AABB[3] = mjPI; } if (AABB[3]-AABB[1] > mjPI) { // SHOULD NOT OCCUR diff --git a/test/engine/engine_ray_test.cc b/test/engine/engine_ray_test.cc index 2726967a..c6d8d3f5 100644 --- a/test/engine/engine_ray_test.cc +++ b/test/engine/engine_ray_test.cc @@ -203,9 +203,9 @@ TEST_F(RayTest, EdgeCases) { mjtNum pnt1[] = {0, 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], -mjPI/2); + EXPECT_FLOAT_EQ(geom_ba[1], 0); EXPECT_FLOAT_EQ(geom_ba[2], mjPI); - EXPECT_FLOAT_EQ(geom_ba[3], mjPI/2); + EXPECT_FLOAT_EQ(geom_ba[3], mjPI); mjtNum vec1[] = {1, 0, 0}; mj_multiRay(m, d, pnt1, vec1, NULL, 1, -1, &rgeomid, &dist, 1); EXPECT_FLOAT_EQ(dist, 0.1); @@ -222,8 +222,8 @@ TEST_F(RayTest, EdgeCases) { // pnt on the boundary of the box mjtNum pnt3[] = {.1, .1, .05}; mju_multiRayPrepare(m, d, pnt3, NULL, NULL, 1, -1, geom_ba, NULL); - EXPECT_FLOAT_EQ(geom_ba[1], -mjPI/2); - EXPECT_FLOAT_EQ(geom_ba[3], mjPI/2); + EXPECT_FLOAT_EQ(geom_ba[1], 0); + EXPECT_FLOAT_EQ(geom_ba[3], mjPI); mjtNum vec3[] = {1, 1, 0}; mj_multiRay(m, d, pnt3, vec3, NULL, 1, -1, &rgeomid, &dist, 1); EXPECT_FLOAT_EQ(dist, -1); @@ -234,9 +234,9 @@ TEST_F(RayTest, EdgeCases) { 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); EXPECT_FLOAT_EQ(geom_ba[0], 0); - EXPECT_FLOAT_EQ(geom_ba[1], -mjPI/2); + EXPECT_FLOAT_EQ(geom_ba[1], mjPI/2); EXPECT_FLOAT_EQ(geom_ba[2], 0); - EXPECT_FLOAT_EQ(geom_ba[3], -mjPI/2); + EXPECT_FLOAT_EQ(geom_ba[3], mjPI/2); mjtNum vec4[] = {1, 0, 0}; mj_multiRay(m, d, pnt4, vec4, NULL, 1, -1, &rgeomid, &dist, 1); EXPECT_FLOAT_EQ(dist, 0.9);