diff --git a/nonclass.cc b/nonclass.cc index a658401..9b19242 100644 --- a/nonclass.cc +++ b/nonclass.cc @@ -34,7 +34,7 @@ namespace LA { template void lawritemat(FILE *file,const T *a,int r,int c,const char *form0, \ int nodim,int modulo, int issym); INSTANTIZE(double) -INSTANTIZE(complex) +INSTANTIZE(std::complex) INSTANTIZE(int) INSTANTIZE(short) INSTANTIZE(char) @@ -100,8 +100,8 @@ void lawritemat(FILE *file,const T *a,int r,int c,const char *form0, ii=j; jj=i; } - fprintf(file, f, ((complex)a[ii*(ii+1)/2+jj]).real(), ((complex)a[ii*(ii+1)/2+jj]).imag()); - } else fprintf(file, f, ((complex)a[(i-1)*c+j-1]).real(), ((complex)a[(i-1)*c+j-1]).imag()); + fprintf(file, f, ((std::complex)a[ii*(ii+1)/2+jj]).real(), ((std::complex)a[ii*(ii+1)/2+jj]).imag()); + } else fprintf(file, f, ((std::complex)a[(i-1)*c+j-1]).real(), ((std::complex)a[(i-1)*c+j-1]).imag()); if (j < n2) fputc(' ',file); } fprintf(file, "\n"); @@ -120,8 +120,8 @@ void lawritemat(FILE *file,const T *a,int r,int c,const char *form0, ii=j; jj=i; } - fprintf(file, f, ((complex)a[ii*(ii+1)/2+jj]).real(), ((complex)a[ii*(ii+1)/2+jj]).imag()); - } else fprintf(file,f,((complex)a[(i-1)*c+j-1]).real(), ((complex)a[(i-1)*c+j-1]).imag()); + fprintf(file, f, ((std::complex)a[ii*(ii+1)/2+jj]).real(), ((std::complex)a[ii*(ii+1)/2+jj]).imag()); + } else fprintf(file,f,((std::complex)a[(i-1)*c+j-1]).real(), ((std::complex)a[(i-1)*c+j-1]).imag()); putc(j > &A, NRMat< complex > *B, complex *det, int n) +void linear_solve(NRMat< std::complex > &A, NRMat< std::complex > *B, std::complex *det, int n) { int r, *ipiv; @@ -280,7 +280,7 @@ void linear_solve(NRMat< complex > &A, NRMat< complex > *B, comp //other version of linear solver based on gesvx //------------------------------------------------------------------------------ -extern "C" void FORNAME(zgesvx)(const char *fact, const char *trans, const FINT *n, const FINT *nrhs, complex *A, const FINT *lda, complex *AF, const FINT *ldaf, const FINT *ipiv, char *equed, double *R,double *C, complex *B, const FINT *ldb, complex *X, const FINT *ldx, double *rcond, double *ferr, double *berr, complex *work, double *rwork, FINT *info); +extern "C" void FORNAME(zgesvx)(const char *fact, const char *trans, const FINT *n, const FINT *nrhs, std::complex *A, const FINT *lda, std::complex *AF, const FINT *ldaf, const FINT *ipiv, char *equed, double *R,double *C, std::complex *B, const FINT *ldb, std::complex *X, const FINT *ldx, double *rcond, double *ferr, double *berr, std::complex *work, double *rwork, FINT *info); extern "C" void FORNAME(dgesvx)(const char *fact, const char *trans, const FINT *n, const FINT *nrhs, double *A, const FINT *lda, double *AF, const FINT *ldaf, const FINT *ipiv, char *equed, double *R,double *C, double *B, const FINT *ldb, double *X, const FINT *ldx, double *rcond, double *ferr, double *berr, double *work, FINT *iwork, FINT *info); //------------------------------------------------------------------------------ // solves set of linear equations using dgesvx @@ -369,7 +369,7 @@ int linear_solve_x(NRMat &_A, double *_B, const int _rhsCount, const int // solution is stored in _B // the info parameter of dgesvx is returned (see man dgesvx) //------------------------------------------------------------------------------ -int linear_solve_x(NRMat > &_A, complex *_B, const int _rhsCount, const int _eqCount, const bool _eq, const bool _saveA, double *_rcond){ +int linear_solve_x(NRMat > &_A, std::complex *_B, const int _rhsCount, const int _eqCount, const bool _eq, const bool _saveA, double *_rcond){ const int A_rows = _A.nrows(); const int A_cols = _A.ncols(); @@ -381,8 +381,8 @@ int linear_solve_x(NRMat > &_A, complex *_B, const int _ laerror("linear_solve_x: invalid input matrices"); } - complex *A; - complex * const _A_data = (complex*)_A; + std::complex *A; + std::complex * const _A_data = (std::complex*)_A; FINT info; const FINT nrhs = _rhsCount; @@ -393,17 +393,17 @@ int linear_solve_x(NRMat > &_A, complex *_B, const int _ double rcond; double ferr[nrhs], berr[nrhs]; double R[n], C[n], rwork[2*n]; - complex work[2*n]; + std::complex work[2*n]; FINT *const ipiv = new FINT[n]; - complex *X = new complex[n*nrhs]; - complex *AF = new complex[ldaf*n]; + std::complex *X = new std::complex[n*nrhs]; + std::complex *AF = new std::complex[ldaf*n]; A = _A_data; if(_eq){ if(_saveA){//store the corresponding submatrix of _A (not needed provided fact=='N') - A = new complex[n*n]; + A = new std::complex[n*n]; int offset1 = 0;int offset2 = 0; for(register int i=0;i > &_A, complex *_B, const int _ // solution is stored in _B // the info parameter of dgesvx is returned (see man dgesvx) //------------------------------------------------------------------------------ -int multiply_by_inverse(NRMat &_A, NRMat &_B, bool _useEq, double *_rcond){ +template<> +int multiply_by_inverse(NRMat &_A, NRMat &_B, bool _useEq, double *_rcond){ const FINT n = _A.nrows(); const FINT m = _A.ncols(); @@ -492,7 +493,8 @@ int multiply_by_inverse(NRMat &_A, NRMat &_B, bool _useEq, doubl // solution is stored in _B // the info parameter of zgesvx is returned (see man zgesvx) //------------------------------------------------------------------------------ -int multiply_by_inverse(NRMat > &_A, NRMat > &_B, bool _useEq, double *_rcond){ +template<> +int multiply_by_inverse >(NRMat > &_A, NRMat > &_B, bool _useEq, double *_rcond){ const FINT n = _A.nrows(); const FINT m = _A.ncols(); @@ -505,20 +507,20 @@ int multiply_by_inverse(NRMat > &_A, NRMat > &_B const char trans = 'N';//because of c-order char equed = 'B';//if fact=='N' then equed is an output argument, therefore not declared as const - complex * const A = (complex*)_A; - complex * const B = (complex*)_B; + std::complex * const A = (std::complex*)_A; + std::complex * const B = (std::complex*)_B; _B.copyonwrite();//even if fact='N', call copyonwrite because the solution is going to be stored in _B FINT info; double rcond; double ferr[n], berr[n]; double R[n], C[n], rwork[2*n]; - complex work[2*n]; + std::complex work[2*n]; FINT *const ipiv = new FINT[n]; - complex *X = new complex[n2]; - complex *AF = new complex[n2]; + std::complex *X = new std::complex[n2]; + std::complex *AF = new std::complex[n2]; FORNAME(zgesvx)(&fact, &trans, &n, &n, B, &n, AF, &n, &ipiv[0], &equed, &R[0], &C[0], A, &n, X, &n, &rcond, ferr, berr, work, rwork, &info); @@ -599,15 +601,15 @@ void diagonalize(NRMat &a, NRVec &w, const bool eivec, extern "C" void FORNAME(zheev)(const char *JOBZ, const char *UPLO, const FINT *N, - complex *A, const FINT *LDA, double *W, complex *WORK, const FINT *LWORK, double *RWORK, FINT *INFO); + std::complex *A, const FINT *LDA, double *W, std::complex *WORK, const FINT *LWORK, double *RWORK, FINT *INFO); extern "C" void FORNAME(zhegv)(const FINT *ITYPE, const char *JOBZ, const char *UPLO, const FINT *N, - complex *A, const FINT *LDA, complex *B, const FINT *LDB, double *W, complex *WORK, const FINT *LWORK, double *RWORK, FINT *INFO); + std::complex *A, const FINT *LDA, std::complex *B, const FINT *LDB, double *W, std::complex *WORK, const FINT *LWORK, double *RWORK, FINT *INFO); // a will contain eigenvectors (columns if corder==1), w eigenvalues -void diagonalize(NRMat > &a, NRVec &w, const bool eivec, - const bool corder, int n, NRMat > *b, const int itype) +void diagonalize(NRMat > &a, NRVec &w, const bool eivec, + const bool corder, int n, NRMat > *b, const int itype) { FINT m = a.nrows(); if (m != a.ncols()) laerror("diagonalize() call with non-square matrix"); @@ -626,7 +628,7 @@ void diagonalize(NRMat > &a, NRVec &w, const bool eivec, char vectors = LAPACK_FORTRANCASE('V'); if (!eivec) vectors = LAPACK_FORTRANCASE('n'); FINT LWORK = -1; - complex WORKX; + std::complex WORKX; FINT ldb=0; if(b) ldb=b->ncols(); std::cout << "test vectors "< *WORK = new complex[LWORK]; + std::complex *WORK = new std::complex[LWORK]; #ifdef FORINT if(b) FORNAME(zhegv)(&itypetmp,&vectors, &U, &ntmp, a, &m, *b, &ldb, w, &WORKX, &LWORK, RWORK, &r ); @@ -710,15 +712,15 @@ void diagonalize(NRSMat &a, NRVec &w, NRMat *v, extern "C" void FORNAME(zhpev)(const char *JOBZ, const char *UPLO, const FINT *N, - complex *AP, double *W, complex *Z, const FINT *LDZ, complex *WORK, double *RWORK, FINT *INFO); + std::complex *AP, double *W, std::complex *Z, const FINT *LDZ, std::complex *WORK, double *RWORK, FINT *INFO); extern "C" void FORNAME(zhpgv)(const FINT *ITYPE, const char *JOBZ, const char *UPLO, const FINT *N, - complex *AP, complex *BP, double *W, complex *Z, const FINT *LDZ, complex *WORK, double *RWORK, FINT *INFO); + std::complex *AP, std::complex *BP, double *W, std::complex *Z, const FINT *LDZ, std::complex *WORK, double *RWORK, FINT *INFO); // v will contain eigenvectors, w eigenvalues -void diagonalize(NRSMat > &a, NRVec &w, NRMat > *v, - const bool corder, int n, NRSMat > *b, const int itype) +void diagonalize(NRSMat > &a, NRVec &w, NRMat > *v, + const bool corder, int n, NRSMat > *b, const int itype) { if(n<=0) n = a.nrows(); if (v) if (v->nrows() != v ->ncols() || n > v->nrows() || n > a.nrows()) @@ -736,17 +738,17 @@ void diagonalize(NRSMat > &a, NRVec &w, NRMat *WORK = new complex[2*n]; + std::complex *WORK = new std::complex[2*n]; double *RWORK = new double[3*n]; FINT ldv=v?v->ncols():n; #ifdef FORINT const FINT itypetmp = itype; FINT ntmp = n; - if(b) FORNAME(zhpgv)(&itypetmp,&job, &U, &ntmp, a, *b, w, v?(*v)[0]:(complex *)0, &ldv, WORK, RWORK, &r ); - else FORNAME(zhpev)(&job, &U, &ntmp, a, w, v?(*v)[0]:(complex *)0, &ldv, WORK, RWORK, &r ); + if(b) FORNAME(zhpgv)(&itypetmp,&job, &U, &ntmp, a, *b, w, v?(*v)[0]:(std::complex *)0, &ldv, WORK, RWORK, &r ); + else FORNAME(zhpev)(&job, &U, &ntmp, a, w, v?(*v)[0]:(std::complex *)0, &ldv, WORK, RWORK, &r ); #else - if(b) FORNAME(zhpgv)(&itype,&job, &U, &n, a, *b, w, v?(*v)[0]:(complex *)0, &ldv, WORK, RWORK, &r ); - else FORNAME(zhpev)(&job, &U, &n, a, w, v?(*v)[0]:(complex *)0, &ldv, WORK, RWORK, &r ); + if(b) FORNAME(zhpgv)(&itype,&job, &U, &n, a, *b, w, v?(*v)[0]:(std::complex *)0, &ldv, WORK, RWORK, &r ); + else FORNAME(zhpev)(&job, &U, &n, a, w, v?(*v)[0]:(std::complex *)0, &ldv, WORK, RWORK, &r ); #endif delete[] WORK; delete[] RWORK; @@ -824,11 +826,11 @@ void singular_decomposition(NRMat &a, NRMat *u, NRVec &s extern "C" void FORNAME(zgesvd)(const char *JOBU, const char *JOBVT, const FINT *M, - const FINT *N, complex *A, const FINT *LDA, double *S, complex *U, const FINT *LDU, - complex *VT, const FINT *LDVT, complex *WORK, const FINT *LWORK, double *RWORK, FINT *INFO ); + const FINT *N, std::complex *A, const FINT *LDA, double *S, std::complex *U, const FINT *LDU, + std::complex *VT, const FINT *LDVT, std::complex *WORK, const FINT *LWORK, double *RWORK, FINT *INFO ); -void singular_decomposition(NRMat > &a, NRMat > *u, NRVec &s, - NRMat > *v, const bool vnotdagger, int m, int n) +void singular_decomposition(NRMat > &a, NRMat > *u, NRVec &s, + NRMat > *v, const bool vnotdagger, int m, int n) { FINT m0 = a.nrows(); FINT n0 = a.ncols(); @@ -852,7 +854,7 @@ void singular_decomposition(NRMat > &a, NRMat > // v should be transposed at the end char jobu = u ? 'A' : 'N'; char jobv = v ? 'A' : 'N'; - complex work0; + std::complex work0; FINT lwork = -1; FINT r; double *rwork = new double[5*nmin]; @@ -868,7 +870,7 @@ void singular_decomposition(NRMat > &a, NRMat > #endif lwork = (FINT) work0.real(); - complex *work = new complex[lwork]; + std::complex *work = new std::complex[lwork]; #ifdef FORINT FORNAME(zgesvd)(&jobv, &jobu, &ntmp, &mtmp, a, &n0, s, v?(*v)[0]:0, &n0, @@ -904,14 +906,14 @@ extern "C" void FORNAME(dggev)(const char *JOBVL, const char *JOBVR, const FINT double *WORK, const FINT *LWORK, FINT *INFO ); extern "C" void FORNAME(zgeev)(const char *JOBVL, const char *JOBVR, const FINT *N, - complex *A, const FINT *LDA, complex *W, complex *VL, const FINT *LDVL, - complex *VR, const FINT *LDVR, complex *WORK, const FINT *LWORK, + std::complex *A, const FINT *LDA, std::complex *W, std::complex *VL, const FINT *LDVL, + std::complex *VR, const FINT *LDVR, std::complex *WORK, const FINT *LWORK, double *RWORK, FINT *INFO ); extern "C" void FORNAME(zggev)(const char *JOBVL, const char *JOBVR, const FINT *N, - complex *A, const FINT *LDA, complex *B, const FINT *LDB, complex *W, complex *WBETA, - complex *VL, const FINT *LDVL, complex *VR, const FINT *LDVR, - complex *WORK, const FINT *LWORK, double *RWORK, FINT *INFO ); + std::complex *A, const FINT *LDA, std::complex *B, const FINT *LDB, std::complex *W, std::complex *WBETA, + std::complex *VL, const FINT *LDVL, std::complex *VR, const FINT *LDVR, + std::complex *WORK, const FINT *LWORK, double *RWORK, FINT *INFO ); @@ -1124,10 +1126,10 @@ void gdiagonalize(NRMat &a, NRVec &wr, NRVec &wi, //most general complex routine template<> -void gdiagonalize(NRMat > &a, NRVec< complex > &w, - NRMat< complex >*vl, NRMat< complex > *vr, +void gdiagonalize(NRMat > &a, NRVec< std::complex > &w, + NRMat< std::complex >*vl, NRMat< std::complex > *vr, const bool corder, int n, const int sorttype, const int biorthonormalize, - NRMat > *b, NRVec > *beta) + NRMat > *b, NRVec > *beta) { if(n<=0) {n = a.nrows(); if(a.ncols()!=a.nrows() ) laerror("gdiagonalize() call for a non-square matrix");} @@ -1152,7 +1154,7 @@ void gdiagonalize(NRMat > &a, NRVec< complex > &w, char jobvl = LAPACK_FORTRANCASE(vl ? 'v' : 'n'); char jobvr = LAPACK_FORTRANCASE(vr ? 'v' : 'n'); - complex work0; + std::complex work0; FINT lwork = -1; FINT r; FINT lda=a.ncols(); @@ -1165,30 +1167,30 @@ void gdiagonalize(NRMat > &a, NRVec< complex > &w, #ifdef FORINT FINT ntmp = n; - if(b) FORNAME(zggev)(&jobvr, &jobvl, &ntmp, a, &lda, *b, &ldb, w, *beta, vr?vr[0]:(complex *)0, - &ldvr, vl?vl[0]:(complex *)0, &ldvl, &work0, &lwork, rwork, &r); - else FORNAME(zgeev)(&jobvr, &jobvl, &ntmp, a, &lda, w, vr?vr[0]:(complex *)0, - &ldvr, vl?vl[0]:(complex *)0, &ldvl, &work0, &lwork, rwork, &r); + if(b) FORNAME(zggev)(&jobvr, &jobvl, &ntmp, a, &lda, *b, &ldb, w, *beta, vr?vr[0]:(std::complex *)0, + &ldvr, vl?vl[0]:(std::complex *)0, &ldvl, &work0, &lwork, rwork, &r); + else FORNAME(zgeev)(&jobvr, &jobvl, &ntmp, a, &lda, w, vr?vr[0]:(std::complex *)0, + &ldvr, vl?vl[0]:(std::complex *)0, &ldvl, &work0, &lwork, rwork, &r); #else - if(b) FORNAME(zggev)(&jobvr, &jobvl, &n, a, &lda, *b, &ldb, w, *beta, vr?vr[0]:(complex *)0, - &ldvr, vl?vl[0]:(complex *)0, &ldvl, &work0, &lwork, rwork, &r); - else FORNAME(zgeev)(&jobvr, &jobvl, &n, a, &lda, w, vr?vr[0]:(complex *)0, - &ldvr, vl?vl[0]:(complex *)0, &ldvl, &work0, &lwork, rwork, &r); + if(b) FORNAME(zggev)(&jobvr, &jobvl, &n, a, &lda, *b, &ldb, w, *beta, vr?vr[0]:(std::complex *)0, + &ldvr, vl?vl[0]:(std::complex *)0, &ldvl, &work0, &lwork, rwork, &r); + else FORNAME(zgeev)(&jobvr, &jobvl, &n, a, &lda, w, vr?vr[0]:(std::complex *)0, + &ldvr, vl?vl[0]:(std::complex *)0, &ldvl, &work0, &lwork, rwork, &r); #endif lwork = (FINT) work0.real(); - complex *work = new complex[lwork]; + std::complex *work = new std::complex[lwork]; #ifdef FORINT - if(b) FORNAME(zggev)(&jobvr, &jobvl, &ntmp, a, &lda, *b, &ldb, w, *beta, vr?vr[0]:(complex *)0, - &ldvr, vl?vl[0]:(complex *)0, &ldvl, work, &lwork, rwork, &r); - else FORNAME(zgeev)(&jobvr, &jobvl, &ntmp, a, &lda, w, vr?vr[0]:(complex *)0, - &ldvr, vl?vl[0]:(complex *)0, &ldvl, work, &lwork, rwork, &r); + if(b) FORNAME(zggev)(&jobvr, &jobvl, &ntmp, a, &lda, *b, &ldb, w, *beta, vr?vr[0]:(std::complex *)0, + &ldvr, vl?vl[0]:(std::complex *)0, &ldvl, work, &lwork, rwork, &r); + else FORNAME(zgeev)(&jobvr, &jobvl, &ntmp, a, &lda, w, vr?vr[0]:(std::complex *)0, + &ldvr, vl?vl[0]:(std::complex *)0, &ldvl, work, &lwork, rwork, &r); #else - if(b) FORNAME(zggev)(&jobvr, &jobvl, &n, a, &lda, *b, &ldb, w, *beta, vr?vr[0]:(complex *)0, - &ldvr, vl?vl[0]:(complex *)0, &ldvl, work, &lwork, rwork, &r); - else FORNAME(zgeev)(&jobvr, &jobvl, &n, a, &lda, w, vr?vr[0]:(complex *)0, - &ldvr, vl?vl[0]:(complex *)0, &ldvl, work, &lwork, rwork, &r); + if(b) FORNAME(zggev)(&jobvr, &jobvl, &n, a, &lda, *b, &ldb, w, *beta, vr?vr[0]:(std::complex *)0, + &ldvr, vl?vl[0]:(std::complex *)0, &ldvl, work, &lwork, rwork, &r); + else FORNAME(zgeev)(&jobvr, &jobvl, &n, a, &lda, w, vr?vr[0]:(std::complex *)0, + &ldvr, vl?vl[0]:(std::complex *)0, &ldvl, work, &lwork, rwork, &r); #endif delete[] work; @@ -1205,7 +1207,7 @@ void gdiagonalize(NRMat > &a, NRVec< complex > &w, for(int i=0; i tmp; + std::complex tmp; cblas_zdotc_sub(n,(*vr)[i],1,(*vl)[i], 1, &tmp); tmp = 1./tmp; std::cout <<"scaling by "< > &a, NRVec< complex > &w, template<> -void gdiagonalize(NRMat &a, NRVec< complex > &w, - NRMat< complex >*vl, NRMat< complex > *vr, +void gdiagonalize(NRMat &a, NRVec< std::complex > &w, + NRMat< std::complex >*vl, NRMat< std::complex > *vr, const bool corder, int n, const int sorttype, const int biorthonormalize, NRMat *b, NRVec *beta) { @@ -1246,7 +1248,7 @@ void gdiagonalize(NRMat &a, NRVec< complex > &w, //process the results into complex matrices int i; - for (i=0; i(wr[i], wi[i]); + for (i=0; i(wr[i], wi[i]); if (rvl || rvr) { i = 0; while (i < n) { @@ -1267,26 +1269,26 @@ void gdiagonalize(NRMat &a, NRVec< complex > &w, for (int j=0; j((*rvl)[i][j], (*rvl)[i+1][j]); - (*vl)[j][i+1] = complex((*rvl)[i][j], -(*rvl)[i+1][j]); + (*vl)[j][i] = std::complex((*rvl)[i][j], (*rvl)[i+1][j]); + (*vl)[j][i+1] = std::complex((*rvl)[i][j], -(*rvl)[i+1][j]); } else { - (*vl)[i][j] = complex((*rvl)[i][j], (*rvl)[i+1][j]); - (*vl)[i+1][j] = complex((*rvl)[i][j], -(*rvl)[i+1][j]); + (*vl)[i][j] = std::complex((*rvl)[i][j], (*rvl)[i+1][j]); + (*vl)[i+1][j] = std::complex((*rvl)[i][j], -(*rvl)[i+1][j]); } } if (vr) for (int j=0; j((*rvr)[i][j], (*rvr)[i+1][j]); - (*vr)[j][i+1] = complex((*rvr)[i][j], -(*rvr)[i+1][j]); + (*vr)[j][i] = std::complex((*rvr)[i][j], (*rvr)[i+1][j]); + (*vr)[j][i+1] = std::complex((*rvr)[i][j], -(*rvr)[i+1][j]); } else { - (*vr)[i][j] = complex((*rvr)[i][j], (*rvr)[i+1][j]); - (*vr)[i+1][j] = complex((*rvr)[i][j], -(*rvr)[i+1][j]); + (*vr)[i][j] = std::complex((*rvr)[i][j], (*rvr)[i+1][j]); + (*vr)[i+1][j] = std::complex((*rvr)[i][j], -(*rvr)[i+1][j]); } } i += 2; @@ -1299,7 +1301,7 @@ void gdiagonalize(NRMat &a, NRVec< complex > &w, template<> -const NRMat realpart > >(const NRMat< complex > &a) +const NRMat realpart > >(const NRMat< std::complex > &a) { NRMat result(a.nrows(), a.ncols()); @@ -1317,7 +1319,7 @@ const NRMat realpart > >(const NRMat< complex -const NRMat imagpart > >(const NRMat< complex > &a) +const NRMat imagpart > >(const NRMat< std::complex > &a) { NRMat result(a.nrows(), a.ncols()); @@ -1336,17 +1338,17 @@ const NRMat imagpart > >(const NRMat< complex -const NRMat< complex > realmatrix > (const NRMat &a) +const NRMat< std::complex > realmatrix > (const NRMat &a) { - NRMat > result(a.nrows(), a.ncols()); + NRMat > result(a.nrows(), a.ncols()); #ifdef CUDALA if(a.location == cpu){ #endif -// NRMat > result(a.nrows(), a.ncols()); +// NRMat > result(a.nrows(), a.ncols()); cblas_dcopy(a.nrows()*a.ncols(), a, 1, (double *)result[0], 2); #ifdef CUDALA }else{ @@ -1358,15 +1360,15 @@ const NRMat< complex > realmatrix > (const NRMat & } template<> -const NRMat< complex > imagmatrix > (const NRMat &a) +const NRMat< std::complex > imagmatrix > (const NRMat &a) { - NRMat< complex > result(a.nrows(), a.ncols()); + NRMat< std::complex > result(a.nrows(), a.ncols()); #ifdef CUDALA if(a.location == cpu){ #endif -// NRMat< complex > result(a.nrows(), a.ncols()); +// NRMat< std::complex > result(a.nrows(), a.ncols()); cblas_dcopy(a.nrows()*a.ncols(), a, 1, (double *)result[0]+1, 2); #ifdef CUDALA }else{ @@ -1378,20 +1380,20 @@ const NRMat< complex > imagmatrix > (const NRMat & } template<> -const NRMat< complex > complexmatrix > (const NRMat &re, const NRMat &im) +const NRMat< std::complex > complexmatrix > (const NRMat &re, const NRMat &im) { if(re.nrows()!=im.nrows() || re.ncols() != im.ncols()) laerror("incompatible sizes of real and imaginary parts"); - NRMat< complex > result(re.nrows(), re.ncols()); + NRMat< std::complex > result(re.nrows(), re.ncols()); cblas_dcopy(re.nrows()*re.ncols(), re, 1, (double *)result[0], 2); cblas_dcopy(re.nrows()*re.ncols(), im, 1, (double *)result[0]+1, 2); return result; } template<> -const SparseSMat< complex > complexmatrix >(const SparseSMat &re, const SparseSMat &im) { +const SparseSMat< std::complex > complexmatrix >(const SparseSMat &re, const SparseSMat &im) { if(re.nrows()!=im.nrows() || re.ncols() != im.ncols()) laerror("incompatible sizes of real and imaginary parts"); - SparseSMat< complex > result(re.nrows(),re.ncols()); - complex tmp; + SparseSMat< std::complex > result(re.nrows(),re.ncols()); + std::complex tmp; SparseSMat::iterator pre(re); for(; pre.notend(); ++pre) { @@ -1401,7 +1403,7 @@ const SparseSMat< complex > complexmatrix >(const Spa SparseSMat::iterator pim(im); for(; pim.notend(); ++pim) { - tmp = complex(0,1)*(pim->elem); + tmp = std::complex(0,1)*(pim->elem); result.add(pim->row,pim->col,tmp,false); } @@ -1409,9 +1411,9 @@ const SparseSMat< complex > complexmatrix >(const Spa } template<> -const SparseSMat< complex > realmatrix >(const SparseSMat &re) { - SparseSMat< complex > result(re.nrows(),re.ncols()); - complex tmp; +const SparseSMat< std::complex > realmatrix >(const SparseSMat &re) { + SparseSMat< std::complex > result(re.nrows(),re.ncols()); + std::complex tmp; SparseSMat::iterator pre(re); for(; pre.notend(); ++pre) { @@ -1423,14 +1425,14 @@ const SparseSMat< complex > realmatrix >(const Sparse } template<> -const SparseSMat< complex > imagmatrix >(const SparseSMat &im) { - SparseSMat< complex > result(im.nrows(),im.ncols()); - complex tmp; +const SparseSMat< std::complex > imagmatrix >(const SparseSMat &im) { + SparseSMat< std::complex > result(im.nrows(),im.ncols()); + std::complex tmp; SparseSMat::iterator pim(im); for(; pim.notend(); ++pim) { - tmp = complex(0,1)*(pim->elem); + tmp = std::complex(0,1)*(pim->elem); result.add(pim->row,pim->col,tmp,false); } @@ -1456,7 +1458,7 @@ NRMat realmatrixfunction(NRMat a, double (*f) (const double)) } -NRMat > complexmatrixfunction(NRMat a, double (*fre) (const double), double (*fim) (const double)) +NRMat > complexmatrixfunction(NRMat a, double (*fre) (const double), double (*fim) (const double)) { int n = a.nrows(); NRVec wre(n),wim(n); @@ -1470,8 +1472,8 @@ NRMat > complexmatrixfunction(NRMat a, double (*fre) (co NRMat t(n,n),tt(n,n); t.gemm(0.0, u, 't', a, 'n', 1.0); tt.gemm(0.0, u, 't', b, 'n', 1.0); - NRMat > r(n, n); - for (int i=0; i(t(i,j),tt(i,j)); + NRMat > r(n, n); + for (int i=0; i(t(i,j),tt(i,j)); return r; } @@ -1480,7 +1482,7 @@ NRMat > complexmatrixfunction(NRMat a, double (*fre) (co // instantize template to an addresable function -complex myccopy (const complex &x) +std::complex myccopy (const std::complex &x) { return x; } @@ -1490,18 +1492,18 @@ double mycopy (const double x) return x; } -complex myclog (const complex &x) +std::complex myclog (const std::complex &x) { return log(x); } -complex mycexp (const complex &x) +std::complex mycexp (const std::complex &x) { return std::exp(x); } -complex sqrtinv (const complex &x) +std::complex sqrtinv (const std::complex &x) { return 1./std::sqrt(x); } @@ -1517,7 +1519,7 @@ NRMat log(const NRMat &a) return matrixfunction(a, &myclog); } -NRMat > log(const NRMat > &a) +NRMat > log(const NRMat > &a) { return matrixfunction(a, &myclog); } @@ -1528,12 +1530,12 @@ NRMat exp0(const NRMat &a) return matrixfunction(a, &mycexp); } -NRMat > exp0(const NRMat > &a) +NRMat > exp0(const NRMat > &a) { return matrixfunction(a, &mycexp); } -NRMat > copytest(const NRMat > &a) +NRMat > copytest(const NRMat > &a) { return matrixfunction(a, &myccopy); } @@ -1565,13 +1567,13 @@ const NRVec diagofproduct(const NRMat &a, const NRMat &b } -const NRVec< complex > diagofproduct(const NRMat< complex > &a, - const NRMat< complex > &b, bool trb, bool conjb) +const NRVec< std::complex > diagofproduct(const NRMat< std::complex > &a, + const NRMat< std::complex > &b, bool trb, bool conjb) { if (trb && (a.nrows() != b.nrows() || a.ncols() != b.ncols()) || !trb && (a.nrows() != b.ncols() || a.ncols() != b.nrows())) laerror("incompatible Mats in diagofproduct()"); - NRVec< complex > result(a.nrows()); + NRVec< std::complex > result(a.nrows()); if (trb) { if (conjb) { for(int i=0; i &a, const NRMat &b, bool trb) } // LV -complex trace2(const NRMat > &a, const NRMat > &b, bool adjb) +std::complex trace2(const NRMat > &a, const NRMat > &b, bool adjb) { if (adjb && (a.nrows() != b.nrows() || a.ncols() != b.ncols()) || !adjb && (a.nrows() != b.ncols() || a.ncols() != b.nrows())) laerror("incompatible Mats in trace2()"); - complex dot; + std::complex dot; if (adjb) { cblas_zdotc_sub(a.nrows()*a.ncols(), b, 1, a, 1, &dot); return dot; } - complex sum = complex(0.,0.); + std::complex sum = std::complex(0.,0.); for (int i=0; i *A, const FINT *LDA, FINT *INFO); +extern "C" void FORNAME(zpotrf)(const char *UPLO, const FINT *N, std::complex *A, const FINT *LDA, FINT *INFO); void cholesky(NRMat &a, bool upper) { @@ -1680,7 +1682,7 @@ else } -void cholesky(NRMat > &a, bool upper) +void cholesky(NRMat > &a, bool upper) { if(a.nrows()!=a.ncols()) laerror("matrix must be square in Cholesky"); FINT lda=a.ncols(); @@ -1700,10 +1702,10 @@ else //various norms -extern "C" double FORNAME(zlange)( const char *NORM, const FINT *M, const FINT *N, complex *A, const FINT *LDA, double *WORK); +extern "C" double FORNAME(zlange)( const char *NORM, const FINT *M, const FINT *N, std::complex *A, const FINT *LDA, double *WORK); extern "C" double FORNAME(dlange)( const char *NORM, const FINT *M, const FINT *N, double *A, const FINT *LDA, double *WORK); -double MatrixNorm(NRMat > &A, const char norm) +double MatrixNorm(NRMat > &A, const char norm) { const char TypNorm = (tolower(norm) == 'o')?'I':'O'; //switch c-order/fortran-order const FINT M = A.nrows(); @@ -1726,23 +1728,23 @@ double MatrixNorm(NRMat &A, const char norm) //condition number -extern "C" void FORNAME(zgecon)( const char *norm, const FINT *n, complex *A, const FINT *LDA, const double *anorm, double *rcond, complex *work, double *rwork, FINT *info); +extern "C" void FORNAME(zgecon)( const char *norm, const FINT *n, std::complex *A, const FINT *LDA, const double *anorm, double *rcond, std::complex *work, double *rwork, FINT *info); extern "C" void FORNAME(dgecon)( const char *norm, const FINT *n, double *A, const FINT *LDA, const double *anorm, double *rcond, double *work, double *rwork, FINT *info); -double CondNumber(NRMat > &A, const char norm) +double CondNumber(NRMat > &A, const char norm) { const char TypNorm = (tolower(norm) == 'o')?'I':'O'; //switch c-order/fortran-order const FINT N = A.nrows(); double Norma(0.0), ret(0.0); FINT info; - complex *work; + std::complex *work; double *rwork; if(N != A.ncols()){ laerror("nonsquare matrix in zgecon"); return 0.0; } - work = new complex[2*N]; + work = new std::complex[2*N]; rwork = new double[2*N]; Norma = MatrixNorm(A, norm); diff --git a/nonclass.h b/nonclass.h index 4e122ad..c103b7c 100644 --- a/nonclass.h +++ b/nonclass.h @@ -100,7 +100,7 @@ extern void singular_decomposition(NRMat &a, NRMat *u, NRVec: /*NOTE!!! all versions of diagonalize DESTROY A and generalized diagonalize also B matrix */ declare_la(double) -declare_la(complex) +declare_la(std::complex) // Separate declarations //general nonsymmetric matrix and generalized diagonalization @@ -110,8 +110,8 @@ extern void gdiagonalize(NRMat &a, NRVec &wr, NRVec &wi, NRMat *b=NULL, NRVec *beta=NULL); //this used real storage of eigenvectors like dgeev template -extern void gdiagonalize(NRMat &a, NRVec< complex > &w, - NRMat< complex >*vl, NRMat< complex > *vr, +extern void gdiagonalize(NRMat &a, NRVec< std::complex > &w, + NRMat< std::complex >*vl, NRMat< std::complex > *vr, const bool corder=1, int n=0, const int sorttype=0, const int biorthonormalize=0, NRMat *b=NULL, NRVec *beta=NULL); //eigenvectors are stored in complex matrices for T both double and complex @@ -129,7 +129,7 @@ extern const typename LA_traits::complextype complexmatrix (const T&, const T //Cholesky decomposition extern void cholesky(NRMat &a, bool upper=1); -extern void cholesky(NRMat > &a, bool upper=1); +extern void cholesky(NRMat > &a, bool upper=1); //inverse by means of linear solve, preserving rhs intact template @@ -207,7 +207,7 @@ int linear_solve_x(NRMat &A, T *B, const int rhsCount, const int eqCount, con // the info parameter of dgesvx is returned (see man dgesvx) //------------------------------------------------------------------------------ template -int multiply_by_inverse(NRMat &A, NRMat &B, bool useEq, double *rcond); +int multiply_by_inverse(NRMat &A, NRMat &B, bool useEq=false, double *rcond=NULL); //general submatrix, INDEX will typically be NRVec or even int* @@ -310,7 +310,7 @@ return r; //matrix functions via diagonalization extern NRMat realmatrixfunction(NRMat a, double (*f) (double)); //a has to by in fact symmetric -extern NRMat > complexmatrixfunction(NRMat a, double (*fre) (double), double (*fim) (double)); //a has to by in fact symmetric +extern NRMat > complexmatrixfunction(NRMat a, double (*fre) (double), double (*fim) (double)); //a has to by in fact symmetric template NRMat matrixfunction(NRSMat a, double (*f) (double)) //of symmetric/hermitian matrix @@ -331,18 +331,18 @@ NRMat matrixfunction(NRSMat a, double (*f) (double)) //of symmetric/hermit template -extern NRMat matrixfunction(NRMat a, complex (*f)(const complex &)) //of a general real/complex matrix +extern NRMat matrixfunction(NRMat a, std::complex (*f)(const std::complex &)) //of a general real/complex matrix { int n = a.nrows(); - NRVec > w(n); - NRMat > u(n,n),v(n,n); + NRVec > w(n); + NRMat > u(n,n),v(n,n); #ifdef debugmf -NRMat > a0=a; +NRMat > a0=a; #endif gdiagonalize(a, w, &u, &v, false,n,0,false,NULL,NULL);//a gets destroyed, eigenvectors are rows - NRVec< complex > z = diagofproduct(u, v, 1, 1); + NRVec< std::complex > z = diagofproduct(u, v, 1, 1); #ifdef debugmf std::cout <<"TEST matrixfunction\n"< > wz(n); +NRVec< std::complex > wz(n); for (int i=0; i > r(n, n); + NRMat< std::complex > r(n, n); r.gemm(0.0, v, 'c', u, 'n', 1.0); return (NRMat) r; //convert back to real if applicable by the explicit decomplexifying constructor; it is NOT checked to which accuracy the imaginary part is actually zero } @@ -369,7 +369,7 @@ std::cout <<"TEST matrixfunction4 "<< a0<< v.transpose(true)*diagonalmatrix(wz)* -extern complex sqrtinv(const complex &); +extern std::complex sqrtinv(const std::complex &); extern double sqrtinv(const double); //functions on matrices @@ -379,9 +379,9 @@ inline NRMat realsqrt(const NRMat &a) { return realmatrixfuncti inline NRMat realsqrtinv(const NRMat &a) { return realmatrixfunction(a,&sqrtinv); } inline NRMat log(const NRSMat &a) { return matrixfunction(a,&std::log); } extern NRMat log(const NRMat &a); -extern NRMat > log(const NRMat > &a); -extern NRMat > exp0(const NRMat > &a); -extern NRMat > copytest(const NRMat > &a); +extern NRMat > log(const NRMat > &a); +extern NRMat > exp0(const NRMat > &a); +extern NRMat > copytest(const NRMat > &a); extern NRMat copytest(const NRMat &a); extern NRMat exp0(const NRMat &a); diff --git a/quaternion.cc b/quaternion.cc index d71f91b..8333fa4 100644 --- a/quaternion.cc +++ b/quaternion.cc @@ -17,8 +17,10 @@ */ #include "quaternion.h" +#include "vecmat3.h" using namespace LA_Quaternion; +using namespace LA_Vecmat3; //do not replicate this code in each object file, therefore not in .h @@ -155,15 +157,149 @@ return derivative; //optionally skip this for microcontrollers if not needed -//note that C++ standard headers should use float version of the functions for T=float +//note that C++ standard headers automatically use float versions of the goniometric functions for T=float template -void Quaternion::normquat2euler(T *e) const +void Quaternion::normquat2eulerzyx(T *e) const { e[0]= atan2(2*q[1]*q[2]-2*q[0]*q[3],2*q[0]*q[0]+2*q[1]*q[1]-1); e[1]= -asin(2*q[1]*q[3]+2*q[0]*q[2]); e[2]= atan2(2*q[2]*q[3]-2*q[0]*q[1],2*q[0]*q[0]+2*q[3]*q[3]-1); } + +template +void Quaternion::normquat2euler(T *eul, const char *type) const +{ +Mat3 rotmat; +normquat2rotmat(*this,rotmat,false); +rotmat2euler(eul,rotmat,type,false,false,false); +} + +//could be done more efficiently since not all rotmat element s are needed in each case, but this is lazy implementation +template +void Quaternion::euler2normquat(const T *e, const char *type) +{ +T s1=sin(e[0]*(T)0.5); +T s2=sin(e[1]*(T)0.5); +T s3=sin(e[2]*(T)0.5); +T c1=cos(e[0]*(T)0.5); +T c2=cos(e[1]*(T)0.5); +T c3=cos(e[2]*(T)0.5); + +switch(Euler_case(type[0],type[1],type[2])) +{ + +case Euler_case('x','z','x'): + { + q[0] = c2*(c1*c3-s1*s3); + q[1] = c2*(s1*c3+c1*s3); + q[2] = -s2*(s1*c3-c1*s3); + q[3] = s2*(c1*c3+s1*s3); + } + break; + +case Euler_case('x','y','x'): + { + q[0] = c2*(c1*c3-s1*s3); + q[1] = c2*(s1*c3+c1*s3); + q[2] = s2*(c1*c3+s1*s3); + q[3] = s2*(s1*c3-c1*s3); + } + break; + +case Euler_case('y','x','y'): + { + q[0] = c2*(c1*c3-s1*s3); + q[1] = s2*(c1*c3+s1*s3); + q[2] = c2*(s1*c3+c1*s3); + q[3] = -s2*(s1*c3-c1*s3); + } + break; + +case Euler_case('y','z','y'): + { + q[0] = c2*(c1*c3-s1*s3); + q[1] = s2*(s1*c3-c1*s3); + q[2] = c2*(s1*c3+c1*s3); + q[3] = s2*(c1*c3+s1*s3); + } + break; + +case Euler_case('z','y','z'): + { + q[0] = c2*(c1*c3-s1*s3); + q[1] = -s2*(s1*c3-c1*s3); + q[2] = s2*(c1*c3+s1*s3); + q[3] = c2*(s1*c3+c1*s3); + } + break; + +case Euler_case('z','x','z'): + { + q[0] = c2*(c1*c3-s1*s3); + q[1] = s2*(c1*c3+s1*s3); + q[2] = s2*(s1*c3-c1*s3); + q[3] = c2*(s1*c3+c1*s3); + } + break; + +case Euler_case('x','z','y'): + { + q[0] = s1*s2*s3 +c1*c2*c3; + q[1] = s1*c2*c3 - c1*s2*s3; + q[2] = -s1*s2*c3+c1*c2*s3; + q[3] = s1*c2*s3 +c1*s2*c3; + } + break; + +case Euler_case('x','y','z'): + { + q[0] = -s1*s2*s3 + c1*c2*c3; + q[1] = s1*c2*c3 +c1*s2*s3; + q[2] = -s1*c2*s3 + c1*s2*c3; + q[3] = s1*s2*c3 + c1*c2*s3; + } + break; + +case Euler_case('y','x','z'): + { + q[0] = s1*s2*s3 + c1*c2*c3; + q[1] = s1*c2*s3 + c1*s2*c3; + q[2] = s1*c2*c3 - c1*s2*s3; + q[3] = -s1*s2*c3 + c1*c2*s3; + } + break; + +case Euler_case('y','z','x'): + { + q[0] = -s1*s2*s3 + c1*c2*c3; + q[1] = s1*s2*c3 + c1*c2*s3; + q[2] = s1*c2*c3 + c1*s2*s3; + q[3] = -s1*c2*s3 + c1*s2*c3; + } + break; + +case Euler_case('z','y','x'): + { + q[0] = c1*c2*c3 + s1*s2*s3; + q[1] = s1*s2*c3 - c1*c2*s3; + q[2] = -s1*c2*s3 - c1*s2*c3; + q[3] = -s1*c2*c3 + c1*s2*s3; + } + break; + +case Euler_case('z','x','y'): + { + q[0] = -s1*s2*s3 + c1*c2*c3; + q[1] = -s1*c2*s3 + c1*s2*c3; + q[2] = s1*s2*c3 + c1*c2*s3; + q[3] = s1*c2*c3 + c1*s2*s3; + } + break; +}//switch +} + + template void Quaternion::axis2normquat(const T *axis, const T &angle) { @@ -188,9 +324,69 @@ axis[2]= q[3]*s; +template +Quaternion LA_Quaternion::exp(const Quaternion &x) +{ +Quaternion r; +T vnorm = sqrt(x[1]*x[1]+x[2]*x[2]+x[3]*x[3]); +r[0] = cos(vnorm); +vnorm = sin(vnorm)/vnorm; +r[1] = x[1] * vnorm; +r[2] = x[2] * vnorm; +r[3] = x[3] * vnorm; +r*= ::exp(x[0]); +return r; +} + + +//NOTE: log(exp(x)) need not be always = x ... log is not unique! +//NOTE2: log(x*y) != log(y*x) != log(x)+log(y) +template +Quaternion LA_Quaternion::log(const Quaternion &x) +{ +Quaternion r; +T vnorm = x[1]*x[1]+x[2]*x[2]+x[3]*x[3]; +T xnorm = vnorm + x[0]*x[0]; +vnorm = sqrt(vnorm); +xnorm = sqrt(xnorm); +r[0] = ::log(xnorm); +T tmp = acos(x[0]/xnorm)/vnorm; +r[1] = x[1] * tmp; +r[2] = x[2] * tmp; +r[3] = x[3] * tmp; +return r; +} + + +template +Quaternion LA_Quaternion::pow(const Quaternion &x, const T &y) +{ +Quaternion r; +T vnorm = x[1]*x[1]+x[2]*x[2]+x[3]*x[3]; +T xnorm = vnorm + x[0]*x[0]; +vnorm = sqrt(vnorm); +xnorm = sqrt(xnorm); +T phi = acos(x[0]/xnorm); +r[0] = cos(y*phi); +T tmp = sin(y*phi)/vnorm; +r[1] = x[1] * tmp; +r[2] = x[2] * tmp; +r[3] = x[3] * tmp; +r *= ::pow(xnorm,y); +return r; +} + + + + + //force instantization #define INSTANTIZE(T) \ template class Quaternion; \ +template Quaternion LA_Quaternion::pow(const Quaternion &x, const T &y); \ +template Quaternion LA_Quaternion::log(const Quaternion &x); \ +template Quaternion LA_Quaternion::exp(const Quaternion &x); \ + INSTANTIZE(float) diff --git a/quaternion.h b/quaternion.h index 79f106e..5408889 100644 --- a/quaternion.h +++ b/quaternion.h @@ -85,8 +85,12 @@ public: Quaternion commutator(const Quaternion &rhs) const {return *this * rhs - rhs * *this;}; //could be made more efficient Quaternion anticommutator(const Quaternion &rhs) const {return *this * rhs + rhs * *this;}; //could be made more efficient - //some conversions - void normquat2euler(T *) const; //"euler" or Tait-Bryan angles [corresponding to meul -r -T xyz -d -t -R] + //some conversions (for all 12 cases of euler angles go via rotation matrices), cf. also the 1977 NASA paper + void normquat2eulerzyx(T *eul) const; //corresponds to [meul -r -T xyz -d -t -R] or euler2rotmat(...,"xyz",true,true,true) + void euler2normquat(const T *eul, const char *type); + void normquat2euler(T *eul, const char *type) const; + inline void eulerzyx2normquat(const T *eul) {euler2normquat(eul,"zyx");}; + //@quaternion to euler via rotation matrix void axis2normquat(const T *axis, const T &angle); void normquat2axis(T *axis, T &angle) const; @@ -98,8 +102,9 @@ public: }; -//stream I/O +//stream I/O ... cannot be moved to .cc, since we do e.g. Quaternion>> #ifndef AVOID_STDSTREAM + template std::istream& operator>>(std::istream &s, Quaternion &x) { @@ -110,8 +115,10 @@ s >> x.q[3]; return s; } + template -std::ostream& operator<<(std::ostream &s, const Quaternion &x) { +std::ostream& operator<<(std::ostream &s, const Quaternion &x) +{ s << x.q[0]<<" "; s << x.q[1]<<" "; s << x.q[2]<<" "; @@ -124,11 +131,24 @@ return s; //the following must be in .h due to the generic M type which is unspecified and can be any type providing [][], either plain C matrix, Mat3 class, or std::matrix or LA matrix NRMat +//maybe we go via T* and recast it to T (*)[3] and move this to .cc to avoid replication of the code in multiple object files? -//conversion between quanternions and rotation matrices +//conversion from normalized quaternion to SU(2) matrix (+/- q yields different SU(2) element) +template +void normquat2su2mat(const Quaternion &q, M &a) +{ +a[0][0] = std::complex(q[0],q[1]); +a[0][1] = std::complex(q[2],q[3]); +a[1][0] = std::complex(-q[2],q[3]); +a[1][1] = std::complex(q[0],-q[1]); +} + + +//use transpose option to match nasa paper definition +//conversion between quanternions and rotation matrices (+/- q yields the same rotation) // template -void normquat2rotmat(const Quaternion &q, M &a) +void normquat2rotmat(const Quaternion &q, M &a, bool transpose=false) { //some explicit common subexpression optimizations { @@ -149,24 +169,34 @@ T q12= q[1]*q[2]; T q13= q[1]*q[3]; T q23= q[2]*q[3]; +if(transpose) //effectively sign change of the temporaries + { + q01 = -q01; + q02 = -q03; + q03 = -q02; + } a[0][1] = 2*(q12+q03); -a[0][2] = 2*(q13-q02); a[1][0] = 2*(q12-q03); -a[1][2] = 2*(q23+q01); +a[0][2] = 2*(q13-q02); a[2][0] = 2*(q13+q02); +a[1][2] = 2*(q23+q01); a[2][1] = 2*(q23-q01); } + +//transpose option to match nasa template -void quat2rotmat(Quaternion q, M &a, const bool already_normalized=false) +void quat2rotmat(Quaternion q, M &a, bool transpose=false, const bool already_normalized=false) { if(!already_normalized) q.normalize(); -normquat2rotmat(q,a); +normquat2rotmat(q,a,transpose); } + +//use transpose option to match nasa //derivative of the rotation matrix by quaternion elements template -void normquat2rotmatder(const Quaternion &q, Quaternion &a) +void normquat2rotmatder(const Quaternion &q, Quaternion &a, bool transpose=false) { //some explicit common subexpression optimizations T q0= q[0]+q[0]; @@ -213,18 +243,29 @@ a[3][1][2]= q2; a[3][2][0]= q1; a[3][2][1]= q2; a[3][2][2]= q3; + +if(transpose) + { + a[0].transposeme(); + a[1].transposeme(); + a[2].transposeme(); + a[3].transposeme(); + } } //normalized quaternion from rotation matrix //convention compatible with the paper on MEMS sensors by Sebastian O.H. Madgwick -//the rotation matrix correcponds to transpose of (4) in Sarabandi and Thomas paper +//the rotation matrix correcponds to transpose of (4) in Sarabandi and Thomas paper or the NASA paper //where the method is described template -void rotmat2normquat(const M &a, Quaternion &q) +void rotmat2normquat(const M &a, Quaternion &q, bool transpose=false) { T tr= a[0][0]+a[1][1]+a[2][2]; +T a12m = transpose? a[1][0]-a[0][1] : a[0][1]-a[1][0]; +T a13m = transpose? a[2][0]-a[0][2] : a[0][2]-a[2][0]; +T a23m = transpose? a[2][1]-a[1][2] : a[1][2]-a[2][1]; if(tr>=0) { q[0] = (T).5*sqrt((T)1. +tr); @@ -235,11 +276,8 @@ if(tr>=0) else { T a12p = a[0][1]+a[1][0]; - T a12m = a[0][1]-a[1][0]; T a13p = a[0][2]+a[2][0]; - T a13m = a[0][2]-a[2][0]; T a23p = a[1][2]+a[2][1]; - T a23m = a[1][2]-a[2][1]; q[0] = (T).5*sqrt((a23m*a23m+a13m*a13m+a12m*a12m)/((T)3.-tr)); q[1] = (T).5*sqrt((a23m*a23m+a12p*a12p+a13p*a13p)/((T)3.-a[0][0]+a[1][1]+a[2][2])); @@ -247,66 +285,30 @@ else q[3] = (T).5*sqrt((a12m*a12m+a13p*a13p+a23p*a23p)/((T)3.+a[0][0]+a[1][1]-a[2][2])); } -if(a[1][2]-a[2][1]<0) q[1] = -q[1]; -if(a[2][0]-a[0][2]<0) q[2] = -q[2]; -if(a[0][1]-a[1][0]<0) q[3] = -q[3]; +if(a23m<0) q[1] = -q[1]; +if(a13m>0) q[2] = -q[2]; +if(a12m<0) q[3] = -q[3]; } -//Functions - cf. https://en.wikipedia.org/wiki/Quaternion +//Quaternion Functions - cf. https://en.wikipedia.org/wiki/Quaternion template -Quaternion exp(const Quaternion &x) -{ -Quaternion r; -T vnorm = sqrt(x[1]*x[1]+x[2]*x[2]+x[3]*x[3]); -r[0] = cos(vnorm); -vnorm = sin(vnorm)/vnorm; -r[1] = x[1] * vnorm; -r[2] = x[2] * vnorm; -r[3] = x[3] * vnorm; -r*= ::exp(x[0]); -return r; -} +Quaternion exp(const Quaternion &x); + + +//NOTE: log(exp(x)) need not be always = x ... log is not unique! +//NOTE2: log(x*y) != log(y*x) != log(x)+log(y) +template +Quaternion log(const Quaternion &x); template -Quaternion log(const Quaternion &x) -{ -Quaternion r; -T vnorm = x[1]*x[1]+x[2]*x[2]+x[3]*x[3]; -T xnorm = vnorm + x[0]*x[0]; -vnorm = sqrt(vnorm); -xnorm = sqrt(xnorm); -r[0] = ::log(xnorm); -T tmp = acos(x[0]/xnorm)/vnorm; -r[1] = x[1] * tmp; -r[2] = x[2] * tmp; -r[3] = x[3] * tmp; -return r; -} +Quaternion pow(const Quaternion &x, const T &y); -template -Quaternion pow(const Quaternion &x, const T &y) -{ -Quaternion r; -T vnorm = x[1]*x[1]+x[2]*x[2]+x[3]*x[3]; -T xnorm = vnorm + x[0]*x[0]; -vnorm = sqrt(vnorm); -xnorm = sqrt(xnorm); -T phi = acos(x[0]/xnorm); -r[0] = cos(y*phi); -T tmp = sin(y*phi)/vnorm; -r[1] = x[1] * tmp; -r[2] = x[2] * tmp; -r[3] = x[3] * tmp; -r *= ::pow(xnorm,y); -return r; -} - } //namespace diff --git a/t.cc b/t.cc index 1585fa2..80e5c0c 100644 --- a/t.cc +++ b/t.cc @@ -1886,7 +1886,7 @@ cout< rotmat; quat2rotmat(r,rotmat); cout << rotmat[0][1]< rotmat2(3,3),rotmat3(3,3); Quaternion > rotmatder; rotmatder[0].resize(3,3); @@ -1905,8 +1905,18 @@ cout <<"orig "< eul; -r.normquat2euler(eul); -cout< rback; +rback.eulerzyx2normquat(eul); +cout <<"q from euler back "< rm; +euler2rotmat((double *)eul,rm,"xyz",false,false,false); +cout < eulback; +rotmat2euler((double *)eulback,rm,"xyz",false,false,false); +cout <<"euler back from rm error "< axis={0,1/sqrt(2),1/sqrt(2)}; Quaternion rrr; rrr.axis2normquat(axis,0.5); @@ -1924,11 +1934,31 @@ Quaternion rrvec = rvec.rotateby(rrr.conjugate()); cout < rotvec; rrr.rotate(rotvec,vec); -Quaternion qq={1.5,2,-3,2.123}; +Quaternion qq={1.5,2,-2,-1.123}; cout << " test "< qq2={-.5,1,2,3.23}; +Mat3 m(1,2,3,4,5,6,7,8,-9); +NRMat mm(m,3,3); +Vec3 v(10,20,30); +NRVec vv(v,3); +cout << m*v< m2 = m.inverse(); +cout< mm2(3,3); mm2=1.; +multiply_by_inverse(mm2,mm); +cout < xeul,yeul; +qq.normquat2euler((double *)xeul,"zyx"); +qq.normquat2eulerzyx((double *)yeul); +cout < xqq; +xqq.euler2normquat((double *)xeul,"zyx"); +cout <<"normquat2euler test "< using namespace LA_Vecmat3; -//cf. https://en.wikipedia.org/wiki/Euler_angles template -void euler2rotmat(const Vec3 &eul, Mat3 a, char *type, bool transpose, bool direction, bool reverse) +const Vec3 Vec3::operator*(const Mat3 &rhs) const + { + Vec3 r; + r[0] = q[0]*rhs.q[0][0] + q[1]*rhs.q[1][0] + q[2]*rhs.q[2][0]; + r[1] = q[0]*rhs.q[0][1] + q[1]*rhs.q[1][1] + q[2]*rhs.q[2][1]; + r[2] = q[0]*rhs.q[0][2] + q[1]*rhs.q[1][2] + q[2]*rhs.q[2][2]; + return r; + }; + + +template +Mat3& Mat3::operator+=(const Mat3 &rhs) +{ +q[0][0]+=rhs.q[0][0];q[0][1]+=rhs.q[0][1];q[0][2]+=rhs.q[0][2]; q[1][0]+=rhs.q[1][0];q[1][1]+=rhs.q[1][1];q[1][2]+=rhs.q[1][2]; q[2][0]+=rhs.q[2][0];q[2][1]+=rhs.q[2][1];q[2][2]+=rhs.q[2][2]; +return *this; +} + +template +Mat3& Mat3::operator-=(const Mat3 &rhs) +{ +q[0][0]-=rhs.q[0][0];q[0][1]-=rhs.q[0][1];q[0][2]-=rhs.q[0][2]; q[1][0]-=rhs.q[1][0];q[1][1]-=rhs.q[1][1];q[1][2]-=rhs.q[1][2]; q[2][0]-=rhs.q[2][0];q[2][1]-=rhs.q[2][1];q[2][2]-=rhs.q[2][2]; +return *this; +} + + +template +const Mat3 Mat3::operator*(const Mat3 &rhs) const + { + Mat3 r; + r[0][0]= q[0][0]*rhs.q[0][0] + q[0][1]*rhs.q[1][0] + q[0][2]*rhs.q[2][0]; + r[0][1]= q[0][0]*rhs.q[0][1] + q[0][1]*rhs.q[1][1] + q[0][2]*rhs.q[2][1]; + r[0][2]= q[0][0]*rhs.q[0][2] + q[0][1]*rhs.q[1][2] + q[0][2]*rhs.q[2][2]; + r[1][0]= q[1][0]*rhs.q[0][0] + q[1][1]*rhs.q[1][0] + q[1][2]*rhs.q[2][0]; + r[1][1]= q[1][0]*rhs.q[0][1] + q[1][1]*rhs.q[1][1] + q[1][2]*rhs.q[2][1]; + r[1][2]= q[1][0]*rhs.q[0][2] + q[1][1]*rhs.q[1][2] + q[1][2]*rhs.q[2][2]; + r[2][0]= q[2][0]*rhs.q[0][0] + q[2][1]*rhs.q[1][0] + q[2][2]*rhs.q[2][0]; + r[2][1]= q[2][0]*rhs.q[0][1] + q[2][1]*rhs.q[1][1] + q[2][2]*rhs.q[2][1]; + r[2][2]= q[2][0]*rhs.q[0][2] + q[2][1]*rhs.q[1][2] + q[2][2]*rhs.q[2][2]; + return r; + } + +template +T Mat3::determinant() const +{ +return q[0][0]*(q[2][2]*q[1][1]-q[2][1]*q[1][2])-q[1][0]*(q[2][2]*q[0][1]-q[2][1]*q[0][2])+q[2][0]*(q[1][2]*q[0][1]-q[1][1]*q[0][2]); +} + +template +void Mat3::transposeme() +{T t; t=q[0][1]; q[0][1]=q[1][0]; q[1][0]=t; t=q[0][2]; q[0][2]=q[2][0]; q[2][0]=t; t=q[1][2]; q[1][2]=q[2][1]; q[2][1]=t;}; + +template +const Mat3 Mat3::inverse() const + { + Mat3 r; + r[0][0]= q[2][2]*q[1][1]-q[2][1]*q[1][2]; + r[0][1]= -q[2][2]*q[0][1]+q[2][1]*q[0][2]; + r[0][2]= q[1][2]*q[0][1]-q[1][1]*q[0][2]; + r[1][0]= -q[2][2]*q[1][0]+q[2][0]*q[1][2]; + r[1][1]= q[2][2]*q[0][0]-q[2][0]*q[0][2]; + r[1][2]= -q[1][2]*q[0][0]+q[1][0]*q[0][2]; + r[2][0]= q[2][1]*q[1][0]-q[2][0]*q[1][1]; + r[2][1]= -q[2][1]*q[0][0]+q[2][0]*q[0][1]; + r[2][2]= q[1][1]*q[0][0]-q[1][0]*q[0][1]; + return r/determinant(); + } + +template +const Vec3 Mat3::operator*(const Vec3 &rhs) const + { + Vec3 r; + r[0] = q[0][0]*rhs.q[0] + q[0][1]*rhs.q[1] + q[0][2]*rhs.q[2]; + r[1] = q[1][0]*rhs.q[0] + q[1][1]*rhs.q[1] + q[1][2]*rhs.q[2]; + r[2] = q[2][0]*rhs.q[0] + q[2][1]*rhs.q[1] + q[2][2]*rhs.q[2]; + return r; + } + + + +//cf. https://en.wikipedia.org/wiki/Euler_angles and NASA paper cited therein +template +void LA_Vecmat3::euler2rotmat(const T *eul, Mat3 &a, const char *type, bool transpose, bool direction, bool reverse) { T c2=cos(eul[1]); T s2=sin(eul[1]); @@ -34,7 +113,9 @@ T s3=sin(eul[reverse?0:2]); if(direction) {s1= -s1; s2= -s2; s3= -s3;} -if(tolower(type[0])=='x' && tolower(type[1])=='z' && tolower(type[2])=='x') +switch(Euler_case(type[0],type[1],type[2])) +{ +case Euler_case('x','z','x'): { a[0][0]= c2; a[0][1]= -c3*s2; @@ -46,8 +127,9 @@ if(tolower(type[0])=='x' && tolower(type[1])=='z' && tolower(type[2])=='x') a[2][1]= c1*s3+c2*c3*s1; a[2][2]= c1*c3-c2*s1*s3; } + break; -if(tolower(type[0])=='x' && tolower(type[1])=='y' && tolower(type[2])=='x') +case Euler_case('x','y','x'): { a[0][0]= c2; a[0][1]= s2*s3; @@ -59,8 +141,9 @@ if(tolower(type[0])=='x' && tolower(type[1])=='y' && tolower(type[2])=='x') a[2][1]= c3*s1+c1*c2*s3; a[2][2]= c1*c2*c3-s1*s3; } + break; -if(tolower(type[0])=='y' && tolower(type[1])=='x' && tolower(type[2])=='y') +case Euler_case('y','x','y'): { a[0][0]= c1*c3-c2*s1*s3; a[0][1]= s1*s2; @@ -72,8 +155,9 @@ if(tolower(type[0])=='y' && tolower(type[1])=='x' && tolower(type[2])=='y') a[2][1]= c1*s2; a[2][2]= c1*c2*c3-s1*s3; } + break; -if(tolower(type[0])=='y' && tolower(type[1])=='z' && tolower(type[2])=='y') +case Euler_case('y','z','y'): { a[0][0]= c1*c2*c3-s1*s3; a[0][1]= -c1*s2; @@ -85,11 +169,12 @@ if(tolower(type[0])=='y' && tolower(type[1])=='z' && tolower(type[2])=='y') a[2][1]= s1*s2; a[2][2]= c1*c3-c2*s1*s3; } + break; -if(tolower(type[0])=='z' && tolower(type[1])=='y' && tolower(type[2])=='z') +case Euler_case('z','y','z'): { a[0][0]= c1*c2*c3-s1*s3; - a[0][1]= -c3*s1; + a[0][1]= -c3*s1-c1*c2*s3; a[0][2]= c1*s2; a[1][0]= c1*s3+c2*c3*s1; a[1][1]= c1*c3-c2*s1*s3; @@ -98,8 +183,9 @@ if(tolower(type[0])=='z' && tolower(type[1])=='y' && tolower(type[2])=='z') a[2][1]= s2*s3; a[2][2]= c2; } + break; -if(tolower(type[0])=='z' && tolower(type[1])=='x' && tolower(type[2])=='z') +case Euler_case('z','x','z'): { a[0][0]= c1*c3-c2*s1*s3; a[0][1]= -c1*s3-c2*c3*s1; @@ -111,10 +197,9 @@ if(tolower(type[0])=='z' && tolower(type[1])=='x' && tolower(type[2])=='z') a[2][1]= c3*s2; a[2][2]= c2; } + break; -//improper angles Tait-Bryan - -if(tolower(type[0])=='x' && tolower(type[1])=='z' && tolower(type[2])=='y') +case Euler_case('x','z','y'): { a[0][0]= c2*c3; a[0][1]= -s2; @@ -126,8 +211,9 @@ if(tolower(type[0])=='x' && tolower(type[1])=='z' && tolower(type[2])=='y') a[2][1]= c2*s1; a[2][2]= c1*c3+s1*s2*s3; } + break; -if(tolower(type[0])=='x' && tolower(type[1])=='y' && tolower(type[2])=='z') +case Euler_case('x','y','z'): { a[0][0]= c2*c3; a[0][1]= -c2*s3; @@ -139,8 +225,9 @@ if(tolower(type[0])=='x' && tolower(type[1])=='y' && tolower(type[2])=='z') a[2][1]= c3*s1+c1*s2*s3; a[2][2]= c1*c2; } + break; -if(tolower(type[0])=='y' && tolower(type[1])=='x' && tolower(type[2])=='z') +case Euler_case('y','x','z'): { a[0][0]= c1*c3+s1*s2*s3; a[0][1]= c3*s1*s2-c1*s3; @@ -152,8 +239,9 @@ if(tolower(type[0])=='y' && tolower(type[1])=='x' && tolower(type[2])=='z') a[2][1]= c1*c3*s2+s1*s3; a[2][2]= c1*c2; } + break; -if(tolower(type[0])=='y' && tolower(type[1])=='z' && tolower(type[2])=='x') +case Euler_case('y','z','x'): { a[0][0]= c1*c2; a[0][1]= s1*s3-c1*c3*s2; @@ -165,8 +253,9 @@ if(tolower(type[0])=='y' && tolower(type[1])=='z' && tolower(type[2])=='x') a[2][1]= c1*s3+c3*s1*s2; a[2][2]= c1*c3-s1*s2*s3; } + break; -if(tolower(type[0])=='z' && tolower(type[1])=='y' && tolower(type[2])=='x') +case Euler_case('z','y','x'): { a[0][0]= c1*c2; a[0][1]= c1*s2*s3-c3*s1; @@ -178,8 +267,9 @@ if(tolower(type[0])=='z' && tolower(type[1])=='y' && tolower(type[2])=='x') a[2][1]= c2*s3; a[2][2]= c2*c3; } + break; -if(tolower(type[0])=='z' && tolower(type[1])=='x' && tolower(type[2])=='y') +case Euler_case('z','x','y'): { a[0][0]= c1*c3-s1*s2*s3; a[0][1]= -c2*s1; @@ -191,7 +281,213 @@ if(tolower(type[0])=='z' && tolower(type[1])=='x' && tolower(type[2])=='y') a[2][1]= s2; a[2][2]= c2*c3; } + break; +}//switch if(transpose) a.transposeme(); } + +template +void LA_Vecmat3::rotmat2euler(T *eul, const Mat3 &a, const char *type, bool transpose, bool direction, bool reverse) +{ +T m11=a[0][0]; +T m22=a[1][1]; +T m33=a[2][2]; +T m12=transpose?a[1][0]:a[0][1]; +T m21=transpose?a[0][1]:a[1][0]; +T m13=transpose?a[2][0]:a[0][2]; +T m31=transpose?a[0][2]:a[2][0]; +T m23=transpose?a[2][1]:a[1][2]; +T m32=transpose?a[1][2]:a[2][1]; + +switch(Euler_case(type[0],type[1],type[2])) +{ + +case Euler_case('x','z','x'): + { + eul[0]=atan2(m31,m21); + eul[1]=atan2(sqrt(1-m11*m11),m11); + eul[2]=atan2(m13,-m12); + } + break; + +case Euler_case('x','y','x'): + { + eul[0]=atan2(m21,-m31); + eul[1]=atan2(sqrt(1-m11*m11),m11); + eul[2]=atan2(m12,m13); + } + break; + +case Euler_case('y','x','y'): + { + eul[0]=atan2(m12,m32); + eul[1]=atan2(sqrt(1-m22*m22),m22); + eul[2]=atan2(m21,-m23); + } + break; + +case Euler_case('y','z','y'): + { + eul[0]=atan2(m32,-m12); + eul[1]=atan2(sqrt(1-m22*m22),m22); + eul[2]=atan2(m23,m21); + } + break; + +case Euler_case('z','y','z'): + { + eul[0]=atan2(m23,m13); + eul[1]=atan2(sqrt(1-m33*m33),m33); + eul[2]=atan2(m32,-m31); + } + break; + +case Euler_case('z','x','z'): + { + eul[0]=atan2(m13,-m23); + eul[1]=atan2(sqrt(1-m33*m33),m33); + eul[2]=atan2(m31,m32); + } + break; + +case Euler_case('x','z','y'): + { + eul[0]=atan2(m32,m22); + eul[1]=atan2(-m12,sqrt(1-m12*m12)); + eul[2]=atan2(m13,m11); + } + break; + +case Euler_case('x','y','z'): + { + eul[0]=atan2(-m23,m33); + eul[1]=atan2(m13,sqrt(1-m13*m13)); + eul[2]=atan2(-m12,m11); + } + break; + +case Euler_case('y','x','z'): + { + eul[0]=atan2(m31,m33); + eul[1]=atan2(-m23,sqrt(1-m23*m23)); + eul[2]=atan2(m21,m22); + } + break; + +case Euler_case('y','z','x'): + { + eul[0]=atan2(-m31,m11); + eul[1]=atan2(m21,sqrt(1-m21*m21)); + eul[2]=atan2(-m23,m22); + } + break; + +case Euler_case('z','y','x'): + { + eul[0]=atan2(m21,m11); + eul[1]=atan2(-m31,sqrt(1-m31*m31)); + eul[2]=atan2(m32,m33); + } + break; + +case Euler_case('z','x','y'): + { + eul[0]=atan2(-m12,m22); + eul[1]=atan2(m32,sqrt(1-m32*m32)); + eul[2]=atan2(-m31,m33); + } + break; + +}//switch + +if(reverse) + { + T t=eul[0]; eul[0]=eul[2]; eul[2]=t; + } +if(direction) + { + eul[0] *= (T)-1; + eul[1] *= (T)-1; + eul[2] *= (T)-1; + } +} + + +//stream I/O +#ifndef AVOID_STDSTREAM +template +std::istream& LA_Vecmat3::operator>>(std::istream &s, Vec3 &x) +{ +s >> x.q[0]; +s >> x.q[1]; +s >> x.q[2]; +return s; +} + +template +std::ostream& LA_Vecmat3::operator<<(std::ostream &s, const Vec3 &x) { +s << x.q[0]<<" "; +s << x.q[1]<<" "; +s << x.q[2]; +return s; +} + +template +std::istream& LA_Vecmat3::operator>>(std::istream &s, Mat3 &x) +{ +s >> x.q[0][0]; +s >> x.q[0][1]; +s >> x.q[0][2]; +s >> x.q[1][0]; +s >> x.q[1][1]; +s >> x.q[1][2]; +s >> x.q[2][0]; +s >> x.q[2][1]; +s >> x.q[2][2]; +return s; +} + +template +std::ostream& LA_Vecmat3::operator<<(std::ostream &s, const Mat3 &x) { +s << x.q[0][0]<<" "<< x.q[0][1]<<" " << x.q[0][2]<; \ +template class Mat3; \ +template void LA_Vecmat3::euler2rotmat(const T *eul, Mat3 &a, const char *type, bool transpose=0, bool direction=0, bool reverse=0); \ +template void LA_Vecmat3::rotmat2euler(T *eul, const Mat3 &a, const char *type, bool transpose=0, bool direction=0, bool reverse=0); \ + + +#ifndef AVOID_STDSTREAM +#define INSTANTIZE2(T) \ +template std::istream& LA_Vecmat3::operator>>(std::istream &s, Vec3 &x); \ +template std::ostream& LA_Vecmat3::operator<<(std::ostream &s, const Vec3 &x); \ +template std::istream& LA_Vecmat3::operator>>(std::istream &s, Mat3 &x); \ +template std::ostream& LA_Vecmat3::operator<<(std::ostream &s, const Mat3 &x); \ + +#endif + + + +INSTANTIZE(float) +#ifndef QUAT_NO_DOUBLE +INSTANTIZE(double) +#endif + +#ifndef AVOID_STDSTREAM +INSTANTIZE2(float) +#ifndef QUAT_NO_DOUBLE +INSTANTIZE2(double) +#endif +#endif diff --git a/vecmat3.h b/vecmat3.h index 3fa3c01..3c85de2 100644 --- a/vecmat3.h +++ b/vecmat3.h @@ -76,14 +76,7 @@ public: T normsqr(void) const {return dot(*this);}; T norm(void) const {return sqrt(normsqr());}; Vec3& normalize(void) {*this /= norm(); return *this;}; - const Vec3 operator*(const Mat3 &rhs) const - { - Vec3 r; - r[0] = q[0]*rhs.q[0][0] + q[1]*rhs.q[1][0] + q[2]*rhs.q[2][0]; - r[1] = q[0]*rhs.q[0][1] + q[1]*rhs.q[1][1] + q[2]*rhs.q[2][1]; - r[2] = q[0]*rhs.q[0][2] + q[1]*rhs.q[1][2] + q[2]*rhs.q[2][2]; - return r; - }; //matrix times vector + const Vec3 operator*(const Mat3 &rhs) const; //C-style IO void fprintf(FILE *f, const char *format) const {::fprintf(f,format,q[0],q[1],q[2]);}; void sprintf(char *f, const char *format) const {::sprintf(f,format,q[0],q[1],q[2]);}; @@ -103,10 +96,12 @@ public: Mat3(void) {}; Mat3(const T x) {memset(q,0,9*sizeof(T)); q[0][0]=q[1][1]=q[2][2]=x;}; //scalar matrix Mat3(const T* x) {memcpy(q,x,9*sizeof(T));} + Mat3(const T x00, const T x01,const T x02,const T x10,const T x11,const T x12,const T x20,const T x21,const T x22) + {q[0][0]=x00; q[0][1]=x01; q[0][2]=x02; q[1][0]=x10; q[1][1]=x11; q[1][2]=x12; q[2][0]=x20; q[2][1]=x21; q[2][2]=x22;}; //get pointer to data transparently - inline operator const T*() const {return q;}; - inline operator T*() {return q;}; + inline operator const T*() const {return &q[0][0];}; + inline operator T*() {return &q[0][0];}; //compiler generates default copy constructor and assignment operator @@ -129,50 +124,17 @@ public: //Mat3 algebra const Mat3 operator-() const {return *this * (T)-1;}; //unary minus - Mat3& operator+=(const Mat3 &rhs) {q[0][0]+=rhs.q[0][0];q[0][1]+=rhs.q[0][1];q[0][2]+=rhs.q[0][2]; q[1][0]+=rhs.q[1][0];q[1][1]+=rhs.q[1][1];q[1][2]+=rhs.q[1][2]; q[2][0]+=rhs.q[2][0];q[2][1]+=rhs.q[2][1];q[2][2]+=rhs.q[2][2]; return *this;}; - Mat3& operator-=(const Mat3 &rhs) {q[0][0]-=rhs.q[0][0];q[0][1]-=rhs.q[0][1];q[0][2]-=rhs.q[0][2]; q[1][0]-=rhs.q[1][0];q[1][1]-=rhs.q[1][1];q[1][2]-=rhs.q[1][2]; q[2][0]-=rhs.q[2][0];q[2][1]-=rhs.q[2][1];q[2][2]-=rhs.q[2][2]; return *this;}; + Mat3& operator+=(const Mat3 &rhs); + Mat3& operator-=(const Mat3 &rhs); const Mat3 operator+(const Mat3 &rhs) const {return Mat3(*this) += rhs;}; const Mat3 operator-(const Mat3 &rhs) const {return Mat3(*this) -= rhs;}; - const Mat3 operator*(const Mat3 &rhs) const - { - Mat3 r; - r[0][0]= q[0][0]*rhs.q[0][0] + q[0][1]*rhs.q[1][0] + q[0][2]*rhs.q[2][0]; - r[0][1]= q[0][0]*rhs.q[0][1] + q[0][1]*rhs.q[1][1] + q[0][2]*rhs.q[2][1]; - r[0][2]= q[0][0]*rhs.q[0][2] + q[0][1]*rhs.q[1][2] + q[0][2]*rhs.q[2][2]; - r[1][0]= q[1][0]*rhs.q[0][0] + q[1][1]*rhs.q[1][0] + q[1][2]*rhs.q[2][0]; - r[1][1]= q[1][0]*rhs.q[0][1] + q[1][1]*rhs.q[1][1] + q[1][2]*rhs.q[2][1]; - r[1][2]= q[1][0]*rhs.q[0][2] + q[1][1]*rhs.q[1][2] + q[1][2]*rhs.q[2][2]; - r[2][0]= q[2][0]*rhs.q[0][0] + q[2][1]*rhs.q[1][0] + q[2][2]*rhs.q[2][0]; - r[2][1]= q[2][0]*rhs.q[0][1] + q[2][1]*rhs.q[1][1] + q[2][2]*rhs.q[2][1]; - r[2][2]= q[2][0]*rhs.q[0][2] + q[2][1]*rhs.q[1][2] + q[2][2]*rhs.q[2][2]; - return r; - }; //matrix product - const Vec3 operator*(const Vec3 &rhs) const - { - Vec3 r; - r[0] = q[0][0]*rhs.q[0] + q[0][1]*rhs.q[1] + q[0][2]*rhs.q[2]; - r[1] = q[1][0]*rhs.q[0] + q[1][1]*rhs.q[1] + q[1][2]*rhs.q[2]; - r[2] = q[2][0]*rhs.q[0] + q[2][1]*rhs.q[1] + q[2][2]*rhs.q[2]; - return r; - }; //matrix times vector + const Mat3 operator*(const Mat3 &rhs) const; //matrix product + const Vec3 operator*(const Vec3 &rhs) const; //matrix times vector T trace() const {return q[0][0]+q[1][1]+q[2][2];}; - T determinant() const {return q[0][0]*(q[2][2]*q[1][1]-q[2][1]*q[1][2])-q[1][0]*(q[2][2]*q[0][1]-q[2][1]*q[0][2])+q[2][0]*(q[1][2]*q[0][1]-q[1][1]*q[0][2]); };//determinant - void transposeme() {T t; t=q[0][1]; q[0][1]=q[1][0]; q[1][0]=t; t=q[0][2]; q[0][2]=q[2][0]; q[2][0]=t; t=q[1][2]; q[1][2]=q[2][1]; q[2][1]=t;}; + T determinant() const; + void transposeme(); const Mat3 transpose() const {Mat3 r(*this); r.transposeme(); return r;}; - const Mat3 inverse() const - { - Mat3 r; - r[0][0]= q[2][2]*q[1][1]-q[2][1]*q[1][2]; - r[0][1]= -q[2][2]*q[0][1]+q[2][1]*q[0][2]; - r[0][2]= q[1][2]*q[0][1]-q[1][1]*q[0][2]; - r[1][0]= -q[2][2]*q[1][0]+q[2][0]*q[1][1]; - r[1][1]= q[2][2]*q[0][0]-q[2][0]*q[0][2]; - r[1][2]= -q[1][2]*q[0][0]+q[1][0]*q[0][2]; - r[2][0]= q[2][1]*q[1][0]-q[2][0]*q[1][1]; - r[2][1]= -q[2][1]*q[0][0]+q[2][0]*q[0][1]; - r[2][2]= q[1][1]*q[0][0]-q[1][0]*q[0][1]; - return r/determinant(); - }; + const Mat3 inverse() const; //C-style IO void fprintf(FILE *f, const char *format) const {::fprintf(f,format,q[0][0],q[0][1],q[0][2]); ::fprintf(f,format,q[1][0],q[1][1],q[1][2]); ::fprintf(f,format,q[2][0],q[2][1],q[2][2]);}; int fscanf(FILE *f, const char *format) const {return ::fscanf(f,format,q[0][0],q[0][1],q[0][2]) + ::fscanf(f,format,q[1][0],q[1][1],q[1][2]) + ::fscanf(f,format,q[2][0],q[2][1],q[2][2]);}; @@ -184,46 +146,29 @@ public: //stream I/O #ifndef AVOID_STDSTREAM template -std::istream& operator>>(std::istream &s, Vec3 &x) -{ -s >> x.q[0]; -s >> x.q[1]; -s >> x.q[2]; -return s; -} +std::istream& operator>>(std::istream &s, Vec3 &x); template -std::ostream& operator<<(std::ostream &s, const Vec3 &x) { -s << x.q[0]<<" "; -s << x.q[1]<<" "; -s << x.q[2]; -return s; -} +std::ostream& operator<<(std::ostream &s, const Vec3 &x); template -std::istream& operator>>(std::istream &s, Mat3 &x) -{ -s >> x.q[0][0]; -s >> x.q[0][1]; -s >> x.q[0][2]; -s >> x.q[1][0]; -s >> x.q[1][1]; -s >> x.q[1][2]; -s >> x.q[2][0]; -s >> x.q[2][1]; -s >> x.q[2][2]; -return s; -} +std::istream& operator>>(std::istream &s, Mat3 &x); template -std::ostream& operator<<(std::ostream &s, const Mat3 &x) { -s << x.q[0][0]<<" "<< x.q[0][1]<<" " << x.q[0][2]< &x); #endif +//euler angles to rotation matrices cf. https://en.wikipedia.org/wiki/Euler_angles and NASA paper cited therein + +#define Euler_case(a,b,c) (((a)-'x')*9+((b)-'x')*3+((c)-'x')) + +template +void euler2rotmat(const T *eul, Mat3 &a, const char *type, bool transpose=0, bool direction=0, bool reverse=0); + +template +void rotmat2euler(T *eul, const Mat3 &a, const char *type, bool transpose=0, bool direction=0, bool reverse=0); + + }//namespace #endif /* _VECMAT3_H_ */