From 77b3533d908b52d9c9bbcae373cd0a6199e2141f Mon Sep 17 00:00:00 2001
From: Yuval Tassa <tassa@google.com>
Date: Tue, 11 Jul 2023 12:34:10 -0700
Subject: [PATCH] Improve `mju_cross` implementation.

- Allow in-place cross product a = a x b using `mju_cross(a, a, b)`.
- Easier for compiler to optimize since tmp cannot alias a or b (verified with godbolt).

PiperOrigin-RevId: 547265908
Change-Id: If965867e08c1bc980e71729e38f6855e32de18b2
---
 src/engine/engine_util_spatial.c | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c
index 247a44d2..576d0274 100644
--- a/src/engine/engine_util_spatial.c
+++ b/src/engine/engine_util_spatial.c
@@ -323,9 +323,14 @@ void mju_trnVecPose(mjtNum res[3], const mjtNum pos[3], const mjtNum quat[4], co
 
 // vector cross-product, 3D
 void mju_cross(mjtNum res[3], const mjtNum a[3], const mjtNum b[3]) {
-  res[0] = a[1]*b[2] - a[2]*b[1];
-  res[1] = a[2]*b[0] - a[0]*b[2];
-  res[2] = a[0]*b[1] - a[1]*b[0];
+  mjtNum tmp[3] = {
+    a[1]*b[2] - a[2]*b[1],
+    a[2]*b[0] - a[0]*b[2],
+    a[0]*b[1] - a[1]*b[0]
+  };
+  res[0] = tmp[0];
+  res[1] = tmp[1];
+  res[2] = tmp[2];
 }