diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index b7f06601..03435f48 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -107,12 +107,12 @@ static mjtNum wrap_circle(mjtNum pnt[4], const mjtNum end[4], const mjtNum* side return -1; } + mjtNum sqrt0 = mju_sqrt(sqlen0 - sqrad); + mjtNum sqrt1 = mju_sqrt(sqlen1 - sqrad); + // construct the two solutions, compute goodness mjtNum sol[2][2][2], good[2]; 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); sol[i][0][0] = (end[0]*sqrad + sgn*radius*end[1]*sqrt0)/sqlen0;