pick near value method

include/feasibility_set.h

@@ -104,6 +104,13 @@ int lp_feasibility_set_contains(const lp_feasibility_set_t* set, const lp_value_
  */
 void lp_feasibility_set_pick_value(const lp_feasibility_set_t* set, lp_value_t* v);
 
+/**
+ * Pick a value from the feasible set (must be non-empty). If an integer value
+ * is available it will be picked. Preference integer < rational < algebraic.
+ * If equal, pick the closer to the near value.
+ */
+void lp_feasibility_set_pick_near_value(const lp_feasibility_set_t* set, lp_value_t* v, const lp_value_t* near_value);
+
 /**
  * Pick a value from the first interval towards -inf.
  */

src/polynomial/feasibility_set.c

@@ -22,6 +22,7 @@
 #include <assert.h>
 #include <string.h>
 #include <limits.h>
+#include <math.h>
 
 #include "poly.h"
 #include "value.h"
@@ -229,6 +230,59 @@ void lp_feasibility_set_pick_value(const lp_feasibility_set_t* set, lp_value_t*
   lp_value_destruct(&current);
 }
 
+// We get smallest integer < rational < algebraic
+// If same we get one with the smallest distance from the near value
+static inline
+int near_or_better_complexity(const lp_value_t* v1, double v1_dbl, const lp_value_t* v2, double v2_dbl, double near_dbl) {
+
+  // See if any is integer
+  int v1_is_int = lp_value_is_integer(v1);
+  int v2_is_int = lp_value_is_integer(v2);
+  if (v1_is_int && !v2_is_int) {
+    return 1;
+  }
+  if (v2_is_int && !v1_is_int) {
+    return 0;
+  }
+
+  // See if any is rational
+  int v1_is_rational = lp_value_is_rational(v1);
+  int v2_is_rational = lp_value_is_rational(v2);
+  if (v1_is_rational && !v2_is_rational) {
+    return 1;
+  }
+  if (v2_is_rational && !v1_is_rational) {
+    return 0;
+  }
+
+  // Same type, compare the distance
+  return fabs(v1_dbl - near_dbl) < fabs(v2_dbl - near_dbl);
+}
+
+void lp_feasibility_set_pick_near_value(const lp_feasibility_set_t* set, lp_value_t* value, const lp_value_t* near_value) {
+  size_t i;
+
+  assert(!lp_feasibility_set_is_empty(set));
+
+  lp_interval_pick_value(set->intervals, value);
+  double value_dbl = lp_value_to_double(value);
+  double near_dbl = lp_value_to_double(near_value);
+
+  lp_value_t current;
+  double current_dbl;
+  lp_value_construct_none(&current);
+  for (i = 1; i < set->size; ++ i) {
+    lp_interval_pick_value(set->intervals + i, &current);
+    current_dbl = lp_value_to_double(&current);
+
+    if (!near_or_better_complexity(value, value_dbl, &current, current_dbl, near_dbl)) {
+      lp_value_swap(value, &current);
+      value_dbl = current_dbl;
+    }
+  }
+  lp_value_destruct(&current);
+}
+
 void lp_feasibility_set_pick_first_value(const lp_feasibility_set_t* set, lp_value_t* value) {
   assert(!lp_feasibility_set_is_empty(set));
   lp_interval_pick_value(set->intervals, value);