From 8a8305d3ca07f2babc7d4a38b2eadc892856f396 Mon Sep 17 00:00:00 2001
From: Nimrod Gileadi <nimrod@google.com>
Date: Thu, 29 Sep 2022 09:33:12 -0700
Subject: [PATCH] Avoid unnecessary copies after of mju_mulQuat.

Since fdbbc8bbe09b4d7ce97f71bdfdf0cb66c549794d, mju_mulQuat allows in-place computations.
There were a few places where the result was unnecessarily assigned to a temporary variable.

PiperOrigin-RevId: 477746005
Change-Id: I7351ef4796e7d70a4a54b930037bfaadee781f12
---
 src/engine/engine_core_smooth.c  | 5 ++---
 src/engine/engine_sensor.c       | 3 +--
 src/engine/engine_util_solve.c   | 6 ++----
 src/engine/engine_util_spatial.c | 5 ++---
 src/engine/engine_vis_interact.c | 7 ++++---
 5 files changed, 11 insertions(+), 15 deletions(-)

diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c
index 3acd5a51..bfcbc460 100644
--- a/src/engine/engine_core_smooth.c
+++ b/src/engine/engine_core_smooth.c
@@ -35,7 +35,7 @@
 // forward kinematics
 void mj_kinematics(const mjModel* m, mjData* d) {
   mjtNum pos[3], quat[4], *bodypos, *bodyquat;
-  mjtNum qloc[4], qtmp[4], vec[3], vec1[3], xanchor[3], xaxis[3];
+  mjtNum qloc[4], vec[3], vec1[3], xanchor[3], xaxis[3];
 
   // set world position and orientation
   mju_zero3(d->xpos);
@@ -124,8 +124,7 @@ void mj_kinematics(const mjModel* m, mjData* d) {
           }
 
           // apply rotation
-          mju_mulQuat(qtmp, quat, qloc);
-          mju_copy4(quat, qtmp);
+          mju_mulQuat(quat, quat, qloc);
 
           // correct for off-center rotation
           mju_sub3(vec, xanchor, pos);
diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c
index 9efd8634..feb3b3fe 100644
--- a/src/engine/engine_sensor.c
+++ b/src/engine/engine_sensor.c
@@ -90,8 +90,7 @@ static void add_noise(const mjModel* m, mjData* d, mjtStage stage) {
         // quaternion
         else if (m->sensor_datatype[i]==mjDATATYPE_QUATERNION) {
           // apply quaternion rotation to quaternion, assign
-          mju_mulQuat(res, d->sensordata+adr, quat);
-          mju_copy4(d->sensordata+adr, res);
+          mju_mulQuat(d->sensordata+adr, d->sensordata+adr, quat);
         }
 
         // unknown datatype
diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c
index 50f152d1..1d1c2b2b 100644
--- a/src/engine/engine_util_solve.c
+++ b/src/engine/engine_util_solve.c
@@ -493,8 +493,7 @@ int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9]
     mju_normalize4(tmp);
 
     // accumulate quaternion rotation
-    mju_mulQuat(tmp+4, quat, tmp);
-    mju_copy4(quat, tmp+4);
+    mju_mulQuat(quat, quat, tmp);
     mju_normalize4(quat);
   }
 
@@ -512,8 +511,7 @@ int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9]
       tmp[0] = 0.707106781186548;     // mju_cos(pi/4) = mju_sin(pi/4)
       tmp[1] = tmp[2] = tmp[3] = 0;
       tmp[(j1+2)%3+1] = tmp[0];
-      mju_mulQuat(tmp+4, quat, tmp);
-      mju_copy4(quat, tmp+4);
+      mju_mulQuat(quat, quat, tmp);
       mju_normalize4(quat);
     }
   }
diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c
index 0eb3955f..7b96d902 100644
--- a/src/engine/engine_util_spatial.c
+++ b/src/engine/engine_util_spatial.c
@@ -240,9 +240,8 @@ void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale) {
   mju_copy3(tmp, vel);
   angle = scale * mju_normalize3(tmp);
   mju_axisAngle2Quat(qrot, tmp, angle);
-  mju_mulQuat(tmp, quat, qrot);
-  mju_normalize4(tmp);
-  mju_copy4(quat, tmp);
+  mju_mulQuat(quat, quat, qrot);
+  mju_normalize4(quat);
 }
 
 
diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c
index 778b2b10..750553e3 100644
--- a/src/engine/engine_vis_interact.c
+++ b/src/engine/engine_vis_interact.c
@@ -374,7 +374,7 @@ void mjv_moveCamera(const mjModel* m, int action, mjtNum reldx, mjtNum reldy,
 void mjv_movePerturb(const mjModel* m, const mjData* d, int action, mjtNum reldx,
                      mjtNum reldy, const mjvScene* scn, mjvPerturb* pert) {
   int sel = pert->select;
-  mjtNum forward[3], vec[3], dif[3], scl, q1[4], q2[4], xiquat[4];
+  mjtNum forward[3], vec[3], scl, q1[4], xiquat[4];
 
   // get camera info and align
   mjv_cameraInModel(NULL, forward, NULL, scn);
@@ -394,8 +394,7 @@ void mjv_movePerturb(const mjModel* m, const mjData* d, int action, mjtNum reldx
 
     // make quaternion and apply
     mju_axisAngle2Quat(q1, vec, scl*mjPI*2);
-    mju_mulQuat(q2, q1, pert->refquat);
-    mju_copy4(pert->refquat, q2);
+    mju_mulQuat(pert->refquat, q1, pert->refquat);
     mju_normalize4(pert->refquat);
 
     // compute xiquat
@@ -404,10 +403,12 @@ void mjv_movePerturb(const mjModel* m, const mjData* d, int action, mjtNum reldx
     // limit rotation relative to selected body
     if (sel>0 && sel<m->nbody) {
       // q2 = neg(selbody) * refquat
+      mjtNum q2[4];
       mju_negQuat(q1, xiquat);
       mju_mulQuat(q2, q1, pert->refquat);
 
       // convert q2 to axis-angle
+      mjtNum dif[3];
       mju_quat2Vel(dif, q2, 1);
       scl = mju_normalize3(dif);