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

Change elevation to [0,pi] in mj_multiRay.

Browse Source 
PiperOrigin-RevId: 529393944
Change-Id: I23f3113fd91e909fa1dc23b620d66e963db59355
This commit is contained in:
Alessio Quaglino
2023-05-04 06:42:03 -07:00
committed by Copybara-Service
parent 7e49d935ba
commit 267299fb59
2 changed files with 11 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_ray.c
+5 -9
View File
@@ -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
test/engine/engine_ray_test.cc
+6 -6
View File
@@ -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);