Avoid square root in mju_makeFrame

yuvaltassa · copybara-github · commit d3664b5d0192 · 2025-04-10T04:41:23.000-07:00
PiperOrigin-RevId: 745974380
Change-Id: I90879fac05e62daf60acd2223f814e833634b242
diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c
@@ -532,7 +532,7 @@ void mju_makeFrame(mjtNum frame[9]) {
   }
 
   // if yaxis undefined, set yaxis to (0,1,0) if possible, otherwise (0,0,1)
-  if (mju_norm3(frame+3) < 0.5) {
+  if (mju_dot3(frame+3, frame+3) < 0.25) {
     mju_zero3(frame+3);
 
     if (frame[1] < 0.5 && frame[1] > -0.5) {

Original file line number	Diff line number	Diff line change
`@@ -532,7 +532,7 @@ void mju_makeFrame(mjtNum frame[9]) {`
`532`	`532`	`}`
`533`	`533`
`534`	`534`	`// if yaxis undefined, set yaxis to (0,1,0) if possible, otherwise (0,0,1)`
`535`		`- if (mju_norm3(frame+3) < 0.5) {`
	`535`	`+ if (mju_dot3(frame+3, frame+3) < 0.25) {`
`536`	`536`	`mju_zero3(frame+3);`
`537`	`537`
`538`	`538`	`if (frame[1] < 0.5 && frame[1] > -0.5) {`