Add a sparse convex solver

This is a port of the dense convex solver to a sparse one.  The syntax
is different enough it isn't worth pretending we can share code.

Change-Id: I16788db62ccc3105ed866cef0a8cefe850ac5dfb
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/solvers/convex.h b/frc971/solvers/convex.h
index aa14c1a..140cac1 100644
--- a/frc971/solvers/convex.h
+++ b/frc971/solvers/convex.h
@@ -1,3 +1,6 @@
+#ifndef FRC971_SOLVERS_CONVEX_H_
+#define FRC971_SOLVERS_CONVEX_H_
+
 #include <sys/types.h>
 #include <unistd.h>
 
@@ -123,8 +126,7 @@
 
   r_dual = derivatives.gradient + derivatives.df.transpose() * lambda +
            derivatives.A.transpose() * v;
-  r_cent = -(Eigen::DiagonalMatrix<double, M>(lambda) * derivatives.f +
-             t_inverse * Eigen::Matrix<double, M, 1>::Ones());
+  r_cent = -lambda.array() * derivatives.f.array() - t_inverse;
   r_pri = derivatives.Axmb;
 
   return result;
@@ -171,8 +173,8 @@
     m1.template block<States, N>(0, States + M) = derivatives.A.transpose();
     m1.template block<M, States>(States, 0) =
         -(Eigen::DiagonalMatrix<double, M>(lambda) * derivatives.df);
-    m1.template block<M, M>(States, States) -=
-        Eigen::DiagonalMatrix<double, M>(derivatives.f);
+    m1.template block<M, M>(States, States) =
+        Eigen::DiagonalMatrix<double, M>(-derivatives.f);
     m1.template block<N, States>(States + M, 0) = derivatives.A;
 
     Eigen::Matrix<double, States + M + N, 1> dy =
@@ -379,3 +381,5 @@
 
 };  // namespace solvers
 };  // namespace frc971
+
+#endif  // FRC971_SOLVERS_CONVEX_H_