1 /*
2 * ECOS - Embedded Conic Solver.
3 * Copyright (C) 2012-2015 A. Domahidi [domahidi@embotech.com],
4 * Automatic Control Lab, ETH Zurich & embotech GmbH, Zurich, Switzerland.
5 *
6 * This program is free software: you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation, either version 3 of the License, or
9 * (at your option) any later version.
10 *
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with this program. If not, see <http://www.gnu.org/licenses/>.
18 */
19
20
21 /*
22 * Sparse linear algebra library for solver, i.e. no memory manager
23 * such as malloc is accessed by this module.
24 */
25
26 #include "spla.h"
27
28 #include <math.h>
29
30
31 /*
32 * Sparse matrix-vector multiply for operations
33 *
34 * y = A*x (if a > 0 && newVector == 1)
35 * y += A*x (if a > 0 && newVector == 0)
36 * y = -A*x (if a < 0 && newVector == 1)
37 * y -= A*x (if a < 0 && newVector == 0)
38 *
39 * where A is a sparse matrix and both x and y are assumed to be dense.
40 */
sparseMV(spmat * A,pfloat * x,pfloat * y,idxint a,idxint newVector)41 void sparseMV(spmat* A, pfloat* x, pfloat* y, idxint a, idxint newVector)
42 {
43 idxint i, j;
44 /* fill y with zeros in case A happens to not have any zero rows */
45 if( newVector > 0 ){ for( i=0; i<A->m; i++ ){ y[i] = 0; } }
46
47 /* if there are no entries in A, just do nothing */
48 if( A->nnz == 0 ) return;
49
50 if( a > 0 ){
51 /* Add A*x */
52 for( j=0; j<A->n; j++ ){
53 for( i=A->jc[j]; i < A->jc[j+1]; i++ ){
54 y[A->ir[i]] += A->pr[i]*x[j];
55 }
56 }
57 } else {
58 /* Subtract A*x */
59 for( j=0; j<A->n; j++ ){
60 for( i=A->jc[j]; i < A->jc[j+1]; i++ ){
61 y[A->ir[i]] -= A->pr[i]*x[j];
62 }
63 }
64 }
65 }
66
67
68 /*
69 * Sparse matrix-transpose-vector multiply with subtraction.
70 *
71 * If newVector > 0, then this computes y = -A'*x,
72 * otherwise y -= A'*x,
73 *
74 * where A is a sparse matrix and both x and y are assumed to be dense.
75 * If skipDiagonal == 1, then the contributions of diagonal elements are
76 * not counted.
77 *
78 * NOTE: The product is calculating without explicitly forming the
79 * transpose.
80 */
sparseMtVm(spmat * A,pfloat * x,pfloat * y,idxint newVector,idxint skipDiagonal)81 void sparseMtVm(spmat* A, pfloat* x, pfloat* y, idxint newVector, idxint skipDiagonal)
82 {
83 idxint i, j, k;
84 /* fill y with zeros in case A happens to not have any zero rows */
85 if( newVector > 0 ){ for( j=0; j<A->n; j++ ){ y[j] = 0; } }
86
87 /* if there are no entries in A, just do nothing */
88 if( A->nnz == 0 ) return;
89
90 if( skipDiagonal ){
91 /* Subtract A'*x */
92 for( j=0; j<A->n; j++ ){
93 for( k=A->jc[j]; k < A->jc[j+1]; k++ ){
94 i = A->ir[k];
95 y[j] -= i==j? 0 : A->pr[k]*x[i];
96 }
97 }
98 } else {
99 /* Subtract A'*x */
100 for( j=0; j<A->n; j++ ){
101 for( k=A->jc[j]; k < A->jc[j+1]; k++ ){
102 y[j] -= A->pr[k]*x[A->ir[k]];
103 }
104 }
105 }
106 }
107
108
109 /*
110 * Vector addition y += x of size n.
111 */
vadd(idxint n,pfloat * x,pfloat * y)112 void vadd(idxint n, pfloat* x, pfloat* y)
113 {
114 idxint i;
115 for( i=0; i<n; i++){ y[i] += x[i]; }
116 }
117
118
119 /*
120 * Vector subtraction with scaling: y -= a*x of size n.
121 */
vsubscale(idxint n,pfloat a,pfloat * x,pfloat * y)122 void vsubscale(idxint n, pfloat a, pfloat* x, pfloat* y)
123 {
124 idxint i;
125 for( i=0; i<n; i++){ y[i] -= a*x[i]; }
126 }
127
128 /*
129 * 2-norm of a vector.
130 */
norm2(pfloat * v,idxint n)131 pfloat norm2(pfloat* v, idxint n)
132 {
133 idxint i;
134 pfloat normsquare = 0;
135 for( i=0; i<n; i++ ){ normsquare += v[i]*v[i]; }
136 return sqrt(normsquare);
137 }
138
139
140 /*
141 * infinity norm of a vector.
142 */
norminf(pfloat * v,idxint n)143 pfloat norminf(pfloat* v, idxint n)
144 {
145 idxint i;
146 pfloat norm = 0;
147 pfloat mv;
148 for( i=0; i<n; i++ ){
149 if( v[i] > norm ){ norm = v[i]; }
150 mv = -v[i];
151 if( mv > norm){ norm = mv; }
152 }
153 return norm;
154 }
155
156 /*
157 * ECOS dot product z = x'*y of size n.
158 */
eddot(idxint n,pfloat * x,pfloat * y)159 pfloat eddot(idxint n, pfloat* x, pfloat* y)
160 {
161 pfloat z = 0; idxint i;
162 for( i=0; i<n; i++ ){ z += x[i]*y[i]; }
163 return z;
164 }
165