FunctionalForm: Treat NaN as undefined form

When constructing `FunctionalForm` from `double`, treat `NaN` values as the `undefined` form instead of `constant`. Fixes RobotLocomotion#2444.
aidiyang · Jun 14, 2016 · 051acd6 · 051acd6
1 parent f226039
commit 051acd6
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 2 deletions.
diff --git a/drake/core/functional_form.cc b/drake/core/functional_form.cc
@@ -2,6 +2,7 @@
 
 #include <algorithm>
 #include <cassert>
+#include <cmath>
 #include <cstddef>
 #include <ostream>
 #include <type_traits>
@@ -59,12 +60,22 @@ class FunctionalForm::Internal {
   static bool need_vars(Form f) {
     return f != Form::kZero && f != Form::kConstant;
   }
+
+  static Form FormFromDouble(double d) {
+    if (d == 0) {
+      return Form::kZero;
+    }
+    if (std::isnan(d)) {
+      return Form::kUndefined;
+    }
+    return Form::kConstant;
+  }
 };
 
 FunctionalForm::FunctionalForm() : FunctionalForm(Form::kUndefined, {}) {}
 
 FunctionalForm::FunctionalForm(double d)
-    : FunctionalForm(d == 0 ? Form::kZero : Form::kConstant, {}) {}
+    : FunctionalForm(Internal::FormFromDouble(d), {}) {}
 
 FunctionalForm::FunctionalForm(Form f, Variables&& v)
     : vars_(std::move(v)), form_(f) {

diff --git a/drake/core/functional_form.h b/drake/core/functional_form.h
@@ -109,7 +109,8 @@ class DRAKECORE_EXPORT FunctionalForm {
   /** Construct an \ref undefined form with no variables. */
   FunctionalForm();
 
-  /** Construct a \ref constant or \ref zero form with no variables.  */
+  /** Construct a \ref constant, \ref zero (0), or \ref undefined (NaN)
+      form with no variables.  */
   explicit FunctionalForm(double d);
 
   /** Return a \ref zero form with no variables. */

diff --git a/drake/core/test/functional_form_test.cc b/drake/core/test/functional_form_test.cc
@@ -1,5 +1,6 @@
 #include "drake/core/functional_form.h"
 
+#include <cmath>
 #include <sstream>
 
 #include <Eigen/Core>
@@ -268,6 +269,9 @@ GTEST_TEST(FunctionalFormTest, Construct) {
 
   FunctionalForm double_nonzero(0.1);
   EXPECT_TRUE(double_nonzero.IsConstant());
+
+  FunctionalForm double_nan(std::nan(""));
+  EXPECT_TRUE(double_nan.IsUndefined());
 }
 
 GTEST_TEST(FunctionalFormTest, Basic) {