commit bcce26ac1640be91a15d3c0c60369f353c51e332
author Austin Schuh <austin.linux@gmail.com> Mon Mar 26 23:41:24 2018 -0700
committer Brian Silverman <bsilver16384@gmail.com> Thu Apr 05 23:09:40 2018 -0400
tree 9564cfbd2adb87593d9ef622646372960cc237e9
parent d0180f18609785dcb224067e110b613baa9a97e8

Drive code works on Tantrum.

Need to write the spring code.  Drive now supports doubles...  What a
pain.

Change-Id: Id589acdc443dcd81242a21e3b0c26f81d6974dc8

frc971/control_loops/coerce_goal.h

diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index 83c401d..b1d5c3c 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -8,22 +8,95 @@
 namespace frc971 {
 namespace control_loops {
 
-Eigen::Matrix<double, 2, 1> DoCoerceGoal(
-    const aos::controls::HVPolytope<2, 4, 4> &region,
-    const Eigen::Matrix<double, 1, 2> &K, double w,
-    const Eigen::Matrix<double, 2, 1> &R, bool *is_inside);
+template <typename Scalar = double>
+Eigen::Matrix<Scalar, 2, 1> DoCoerceGoal(
+    const aos::controls::HVPolytope<2, 4, 4, Scalar> &region,
+    const Eigen::Matrix<Scalar, 1, 2> &K, Scalar w,
+    const Eigen::Matrix<Scalar, 2, 1> &R, bool *is_inside);
 
 // Intersects a line with a region, and finds the closest point to R.
 // Finds a point that is closest to R inside the region, and on the line
 // defined by K X = w.  If it is not possible to find a point on the line,
 // finds a point that is inside the region and closest to the line.
-static inline Eigen::Matrix<double, 2, 1> CoerceGoal(
-    const aos::controls::HVPolytope<2, 4, 4> &region,
-    const Eigen::Matrix<double, 1, 2> &K, double w,
-    const Eigen::Matrix<double, 2, 1> &R) {
+template <typename Scalar = double>
+static inline Eigen::Matrix<Scalar, 2, 1> CoerceGoal(
+    const aos::controls::HVPolytope<2, 4, 4, Scalar> &region,
+    const Eigen::Matrix<Scalar, 1, 2> &K, Scalar w,
+    const Eigen::Matrix<Scalar, 2, 1> &R) {
   return DoCoerceGoal(region, K, w, R, nullptr);
 }
 
+template <typename Scalar>
+Eigen::Matrix<Scalar, 2, 1> DoCoerceGoal(
+    const aos::controls::HVPolytope<2, 4, 4, Scalar> &region,
+    const Eigen::Matrix<Scalar, 1, 2> &K, Scalar w,
+    const Eigen::Matrix<Scalar, 2, 1> &R, bool *is_inside) {
+  if (region.IsInside(R)) {
+    if (is_inside) *is_inside = true;
+    return R;
+  }
+  Eigen::Matrix<Scalar, 2, 1> parallel_vector;
+  Eigen::Matrix<Scalar, 2, 1> perpendicular_vector;
+  perpendicular_vector = K.transpose().normalized();
+  parallel_vector << perpendicular_vector(1, 0), -perpendicular_vector(0, 0);
+
+  Eigen::Matrix<Scalar, 4, 1> projectedh = region.static_H() * parallel_vector;
+  Eigen::Matrix<Scalar, 4, 1> projectedk =
+      region.static_k() - region.static_H() * perpendicular_vector * w;
+
+  Scalar min_boundary = ::std::numeric_limits<Scalar>::lowest();
+  Scalar max_boundary = ::std::numeric_limits<Scalar>::max();
+  for (int i = 0; i < 4; ++i) {
+    if (projectedh(i, 0) > 0) {
+      max_boundary =
+          ::std::min(max_boundary, projectedk(i, 0) / projectedh(i, 0));
+    } else {
+      min_boundary =
+          ::std::max(min_boundary, projectedk(i, 0) / projectedh(i, 0));
+    }
+  }
+
+  Eigen::Matrix<Scalar, 1, 2> vertices;
+  vertices << max_boundary, min_boundary;
+
+  if (max_boundary > min_boundary) {
+    Scalar min_distance_sqr = 0;
+    Eigen::Matrix<Scalar, 2, 1> closest_point;
+    for (int i = 0; i < vertices.innerSize(); i++) {
+      Eigen::Matrix<Scalar, 2, 1> point;
+      point = parallel_vector * vertices(0, i) + perpendicular_vector * w;
+      const Scalar length = (R - point).squaredNorm();
+      if (i == 0 || length < min_distance_sqr) {
+        closest_point = point;
+        min_distance_sqr = length;
+      }
+    }
+    if (is_inside) *is_inside = true;
+    return closest_point;
+  } else {
+    Eigen::Matrix<Scalar, 2, 4> region_vertices = region.StaticVertices();
+#ifdef __linux__
+    CHECK_GT(region_vertices.outerSize(), 0);
+#else
+    assert(region_vertices.outerSize() > 0);
+#endif
+    Scalar min_distance = INFINITY;
+    int closest_i = 0;
+    for (int i = 0; i < region_vertices.outerSize(); i++) {
+      const Scalar length = ::std::abs(
+          (perpendicular_vector.transpose() * (region_vertices.col(i)))(0, 0));
+      if (i == 0 || length < min_distance) {
+        closest_i = i;
+        min_distance = length;
+      }
+    }
+    if (is_inside) *is_inside = false;
+    return (Eigen::Matrix<Scalar, 2, 1>() << region_vertices(0, closest_i),
+            region_vertices(1, closest_i))
+        .finished();
+  }
+}
+
 }  // namespace control_loops
 }  // namespace frc971