Improve wrap_circle.

PiperOrigin-RevId: 756704385
Change-Id: Id44ba317c6072790763a958f4227a76278d3ab74

Author: thowell

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;