Squashed 'third_party/osqp/' content from commit 33454b3e23
Change-Id: I056df0582ca06664e86554c341a94c47ab932001
git-subtree-dir: third_party/osqp
git-subtree-split: 33454b3e236f1f44193bfbbb6b8c8e71f8f04e9a
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/src/proj.c b/src/proj.c
new file mode 100644
index 0000000..c7fdb45
--- /dev/null
+++ b/src/proj.c
@@ -0,0 +1,29 @@
+#include "proj.h"
+
+
+void project(OSQPWorkspace *work, c_float *z) {
+ c_int i, m;
+
+ m = work->data->m;
+
+ for (i = 0; i < m; i++) {
+ z[i] = c_min(c_max(z[i],
+ work->data->l[i]), // Between lower
+ work->data->u[i]); // and upper bounds
+ }
+}
+
+void project_normalcone(OSQPWorkspace *work, c_float *z, c_float *y) {
+ c_int i, m;
+
+ // NB: Use z_prev as temporary vector
+
+ m = work->data->m;
+
+ for (i = 0; i < m; i++) {
+ work->z_prev[i] = z[i] + y[i];
+ z[i] = c_min(c_max(work->z_prev[i], work->data->l[i]),
+ work->data->u[i]);
+ y[i] = work->z_prev[i] - z[i];
+ }
+}