Add mju_mulVecMatVec, mutiplies a square matrix M by a vector x on both sides. Returns x^T * M * x.

PiperOrigin-RevId: 474292806
Change-Id: I3432469dbe1f02ccf5a13241c7aa12d824cbe034

src/engine/engine_solver.c

@@ -190,7 +190,7 @@ static void residual(const mjModel* m, mjData* d, mjtNum* res, int i, int dim, i
 // compute cost change
 static mjtNum costChange(const mjtNum* A, mjtNum* force, const mjtNum* oldforce,
                          const mjtNum* res, int dim) {
-  mjtNum delta[6], v[6], change;
+  mjtNum delta[6], change;
 
   // compute change
   if (dim==1) {
@@ -198,8 +198,7 @@ static mjtNum costChange(const mjtNum* A, mjtNum* force, const mjtNum* oldforce,
     change = 0.5*delta[0]*delta[0]*A[0] + delta[0]*res[0];
   } else {
     mju_sub(delta, force, oldforce, dim);
-    mju_mulMatVec(v, A, delta, dim, dim);
-    change = 0.5*mju_dot(delta, v, dim) + mju_dot(delta, res, dim);
+    change = 0.5*mju_mulVecMatVec(delta, A, delta, dim) + mju_dot(delta, res, dim);
   }
 
   // positive change: restore

src/engine/engine_util_blas.c

@@ -685,8 +685,7 @@ mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, const int n) {
 //------------------------------ matrix-vector operations ------------------------------------------
 
 // multiply matrix and vector
-void mju_mulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
-                   int nr, int nc) {
+void mju_mulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc) {
   for (int r=0; r<nr; r++) {
     res[r] = mju_dot(mat + r*nc, vec, nc);
   }
@@ -695,8 +694,7 @@ void mju_mulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
 
 
 // multiply transposed matrix and vector
-void mju_mulMatTVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
-                    int nr, int nc) {
+void mju_mulMatTVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc) {
   mjtNum tmp;
   mju_zero(res, nc);
 
@@ -709,6 +707,17 @@ void mju_mulMatTVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
 
 
 
+// multiply square matrix with vectors on both sides: return vec1'*mat*vec2
+mjtNum mju_mulVecMatVec(const mjtNum* vec1, const mjtNum* mat, const mjtNum* vec2, int n) {
+  mjtNum res = 0;
+  for (int i=0; i<n; i++) {
+    res += vec1[i] * mju_dot(mat + i*n, vec2, n);
+  }
+  return res;
+}
+
+
+
 //------------------------------ matrix-matrix operations ------------------------------------------
 
 // transpose matrix

src/engine/engine_util_blas.h

@@ -180,6 +180,9 @@ MJAPI void mju_mulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
 MJAPI void mju_mulMatTVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
                           int nr, int nc);
 
+// multiply square matrix with vectors on both sides: return vec1'*mat*vec2
+MJAPI mjtNum mju_mulVecMatVec(const mjtNum* vec1, const mjtNum* mat, const mjtNum* vec2, int n);
+
 
 //------------------------------ matrix-matrix operations ------------------------------------------
 

src/engine/engine_util_solve.c

@@ -934,8 +934,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index,               // outputs
     }
 
     // compute objective: value = 0.5*res'*H*res + res'*g
-    mju_mulMatVec(temp, H, res, n, n);  // TODO(b/246267542): do this in one call
-    value = 0.5 * mju_dot(res, temp, n) + mju_dot(res, g, n);
+    value = 0.5 * mju_mulVecMatVec(res, H, res, n) + mju_dot(res, g, n);
 
     // save last value
     oldvalue = value;
@@ -1056,8 +1055,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index,               // outputs
       }
 
       // new objective value
-      mju_mulMatVec(temp, H, candidate, n, n);
-      value = 0.5 * mju_dot(candidate, temp, n) + mju_dot(candidate, g, n);
+      value = 0.5 * mju_mulVecMatVec(candidate, H, candidate, n) + mju_dot(candidate, g, n);
 
       // increment and break if step is too small
       nstep++;