perf(kinbody): skip redundant GetDOFValues recompute in SetDOFValues

src/libopenrave/kinbody.cpp

@@ -2271,27 +2271,44 @@ void KinBody::SetDOFValues(const dReal* pJointValues, int dof, uint32_t checklim
     if( dof == 0 || _veclinks.size() == 0) {
         return;
     }
-    int expecteddof = dofindices.size() > 0 ? (int)dofindices.size() : GetDOF();
-    OPENRAVE_ASSERT_OP_FORMAT((int)dof,>=,expecteddof, "env=%s, body '%s' not enough values %d<%d", GetEnv()->GetNameId()%GetName()%dof%GetDOF(),ORE_InvalidArguments);
+    const int expecteddof = dofindices.size() > 0 ? (int)dofindices.size() : GetDOF();
+    OPENRAVE_ASSERT_OP_FORMAT((int)dof, >=, expecteddof,
+                              "env=%s, body '%s' not enough values %d<%d", GetEnv()->GetNameId() % GetName() % dof % expecteddof,
+                              ORE_InvalidArguments);
 
-    GetDOFValues(_vTempJoints);
     if( dofindices.size() > 0 ) {
         // user only set a certain number of indices, so have to fill the temporary array with the full set of values first
         // and then overwrite with the user set values
+        GetDOFValues(_vTempJoints);
         for(size_t i = 0; i < dofindices.size(); ++i) {
             if( !std::isnan(pJointValues[i]) ) {
                 _vTempJoints.at(dofindices[i]) = pJointValues[i];
             }
         }
+        pJointValues = &_vTempJoints[0];
     }
     else {
-        for(size_t i = 0; i < _vTempJoints.size(); ++i) {
-            if( !std::isnan(pJointValues[i]) ) {
-                _vTempJoints[i] = pJointValues[i];
+        // When setting values to all joints, the input pJointValues already holds every value so use it directly and
+        // skip the expensive GetDOFValues. Only when pJointValues contains NaN (meaning "keep the current value") do we
+        // fetch the current values and fill in the rest.
+        _vTempJoints.resize(expecteddof);
+        bool bHasNaN = false;
+        for(int i = 0; i < expecteddof; ++i) {
+            if( std::isnan(pJointValues[i]) ) {
+                bHasNaN = true;
+                break;
+            }
+        }
+        if( bHasNaN ) {
+            GetDOFValues(_vTempJoints);
+            for(int i = 0; i < expecteddof; ++i) {
+                if( !std::isnan(pJointValues[i]) ) {
+                    _vTempJoints[i] = pJointValues[i];
+                }
             }
+            pJointValues = &_vTempJoints[0];
         }
     }
-    pJointValues = &_vTempJoints[0];
 
     if( checklimits != CLA_Nothing ) {
         dReal* ptempjoints = &_vTempJoints[0];