From 08b2db2f621a00b7d24bc332359923d95c791212 Mon Sep 17 00:00:00 2001
From: Yuval Tassa <tassa@google.com>
Date: Mon, 20 Jun 2022 01:57:05 -0700
Subject: [PATCH] Allow in-place mju_rotVecMat and mju_rotVecMatT (`res=vec`)

PiperOrigin-RevId: 456021131
Change-Id: Id38ff5fabd15706d3087bc95d341b4f7a673449f
---
 src/engine/engine_util_blas.c | 22 ++++++++++++++++------
 1 file changed, 16 insertions(+), 6 deletions(-)

diff --git a/src/engine/engine_util_blas.c b/src/engine/engine_util_blas.c
index b40d901a..d686a925 100644
--- a/src/engine/engine_util_blas.c
+++ b/src/engine/engine_util_blas.c
@@ -154,18 +154,28 @@ mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
 
 // multiply vector by 3D rotation matrix
 void mju_rotVecMat(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) {
-  res[0] = mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2];
-  res[1] = mat[3]*vec[0] + mat[4]*vec[1] + mat[5]*vec[2];
-  res[2] = mat[6]*vec[0] + mat[7]*vec[1] + mat[8]*vec[2];
+  mjtNum tmp[3] = {
+    mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2],
+    mat[3]*vec[0] + mat[4]*vec[1] + mat[5]*vec[2],
+    mat[6]*vec[0] + mat[7]*vec[1] + mat[8]*vec[2]
+  };
+  res[0] = tmp[0];
+  res[1] = tmp[1];
+  res[2] = tmp[2];
 }
 
 
 
 // multiply vector by transposed 3D rotation matrix
 void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) {
-  res[0] = mat[0]*vec[0] + mat[3]*vec[1] + mat[6]*vec[2];
-  res[1] = mat[1]*vec[0] + mat[4]*vec[1] + mat[7]*vec[2];
-  res[2] = mat[2]*vec[0] + mat[5]*vec[1] + mat[8]*vec[2];
+  mjtNum tmp[3] = {
+    mat[0]*vec[0] + mat[3]*vec[1] + mat[6]*vec[2],
+    mat[1]*vec[0] + mat[4]*vec[1] + mat[7]*vec[2],
+    mat[2]*vec[0] + mat[5]*vec[1] + mat[8]*vec[2]
+  };
+  res[0] = tmp[0];
+  res[1] = tmp[1];
+  res[2] = tmp[2];
 }