Line data Source code
1 : /*
2 : * The MIT License (MIT)
3 : *
4 : * Copyright (c) 2017 Pantelis Sopasakis (https://alphaville.github.io),
5 : * Krina Menounou (https://www.linkedin.com/in/krinamenounou),
6 : * Panagiotis Patrinos (http://homes.esat.kuleuven.be/~ppatrino)
7 : * Copyright (c) 2012 Brendan O'Donoghue (bodonoghue85@gmail.com)
8 : *
9 : * Permission is hereby granted, free of charge, to any person obtaining a copy
10 : * of this software and associated documentation files (the "Software"), to deal
11 : * in the Software without restriction, including without limitation the rights
12 : * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 : * copies of the Software, and to permit persons to whom the Software is
14 : * furnished to do so, subject to the following conditions:
15 : *
16 : * The above copyright notice and this permission notice shall be included in all
17 : * copies or substantial portions of the Software.
18 : *
19 : * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 : * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 : * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 : * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 : * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 : * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
25 : * SOFTWARE.
26 : *
27 : */
28 : #include "linAlg.h"
29 : #include <math.h>
30 : #include <stdio.h>
31 :
32 :
33 : #ifdef LAPACK_LIB_FOUND
34 :
35 : #define SCS_NO_SUFFIX
36 : #define SCS_IAMAX stitch__(i, BLAS(amax), SCS_NO_SUFFIX)
37 :
38 : extern blasint SCS_IAMAX(
39 : const blasint *n,
40 : const scs_float *x,
41 : const blasint *inc_x);
42 :
43 : extern scs_float BLAS(nrm2)(
44 : const blasint *n,
45 : const scs_float *x,
46 : const blasint *incx);
47 :
48 : /* y += ax */
49 : extern void BLAS(axpy)(
50 : const blasint* n,
51 : const scs_float* alpha,
52 : const scs_float* x,
53 : const blasint* inc_x,
54 : scs_float* y,
55 : const blasint* inc_y);
56 :
57 : extern void BLAS(scal)(
58 : const blasint* n,
59 : const scs_float* alpha,
60 : scs_float* x,
61 : const blasint* inc_x);
62 :
63 : extern scs_float BLAS(dot)(
64 : const blasint* n,
65 : const scs_float* x,
66 : const blasint* inc_x,
67 : const scs_float* y,
68 : const blasint* inc_y);
69 :
70 : extern void BLAS(gemm)(
71 : const char* trans_a,
72 : const char* trans_b,
73 : const blasint* m,
74 : const blasint* n,
75 : const blasint* k,
76 : const scs_float* alpha,
77 : const scs_float* A,
78 : const blasint* lda,
79 : const scs_float* B,
80 : const blasint* ldb,
81 : const scs_float* beta,
82 : scs_float* C,
83 : const blasint* ldc);
84 :
85 : extern void LPCK(gels)(
86 : const char* trans,
87 : const blasint* m,
88 : const blasint* n,
89 : const blasint* nrhs,
90 : const scs_float* a,
91 : const blasint* lda,
92 : scs_float* b,
93 : const blasint* ldb,
94 : scs_float* work,
95 : blasint* lwork,
96 : blasint* info);
97 :
98 : extern void LPCK(gelss)(
99 : const blasint* m,
100 : const blasint* n,
101 : const blasint* nrhs,
102 : const scs_float* A,
103 : const blasint* lda,
104 : scs_float* b,
105 : const blasint* ldb,
106 : scs_float* S,
107 : const scs_float* rcond,
108 : blasint* rank,
109 : scs_float* work,
110 : blasint* lwork,
111 : blasint* info);
112 :
113 : #define scs_iamax_ SCS_IAMAX
114 : #define scs_nrm2_ BLAS(nrm2)
115 : #define scs_axpy_ BLAS(axpy)
116 : #define scs_scal_ BLAS(scal)
117 : #define scs_dot_ BLAS(dot)
118 : #define scs_gemm_ BLAS(gemm)
119 : #define scs_gels_ LPCK(gels)
120 : #define scs_dgelss_ LPCK(gelss)
121 : #endif
122 :
123 : /* static void printVec(char** name, int len, scs_float* x) {
124 : int i;
125 : for (i = 0; i < len; ++i) {
126 : printf("%s[%d]=%g\n", name, i, x[i]);
127 : }
128 : printf("\n");
129 : }*/
130 :
131 :
132 : /* LCOV_EXCL_START */
133 : #define SCS_DGEMM_NN_MC 384
134 : #define SCS_DGEMM_NN_KC 384
135 : #define SCS_DGEMM_NN_NC 4096
136 :
137 : #define SCS_DGEMM_NN_MR 4
138 : #define SCS_DGEMM_NN_NR 4
139 :
140 : /*
141 : * Local buffers for storing panels from A, B and C
142 : */
143 : static double SCS_DGEMM_NN__A[SCS_DGEMM_NN_MC*SCS_DGEMM_NN_KC];
144 : static double SCS_DGEMM_NN__B[SCS_DGEMM_NN_KC*SCS_DGEMM_NN_NC];
145 : static double SCS_DGEMM_NN__C[SCS_DGEMM_NN_MR*SCS_DGEMM_NN_NR];
146 :
147 : /*
148 : * Packing complete panels from A (i.e. without padding)
149 : */
150 : static void
151 : scs_pack_MRxk(int k, const double *A, int incRowA, int incColA,
152 : double *buffer) {
153 : int i, j;
154 :
155 : for (j = 0; j < k; ++j) {
156 : for (i = 0; i < SCS_DGEMM_NN_MR; ++i) {
157 : buffer[i] = A[i * incRowA];
158 : }
159 : buffer += SCS_DGEMM_NN_MR;
160 : A += incColA;
161 : }
162 : }
163 :
164 : /*
165 : * Packing panels from A with padding if required
166 : */
167 : static void
168 : scs_pack_A(int mc, int kc, const double *A, int incRowA, int incColA,
169 : double *buffer) {
170 : int mp = mc / SCS_DGEMM_NN_MR;
171 : int _mr = mc % SCS_DGEMM_NN_MR;
172 :
173 : int i;
174 :
175 : for (i = 0; i < mp; ++i) {
176 : scs_pack_MRxk(kc, A, incRowA, incColA, buffer);
177 : buffer += kc*SCS_DGEMM_NN_MR;
178 : A += SCS_DGEMM_NN_MR*incRowA;
179 : }
180 : if (_mr > 0) {
181 : int j;
182 : for (j = 0; j < kc; ++j) {
183 : for (i = 0; i < _mr; ++i) {
184 : buffer[i] = A[i * incRowA];
185 : }
186 : for (i = _mr; i < SCS_DGEMM_NN_MR; ++i) {
187 : buffer[i] = 0.0;
188 : }
189 : buffer += SCS_DGEMM_NN_MR;
190 : A += incColA;
191 : }
192 : }
193 : }
194 :
195 : /*
196 : * Packing complete panels from B (i.e. without padding)
197 : */
198 : static void
199 : scs_pack_kxNR(int k, const double *B, int incRowB, int incColB,
200 : double *buffer) {
201 : int i, j;
202 :
203 : for (i = 0; i < k; ++i) {
204 : for (j = 0; j < SCS_DGEMM_NN_NR; ++j) {
205 : buffer[j] = B[j * incColB];
206 : }
207 : buffer += SCS_DGEMM_NN_NR;
208 : B += incRowB;
209 : }
210 : }
211 :
212 : /*
213 : * Packing panels from B with padding if required
214 : */
215 : static void
216 : scs_pack_B(int kc, int nc, const double *B, int incRowB, int incColB,
217 : double *buffer) {
218 : int np = nc / SCS_DGEMM_NN_NR;
219 : int _nr = nc % SCS_DGEMM_NN_NR;
220 :
221 : int j;
222 :
223 : for (j = 0; j < np; ++j) {
224 : scs_pack_kxNR(kc, B, incRowB, incColB, buffer);
225 : buffer += kc*SCS_DGEMM_NN_NR;
226 : B += SCS_DGEMM_NN_NR*incColB;
227 : }
228 : if (_nr > 0) {
229 : int i;
230 : for (i = 0; i < kc; ++i) {
231 : for (j = 0; j < _nr; ++j) {
232 : buffer[j] = B[j * incColB];
233 : }
234 : for (j = _nr; j < SCS_DGEMM_NN_NR; ++j) {
235 : buffer[j] = 0.0;
236 : }
237 : buffer += SCS_DGEMM_NN_NR;
238 : B += incRowB;
239 : }
240 : }
241 : }
242 :
243 : /*
244 : * Micro kernel for multiplying panels from A and B.
245 : */
246 : static void
247 : scs_dgemm_micro_kernel(int kc,
248 : double alpha, const double *A, const double *B,
249 : double beta,
250 : double *C, int incRowC, int incColC) {
251 : double AB[SCS_DGEMM_NN_MR * SCS_DGEMM_NN_NR];
252 :
253 : int i, j, l;
254 :
255 : /* Compute AB = A*B */
256 : for (l = 0; l < SCS_DGEMM_NN_MR * SCS_DGEMM_NN_NR; ++l) {
257 : AB[l] = 0;
258 : }
259 : for (l = 0; l < kc; ++l) {
260 : for (j = 0; j < SCS_DGEMM_NN_NR; ++j) {
261 : for (i = 0; i < SCS_DGEMM_NN_MR; ++i) {
262 : AB[i + j * SCS_DGEMM_NN_MR] += A[i] * B[j];
263 : }
264 : }
265 : A += SCS_DGEMM_NN_MR;
266 : B += SCS_DGEMM_NN_NR;
267 : }
268 :
269 : /*
270 : * Update C <- beta*C
271 : */
272 : if (beta == 0.0) {
273 : for (j = 0; j < SCS_DGEMM_NN_NR; ++j) {
274 : for (i = 0; i < SCS_DGEMM_NN_MR; ++i) {
275 : C[i * incRowC + j * incColC] = 0.0;
276 : }
277 : }
278 : } else if (beta != 1.0) {
279 : for (j = 0; j < SCS_DGEMM_NN_NR; ++j) {
280 : for (i = 0; i < SCS_DGEMM_NN_MR; ++i) {
281 : C[i * incRowC + j * incColC] *= beta;
282 : }
283 : }
284 : }
285 :
286 : /*
287 : * Update C <- C + alpha*AB (note: the case alpha==0.0 was already treated in
288 : * the above layer dgemm_nn)
289 : */
290 : if (alpha == 1.0) {
291 : for (j = 0; j < SCS_DGEMM_NN_NR; ++j) {
292 : for (i = 0; i < SCS_DGEMM_NN_MR; ++i) {
293 : C[i * incRowC + j * incColC] += AB[i + j * SCS_DGEMM_NN_MR];
294 : }
295 : }
296 : } else {
297 : for (j = 0; j < SCS_DGEMM_NN_NR; ++j) {
298 : for (i = 0; i < SCS_DGEMM_NN_MR; ++i) {
299 : C[i * incRowC + j * incColC] += alpha * AB[i + j * SCS_DGEMM_NN_MR];
300 : }
301 : }
302 : }
303 : }
304 :
305 : /*
306 : * Compute Y += alpha*X
307 : */
308 : static void
309 : scs_dgeaxpy(int m,
310 : int n,
311 : double alpha,
312 : const double *X,
313 : int incRowX,
314 : int incColX,
315 : double *Y,
316 : int incRowY,
317 : int incColY) {
318 : int i, j;
319 :
320 :
321 : if (alpha != 1.0) {
322 : for (j = 0; j < n; ++j) {
323 : for (i = 0; i < m; ++i) {
324 : Y[i * incRowY + j * incColY] += alpha * X[i * incRowX + j * incColX];
325 : }
326 : }
327 : } else {
328 : for (j = 0; j < n; ++j) {
329 : for (i = 0; i < m; ++i) {
330 : Y[i * incRowY + j * incColY] += X[i * incRowX + j * incColX];
331 : }
332 : }
333 : }
334 : }
335 :
336 : /*
337 : * Compute X *= alpha
338 : */
339 : static void
340 : scs_dgescal(int m,
341 : int n,
342 : double alpha,
343 : double *X,
344 : int incRowX,
345 : int incColX) {
346 : int i, j;
347 :
348 : if (alpha != 0.0) {
349 : for (j = 0; j < n; ++j) {
350 : for (i = 0; i < m; ++i) {
351 : X[i * incRowX + j * incColX] *= alpha;
352 : }
353 : }
354 : } else {
355 : for (j = 0; j < n; ++j) {
356 : for (i = 0; i < m; ++i) {
357 : X[i * incRowX + j * incColX] = 0.0;
358 : }
359 : }
360 : }
361 : }
362 :
363 : /*
364 : * Macro Kernel for the multiplication of blocks of A and B. We assume that
365 : * these blocks were previously packed to buffers _A and _B.
366 : */
367 : static void
368 : scs_dgemm_macro_kernel(int mc,
369 : int nc,
370 : int kc,
371 : double alpha,
372 : double beta,
373 : double *C,
374 : int incRowC,
375 : int incColC) {
376 : int mp = (mc + SCS_DGEMM_NN_MR - 1) / SCS_DGEMM_NN_MR;
377 : int np = (nc + SCS_DGEMM_NN_NR - 1) / SCS_DGEMM_NN_NR;
378 :
379 : int _mr = mc % SCS_DGEMM_NN_MR;
380 : int _nr = nc % SCS_DGEMM_NN_NR;
381 :
382 : int mr;
383 : int i, j;
384 :
385 : for (j = 0; j < np; ++j) {
386 : int nr;
387 : nr = (j != np - 1 || _nr == 0) ? SCS_DGEMM_NN_NR : _nr;
388 :
389 : for (i = 0; i < mp; ++i) {
390 : mr = (i != mp - 1 || _mr == 0) ? SCS_DGEMM_NN_MR : _mr;
391 :
392 : if (mr == SCS_DGEMM_NN_MR && nr == SCS_DGEMM_NN_NR) {
393 : scs_dgemm_micro_kernel(kc, alpha, &SCS_DGEMM_NN__A[i * kc * SCS_DGEMM_NN_MR], &SCS_DGEMM_NN__B[j * kc * SCS_DGEMM_NN_NR],
394 : beta,
395 : &C[i * SCS_DGEMM_NN_MR * incRowC + j * SCS_DGEMM_NN_NR * incColC],
396 : incRowC, incColC);
397 : } else {
398 : scs_dgemm_micro_kernel(kc, alpha, &SCS_DGEMM_NN__A[i * kc * SCS_DGEMM_NN_MR], &SCS_DGEMM_NN__B[j * kc * SCS_DGEMM_NN_NR],
399 : 0.0,
400 : SCS_DGEMM_NN__C, 1, SCS_DGEMM_NN_MR);
401 : scs_dgescal(mr, nr, beta,
402 : &C[i * SCS_DGEMM_NN_MR * incRowC + j * SCS_DGEMM_NN_NR * incColC], incRowC, incColC);
403 : scs_dgeaxpy(mr, nr, 1.0, SCS_DGEMM_NN__C, 1, SCS_DGEMM_NN_MR,
404 : &C[i * SCS_DGEMM_NN_MR * incRowC + j * SCS_DGEMM_NN_NR * incColC], incRowC, incColC);
405 : }
406 : }
407 : }
408 : }
409 :
410 : /* Compute C <- beta*C + alpha*A*B */
411 :
412 : /**
413 : * Perofrms the operation
414 : * \f[
415 : * C \leftarrow \beta C + \alpha A B
416 : * \f]
417 : *
418 : * @param m number of rows of matrix \f$A\f$
419 : * @param n number of columns of matrix \f$B\f$
420 : * @param k number of rows of matrix \f$B\f$ (columns of \f$A\f$)
421 : * @param alpha coefficient \f$\alpha\f$
422 : * @param A pointer to matrix \f$A\f$
423 : * @param incRowA increment in traversing the rows of \f$A\f$
424 : * @param incColA increment in traversing the columns of \f$A\f$
425 : * @param B pointer to matrix \f$B\f$
426 : * @param incRowB increment in traversing the rows of \f$B\f$
427 : * @param incColB increment in traversing the columns of \f$B\f$
428 : * @param beta coefficient \f$\beta\f$
429 : * @param C pointer to matrix \f$C\f$
430 : * @param incRowC increment in traversing the rows of \f$C\f$
431 : * @param incColC increment in traversing the columns of \f$C\f$
432 : *
433 : * @see ::scs_matrix_multiply
434 : *
435 : * \note The implementation of this method is that of
436 : * [ULMBLAS](http://apfel.mathematik.uni-ulm.de/~lehn/sghpc/gemm/page13/index.html).
437 : *
438 : * \note The original source code is available at
439 : * [this link](http://apfel.mathematik.uni-ulm.de/~lehn/sghpc/gemm/page13/index.html).
440 : *
441 : * \note The [ULMBLAS project](https://github.com/michael-lehn/ulmBLAS) is available
442 : * on github and is licensed with the
443 : * [new BSD licence](https://github.com/michael-lehn/ulmBLAS/blob/master/LICENSE).
444 : *
445 : * \warning This function works only with \c double precision data.
446 : *
447 : */
448 : void
449 : scs_dgemm_nn(int m,
450 : int n,
451 : int k,
452 : double alpha,
453 : const double *A,
454 : int incRowA,
455 : int incColA,
456 : const double *B,
457 : int incRowB,
458 : int incColB,
459 : double beta,
460 : double *C,
461 : int incRowC,
462 : int incColC) {
463 : int mb = (m + SCS_DGEMM_NN_MC - 1) / SCS_DGEMM_NN_MC;
464 : int nb = (n + SCS_DGEMM_NN_NC - 1) / SCS_DGEMM_NN_NC;
465 : int kb = (k + SCS_DGEMM_NN_KC - 1) / SCS_DGEMM_NN_KC;
466 :
467 : int _mc = m % SCS_DGEMM_NN_MC;
468 : int _nc = n % SCS_DGEMM_NN_NC;
469 : int _kc = k % SCS_DGEMM_NN_KC;
470 :
471 : int mc, kc;
472 : int i, j, l;
473 :
474 : double _beta;
475 :
476 : if (alpha == 0.0 || k == 0) {
477 : scs_dgescal(m, n, beta, C, incRowC, incColC);
478 : return;
479 : }
480 :
481 : for (j = 0; j < nb; ++j) {
482 : int nc;
483 : nc = (j != nb - 1 || _nc == 0) ? SCS_DGEMM_NN_NC : _nc;
484 :
485 : for (l = 0; l < kb; ++l) {
486 : kc = (l != kb - 1 || _kc == 0) ? SCS_DGEMM_NN_KC : _kc;
487 : _beta = (l == 0) ? beta : 1.0;
488 :
489 : scs_pack_B(kc, nc,
490 : &B[l * SCS_DGEMM_NN_KC * incRowB + j * SCS_DGEMM_NN_NC * incColB], incRowB, incColB,
491 : SCS_DGEMM_NN__B);
492 :
493 : for (i = 0; i < mb; ++i) {
494 : mc = (i != mb - 1 || _mc == 0) ? SCS_DGEMM_NN_MC : _mc;
495 :
496 : scs_pack_A(mc, kc,
497 : &A[i * SCS_DGEMM_NN_MC * incRowA + l * SCS_DGEMM_NN_KC * incColA], incRowA, incColA,
498 : SCS_DGEMM_NN__A);
499 :
500 : scs_dgemm_macro_kernel(mc, nc, kc, alpha, _beta,
501 : &C[i * SCS_DGEMM_NN_MC * incRowC + j * SCS_DGEMM_NN_NC * incColC],
502 : incRowC, incColC);
503 : }
504 : }
505 : }
506 : }
507 :
508 : /* LCOV_EXCL_STOP */
509 :
510 :
511 :
512 56 : void scs_matrix_multiply(
513 : scs_int m,
514 : scs_int n,
515 : scs_int k,
516 : scs_float alpha,
517 : const scs_float * RESTRICT A,
518 : scs_float beta,
519 : const scs_float * RESTRICT B,
520 : scs_float *C) {
521 : #ifdef LAPACK_LIB_FOUND
522 : /* Use BLAS to multiply the two matrices */
523 56 : const char no_transpose = 'N';
524 56 : const blasint m_ = m;
525 56 : const blasint n_ = n;
526 56 : const blasint k_ = k;
527 56 : scs_gemm_(&no_transpose, &no_transpose,
528 : &m_, &n_, &k_, &alpha, A, &m_, B, &k_, &beta, C, &m_);
529 : #else
530 : scs_dgemm_nn(m, n, k, alpha, A, 1, m, B, 1, k, beta, C, 1, m);
531 : #endif
532 :
533 56 : }
534 :
535 15 : void scs_matrix_transpose_multiply(
536 : scs_int m,
537 : scs_int n,
538 : scs_int k,
539 : scs_float alpha,
540 : const scs_float *A,
541 : scs_float beta,
542 : const scs_float *B,
543 : scs_float *C) {
544 :
545 : #ifdef LAPACK_LIB_FOUND
546 15 : const char no_transpose = 'N';
547 15 : const char transpose = 'T';
548 15 : const blasint m_ = m;
549 15 : const blasint n_ = n;
550 15 : const blasint k_ = k;
551 15 : scs_gemm_(&transpose, &no_transpose,
552 : &m_, &n_, &k_, &alpha, A, &k_, B, &k_, &beta, C, &m_);
553 : #else
554 : scs_dgemm_nn(m, n, k, alpha, A, k, 1, B, 1, k, beta, C, 1, m);
555 : #endif
556 :
557 :
558 15 : }
559 :
560 : /* x = b*a */
561 200159 : void scs_set_as_scaled_array(
562 : scs_float * RESTRICT x,
563 : const scs_float * RESTRICT a,
564 : const scs_float b,
565 : scs_int len) {
566 : #ifdef LAPACK_LIB_FOUND
567 200159 : memcpy(x, a, len * sizeof (*x));
568 200159 : scs_scale_array(x, b, len);
569 : #else
570 : register scs_int j;
571 : const scs_int block_size = 4;
572 : const scs_int block_len = len >> 2;
573 : const scs_int remaining = len % block_size;
574 : j = 0;
575 : while (j < block_len * block_size) {
576 : x[j] = b * a[j];
577 : ++j;
578 : x[j] = b * a[j];
579 : ++j;
580 : x[j] = b * a[j];
581 : ++j;
582 : x[j] = b * a[j];
583 : ++j;
584 : }
585 : j = block_size * block_len;
586 : switch (remaining) {
587 : case 3: x[j + 2] = b * a[j + 2];
588 : case 2: x[j + 1] = b * a[j + 1];
589 : case 1: x[j] = b * a[j];
590 : case 0:;
591 : }
592 : #endif
593 200159 : }
594 :
595 : /* a *= b */
596 474909 : void scs_scale_array(scs_float * RESTRICT a, const scs_float b, scs_int len) {
597 : #ifdef LAPACK_LIB_FOUND
598 474909 : const blasint one = 1;
599 474909 : const blasint len_ = len;
600 474909 : scs_scal_(&len_, &b, a, &one);
601 : #else
602 : register scs_int j;
603 : const scs_int block_size = 4;
604 : const scs_int block_len = len >> 2;
605 : const scs_int remaining = len % block_size;
606 : j = 0;
607 : while (j < block_len * block_size) {
608 : a[j] *= b;
609 : ++j;
610 : a[j] *= b;
611 : ++j;
612 : a[j] *= b;
613 : ++j;
614 : a[j] *= b;
615 : ++j;
616 : }
617 : j = block_size * block_len;
618 : switch (remaining) {
619 : case 3: a[j + 2] *= b;
620 : case 2: a[j + 1] *= b;
621 : case 1: a[j] *= b;
622 : case 0:;
623 : }
624 : #endif
625 474909 : }
626 :
627 : /* x'*y */
628 778615 : scs_float scs_inner_product(
629 : const scs_float * RESTRICT x,
630 : const scs_float * RESTRICT y,
631 : scs_int len) {
632 : #ifdef LAPACK_LIB_FOUND
633 778615 : blasint one = 1;
634 778615 : const blasint len_ = len;
635 778615 : scs_float dot_product = scs_dot_(&len_, x, &one, y, &one);
636 778615 : return dot_product;
637 : #else
638 : register scs_int j;
639 : register scs_float ip = 0.;
640 : register scs_float s0 = 0.;
641 : register scs_float s1 = 0.;
642 : register scs_float s2 = 0.;
643 : register scs_float s3 = 0.;
644 : static const scs_int block_size = 4;
645 : const scs_int block_len = len >> 2;
646 : const scs_int remaining = len % block_size;
647 :
648 : j = 0;
649 : while (j < block_len * block_size) {
650 : s0 += x[j] * y[j];
651 : ++j;
652 : s1 += x[j] * y[j];
653 : ++j;
654 : s2 += x[j] * y[j];
655 : ++j;
656 : s3 += x[j] * y[j];
657 : ++j;
658 : }
659 : ip = s0 + s1 + s2 + s3;
660 : j = block_size * block_len;
661 : switch (remaining) {
662 : case 3: ip += x[j + 2] * y[j + 2];
663 : case 2: ip += x[j + 1] * y[j + 1];
664 : case 1: ip += x[j] * y[j];
665 : case 0:;
666 : }
667 : return ip;
668 : #endif
669 : }
670 :
671 : /* ||v||_2^2 */
672 193729 : scs_float scs_norm_squared(const scs_float * RESTRICT v, scs_int len) {
673 193729 : return scs_inner_product(v, v, len);
674 : }
675 :
676 : /* ||v||_2 */
677 123947 : scs_float scs_norm(const scs_float * RESTRICT v, scs_int len) {
678 : #ifdef LAPACK_LIB_FOUND
679 123947 : blasint one = 1;
680 123947 : blasint len_ = len;
681 123947 : return scs_nrm2_(&len_, v, &one);
682 : #else
683 : return SQRTF(scs_norm_squared(v, len));
684 : #endif
685 : }
686 :
687 2 : scs_float scs_norm_infinity(
688 : const scs_float * RESTRICT a,
689 : scs_int l) {
690 : #ifdef LAPACK_LIB_FOUND
691 2 : blasint one = 1;
692 2 : blasint len_ = l;
693 2 : blasint idx_max = scs_iamax_(&len_, a, &one);
694 2 : return a[idx_max];
695 : #else
696 : scs_float tmp, max = 0.0;
697 : scs_int i;
698 : for (i = 0; i < l; ++i) {
699 : tmp = ABS(a[i]);
700 : if (tmp > max)
701 : max = tmp;
702 : }
703 : return max;
704 : #endif
705 : }
706 :
707 : /* saxpy a += sc*b */
708 997518 : void scs_add_scaled_array(
709 : scs_float * RESTRICT a,
710 : const scs_float * RESTRICT b,
711 : scs_int len,
712 : const scs_float sc) {
713 : #ifdef LAPACK_LIB_FOUND
714 997518 : blasint one = 1;
715 997518 : const blasint len_ = len;
716 997518 : scs_axpy_(&len_, &sc, b, &one, a, &one);
717 : #else
718 : register scs_int j;
719 : const scs_int block_size = 4;
720 : const scs_int block_len = len >> 2; /* divide by 4*/
721 : const scs_int remaining = len % block_size;
722 : j = 0;
723 : while (j < block_len * block_size) {
724 : a[j] += sc * b[j];
725 : ++j;
726 : a[j] += sc * b[j];
727 : ++j;
728 : a[j] += sc * b[j];
729 : ++j;
730 : a[j] += sc * b[j];
731 : ++j;
732 : }
733 : j = block_size * block_len;
734 : switch (remaining) {
735 : case 3: a[j + 2] += sc * b[j + 2];
736 : case 2: a[j + 1] += sc * b[j + 1];
737 : case 1: a[j] += sc * b[j];
738 : case 0:;
739 : }
740 : #endif
741 997518 : }
742 :
743 412485 : void scs_axpy(
744 : scs_float * RESTRICT x,
745 : const scs_float * RESTRICT u,
746 : const scs_float * RESTRICT v,
747 : scs_float alpha,
748 : scs_float beta,
749 : scs_int n) {
750 : #ifdef LAPACK_LIB_FOUND
751 412485 : scs_float tol = 1e-16;
752 412485 : if (x != u) {
753 409342 : if (ABS(alpha - 1) > tol) {
754 : /* x = a * u */
755 116109 : scs_set_as_scaled_array(x, u, alpha, n);
756 : } else {
757 293233 : memcpy(x, u, n * sizeof (*x));
758 : }
759 : } else {
760 3143 : scs_scale_array(x, alpha, n);
761 : }
762 : /* x += b * v */
763 412485 : scs_add_scaled_array(x, v, n, beta);
764 : #else
765 : register scs_int j;
766 : const scs_int block_size = 4;
767 : const scs_int block_len = n >> 2; /* divide by 4*/
768 : const scs_int remaining = n % block_size;
769 : j = 0;
770 : while (j < block_len * block_size) {
771 : x[j] = alpha * u[j] + beta * v[j];
772 : ++j;
773 : x[j] = alpha * u[j] + beta * v[j];
774 : ++j;
775 : x[j] = alpha * u[j] + beta * v[j];
776 : ++j;
777 : x[j] = alpha * u[j] + beta * v[j];
778 : ++j;
779 : }
780 : j = block_size * block_len;
781 : switch (remaining) {
782 : case 3: x[j + 2] = alpha * u[j + 2] + beta * v[j + 2];
783 : case 2: x[j + 1] = alpha * u[j + 1] + beta * v[j + 1];
784 : case 1: x[j] = alpha * u[j] + beta * v[j];
785 : case 0:;
786 : }
787 : #endif
788 412485 : }
789 :
790 35567 : void scs_add_array(
791 : scs_float * RESTRICT a,
792 : const scs_float * RESTRICT b,
793 : scs_int len) {
794 : #ifdef LAPACK_LIB_FOUND
795 35567 : scs_add_scaled_array(a, b, len, 1.0);
796 : #else
797 : register scs_int j = 0;
798 : const scs_int block_size = 4;
799 : const scs_int block_len = len >> 2;
800 : const scs_int remaining = len % block_size;
801 : while (j < block_len * block_size) {
802 : a[j] += b[j];
803 : ++j;
804 : a[j] += b[j];
805 : ++j;
806 : a[j] += b[j];
807 : ++j;
808 : a[j] += b[j];
809 : ++j;
810 : }
811 : j = block_size * block_len;
812 : switch (remaining) {
813 : case 3: a[j + 2] += b[j + 2];
814 : case 2: a[j + 1] += b[j + 1];
815 : case 1: a[j] += b[j];
816 : case 0:;
817 : }
818 : #endif
819 35567 : }
820 :
821 9 : void scs_subtract_array(
822 : scs_float * RESTRICT a,
823 : const scs_float * RESTRICT b,
824 : scs_int len) {
825 : #ifdef LAPACK_LIB_FOUND
826 9 : scs_add_scaled_array(a, b, len, -1.0);
827 : #else
828 : register scs_int j = 0;
829 : const scs_int block_size = 4;
830 : const scs_int block_len = len >> 2;
831 : const scs_int remaining = len % block_size;
832 :
833 : j = 0;
834 : while (j < block_len * block_size) {
835 : a[j] -= b[j];
836 : ++j;
837 : a[j] -= b[j];
838 : ++j;
839 : a[j] -= b[j];
840 : ++j;
841 : a[j] -= b[j];
842 : ++j;
843 : }
844 : j = block_size * block_len;
845 : switch (remaining) {
846 : case 3: a[j + 2] -= b[j + 2];
847 : case 2: a[j + 1] -= b[j + 1];
848 : case 1: a[j] -= b[j];
849 : case 0:;
850 : }
851 : #endif
852 9 : }
853 :
854 1 : scs_float scs_norm_difference(
855 : const scs_float * RESTRICT a,
856 : const scs_float * RESTRICT b,
857 : scs_int l) {
858 1 : scs_float nmDiff = 0.0, tmp;
859 : scs_int i;
860 11 : for (i = 0; i < l; ++i) {
861 10 : tmp = (a[i] - b[i]);
862 10 : nmDiff += tmp * tmp;
863 : }
864 1 : return SQRTF(nmDiff);
865 : }
866 :
867 66 : scs_float scs_norm_infinity_difference(
868 : const scs_float * RESTRICT a,
869 : const scs_float * RESTRICT b,
870 : scs_int l) {
871 66 : scs_float tmp, max = 0.0;
872 : scs_int i;
873 11435 : for (i = 0; i < l; ++i) {
874 11369 : tmp = ABS(a[i] - b[i]);
875 11369 : if (tmp > max)
876 58 : max = tmp;
877 : }
878 66 : return max;
879 : }
880 :
881 3 : scs_float * scs_cgls_malloc_workspace(scs_int m, scs_int n) {
882 3 : const scs_int maxmn = m > n ? m : n;
883 3 : if (m <= 0 || n <= 0) {
884 : return SCS_NULL;
885 : }
886 3 : return malloc((maxmn + m + 2 * n) * sizeof (scs_float));
887 : }
888 :
889 3 : scs_int scs_cgls(
890 : scs_int m,
891 : scs_int n,
892 : const scs_float * RESTRICT A,
893 : const scs_float * RESTRICT b,
894 : scs_float * RESTRICT x,
895 : scs_float tol,
896 : scs_int * RESTRICT maxiter,
897 : scs_float * RESTRICT workspace
898 : ) {
899 3 : const scs_int maxmn = m > n ? m : n;
900 3 : scs_float * r = workspace;
901 3 : scs_float * p = r + n;
902 3 : scs_float * t = p;
903 3 : scs_float * xi = p + maxmn;
904 3 : scs_float * phi = xi + n;
905 : scs_float r_norm_old;
906 : scs_float r_norm_new;
907 : scs_int k;
908 :
909 :
910 : /* t = b */
911 3 : memcpy(t, b, m * sizeof (*t));
912 : /* t = t - Ax */
913 3 : scs_matrix_multiply(m, 1, n, -1.0, A, 1.0, x, t);
914 : /* r = A' * t */
915 3 : scs_matrix_transpose_multiply(n, 1, m, 1.0, A, 0.0, t, r);
916 : /* p = r */
917 3 : memcpy(p, r, n * sizeof (*p));
918 : /* r_norm_old = norm(r)^2 */
919 3 : r_norm_old = scs_norm_squared(r, n);
920 :
921 11 : for (k = 0; k < *maxiter; ++k) {
922 : double alpha;
923 : /* phi = A * p */
924 11 : scs_matrix_multiply(m, 1, n, 1.0, A, 0.0, p, phi);
925 : /* xi = A' * phi */
926 11 : scs_matrix_transpose_multiply(n, 1, m, 1.0, A, 0.0, phi, xi);
927 : /* alpha = r_norm_old / (p'*xi) */
928 11 : alpha = r_norm_old / scs_inner_product(p, xi, n);
929 : /* x = x + alpha * p */
930 11 : scs_axpy(x, x, p, 1.0, alpha, n);
931 : /* r = r - alpha * xi */
932 11 : scs_axpy(r, r, xi, 1.0, -alpha, n);
933 : /* r_norm_new = norm(r)^2 */
934 11 : r_norm_new = scs_norm_squared(r, n);
935 11 : if (sqrt(r_norm_new) < tol) {
936 : break;
937 : }
938 : /* p = beta * p + r*/
939 8 : scs_axpy(p, p, r, r_norm_new / r_norm_old, 1.0, n);
940 8 : r_norm_old = r_norm_new;
941 : }
942 :
943 3 : if (k == *maxiter) {
944 : return 1;
945 : }
946 :
947 3 : *maxiter = k + 1;
948 :
949 3 : return 0;
950 : }
951 :
952 : #ifdef LAPACK_LIB_FOUND
953 :
954 1 : scs_int scs_qr_workspace_size(
955 : scs_int m,
956 : scs_int n
957 : ) {
958 1 : blasint lwork = -1;
959 : blasint status;
960 1 : blasint nrhs = 1;
961 1 : blasint lda = m;
962 1 : blasint ldb = m;
963 : scs_float wkopt;
964 1 : blasint m_ = m;
965 1 : blasint n_ = n;
966 1 : if (m <= 0 || n <= 0) {
967 : return 0;
968 : }
969 1 : scs_gels_((char *) "No transpose", &m_, &n_, &nrhs, 0, &lda, 0, &ldb, &wkopt, &lwork,
970 : &status);
971 1 : return (scs_int) wkopt;
972 : }
973 :
974 1 : scs_int scs_qrls(
975 : scs_int m,
976 : scs_int n,
977 : scs_float * RESTRICT A,
978 : scs_float * RESTRICT b,
979 : scs_float * RESTRICT wspace,
980 : scs_int wsize
981 : ) {
982 : blasint status;
983 1 : blasint nrhs = 1;
984 1 : blasint lda = m;
985 1 : blasint ldb = m;
986 1 : blasint m_ = m;
987 1 : blasint n_ = n;
988 1 : blasint wsize_ = wsize;
989 1 : scs_gels_("No transpose", &m_, &n_, &nrhs, A, &lda, b, &ldb, wspace, &wsize_, &status);
990 1 : return status;
991 : }
992 :
993 5 : scs_int scs_svd_workspace_size(
994 : scs_int m,
995 : scs_int n
996 : ) {
997 : blasint status;
998 5 : blasint nrhs = 1;
999 5 : scs_float rcond = 1.;
1000 : scs_float wkopt;
1001 : scs_float singular_values;
1002 : blasint rank;
1003 5 : blasint lwork = -1;
1004 5 : blasint m_ = m;
1005 5 : blasint n_ = n;
1006 :
1007 5 : if (m <= 0 || n <= 0) {
1008 : return 0;
1009 : }
1010 :
1011 5 : scs_dgelss_(&m_, &n_, &nrhs, 0, &m_, 0, &m_,
1012 : &singular_values, &rcond, &rank,
1013 : &wkopt, &lwork, &status);
1014 :
1015 5 : return (scs_int) wkopt;
1016 : }
1017 :
1018 43 : scs_int scs_svdls(
1019 : scs_int m,
1020 : scs_int n,
1021 : scs_float * RESTRICT A,
1022 : scs_float * RESTRICT b,
1023 : scs_float * RESTRICT wspace,
1024 : scs_int wsize,
1025 : scs_float rcond,
1026 : scs_float * RESTRICT singular_values,
1027 : scs_int * RESTRICT rank
1028 : ) {
1029 :
1030 : blasint status;
1031 43 : blasint nrhs = 1;
1032 43 : blasint m_ = m;
1033 43 : blasint n_ = n;
1034 43 : blasint wsize_ = wsize;
1035 43 : blasint rank_ = *rank;
1036 43 : scs_dgelss_(&m_, &n_, &nrhs, A, &m_, b, &m_,
1037 : singular_values, &rcond, &rank_,
1038 : wspace, &wsize_, &status);
1039 43 : *rank = rank_;
1040 43 : return (scs_int) status;
1041 : }
1042 : #endif /* #ifdef LAPACK_LIB_FOUND */
|
