68 for( i=0; i<FROM->
nRows; ++i )
69 for( j=0; j<FROM->
nCols; ++j )
98 _THIS->
val[i*lD+j] = v[i*n+j];
112 if ( ( i < 0 ) || ( i >= _THIS->
nRows ) || ( i >= _THIS->
nCols ) )
115 return _THIS->
val[i*((_THIS->
leaDim)+1)];
126 for ( i=0; i<_THIS->
nRows; ++i )
127 for ( j=0; j<i; ++j )
153 for (i = 0; i < icols->
length; i++)
156 for (i = 0; i < icols->
length; i++)
159 for (i = 0; i < icols->
length; i++)
165 for (i = 0; i < _THIS->
nCols; i++)
166 row[i] = _THIS->
val[rNum*(_THIS->
leaDim)+i];
168 for (i = 0; i < _THIS->
nCols; i++)
169 row[i] = -_THIS->
val[rNum*(_THIS->
leaDim)+i];
171 for (i = 0; i < _THIS->
nCols; i++)
172 row[i] = alpha*_THIS->
val[rNum*(_THIS->
leaDim)+i];
182 for (i = 0; i < irows->
length; i++)
185 for (i = 0; i < irows->
length; i++)
188 for (i = 0; i < irows->
length; i++)
200 unsigned long _xN = (
unsigned long)xN;
201 unsigned long _nRows = (
unsigned long)(_THIS->
nRows);
202 unsigned long _nCols = (
unsigned long)(_THIS->
nCols);
208 GEMM(
"TRANS",
"NOTRANS", &_nRows, &_xN, &_nCols, &alpha, _THIS->
val, &_leaDim, x, &_xLD, &beta, y, &_yLD);
217 unsigned long _xN = (
unsigned long)xN;
218 unsigned long _nRows = (
unsigned long)(_THIS->
nRows);
219 unsigned long _nCols = (
unsigned long)(_THIS->
nCols);
225 GEMM(
"NOTRANS",
"NOTRANS", &_nCols, &_xN, &_nRows, &alpha, _THIS->
val, &_leaDim, x, &_xLD, &beta, y, &_yLD);
236 int i, j, k,
row,
col, iy, irA;
241 for (k = 0; k < xN; k++)
242 for (j = 0; j < irows->
length; j++)
245 for (k = 0; k < xN; k++)
246 for (j = 0; j < irows->
length; j++)
247 y[j+k*yLD] = -y[j+k*yLD];
249 for (k = 0; k < xN; k++)
250 for (j = 0; j < irows->
length; j++)
255 for (k = 0; k < xN; k++)
256 for (j = 0; j < irows->
length; j++)
258 row = irows->
iSort[j];
261 for (i = 0; i < _THIS->
nCols; i++)
262 y[iy] += _THIS->
val[irA+i] * x[k*xLD+i];
265 for (k = 0; k < xN; k++)
266 for (j = 0; j < irows->
length; j++)
268 row = irows->
iSort[j];
271 for (i = 0; i < _THIS->
nCols; i++)
272 y[iy] -= _THIS->
val[irA+i] * x[k*xLD+i];
275 for (k = 0; k < xN; k++)
276 for (j = 0; j < irows->
length; j++)
278 row = irows->
iSort[j];
281 for (i = 0; i < _THIS->
nCols; i++)
282 y[iy] += alpha * _THIS->
val[irA+i] * x[k*xLD+i];
286 for (k = 0; k < xN; k++)
287 for (j = 0; j < irows->
length; j++)
289 row = irows->
iSort[j];
292 for (i = 0; i < icols->
length; i++)
294 col = icols->
iSort[i];
299 for (k = 0; k < xN; k++)
300 for (j = 0; j < irows->
length; j++)
302 row = irows->
iSort[j];
305 for (i = 0; i < icols->
length; i++)
307 col = icols->
iSort[i];
312 for (k = 0; k < xN; k++)
313 for (j = 0; j < irows->
length; j++)
315 row = irows->
iSort[j];
318 for (i = 0; i < icols->
length; i++)
320 col = icols->
iSort[i];
328 for (k = 0; k < xN; k++)
329 for (j = 0; j < irows->
length; j++)
330 y[irows->
number[j]+k*yLD] = 0.0;
332 for (k = 0; k < xN; k++)
333 for (j = 0; j < irows->
length; j++)
334 y[irows->
number[j]+k*yLD] = -y[j+k*yLD];
336 for (k = 0; k < xN; k++)
337 for (j = 0; j < irows->
length; j++)
338 y[irows->
number[j]+k*yLD] *= beta;
342 for (k = 0; k < xN; k++)
343 for (j = 0; j < irows->
length; j++)
347 irA = row * (_THIS->
leaDim);
348 for (i = 0; i < _THIS->
nCols; i++)
349 y[iy] += _THIS->
val[irA+i] * x[k*xLD+i];
352 for (k = 0; k < xN; k++)
353 for (j = 0; j < irows->
length; j++)
357 irA = row * (_THIS->
leaDim);
358 for (i = 0; i < _THIS->
nCols; i++)
359 y[iy] -= _THIS->
val[irA+i] * x[k*xLD+i];
362 for (k = 0; k < xN; k++)
363 for (j = 0; j < irows->
length; j++)
367 irA = row * (_THIS->
leaDim);
368 for (i = 0; i < _THIS->
nCols; i++)
369 y[iy] += alpha * _THIS->
val[irA+i] * x[k*xLD+i];
373 for (k = 0; k < xN; k++)
374 for (j = 0; j < irows->
length; j++)
378 irA = row * (_THIS->
leaDim);
379 for (i = 0; i < icols->
length; i++)
381 col = icols->
iSort[i];
386 for (k = 0; k < xN; k++)
387 for (j = 0; j < irows->
length; j++)
391 irA = row * (_THIS->
leaDim);
392 for (i = 0; i < icols->
length; i++)
394 col = icols->
iSort[i];
399 for (k = 0; k < xN; k++)
400 for (j = 0; j < irows->
length; j++)
404 irA = row * (_THIS->
leaDim);
405 for (i = 0; i < icols->
length; i++)
407 col = icols->
iSort[i];
424 for (k = 0; k < xN; k++)
425 for (j = 0; j < icols->
length; j++)
428 for (k = 0; k < xN; k++)
429 for (j = 0; j < icols->
length; j++)
430 y[j+k*yLD] = -y[j+k*yLD];
432 for (k = 0; k < xN; k++)
433 for (j = 0; j < icols->
length; j++)
437 for (k = 0; k < xN; k++)
438 for (j = 0; j < irows->
length; j++)
440 row = irows->
iSort[j];
441 for (i = 0; i < icols->
length; i++)
443 col = icols->
iSort[i];
448 for (k = 0; k < xN; k++)
449 for (j = 0; j < irows->
length; j++)
451 row = irows->
iSort[j];
452 for (i = 0; i < icols->
length; i++)
454 col = icols->
iSort[i];
459 for (k = 0; k < xN; k++)
460 for (j = 0; j < irows->
length; j++)
462 row = irows->
iSort[j];
463 for (i = 0; i < icols->
length; i++)
465 col = icols->
iSort[i];
477 for (i = 0; i < _THIS->
nRows && i < _THIS->
nCols; i++)
478 _THIS->
val[i*((_THIS->
leaDim)+1)] += alpha;
500 for (ii = 0; ii < xN; ii++)
501 for (jj = 0; jj < xN; jj++)
504 for (i=0;i<icols->
length * xN;++i)
508 for (j = 0; j < icols->
length; j++) {
510 for (i = 0; i < icols->
length; i++)
513 for (k = 0; k < xN; k++)
514 Ax[j + k * icols->
length] += h * x[k*xLD+icols->
number[i]];
518 for (ii = 0; ii < icols->
length; ++ii) {
520 for (jj = 0; jj < xN; ++jj) {
521 for (kk = 0; kk < xN; ++kk) {
522 y[kk + jj*yLD] += x[col + jj*xLD] * Ax[ii + kk*icols->
length];
531 void dgemm_(
const char *TRANSA,
const char *TRANSB,
532 const unsigned long *
M,
const unsigned long *
N,
const unsigned long *K,
533 const double *ALPHA,
const double *
A,
const unsigned long *LDA,
const double *B,
const unsigned long *LDB,
534 const double *BETA,
double *C,
const unsigned long *LDC
537 unsigned int i, j, k;
540 for (k = 0; k < *
N; k++)
541 for (j = 0; j < *
M; j++)
544 for (k = 0; k < *
N; k++)
545 for (j = 0; j < *
M; j++)
546 C[j+(*LDC)*k] = -C[j+(*LDC)*k];
548 for (k = 0; k < *
N; k++)
549 for (j = 0; j < *
M; j++)
550 C[j+(*LDC)*k] *= *BETA;
552 if (TRANSA[0] ==
'N')
554 for (k = 0; k < *
N; k++)
555 for (j = 0; j < *
M; j++)
556 for (i = 0; i < *K; i++)
557 C[j+(*LDC)*k] += A[j+(*LDA)*i] * B[i+(*LDB)*k];
559 for (k = 0; k < *
N; k++)
560 for (j = 0; j < *
M; j++)
561 for (i = 0; i < *K; i++)
562 C[j+(*LDC)*k] -= A[j+(*LDA)*i] * B[i+(*LDB)*k];
564 for (k = 0; k < *
N; k++)
565 for (j = 0; j < *
M; j++)
566 for (i = 0; i < *K; i++)
567 C[j+(*LDC)*k] += *ALPHA * A[j+(*LDA)*i] * B[i+(*LDB)*k];
570 for (k = 0; k < *
N; k++)
571 for (j = 0; j < *
M; j++)
572 for (i = 0; i < *K; i++)
573 C[j+(*LDC)*k] += A[i+(*LDA)*j] * B[i+(*LDB)*k];
575 for (k = 0; k < *
N; k++)
576 for (j = 0; j < *
M; j++)
577 for (i = 0; i < *K; i++)
578 C[j+(*LDC)*k] -= A[i+(*LDA)*j] * B[i+(*LDB)*k];
580 for (k = 0; k < *
N; k++)
581 for (j = 0; j < *
M; j++)
582 for (i = 0; i < *K; i++)
583 C[j+(*LDC)*k] += *ALPHA * A[i+(*LDA)*j] * B[i+(*LDB)*k];
586 void sgemm_(
const char *TRANSA,
const char *TRANSB,
587 const unsigned long *
M,
const unsigned long *
N,
const unsigned long *K,
588 const float *ALPHA,
const float *
A,
const unsigned long *LDA,
const float *B,
const unsigned long *LDB,
589 const float *BETA,
float *C,
const unsigned long *LDC
592 unsigned int i, j, k;
595 for (k = 0; k < *
N; k++)
596 for (j = 0; j < *
M; j++)
599 for (k = 0; k < *
N; k++)
600 for (j = 0; j < *
M; j++)
601 C[j+(*LDC)*k] = -C[j+(*LDC)*k];
603 for (k = 0; k < *
N; k++)
604 for (j = 0; j < *
M; j++)
605 C[j+(*LDC)*k] *= *BETA;
607 if (TRANSA[0] ==
'N')
609 for (k = 0; k < *
N; k++)
610 for (j = 0; j < *
M; j++)
611 for (i = 0; i < *K; i++)
612 C[j+(*LDC)*k] += A[j+(*LDA)*i] * B[i+(*LDB)*k];
614 for (k = 0; k < *
N; k++)
615 for (j = 0; j < *
M; j++)
616 for (i = 0; i < *K; i++)
617 C[j+(*LDC)*k] -= A[j+(*LDA)*i] * B[i+(*LDB)*k];
619 for (k = 0; k < *
N; k++)
620 for (j = 0; j < *
M; j++)
621 for (i = 0; i < *K; i++)
622 C[j+(*LDC)*k] += *ALPHA * A[j+(*LDA)*i] * B[i+(*LDB)*k];
625 for (k = 0; k < *
N; k++)
626 for (j = 0; j < *
M; j++)
627 for (i = 0; i < *K; i++)
628 C[j+(*LDC)*k] += A[i+(*LDA)*j] * B[i+(*LDB)*k];
630 for (k = 0; k < *
N; k++)
631 for (j = 0; j < *
M; j++)
632 for (i = 0; i < *K; i++)
633 C[j+(*LDC)*k] -= A[i+(*LDA)*j] * B[i+(*LDB)*k];
635 for (k = 0; k < *
N; k++)
636 for (j = 0; j < *
M; j++)
637 for (i = 0; i < *K; i++)
638 C[j+(*LDC)*k] += *ALPHA * A[i+(*LDA)*j] * B[i+(*LDB)*k];
643 void dpotrf_(
const char *uplo,
const unsigned long *_n,
double *a,
644 const unsigned long *_lda,
long *info
649 long n = (long)(*_n);
650 long lda = (long)(*_lda);
657 for( k=(i-1); k>=0; --k )
658 sum -= a[k+lda*i] * a[k+lda*i];
670 for( j=(i+1); j<n; ++j )
674 for( k=(i-1); k>=0; --k )
675 sum -= a[k+lda*i] * a[k+lda*j];
677 a[i+lda*j] = sum / a[i+lda*i];
685 void spotrf_(
const char *uplo,
const unsigned long *_n,
float *a,
686 const unsigned long *_lda,
long *info
691 long n = (long)(*_n);
692 long lda = (long)(*_lda);
699 for( k=(i-1); k>=0; --k )
700 sum -= a[k+lda*i] * a[k+lda*i];
712 for( j=(i+1); j<n; ++j )
716 for( k=(i-1); k>=0; --k )
717 sum -= a[k+lda*i] * a[k+lda*j];
719 a[i+lda*j] = sum / a[i+lda*i];
static BooleanType qpOASES_isEqual(real_t x, real_t y, real_t TOL)
real_t DenseMatrix_getNorm(DenseMatrix *_THIS, int type)
returnValue DenseMatrix_getCol(DenseMatrix *_THIS, int cNum, const Indexlist *const irows, real_t alpha, real_t *col)
returnValue DenseMatrix_bilinear(DenseMatrix *_THIS, const Indexlist *const icols, int xN, const real_t *x, int xLD, real_t *y, int yLD)
real_t DenseMatrix_getRowNorm(DenseMatrix *_THIS, int rNum, int type)
returnValue DenseMatrix_subTimes(DenseMatrix *_THIS, const Indexlist *const irows, const Indexlist *const icols, int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD, BooleanType yCompr)
Allows to pass back messages to the calling function.
BEGIN_NAMESPACE_QPOASES void DenseMatrixCON(DenseMatrix *_THIS, int m, int n, int lD, real_t *v)
returnValue DenseMatrix_subTransTimes(DenseMatrix *_THIS, const Indexlist *const irows, const Indexlist *const icols, int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD)
returnValue qpOASES_printM(const real_t *const M, int nrow, int ncol)
returnValue DenseMatrix_print(DenseMatrix *_THIS)
void spotrf_(const char *uplo, const unsigned long *_n, float *a, const unsigned long *_lda, long *info)
static BooleanType qpOASES_isZero(real_t x, real_t TOL)
returnValue DenseMatrix_init(DenseMatrix *_THIS, int m, int n, int lD, real_t *v)
void DenseMatrixCPY(DenseMatrix *FROM, DenseMatrix *TO)
Interfaces matrix-vector operations tailored to general dense matrices.
BooleanType DenseMatrix_isDiag(DenseMatrix *_THIS)
static real_t qpOASES_getMax(real_t x, real_t y)
void dgemm_(const char *TRANSA, const char *TRANSB, const unsigned long *M, const unsigned long *N, const unsigned long *K, const double *ALPHA, const double *A, const unsigned long *LDA, const double *B, const unsigned long *LDB, const double *BETA, double *C, const unsigned long *LDC)
static real_t qpOASES_getSqrt(real_t x)
real_t qpOASES_getNorm(const real_t *const v, int n, int type)
static const real_t QPOASES_INFTY
#define END_NAMESPACE_QPOASES
int number[INDEXLISTFACTOR *(NVMAX+NCMAX)]
real_t DenseMatrix_diag(DenseMatrix *_THIS, int i)
Stores and manages index lists.
returnValue DenseMatrix_transTimes(DenseMatrix *_THIS, int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD)
static const real_t QPOASES_EPS
returnValue DenseMatrix_times(DenseMatrix *_THIS, int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD)
void sgemm_(const char *TRANSA, const char *TRANSB, const unsigned long *M, const unsigned long *N, const unsigned long *K, const float *ALPHA, const float *A, const unsigned long *LDA, const float *B, const unsigned long *LDB, const float *BETA, float *C, const unsigned long *LDC)
#define BEGIN_NAMESPACE_QPOASES
static const real_t QPOASES_TOL
returnValue DenseMatrix_getRow(DenseMatrix *_THIS, int rNum, const Indexlist *const icols, real_t alpha, real_t *row)
void DenseMatrix_free(DenseMatrix *_THIS)
void dpotrf_(const char *uplo, const unsigned long *_n, double *a, const unsigned long *_lda, long *info)
#define REFER_NAMESPACE_QPOASES
returnValue DenseMatrix_addToDiag(DenseMatrix *_THIS, real_t alpha)