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

Improve wrap_circle.

Browse Source 
PiperOrigin-RevId: 756704385
Change-Id: Id44ba317c6072790763a958f4227a76278d3ab74
This commit is contained in:
Taylor Howell
2025-05-09 04:12:17 -07:00
committed by Copybara-Service
parent ad80a4625b
commit baf84265b8
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_util_misc.c
+3 -3
View File
@@ -107,12 +107,12 @@ static mjtNum wrap_circle(mjtNum pnt[4], const mjtNum end[4], const mjtNum* side
return -1; return -1;
} }
mjtNum sqrt0 = mju_sqrt(sqlen0 - sqrad);
mjtNum sqrt1 = mju_sqrt(sqlen1 - sqrad);
// construct the two solutions, compute goodness // construct the two solutions, compute goodness
mjtNum sol[2][2][2], good[2]; mjtNum sol[2][2][2], good[2];
for (int i=0; i < 2; i++) { for (int i=0; i < 2; i++) {
mjtNum sqrt0 = mju_sqrt(sqlen0 - sqrad);
mjtNum sqrt1 = mju_sqrt(sqlen1 - sqrad);
int sgn = (i == 0 ? 1 : -1); int sgn = (i == 0 ? 1 : -1);
sol[i][0][0] = (end[0]*sqrad + sgn*radius*end[1]*sqrt0)/sqlen0; sol[i][0][0] = (end[0]*sqrad + sgn*radius*end[1]*sqrt0)/sqlen0;