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/lin_alg.c b/src/lin_alg.c
new file mode 100644
index 0000000..ba896f5
--- /dev/null
+++ b/src/lin_alg.c
@@ -0,0 +1,413 @@
+#include "lin_alg.h"
+
+
+/* VECTOR FUNCTIONS ----------------------------------------------------------*/
+
+
+void vec_add_scaled(c_float *c,
+ const c_float *a,
+ const c_float *b,
+ c_int n,
+ c_float sc) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ c[i] = a[i] + sc * b[i];
+ }
+}
+
+c_float vec_scaled_norm_inf(const c_float *S, const c_float *v, c_int l) {
+ c_int i;
+ c_float abs_Sv_i;
+ c_float max = 0.0;
+
+ for (i = 0; i < l; i++) {
+ abs_Sv_i = c_absval(S[i] * v[i]);
+
+ if (abs_Sv_i > max) max = abs_Sv_i;
+ }
+ return max;
+}
+
+c_float vec_norm_inf(const c_float *v, c_int l) {
+ c_int i;
+ c_float abs_v_i;
+ c_float max = 0.0;
+
+ for (i = 0; i < l; i++) {
+ abs_v_i = c_absval(v[i]);
+
+ if (abs_v_i > max) max = abs_v_i;
+ }
+ return max;
+}
+
+c_float vec_norm_inf_diff(const c_float *a, const c_float *b, c_int l) {
+ c_float nmDiff = 0.0, tmp;
+ c_int i;
+
+ for (i = 0; i < l; i++) {
+ tmp = c_absval(a[i] - b[i]);
+
+ if (tmp > nmDiff) nmDiff = tmp;
+ }
+ return nmDiff;
+}
+
+c_float vec_mean(const c_float *a, c_int n) {
+ c_float mean = 0.0;
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ mean += a[i];
+ }
+ mean /= (c_float)n;
+
+ return mean;
+}
+
+void int_vec_set_scalar(c_int *a, c_int sc, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ a[i] = sc;
+ }
+}
+
+void vec_set_scalar(c_float *a, c_float sc, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ a[i] = sc;
+ }
+}
+
+void vec_add_scalar(c_float *a, c_float sc, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ a[i] += sc;
+ }
+}
+
+void vec_mult_scalar(c_float *a, c_float sc, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ a[i] *= sc;
+ }
+}
+
+#ifndef EMBEDDED
+c_float* vec_copy(c_float *a, c_int n) {
+ c_float *b;
+ c_int i;
+
+ b = c_malloc(n * sizeof(c_float));
+ if (!b) return OSQP_NULL;
+
+ for (i = 0; i < n; i++) {
+ b[i] = a[i];
+ }
+
+ return b;
+}
+
+#endif // end EMBEDDED
+
+
+void prea_int_vec_copy(const c_int *a, c_int *b, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ b[i] = a[i];
+ }
+}
+
+void prea_vec_copy(const c_float *a, c_float *b, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ b[i] = a[i];
+ }
+}
+
+void vec_ew_recipr(const c_float *a, c_float *b, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ b[i] = (c_float)1.0 / a[i];
+ }
+}
+
+c_float vec_prod(const c_float *a, const c_float *b, c_int n) {
+ c_float prod = 0.0;
+ c_int i; // Index
+
+ for (i = 0; i < n; i++) {
+ prod += a[i] * b[i];
+ }
+
+ return prod;
+}
+
+void vec_ew_prod(const c_float *a, const c_float *b, c_float *c, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ c[i] = b[i] * a[i];
+ }
+}
+
+#if EMBEDDED != 1
+void vec_ew_sqrt(c_float *a, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ a[i] = c_sqrt(a[i]);
+ }
+}
+
+void vec_ew_max(c_float *a, c_int n, c_float max_val) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ a[i] = c_max(a[i], max_val);
+ }
+}
+
+void vec_ew_min(c_float *a, c_int n, c_float min_val) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ a[i] = c_min(a[i], min_val);
+ }
+}
+
+void vec_ew_max_vec(const c_float *a, const c_float *b, c_float *c, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ c[i] = c_max(a[i], b[i]);
+ }
+}
+
+void vec_ew_min_vec(const c_float *a, const c_float *b, c_float *c, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ c[i] = c_min(a[i], b[i]);
+ }
+}
+
+#endif // EMBEDDED != 1
+
+
+/* MATRIX FUNCTIONS ----------------------------------------------------------*/
+
+/* multiply scalar to matrix */
+void mat_mult_scalar(csc *A, c_float sc) {
+ c_int i, nnzA;
+
+ nnzA = A->p[A->n];
+
+ for (i = 0; i < nnzA; i++) {
+ A->x[i] *= sc;
+ }
+}
+
+void mat_premult_diag(csc *A, const c_float *d) {
+ c_int j, i;
+
+ for (j = 0; j < A->n; j++) { // Cycle over columns
+ for (i = A->p[j]; i < A->p[j + 1]; i++) { // Cycle every row in the column
+ A->x[i] *= d[A->i[i]]; // Scale by corresponding element
+ // of d for row i
+ }
+ }
+}
+
+void mat_postmult_diag(csc *A, const c_float *d) {
+ c_int j, i;
+
+ for (j = 0; j < A->n; j++) { // Cycle over columns j
+ for (i = A->p[j]; i < A->p[j + 1]; i++) { // Cycle every row i in column j
+ A->x[i] *= d[j]; // Scale by corresponding element
+ // of d for column j
+ }
+ }
+}
+
+void mat_vec(const csc *A, const c_float *x, c_float *y, c_int plus_eq) {
+ c_int i, j;
+
+ if (!plus_eq) {
+ // y = 0
+ for (i = 0; i < A->m; i++) {
+ y[i] = 0;
+ }
+ }
+
+ // if A is empty
+ if (A->p[A->n] == 0) {
+ return;
+ }
+
+ if (plus_eq == -1) {
+ // y -= A*x
+ for (j = 0; j < A->n; j++) {
+ for (i = A->p[j]; i < A->p[j + 1]; i++) {
+ y[A->i[i]] -= A->x[i] * x[j];
+ }
+ }
+ } else {
+ // y += A*x
+ for (j = 0; j < A->n; j++) {
+ for (i = A->p[j]; i < A->p[j + 1]; i++) {
+ y[A->i[i]] += A->x[i] * x[j];
+ }
+ }
+ }
+}
+
+void mat_tpose_vec(const csc *A, const c_float *x, c_float *y,
+ c_int plus_eq, c_int skip_diag) {
+ c_int i, j, k;
+
+ if (!plus_eq) {
+ // y = 0
+ for (i = 0; i < A->n; i++) {
+ y[i] = 0;
+ }
+ }
+
+ // if A is empty
+ if (A->p[A->n] == 0) {
+ return;
+ }
+
+ if (plus_eq == -1) {
+ // y -= A*x
+ if (skip_diag) {
+ for (j = 0; j < A->n; j++) {
+ for (k = A->p[j]; k < A->p[j + 1]; k++) {
+ i = A->i[k];
+ y[j] -= i == j ? 0 : A->x[k] * x[i];
+ }
+ }
+ } else {
+ for (j = 0; j < A->n; j++) {
+ for (k = A->p[j]; k < A->p[j + 1]; k++) {
+ y[j] -= A->x[k] * x[A->i[k]];
+ }
+ }
+ }
+ } else {
+ // y += A*x
+ if (skip_diag) {
+ for (j = 0; j < A->n; j++) {
+ for (k = A->p[j]; k < A->p[j + 1]; k++) {
+ i = A->i[k];
+ y[j] += i == j ? 0 : A->x[k] * x[i];
+ }
+ }
+ } else {
+ for (j = 0; j < A->n; j++) {
+ for (k = A->p[j]; k < A->p[j + 1]; k++) {
+ y[j] += A->x[k] * x[A->i[k]];
+ }
+ }
+ }
+ }
+}
+
+#if EMBEDDED != 1
+void mat_inf_norm_cols(const csc *M, c_float *E) {
+ c_int j, ptr;
+
+ // Initialize zero max elements
+ for (j = 0; j < M->n; j++) {
+ E[j] = 0.;
+ }
+
+ // Compute maximum across columns
+ for (j = 0; j < M->n; j++) {
+ for (ptr = M->p[j]; ptr < M->p[j + 1]; ptr++) {
+ E[j] = c_max(c_absval(M->x[ptr]), E[j]);
+ }
+ }
+}
+
+void mat_inf_norm_rows(const csc *M, c_float *E) {
+ c_int i, j, ptr;
+
+ // Initialize zero max elements
+ for (j = 0; j < M->m; j++) {
+ E[j] = 0.;
+ }
+
+ // Compute maximum across rows
+ for (j = 0; j < M->n; j++) {
+ for (ptr = M->p[j]; ptr < M->p[j + 1]; ptr++) {
+ i = M->i[ptr];
+ E[i] = c_max(c_absval(M->x[ptr]), E[i]);
+ }
+ }
+}
+
+void mat_inf_norm_cols_sym_triu(const csc *M, c_float *E) {
+ c_int i, j, ptr;
+ c_float abs_x;
+
+ // Initialize zero max elements
+ for (j = 0; j < M->n; j++) {
+ E[j] = 0.;
+ }
+
+ // Compute maximum across columns
+ // Note that element (i, j) contributes to
+ // -> Column j (as expected in any matrices)
+ // -> Column i (which is equal to row i for symmetric matrices)
+ for (j = 0; j < M->n; j++) {
+ for (ptr = M->p[j]; ptr < M->p[j + 1]; ptr++) {
+ i = M->i[ptr];
+ abs_x = c_absval(M->x[ptr]);
+ E[j] = c_max(abs_x, E[j]);
+
+ if (i != j) {
+ E[i] = c_max(abs_x, E[i]);
+ }
+ }
+ }
+}
+
+#endif /* if EMBEDDED != 1 */
+
+
+c_float quad_form(const csc *P, const c_float *x) {
+ c_float quad_form = 0.;
+ c_int i, j, ptr; // Pointers to iterate over
+ // matrix: (i,j) a element
+ // pointer
+
+ for (j = 0; j < P->n; j++) { // Iterate over columns
+ for (ptr = P->p[j]; ptr < P->p[j + 1]; ptr++) { // Iterate over rows
+ i = P->i[ptr]; // Row index
+
+ if (i == j) { // Diagonal element
+ quad_form += (c_float).5 * P->x[ptr] * x[i] * x[i];
+ }
+ else if (i < j) { // Off-diagonal element
+ quad_form += P->x[ptr] * x[i] * x[j];
+ }
+ else { // Element in lower diagonal
+ // part
+#ifdef PRINTING
+ c_eprint("quad_form matrix is not upper triangular");
+#endif /* ifdef PRINTING */
+ return OSQP_NULL;
+ }
+ }
+ }
+ return quad_form;
+}