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_