dims_clapack.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * The University of Tokyo
00008  */
00016 #include <dims_common.h>
00017 #ifdef __darwin__
00018 typedef int integer;
00019 typedef double doublereal;
00020 #include <stdlib.h>
00021 #else
00022 #include <f2c.h>
00023 #include <malloc.h>
00024 #endif
00025 #include <stdio.h>
00026 
00027 #ifdef USE_CLAPACK_INTERFACE
00028 #ifdef __WIN32__
00029 #undef USE_CLAPACK_INTERFACE
00030 #include <blaswrap.h>
00031 #else
00032 #if defined(__darwin__) || defined(__linux__)
00033 extern "C"{
00034 #endif
00035 #include <cblas.h>
00036 #if defined(__darwin__) || defined(__linux__)
00037 }
00038 #endif
00039 #endif
00040 #endif
00041 
00042 /* CLAPACK native functions */
00043 extern "C" 
00044 {
00045 int dgesvx_(
00046                 char *fact, char *trans, integer *n,
00047                 integer *nrhs, doublereal *a, integer *lda,
00048                 doublereal *af, integer *ldaf, integer *ipiv,
00049                 char *equed, doublereal *r, doublereal *c,
00050                 doublereal *b, integer *ldb, doublereal *x,
00051                 integer *ldx, doublereal *rcond, doublereal *ferr,
00052                 doublereal *berr, doublereal *work, integer *iwork,
00053                 integer *info);
00054 
00055 int dgesvd_(
00056                 char* jobu, char* jobvt, integer* m, integer* n,
00057                 doublereal *a, integer *lda, doublereal *s,
00058                 doublereal *u, integer *ldu, doublereal *vt,
00059                 integer *ldvt, doublereal* work, integer *lwork,
00060                 integer *info);
00061 
00062 int dgelss_(
00063                 integer* m, integer* n, integer* nrhs,
00064                 doublereal* A, integer* lda,
00065                 doublereal* B, integer* ldb,
00066                 doublereal* S, doublereal* rcond, integer* rank,
00067                 doublereal* work, integer* lwork, integer* info);
00068 
00069 int dporfs_(
00070                 char* uplo, integer* n, integer* nrhs,
00071                 doublereal* A, integer* lda,
00072                 doublereal* AF, integer* ldaf,
00073                 doublereal* b, integer* ldb,
00074                 doublereal* x, integer* ldx,
00075                 doublereal* ferr, doublereal* berr,
00076                 doublereal* work, integer* iwork,
00077                 integer* info);
00078 
00079 int dpotrf_(char* uplo, integer* n, doublereal* A, integer* lda,
00080                         integer* info);
00081 
00082 int dpotrs_(char* uplo, integer* n, integer* nrhs,
00083                         doublereal* A, integer* lda,
00084                         doublereal* x, integer* ldb,
00085                         integer* info);
00086 
00087 int dposv_(
00088                 char* uplo, integer* n, integer* nrhs,
00089                 doublereal* A, integer* lda,
00090                 doublereal* b, integer* ldb,
00091                 integer* info);
00092 
00093 int dposvx_(
00094                 char* fact, char* uplo, integer* n, integer* nrhs,
00095                 doublereal* A, integer* lda, doublereal* AF, integer* ldaf,
00096                 char* equed, doublereal* S,
00097                 doublereal* B, integer* ldb,
00098                 doublereal* X, integer* ldx,
00099                 doublereal* rcond, doublereal* ferr, doublereal* berr,
00100                 doublereal* work, integer* iwork, integer* info);
00101 
00102 int dgesvd_(
00103                 char* jobu, char* jobvt, integer* m, integer* n,
00104                 doublereal* A, integer* lda,
00105                 doublereal* S, doublereal* U, integer* ldu,
00106                 doublereal* VT, integer* ldvt,
00107                 doublereal* work, integer* lwork, integer* info);
00108 
00109 int dsyev_(char* jobz, char* uplo, integer* m, doublereal* a, integer* n, doublereal* w, doublereal* work, integer* lwork, integer* info);
00110 
00111 int dgeev_(char *jobvl, char *jobvr, integer *n, doublereal *a, 
00112            integer *lda, doublereal *wr, doublereal *wi, doublereal *vl, 
00113            integer *ldvl, doublereal *vr, integer *ldvr, doublereal *work, 
00114            integer *lwork, integer *info);
00115 
00116 int dgetrf_(integer *m, integer *n, doublereal *a, 
00117             integer *lda, integer *ipiv, integer *info);
00118 
00119 #ifndef USE_CLAPACK_INTERFACE
00120 int f2c_dscal(
00121                 integer* n, doublereal* alpha, doublereal* x, integer* incx);
00122 
00123 int f2c_dcopy(
00124                 integer* n, doublereal* x, integer* incx, doublereal* y, integer* incy);
00125 double f2c_ddot(
00126                 integer* n, doublereal* x, integer* incx, doublereal* y, integer* incy);
00127 int f2c_dgemv(
00128                 char* trans, integer* m, integer* n,
00129                 doublereal* alpha, doublereal* A, integer* lda,
00130                 doublereal* x, integer* incx,
00131                 doublereal* beta, doublereal* y, integer* incy);
00132 
00133 int f2c_dgemm(
00134                 char *transa, char *transb, integer *m, integer *n, integer *k,
00135                 doublereal *alpha, doublereal *a, integer *lda, 
00136                 doublereal *b, integer *ldb,
00137                 doublereal *beta, doublereal *c, integer *ldc);
00138 
00139 int f2c_dsyrk(
00140                 char* uplo, char* trans, integer* n, integer* k,
00141                 doublereal* alpha, doublereal* A, integer* lda,
00142                 doublereal* beta, doublereal* C, integer* ldc);
00143                 
00144 int f2c_daxpy(
00145                 integer* n, doublereal* alpha, doublereal* x, integer* incx,
00146                 doublereal* y, integer* incy);
00147 #endif
00148 }
00149 
00150 double dims_dot(double* _x, double* _y, int _n)
00151 {
00152 #ifndef USE_CLAPACK_INTERFACE
00153         integer incx = 1, incy = 1;
00154         integer n = _n;
00155         return f2c_ddot(&n, _x, &incx, _y, &incy);
00156 #else
00157         const int n=_n, incX=1, incY=1;
00158         return cblas_ddot(n, _x, incX, _y, incY);
00159 #endif
00160 
00161 }
00162 
00163 int dims_copy(double* _x, double* _y, int _n)
00164 {
00165 #ifndef USE_CLAPACK_INTERFACE
00166         integer incx = 1, incy = 1;
00167         integer n = _n;
00168         f2c_dcopy(&n, _x, &incx, _y, &incy);
00169 #else
00170         const int n = _n, incX = 1, incY = 1;
00171         cblas_dcopy(n, _x, incX, _y, incY);
00172 #endif
00173         return 0;
00174 }
00175 
00176 int dims_scale_myself(double* _x, double _alpha, int _n)
00177 {
00178 #ifndef USE_CLAPACK_INTERFACE
00179         integer incx = 1;
00180         integer n = _n;
00181         f2c_dscal(&n, &_alpha, _x, &incx);
00182 #else
00183         const int n = _n, incX = 1;
00184         const double alpha = _alpha;
00185         cblas_dscal(n, alpha, _x, incX);
00186 #endif
00187         return 0;
00188 }
00189 
00190 int dims_scale(double* _x, double _alpha, int _n, double*_y)
00191 {
00192 #ifndef USE_CLAPACK_INTERFACE
00193         integer incx = 1, incy = 1;
00194         integer n = _n;
00195         f2c_dcopy(&n, _x, &incx, _y, &incy);
00196         f2c_dscal(&n, &_alpha, _y, &incx);
00197 #else
00198         const int incX = 1, incY = 1;
00199         cblas_dcopy(_n, _x, incX, _y, incY);
00200         cblas_dscal(_n, _alpha, _x, incX);
00201 #endif
00202         return 0;
00203 }
00204 
00205 int dims_dgemv(double* _A, int _m, int _n, double* _x, double* _y)
00206 {
00207 #ifndef USE_CLAPACK_INTERFACE
00208         char trans = 'N';
00209         integer m = _m;
00210         integer n = _n;
00211         integer lda = _m;
00212         doublereal alpha = 1.0;
00213         integer incx = 1;
00214         doublereal beta = 0.0;
00215         integer incy = 1;
00216         f2c_dgemv(&trans, &m, &n, &alpha, _A, &lda, _x, &incx, &beta, _y, &incy);
00217 #else
00218         cblas_dgemv(CblasColMajor, CblasNoTrans,
00219                     _m, _n, 1.0, _A,_m, _x, 1, 0.0, _y, 1);
00220 #endif
00221         return 0;
00222 }
00223 
00224 int dims_dgemv_tran(double* _A, int _m, int _n, double* _x, double* _y)
00225 {
00226 #ifndef USE_CLAPACK_INTERFACE
00227         char trans = 'T';
00228         integer m = _m;
00229         integer n = _n;
00230         integer lda = _m;
00231         doublereal alpha = 1.0;
00232         integer incx = 1;
00233         doublereal beta = 0.0;
00234         integer incy = 1;
00235         f2c_dgemv(&trans, &m, &n, &alpha, _A, &lda, _x, &incx, &beta, _y, &incy);
00236 #else
00237         cblas_dgemv(CblasColMajor, CblasTrans,
00238                     _m, _n, 1.0, _A, _m, _x, 1, 0.0, _y, 1);
00239 #endif
00240         return 0;
00241 }
00242 
00243 int dims_dgemm(double* _A, double* _B, int _m, int _n, int _k, double* _C)
00244 {
00245 #ifndef USE_CLAPACK_INTERFACE
00246         char transa = 'N';
00247         char transb = 'N';
00248         integer m = _m;
00249         integer n = _n;
00250         integer k = _k;
00251         doublereal alpha = 1.0;
00252         integer lda = _m;
00253         integer ldb = _k;
00254         doublereal beta = 0.0;
00255         integer ldc = _m;
00256         f2c_dgemm(&transa, &transb, &m, &n, &k, &alpha, _A, &lda, _B, &ldb, &beta, _C, &ldc);
00257 #else
00258         cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
00259                     _m, _n, _k, 1.0,
00260                     _A, _m, // lda
00261                     _B, _k, // ldb
00262                     0.0, 
00263                     _C, _m  // ldc
00264                     );
00265 #endif
00266         return 0;
00267 }
00268 
00269 int dims_dsyrk(double* _A, int _n, int _k, double* _C)
00270 {
00271 #ifndef USE_CLAPACK_INTERFACE
00272         char uplo = 'U';
00273         char trans = 'N';
00274         integer n = _n;
00275         integer k = _k;
00276         doublereal alpha = 1.0;
00277         integer lda = _n;
00278         doublereal beta = 0.0;
00279         integer ldc = _n;
00280 
00281         f2c_dsyrk(&uplo, &trans, &n, &k, &alpha, _A, &lda, &beta, _C, &ldc);
00282 #else
00283         cblas_dsyrk(CblasColMajor, CblasUpper, CblasNoTrans,
00284                     _n, _k, 1.0, _A, _n, 0.0, _C, _n);
00285 #endif
00286         return 0;
00287 }
00288 
00289 int dims_dsyrk_trans_first(double* _A, int _n, int _k, double* _C)
00290 {
00291 #ifndef USE_CLAPACK_INTERFACE
00292         char uplo = 'U';
00293         char trans = 'T';
00294         integer n = _n;
00295         integer k = _k;
00296         doublereal alpha = 1.0;
00297         integer lda = _k;
00298         doublereal beta = 0.0;
00299         integer ldc = _n;
00300         f2c_dsyrk(&uplo, &trans, &n, &k, &alpha, _A, &lda, &beta, _C, &ldc);
00301 #else
00302         cblas_dsyrk(CblasColMajor, CblasUpper, CblasTrans,
00303                     _n, _k, 1.0, _A, _n, 0.0, _C, _n);
00304 #endif
00305         return 0;
00306 }
00307 
00308 int dims_daxpy(int _n, double _alpha, double* _x, double* _y)
00309 {
00310 #ifndef USE_CLAPACK_INTERFACE
00311         integer n = _n;
00312         doublereal alpha = _alpha;
00313         integer incx = 1;
00314         integer incy = 1;
00315         f2c_daxpy(&n, &alpha, _x, &incx, _y, &incy);
00316 #else
00317         cblas_daxpy(_n, _alpha, _x, 1, _y, 1);
00318 #endif
00319         return 0;
00320 }
00321 
00322 int dims_dporfs(double* _a, double* _x, double* _b, int _m, int _nrhs)
00323 {
00324         char uplo = 'U';
00325         integer n = _m;
00326         integer nrhs = _nrhs;
00327         integer lda = _m;
00328         integer ldaf = _m;
00329         integer ldb = _m;
00330         integer ldx = _m;
00331         integer info;
00332         integer a_ndata = n*n;
00333         integer b_ndata = n*nrhs;
00334         integer i;
00335         doublereal* a = (doublereal *)malloc(sizeof(doublereal)*a_ndata);
00336         doublereal* u = (doublereal *)malloc(sizeof(doublereal)*a_ndata);
00337         doublereal* b = (doublereal *)malloc(sizeof(doublereal)*b_ndata);
00338         doublereal* x = (doublereal *)malloc(sizeof(doublereal)*b_ndata);
00339         doublereal* ferr = (doublereal *)malloc(sizeof(doublereal)*nrhs);
00340         doublereal* berr = (doublereal *)malloc(sizeof(doublereal)*nrhs);
00341         doublereal* work = (doublereal *)malloc(sizeof(doublereal)*3*n);
00342         integer* iwork = (integer*)malloc(sizeof(integer)*n);
00343         /* copy data in _a because they are overwritten by dposv() */
00344         for(i=0; i<a_ndata; i++)
00345         {
00346                 a[i] = _a[i];
00347                 u[i] = _a[i];
00348         }
00349         for(i=0; i<b_ndata; i++)
00350         {
00351                 b[i] = _b[i];
00352                 x[i] = _b[i];
00353         }
00354         dpotrf_(&uplo, &n, u, &lda, &info);
00355         dpotrs_(&uplo, &n, &nrhs, u, &lda, x, &ldb, &info);
00356 #if 0
00357         printf("info=%d\n", info);
00358         printf("old x = ");
00359         for(i=0; i<b_ndata; i++)
00360         {
00361                 printf("\t%.8g", x[i]);
00362         }
00363         printf("\n");
00364 #endif
00365 /*      dposv_(&uplo, &n, &nrhs, a, &lda, x, &ldb, &info);*/
00366         dporfs_(&uplo, &n, &nrhs, a, &lda, u, &ldaf, b, &ldb, x, &ldx, ferr, berr, work, iwork, &info);
00367         for(i=0; i<b_ndata; i++)
00368         {
00369                 _x[i] = x[i];
00370         }
00371 #if 0
00372         printf("info=%d\n", info);
00373         printf("new x = ");
00374         for(i=0; i<b_ndata; i++)
00375         {
00376                 printf("\t%.8g", x[i]);
00377         }
00378         printf("\n");
00379 #endif
00380         free(a);
00381         free(u);
00382         free(b);
00383         free(x);
00384         free(ferr);
00385         free(berr);
00386         free(work);
00387         free(iwork);
00388         return info;
00389 }
00390 
00391 int dims_dposvx(double* _a, double* _x, double* _b, int _m, int _nrhs, double* _rcond)
00392 {
00393         char fact = 'E';
00394         char uplo = 'U';
00395         integer n = _m;
00396         integer nrhs = _nrhs;
00397         integer lda = _m;
00398         integer ldaf = _m;
00399         char equed = 'N';
00400         integer ldb = _m;
00401         integer ldx = _m;
00402         doublereal rcond;
00403         integer info;
00404         integer a_ndata = n*n;
00405         integer b_ndata = n*nrhs;
00406         integer i;
00407         doublereal* a = (doublereal *)malloc(sizeof(doublereal)*a_ndata);
00408         doublereal* af = (doublereal *)malloc(sizeof(doublereal)*a_ndata);
00409         doublereal* s = (doublereal *)malloc(sizeof(doublereal)*n);
00410         doublereal* b = (doublereal *)malloc(sizeof(doublereal)*b_ndata);
00411         doublereal* x = (doublereal *)malloc(sizeof(doublereal)*b_ndata);
00412         doublereal* ferr = (doublereal *)malloc(sizeof(doublereal)*nrhs);
00413         doublereal* berr = (doublereal *)malloc(sizeof(doublereal)*nrhs);
00414         doublereal* work = (doublereal *)malloc(sizeof(doublereal)*3*n);
00415         integer* iwork = (integer*)malloc(sizeof(integer)*n);
00416         /* copy data in _a because they are overwritten by dposv() */
00417         for(i=0; i<a_ndata; i++)
00418         {
00419                 a[i] = _a[i];
00420         }
00421         for(i=0; i<b_ndata; i++)
00422         {
00423                 b[i] = _b[i];
00424         }
00425         dposvx_(&fact, &uplo, &n, &nrhs, a, &lda, af, &ldaf, &equed, s, b, &ldb, x, &ldx, &rcond, ferr, berr, work, iwork, &info);
00426         for(i=0; i<b_ndata; i++)
00427         {
00428                 _x[i] = x[i];
00429         }
00430 #if 0
00431         printf("scale=[");
00432         for(i=0; i<n; i++)
00433         {
00434                 printf("\t%.8g", s[i]);
00435         }
00436         printf("]\n");
00437 #endif
00438         *_rcond = rcond;
00439         free(a);
00440         free(af);
00441         free(s);
00442         free(b);
00443         free(x);
00444         free(ferr);
00445         free(berr);
00446         free(work);
00447         free(iwork);
00448         return info;
00449 }
00450 
00451 int dims_dposv(double* _a, double* _x, double* _b, int _m, int _nrhs)
00452 {
00453         char uplo = 'U';
00454         integer n = _m;
00455         integer nrhs = _nrhs;
00456         integer lda = _m;
00457         integer ldb = _m;
00458         integer info;
00459         integer a_ndata = n*n;
00460         integer b_ndata = n*nrhs;
00461         integer i;
00462         doublereal* a = (doublereal *)malloc(sizeof(doublereal)*a_ndata);
00463         doublereal* b = (doublereal *)malloc(sizeof(doublereal)*b_ndata);
00464         /* copy data in _a because they are overwritten by dposv() */
00465         for(i=0; i<a_ndata; i++)
00466         {
00467                 a[i] = _a[i];
00468         }
00469         for(i=0; i<b_ndata; i++)
00470         {
00471                 b[i] = _b[i];
00472         }
00473         dposv_(&uplo, &n, &nrhs, a, &lda, b, &ldb, &info);
00474         for(i=0; i<b_ndata; i++)
00475         {
00476                 _x[i] = b[i];
00477         }
00478         free(a);
00479         free(b);
00480         return info;
00481 }
00482 
00483 int dims_dgesvx(double* _a, double* _x, double* _b, int _n, int _nrhs)
00484 {
00485         char fact = 'N';
00486         char trans = 'N';
00487         integer n = (integer)_n;
00488         integer nrhs = (integer)_nrhs;
00489         doublereal* a = (doublereal*)_a;
00490         integer lda = n;
00491         doublereal* af = (doublereal*)malloc(sizeof(doublereal)*n*n);
00492         integer ldaf = n;
00493         integer *ipiv=(integer *)malloc(sizeof(integer)*n);
00494         char equed = 'N';
00495         doublereal *r=(doublereal *)malloc(sizeof(doublereal)*n);
00496         doublereal *c=(doublereal *)malloc(sizeof(doublereal)*n);
00497         doublereal* b = (doublereal*)_b;
00498         integer ldb = n;
00499         doublereal *x = (doublereal *)_x;
00500         integer ldx = n;
00501 
00502         doublereal rcond;
00503         doublereal *ferr = (doublereal *)malloc(sizeof(doublereal)*nrhs);
00504         doublereal *berr = (doublereal *)malloc(sizeof(doublereal)*nrhs);
00505         doublereal *work = (doublereal *)malloc(sizeof(doublereal)*4*n);
00506         integer *iwork = (integer *)malloc(sizeof(integer)*n);
00507 
00508         integer info;
00509         dgesvx_(&fact, &trans, &n, &nrhs, a, &lda, af, &ldaf, ipiv,
00510                         &equed, r, c, b, &ldb, x, &ldx, &rcond, ferr, berr,
00511                         work, iwork, &info);
00512         free(ipiv);
00513         free(r);
00514         free(c);
00515         free(iwork);
00516         free(work);
00517         free(berr);
00518         free(ferr);
00519         free(af);
00520         return info;
00521 }
00522 
00523 /* inputs: A(m x n), b(m x nrhs), x(n x nrhs) */
00524 int dims_dgelss(double* _a, double* _x, double* _b, int _m, int _n, int _nrhs,
00525                    double* _s, int* _rank, int _lwork)
00526 {
00527         int i, mm, tmp1, tmp2;
00528         integer m = (integer)_m;  /* row of A */
00529         integer n = (integer)_n;  /* col of A */
00530         integer nrhs = (integer)_nrhs;  /* row of b and x */
00531         integer lda, ldb, rank, lwork, info;
00532         doublereal *a, *b, *s, rcond, *work;
00533         mm = _m*_n;
00534         a = (doublereal*)malloc(sizeof(doublereal)*mm);  /* A is overwritten by its singular vectors */
00535         for(i=0; i<mm; i++) a[i] = _a[i];
00536         lda = MAX(1, _m);   /* row of A */
00537         ldb = MAX3(1, _m, _n);    /* row of B */
00538         mm = ldb*_nrhs;
00539         b = (doublereal*)malloc(sizeof(doublereal)*mm);  /* note that b is overwritten by lapack function */
00540         for(i=0; i<mm; i++) b[i] = _b[i];  /* copy b */
00541         s = (doublereal*)_s;
00542         rcond = -1;  /* rcond < 0 -> use machine precision */
00543         if(_lwork > 0) lwork = _lwork;
00544         else
00545         {
00546                 lwork = MIN(_m, _n);  /* ? size of work */
00547                 lwork *= 3;
00548                 tmp1 = MIN(_m, _n);
00549                 tmp2 = MAX(_m, _n);
00550                 tmp1 *= 2;
00551                 lwork += MAX3(tmp1, tmp2, _nrhs);
00552                 lwork *= 5;
00553         }
00554         work = (doublereal*)malloc(sizeof(doublereal)*lwork);
00555         dgelss_(&m, &n, &nrhs, a, &lda, b, &ldb, s, &rcond, &rank,
00556                         work, &lwork, &info);
00557 
00558         mm = _n*_nrhs;
00559         for(i=0; i<mm; i++) _x[i] = b[i];
00560         *_rank = (int)rank;
00561 
00562         free(a);
00563         free(b);
00564         free(work);
00565         return info;
00566 }
00567  
00568 int dims_svd(double* _a, int _m, int _n, double* _u, double* _sigma, double* _vt)
00569 {
00570         char jobu = 'A';  /* compute all columns of U */
00571         char jobvt = 'A';  /* compute all rows of VT */
00572         int mn = _m * _n, i, large = MAX(_m, _n), small = MIN(_m, _n);
00573         integer m = (integer)_m;
00574         integer n = (integer)_n;
00575         integer lda = m;
00576         integer ldu = m;
00577         integer ldvt = n;
00578         integer lwork, info;
00579         doublereal* a, *work;
00580         doublereal* u = (doublereal*)_u;
00581         doublereal* s = (doublereal*)_sigma;
00582         doublereal* vt = (doublereal*)_vt;
00583         /* copy a - contents of a will be destroyed */
00584         a = (doublereal*)malloc(sizeof(doublereal) * mn);
00585         for(i=0; i<mn; i++) a[i] = _a[i];
00586         /* compute lwork */
00587         lwork = MAX(3*small+large, 5*small-4);
00588         lwork *= 5;  /* larger is better */
00589         /* create work */
00590         work = (doublereal*)malloc(sizeof(doublereal) * lwork);
00591         /* call dgesvd */
00592         dgesvd_(&jobu, &jobvt, &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, work, &lwork, &info);
00593 
00594         free(a);
00595         free(work);
00596         return info;
00597 }
00598 
00599 
00600 int dims_eigs(int _n, double *_a, double *w)
00601 {
00602         char     jobz='V'; /* compute both eigenvalues and vectors */
00603         char     uplo='U'; /* store the upper triangle */
00604         integer  lwork;
00605         double   *work;
00606         double   *a;
00607         integer  info;
00608         int      i;
00609         integer  n = (integer)_n;
00610   
00611         a=(double*)malloc(sizeof(double)*n*n);
00612         lwork=3*n;
00613         lwork=lwork*5;
00614         work=(double*)malloc(sizeof(double)*lwork);
00615 
00616         for(i=0;i<n*n;i++){
00617                 a[i]=_a[i];
00618         }
00619         dsyev_(&jobz,&uplo,&n,a,&n,w,work,&lwork,&info);
00620         for(i=0;i<n*n;i++)
00621         {
00622                 _a[i]=a[i];
00623         }
00624         free(a);
00625         free(work);
00626         return info;
00627 }
00628 
00629 int dims_eigs2(int _n, double *_a, double *w)
00630 {
00631         char     jobz='V'; /* computes both eigenvalues and vectors */
00632         char     uplo='U'; /* store the upper triangle */
00633         integer  lwork;
00634         double   *work=NULL;
00635         double   *a=NULL;
00636         integer  info;
00637         int      i;
00638         integer  n = (integer)_n;
00639 
00640         if(!a)
00641                 a=(double*)malloc(sizeof(double)*n*n);
00642         lwork=3*n;
00643         lwork=lwork*5;
00644         if(!work)
00645                 work=(double*)malloc(sizeof(double)*lwork);
00646 
00647         for(i=0;i<n*n;i++)
00648         {
00649                 a[i]=_a[i];
00650         }
00651         dsyev_(&jobz,&uplo,&n,a,&n,w,work,&lwork,&info);
00652         for(i=0;i<n*n;i++)
00653         {
00654                 _a[i]=a[i];
00655         }
00656         free(a);
00657         free(work);
00658 
00659         return info;
00660 }
00661 
00662 int dims_dgeev(int _n, double* _a, double* _wr, double* _wi, double* _vr)
00663 {
00664         int i, nn;
00665         char     jobvl = 'N';
00666         char     jobvr = 'V';
00667         integer n, lda, ldvl, ldvr, lwork, info;
00668         doublereal *a, *wr, *wi, *vl, *vr, *work;
00669   
00670         n = (integer)_n;  
00671         lda = n;
00672         ldvl = 1;
00673         ldvr = n;
00674         lwork = 4*n;
00675         nn = _n*_n;
00676 
00677         a = (doublereal*)malloc(sizeof(doublereal)*nn);
00678         wr = (doublereal*)malloc(sizeof(doublereal)*n);
00679         wi = (doublereal*)malloc(sizeof(doublereal)*n);
00680         vl = (doublereal*)malloc(sizeof(doublereal)*ldvl*n);
00681         vr = (doublereal*)malloc(sizeof(doublereal)*ldvr*n);
00682         work = (doublereal *)malloc(sizeof(doublereal)*lwork);
00683 
00684         for(i=0; i<nn; i++) a[i] = _a[i];
00685   
00686         dgeev_(&jobvl, &jobvr, &n, 
00687                    a, &lda, 
00688                    wr, wi,
00689                    vl, &ldvl,
00690                    vr, &ldvr,
00691                    work, &lwork, &info);
00692 
00693         for(i=0; i<n; i++) _wr[i] = wr[i];
00694         for(i=0; i<n; i++) _wi[i] = wi[i];
00695         for(i=0; i<nn; i++) _vr[i] = vr[i];
00696         free(a);
00697         free(wr);
00698         free(wi);
00699         free(vl);
00700         free(vr);
00701         free(work);
00702         return info;
00703 }
00704 
00705 int dims_dgeev_simple(int _n, double* _a, double* _wr, double* _wi)
00706 {
00707         int i, nn;
00708         char     jobvl = 'N';
00709         char     jobvr = 'N';
00710         integer n, lda, ldvl, ldvr, lwork, info;
00711         doublereal *a, *wr, *wi, *vl, *vr, *work;
00712   
00713         n = (integer)_n;  
00714         lda = n;
00715         ldvl = 1;
00716         ldvr = n;
00717         lwork = 3*n;
00718         nn = _n*_n;
00719 
00720         a = (doublereal*)malloc(sizeof(doublereal)*nn);
00721         wr = (doublereal*)malloc(sizeof(doublereal)*n);
00722         wi = (doublereal*)malloc(sizeof(doublereal)*n);
00723         vl = (doublereal*)malloc(sizeof(doublereal)*ldvl*n);
00724         vr = (doublereal*)malloc(sizeof(doublereal)*ldvr*n);
00725         work = (doublereal *)malloc(sizeof(doublereal)*lwork);
00726 
00727         for(i=0; i<nn; i++) a[i] = _a[i];
00728 
00729         dgeev_(&jobvl, &jobvr, &n, 
00730                    a, &lda, 
00731                    wr, wi,
00732                    vl, &ldvl,
00733                    vr, &ldvr,
00734                    work, &lwork, &info);
00735 
00736         for(i=0; i<n; i++)  _wr[i] = wr[i];
00737         for(i=0; i<n; i++) _wi[i] = wi[i];
00738 
00739         free(a);
00740         free(wr);
00741         free(wi);
00742         free(vl);
00743         free(vr);
00744         free(work);
00745         return info;
00746 }
00747 
00748 int dims_det(int _n, double* _a, double* _x)
00749 {
00750         int i, nn, cnt = 0;
00751         const double sign[2] = {1.0, -1.0};
00752 
00753         integer n, info;
00754         doublereal *a;
00755         integer* ipiv;
00756   
00757         n = (integer)_n;  
00758         nn = _n*_n;
00759   
00760         a = (doublereal*)malloc(sizeof(doublereal)*nn);
00761         ipiv = (integer*)malloc(sizeof(integer)*n);
00762   
00763         for(i=0; i<nn; i++) a[i] = _a[i];
00764   
00765         dgetrf_(&n, &n,
00766                         a, &n, 
00767                         ipiv, &info);
00768   
00769         *_x = 1.0;
00770         for(i=0; i<n; i++)
00771     {
00772                 *_x *= a[i*n+i];
00773                 cnt += ((i+1)!=ipiv[i]);
00774     }
00775         *_x *= sign[cnt%2];
00776 /*      if(cnt%2 != 0) *_x *= -1.0; */
00777         free(a);
00778         free(ipiv);
00779         return info;
00780 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53