Merge pull request #3600 from ComputationalBiomechanicsLab/FixQuinticDomainBug

Fixes bug introduced by #3596 when handling NaNs.

Author: pepbos

OpenSim/Common/SmoothSegmentedFunction.cpp

@@ -584,50 +584,52 @@ ________________________________________________________________________
 
 double SmoothSegmentedFunction::calcDerivative(double x, int order) const
 {
-    {
+    const double x0 = _smoothData->_x0;
+    const double x1 = _smoothData->_x1;
+
+    if (x < x0) {
         const double y0 = _smoothData->_y0;
-        const double x0 = _smoothData->_x0;
         const double dydx0 = _smoothData->_dydx0;
-        if (x < x0) {
-            switch (order) {
-                case 0: return y0 + dydx0*(x-x0);
-                case 1: return dydx0;
-                default: return 0.;
-            }
+        switch (order) {
+            case 0: return y0 + dydx0*(x-x0);
+            case 1: return dydx0;
+            default: return 0.;
         }
     }
 
-    {
-        const double x1 = _smoothData->_x1;
+    if (x <= x1) {
+        const SimTK::Array_<SimTK::Vec6>& ctrlPtsX = _smoothData->_ctrlPtsX;
+        const int idx  = SegmentedQuinticBezierToolkit::calcIndex(x,ctrlPtsX);
+
+        const SimTK::Array_<SimTK::Spline>& arraySplineUX =
+            _smoothData->_arraySplineUX;
+        const double u = SegmentedQuinticBezierToolkit::calcU(
+                x,
+                ctrlPtsX[idx],
+                arraySplineUX[idx],
+                UTOL,
+                MAXITER);
+
+        const SimTK::Array_<SimTK::Vec6>& ctrlPtsY = _smoothData->_ctrlPtsY;
+        return SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivDYDX(
+                u,
+                ctrlPtsX[idx],
+                ctrlPtsY[idx],
+                order);
+    }
+
+    if (x > x1) {
         const double y1 = _smoothData->_y1;
         const double dydx1 = _smoothData->_dydx1;
-        if (x > x1) {
-            switch (order) {
-                case 0: return y1 + dydx1*(x-x1);
-                case 1: return dydx1;
-                default: return 0.;
-            }
+        switch (order) {
+            case 0: return y1 + dydx1*(x-x1);
+            case 1: return dydx1;
+            default: return 0.;
         }
     }
 
-    const SimTK::Array_<SimTK::Vec6>& ctrlPtsX = _smoothData->_ctrlPtsX;
-    const int idx  = SegmentedQuinticBezierToolkit::calcIndex(x,ctrlPtsX);
-
-    const SimTK::Array_<SimTK::Spline>& arraySplineUX =
-        _smoothData->_arraySplineUX;
-    const double u = SegmentedQuinticBezierToolkit::calcU(
-            x,
-            ctrlPtsX[idx],
-            arraySplineUX[idx],
-            UTOL,
-            MAXITER);
-
-    const SimTK::Array_<SimTK::Vec6>& ctrlPtsY = _smoothData->_ctrlPtsY;
-    return SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivDYDX(
-            u,
-            ctrlPtsX[idx],
-            ctrlPtsY[idx],
-            order);
+    // In case of NaN return NaN.
+    return SimTK::NaN;
 }
 
 double SmoothSegmentedFunction::

OpenSim/Common/Test/testSmoothSegmentedFunctionFactory.cpp

@@ -1122,6 +1122,22 @@ void testMonotonicity(SimTK::Matrix mcfSample)
     cout << endl;
 }
 
+/*
+ 5. Tests the output when NaN is the input.
+*/
+template<typename F>
+void testMuscleCurveNaNBehavior(const F& curve)
+{
+    cout << "   TEST: NaN behavior. " << endl;
+
+    // Curve value at NaN gives NaN.
+    SimTK_TEST(std::isnan(curve.calcValue(SimTK::NaN)));
+    SimTK_TEST(std::isnan(curve.calcDerivative(SimTK::NaN, 1)));
+    SimTK_TEST(std::isnan(curve.calcDerivative(SimTK::NaN, 2)));
+
+    cout << "    passed testing NaN behavior." << endl;
+}
+
 //______________________________________________________________________________
 /**
  * Create a muscle bench marking system. The bench mark consists of a single muscle 
@@ -1209,8 +1225,10 @@ int main(int argc, char* argv[])
             testMuscleCurveC2Continuity(tendonCurve,tendonCurveSample);
         //4. Test for monotonicity where appropriate
             testMonotonicity(tendonCurveSample);
+        //5. Test NaN behavior.
+            testMuscleCurveNaNBehavior(tendonCurve);
 
-        //5. Testing Exceptions
+        //6. Testing Exceptions
             cout << endl;
             cout << "   Exception Testing" << endl;
             SimTK_TEST_MUST_THROW(/*SmoothSegmentedFunction* tendonCurveEX
@@ -1279,8 +1297,10 @@ int main(int argc, char* argv[])
         //4. Test for monotonicity where appropriate
 
             testMonotonicity(fiberFLCurveSample);
+        //5. Test NaN behavior.
+            testMuscleCurveNaNBehavior(fiberFLCurve);
 
-        //5. Testing Exceptions
+        //6. Testing Exceptions
             cout << endl;
             cout << "   Exception Testing" << endl;
             SimTK_TEST_MUST_THROW(/*SmoothSegmentedFunction* fiberFLCurveEX 
@@ -1348,7 +1368,9 @@ int main(int argc, char* argv[])
         //4. Test for monotonicity where appropriate
 
             testMonotonicity(fiberCECurveSample);
-        //5. Testing Exceptions
+        //5. Test NaN behavior.
+            testMuscleCurveNaNBehavior(fiberCECurve);
+        //6. Testing Exceptions
             cout << endl;
             cout << "   Exception Testing" << endl;
             SimTK_TEST_MUST_THROW(/*SmoothSegmentedFunction* fiberCECurveEX 
@@ -1412,7 +1434,9 @@ int main(int argc, char* argv[])
             testMuscleCurveC2Continuity(fiberCEPhiCurve,fiberCEPhiCurveSample);
         //4. Test for monotonicity where appropriate
             testMonotonicity(fiberCEPhiCurveSample);
-        //5. Testing Exceptions
+        //5. Test NaN behavior.
+            testMuscleCurveNaNBehavior(fiberCEPhiCurve);
+        //6. Testing Exceptions
             cout << endl;
             cout << "   Exception Testing" << endl;
             SimTK_TEST_MUST_THROW(/*SmoothSegmentedFunction* fiberCEPhiCurveEX 
@@ -1485,7 +1509,9 @@ int main(int argc, char* argv[])
         //4. Test for monotonicity where appropriate
 
             testMonotonicity(fiberCECosPhiCurveSample);
-        //5. Test exceptions
+        //5. Test NaN behavior.
+            testMuscleCurveNaNBehavior(fiberCECosPhiCurve);
+        //6. Test exceptions
             cout << endl;
         cout << "   Exception Testing" << endl;
             SimTK_TEST_MUST_THROW(/*SmoothSegmentedFunction* fiberCECosPhiCurveEX 
@@ -1575,7 +1601,9 @@ int main(int argc, char* argv[])
         //4. Test for monotonicity where appropriate
 
             testMonotonicity(fiberFVCurveSample);
-        //5. Exception testing
+        //5. Test NaN behavior.
+            testMuscleCurveNaNBehavior(fiberFVCurve);
+        //6. Exception testing
             cout << endl;    
             cout << "   Exception Testing" << endl;
 
@@ -1686,8 +1714,10 @@ int main(int argc, char* argv[])
         //4. Test for monotonicity where appropriate
 
             testMonotonicity(fiberFVInvCurveSample);
+        //5. Test NaN behavior.
+            testMuscleCurveNaNBehavior(fiberFVInvCurve);
 
-        //5. Testing the exceptions
+        //6. Testing the exceptions
 
             //5. Exception testing
             cout << endl;