commit 4e1b066e16215c4ba2094e5261e89e29317e01bd
author Brian Silverman <brians> Sun Jan 31 18:03:19 2016 -0500
committer Brian Silverman <brians> Sun Jan 31 18:04:03 2016 -0500
tree 13cca07d81c4b386b565e1d3ba3ad1d3ba376719
parent aba7bf615ac0220f82694363fe8a826b53924fb1

Re-enable polytopes for closed loop driving

I reproduced the original bug which resulted in this being disabled in a
test and fixed it already (I99a1299e538a395685c2e7e555dfccab48055349).
This commit fixes another issue observed in testing and re-enables the
polytopes.

Change-Id: Ic27e0ab8b2ea6dd480d7998c44530918443874db

frc971/control_loops/coerce_goal.cc

diff --git a/frc971/control_loops/coerce_goal.cc b/frc971/control_loops/coerce_goal.cc
index 6482cc7..0868c2c 100644
--- a/frc971/control_loops/coerce_goal.cc
+++ b/frc971/control_loops/coerce_goal.cc
@@ -7,11 +7,10 @@
 namespace frc971 {
 namespace control_loops {
 
-Eigen::Matrix<double, 2, 1> DoCoerceGoal(const aos::controls::HPolytope<2> &region,
-                                         const Eigen::Matrix<double, 1, 2> &K,
-                                         double w,
-                                         const Eigen::Matrix<double, 2, 1> &R,
-                                         bool *is_inside) {
+Eigen::Matrix<double, 2, 1> DoCoerceGoal(
+    const aos::controls::HPolytope<2> &region,
+    const Eigen::Matrix<double, 1, 2> &K, double w,
+    const Eigen::Matrix<double, 2, 1> &R, bool *is_inside) {
   if (region.IsInside(R)) {
     if (is_inside) *is_inside = true;
     return R;
@@ -43,6 +42,7 @@
   } else {
     Eigen::Matrix<double, 2, Eigen::Dynamic> region_vertices =
         region.Vertices();
+    CHECK_GT(region_vertices.outerSize(), 0);
     double min_distance = INFINITY;
     int closest_i = 0;
     for (int i = 0; i < region_vertices.outerSize(); i++) {