Add mju_mat2Rot.

This function extracts the 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion. It is based on the paper "A robust method to extract the rotational part of deformations" by Müller, Matthias, Jan Bender, Nuttapong Chentanez, and Miles Macklin.

PiperOrigin-RevId: 700006006
Change-Id: I77550993233dea9cdf68601762a3ae7ded749bdf

doc/APIreference/functions.rst

@@ -3555,6 +3555,16 @@ Integrate quaternion given 3D angular velocity.
 
 Construct quaternion performing rotation from z-axis to given vector.
 
+.. _mju_mat2Rot:
+
+`mju_mat2Rot <#mju_mat2Rot>`__
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. mujoco-include:: mju_mat2Rot
+
+extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion
+returns the number of iterations required to converge
+
 .. _mju_euler2Quat:
 
 `mju_euler2Quat <#mju_euler2Quat>`__

doc/includes/references.h

@@ -3481,6 +3481,7 @@ void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]);
 void mju_derivQuat(mjtNum res[4], const mjtNum quat[4], const mjtNum vel[3]);
 void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale);
 void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]);
+int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]);
 void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq);
 void mju_mulPose(mjtNum posres[3], mjtNum quatres[4],
                  const mjtNum pos1[3], const mjtNum quat1[4],