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

mj_multiRay: Reset elevation angle if crossing the coordinate singularity.

Browse Source 
PiperOrigin-RevId: 529341397
Change-Id: I0ca6ed5b97bcc585b0f426d6fb8a6b4cf9367a2b
This commit is contained in:
Alessio Quaglino
2023-05-04 01:49:41 -07:00
committed by Copybara-Service
parent 3d82d2a40f
commit 7e49d935ba
2 changed files with 9 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_ray.c
+7 -1
View File
@@ -56,7 +56,11 @@ static mjtNum longitude(const mjtNum vec[3]) {
// map to elevation angle in spherical coordinates
static mjtNum latitude(const mjtNum vec[3]) {
return mju_atan2(mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1]), vec[2]);
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;
}
}
@@ -919,7 +923,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[2] = mjPI;
AABB[3] = mjPI/2;
}
if (AABB[3]-AABB[1] > mjPI) { // SHOULD NOT OCCUR