Fix another couple of bugs in CoerceGoal
-We weren't properly handling the case where the line was outside of the
box.
-We weren't properly handling the case where w != 0.
-Rearrange the tests.
-Write some new tests.
Change-Id: Ib76b4e1670727d567c943f730c91d9536eb67ea7
diff --git a/aos/controls/polytope.h b/aos/controls/polytope.h
index c63166f..28f011e 100644
--- a/aos/controls/polytope.h
+++ b/aos/controls/polytope.h
@@ -109,13 +109,15 @@
#ifdef __linux__
+
template <int number_of_dimensions>
class HPolytope : public Polytope<number_of_dimensions> {
public:
// Constructs a polytope given the H and k matrices.
- HPolytope(Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic,
- number_of_dimensions>> H,
- Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k)
+ HPolytope(
+ Eigen::Ref<
+ const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>> H,
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k)
: H_(H), k_(k), vertices_(CalculateVertices(H, k)) {}
// This is an initialization function shared across all instantiations of this
@@ -133,6 +135,8 @@
return k_;
}
+ // NOTE: If you are getting bizarre errors that you are tracing back to the
+ // vertex matrix being funky, please check that you correctly called Init().
Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices()
const override {
return vertices_;