commit c7a0a3da74a137c6c0a9757fec0fb2ca978a959c
author Austin Schuh <austin.linux@gmail.com> Sat Oct 15 16:22:47 2016 -0700
committer Austin Schuh <austin.linux@gmail.com> Sat Oct 29 13:49:17 2016 -0700
tree fd54badbdc6b6c98d963f54fd3ba7ac2c3061aae
parent e5f194c6c20dcb61aea387f401f16facb125edc9

Removed mallocs from polydrive code

Since most of the places where we create a polytope are just affine
transformations of another polytope, we can transform the original
vertices to avoid needing libcdd at runtime.  This is in preparation
for the battlebot.

Change-Id: Ic35af2d918095dc23b90a4ab248d0c142cf3588a

frc971/control_loops/coerce_goal.cc

diff --git a/frc971/control_loops/coerce_goal.cc b/frc971/control_loops/coerce_goal.cc
index 0868c2c..0c98560 100644
--- a/frc971/control_loops/coerce_goal.cc
+++ b/frc971/control_loops/coerce_goal.cc
@@ -8,7 +8,7 @@
 namespace control_loops {
 
 Eigen::Matrix<double, 2, 1> DoCoerceGoal(
-    const aos::controls::HPolytope<2> &region,
+    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) {
   if (region.IsInside(R)) {
@@ -20,12 +20,26 @@
   perpendicular_vector = K.transpose().normalized();
   parallel_vector << perpendicular_vector(1, 0), -perpendicular_vector(0, 0);
 
-  aos::controls::HPolytope<1> t_poly(
-      region.H() * parallel_vector,
-      region.k() - region.H() * perpendicular_vector * w);
+  Eigen::Matrix<double, 4, 1> projectedh = region.static_H() * parallel_vector;
+  Eigen::Matrix<double, 4, 1> projectedk =
+      region.static_k() - region.static_H() * perpendicular_vector * w;
 
-  Eigen::Matrix<double, 1, Eigen::Dynamic> vertices = t_poly.Vertices();
-  if (vertices.innerSize() > 0) {
+  double min_boundary = ::std::numeric_limits<double>::lowest();
+  double max_boundary = ::std::numeric_limits<double>::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<double, 1, 2> vertices;
+  vertices << max_boundary, min_boundary;
+
+  if (max_boundary > min_boundary) {
     double min_distance_sqr = 0;
     Eigen::Matrix<double, 2, 1> closest_point;
     for (int i = 0; i < vertices.innerSize(); i++) {
@@ -40,8 +54,8 @@
     if (is_inside) *is_inside = true;
     return closest_point;
   } else {
-    Eigen::Matrix<double, 2, Eigen::Dynamic> region_vertices =
-        region.Vertices();
+    Eigen::Matrix<double, 2, 4> region_vertices =
+        region.StaticVertices();
     CHECK_GT(region_vertices.outerSize(), 0);
     double min_distance = INFINITY;
     int closest_i = 0;