jiri
/
LA_library
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

*** empty log message ***

This commit is contained in:
jiri 2020-01-06 20:50:34 +00:00
parent 086c2202be
commit ef02c16f28
7 changed files with 786 additions and 315 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

252
nonclass.cc
View File

@ -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<double>)
INSTANTIZE(std::complex<double>)
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<double>)a[ii*(ii+1)/2+jj]).real(), ((complex<double>)a[ii*(ii+1)/2+jj]).imag());
} else fprintf(file, f, ((complex<double>)a[(i-1)*c+j-1]).real(), ((complex<double>)a[(i-1)*c+j-1]).imag());
fprintf(file, f, ((std::complex<double>)a[ii*(ii+1)/2+jj]).real(), ((std::complex<double>)a[ii*(ii+1)/2+jj]).imag());
} else fprintf(file, f, ((std::complex<double>)a[(i-1)*c+j-1]).real(), ((std::complex<double>)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<double>)a[ii*(ii+1)/2+jj]).real(), ((complex<double>)a[ii*(ii+1)/2+jj]).imag());
} else fprintf(file,f,((complex<double>)a[(i-1)*c+j-1]).real(), ((complex<double>)a[(i-1)*c+j-1]).imag());
fprintf(file, f, ((std::complex<double>)a[ii*(ii+1)/2+jj]).real(), ((std::complex<double>)a[ii*(ii+1)/2+jj]).imag());
} else fprintf(file,f,((std::complex<double>)a[(i-1)*c+j-1]).real(), ((std::complex<double>)a[(i-1)*c+j-1]).imag());
putc(j<c?' ':'\n',file);
}
}
@ -246,7 +246,7 @@ linear_solve_do(a,&B[0],1,a.nrows(),det,n);
extern "C" void FORNAME(zgesv)(const int *N, const int *NRHS, double *A, const int *LDA,
int *IPIV, double *B, const int *LDB, int *INFO);
void linear_solve(NRMat< complex<double> > &A, NRMat< complex<double> > *B, complex<double> *det, int n)
void linear_solve(NRMat< std::complex<double> > &A, NRMat< std::complex<double> > *B, std::complex<double> *det, int n)
{
int r, *ipiv;
@ -280,7 +280,7 @@ void linear_solve(NRMat< complex<double> > &A, NRMat< complex<double> > *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<double> *A, const FINT *lda, complex<double> *AF, const FINT *ldaf, const FINT *ipiv, char *equed, double *R,double *C, complex<double> *B, const FINT *ldb, complex<double> *X, const FINT *ldx, double *rcond, double *ferr, double *berr, complex<double> *work, double *rwork, FINT *info);
extern "C" void FORNAME(zgesvx)(const char *fact, const char *trans, const FINT *n, const FINT *nrhs, std::complex<double> *A, const FINT *lda, std::complex<double> *AF, const FINT *ldaf, const FINT *ipiv, char *equed, double *R,double *C, std::complex<double> *B, const FINT *ldb, std::complex<double> *X, const FINT *ldx, double *rcond, double *ferr, double *berr, std::complex<double> *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<double> &_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<complex<double> > &_A, complex<double> *_B, const int _rhsCount, const int _eqCount, const bool _eq, const bool _saveA, double *_rcond){
int linear_solve_x(NRMat<std::complex<double> > &_A, std::complex<double> *_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<complex<double> > &_A, complex<double> *_B, const int _
laerror("linear_solve_x: invalid input matrices");
}
complex<double> *A;
complex<double> * const _A_data = (complex<double>*)_A;
std::complex<double> *A;
std::complex<double> * const _A_data = (std::complex<double>*)_A;
FINT info;
const FINT nrhs = _rhsCount;
@ -393,17 +393,17 @@ int linear_solve_x(NRMat<complex<double> > &_A, complex<double> *_B, const int _
double rcond;
double ferr[nrhs], berr[nrhs];
double R[n], C[n], rwork[2*n];
complex<double> work[2*n];
std::complex<double> work[2*n];
FINT *const ipiv = new FINT[n];
complex<double> *X = new complex<double>[n*nrhs];
complex<double> *AF = new complex<double>[ldaf*n];
std::complex<double> *X = new std::complex<double>[n*nrhs];
std::complex<double> *AF = new std::complex<double>[ldaf*n];
A = _A_data;
if(_eq){
if(_saveA){//store the corresponding submatrix of _A (not needed provided fact=='N')
A = new complex<double>[n*n];
A = new std::complex<double>[n*n];
int offset1 = 0;int offset2 = 0;
for(register int i=0;i<n;i++){
cblas_zcopy(n, _A_data + offset1, 1, A + offset2, 1);
@ -441,7 +441,8 @@ int linear_solve_x(NRMat<complex<double> > &_A, complex<double> *_B, const int _
// solution is stored in _B
// the info parameter of dgesvx is returned (see man dgesvx)
//------------------------------------------------------------------------------
int multiply_by_inverse(NRMat<double> &_A, NRMat<double> &_B, bool _useEq, double *_rcond){
template<>
int multiply_by_inverse<double>(NRMat<double> &_A, NRMat<double> &_B, bool _useEq, double *_rcond){
const FINT n = _A.nrows();
const FINT m = _A.ncols();
@ -492,7 +493,8 @@ int multiply_by_inverse(NRMat<double> &_A, NRMat<double> &_B, bool _useEq, doubl
// solution is stored in _B
// the info parameter of zgesvx is returned (see man zgesvx)
//------------------------------------------------------------------------------
int multiply_by_inverse(NRMat<complex<double> > &_A, NRMat<complex<double> > &_B, bool _useEq, double *_rcond){
template<>
int multiply_by_inverse<std::complex<double> >(NRMat<std::complex<double> > &_A, NRMat<std::complex<double> > &_B, bool _useEq, double *_rcond){
const FINT n = _A.nrows();
const FINT m = _A.ncols();
@ -505,20 +507,20 @@ int multiply_by_inverse(NRMat<complex<double> > &_A, NRMat<complex<double> > &_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<double> * const A = (complex<double>*)_A;
complex<double> * const B = (complex<double>*)_B;
std::complex<double> * const A = (std::complex<double>*)_A;
std::complex<double> * const B = (std::complex<double>*)_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<double> work[2*n];
std::complex<double> work[2*n];
FINT *const ipiv = new FINT[n];
complex<double> *X = new complex<double>[n2];
complex<double> *AF = new complex<double>[n2];
std::complex<double> *X = new std::complex<double>[n2];
std::complex<double> *AF = new std::complex<double>[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<double> &a, NRVec<double> &w, const bool eivec,
extern "C" void FORNAME(zheev)(const char *JOBZ, const char *UPLO, const FINT *N,
complex<double> *A, const FINT *LDA, double *W, complex<double> *WORK, const FINT *LWORK, double *RWORK, FINT *INFO);
std::complex<double> *A, const FINT *LDA, double *W, std::complex<double> *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<double> *A, const FINT *LDA, complex<double> *B, const FINT *LDB, double *W, complex<double> *WORK, const FINT *LWORK, double *RWORK, FINT *INFO);
std::complex<double> *A, const FINT *LDA, std::complex<double> *B, const FINT *LDB, double *W, std::complex<double> *WORK, const FINT *LWORK, double *RWORK, FINT *INFO);
// a will contain eigenvectors (columns if corder==1), w eigenvalues
void diagonalize(NRMat<complex<double> > &a, NRVec<double> &w, const bool eivec,
const bool corder, int n, NRMat<complex<double> > *b, const int itype)
void diagonalize(NRMat<std::complex<double> > &a, NRVec<double> &w, const bool eivec,
const bool corder, int n, NRMat<std::complex<double> > *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<complex<double> > &a, NRVec<double> &w, const bool eivec,
char vectors = LAPACK_FORTRANCASE('V');
if (!eivec) vectors = LAPACK_FORTRANCASE('n');
FINT LWORK = -1;
complex<double> WORKX;
std::complex<double> WORKX;
FINT ldb=0; if(b) ldb=b->ncols();
std::cout << "test vectors "<<vectors<<std::endl;
@ -643,7 +645,7 @@ std::cout << "test vectors "<<vectors<<std::endl;
#endif
LWORK = (FINT)WORKX.real();
complex<double> *WORK = new complex<double>[LWORK];
std::complex<double> *WORK = new std::complex<double>[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<double> &a, NRVec<double> &w, NRMat<double> *v,
extern "C" void FORNAME(zhpev)(const char *JOBZ, const char *UPLO, const FINT *N,
complex<double> *AP, double *W, complex<double> *Z, const FINT *LDZ, complex<double> *WORK, double *RWORK, FINT *INFO);
std::complex<double> *AP, double *W, std::complex<double> *Z, const FINT *LDZ, std::complex<double> *WORK, double *RWORK, FINT *INFO);
extern "C" void FORNAME(zhpgv)(const FINT *ITYPE, const char *JOBZ, const char *UPLO, const FINT *N,
complex<double> *AP, complex<double> *BP, double *W, complex<double> *Z, const FINT *LDZ, complex<double> *WORK, double *RWORK, FINT *INFO);
std::complex<double> *AP, std::complex<double> *BP, double *W, std::complex<double> *Z, const FINT *LDZ, std::complex<double> *WORK, double *RWORK, FINT *INFO);
// v will contain eigenvectors, w eigenvalues
void diagonalize(NRSMat<complex<double> > &a, NRVec<double> &w, NRMat<complex<double> > *v,
const bool corder, int n, NRSMat<complex<double> > *b, const int itype)
void diagonalize(NRSMat<std::complex<double> > &a, NRVec<double> &w, NRMat<std::complex<double> > *v,
const bool corder, int n, NRSMat<std::complex<double> > *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<complex<double> > &a, NRVec<double> &w, NRMat<complex<do
char U = LAPACK_FORTRANCASE('u');
char job = LAPACK_FORTRANCASE(v ? 'v' : 'n');
complex<double> *WORK = new complex<double>[2*n];
std::complex<double> *WORK = new std::complex<double>[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<double> *)0, &ldv, WORK, RWORK, &r );
else FORNAME(zhpev)(&job, &U, &ntmp, a, w, v?(*v)[0]:(complex<double> *)0, &ldv, WORK, RWORK, &r );
if(b) FORNAME(zhpgv)(&itypetmp,&job, &U, &ntmp, a, *b, w, v?(*v)[0]:(std::complex<double> *)0, &ldv, WORK, RWORK, &r );
else FORNAME(zhpev)(&job, &U, &ntmp, a, w, v?(*v)[0]:(std::complex<double> *)0, &ldv, WORK, RWORK, &r );
#else
if(b) FORNAME(zhpgv)(&itype,&job, &U, &n, a, *b, w, v?(*v)[0]:(complex<double> *)0, &ldv, WORK, RWORK, &r );
else FORNAME(zhpev)(&job, &U, &n, a, w, v?(*v)[0]:(complex<double> *)0, &ldv, WORK, RWORK, &r );
if(b) FORNAME(zhpgv)(&itype,&job, &U, &n, a, *b, w, v?(*v)[0]:(std::complex<double> *)0, &ldv, WORK, RWORK, &r );
else FORNAME(zhpev)(&job, &U, &n, a, w, v?(*v)[0]:(std::complex<double> *)0, &ldv, WORK, RWORK, &r );
#endif
delete[] WORK;
delete[] RWORK;
@ -824,11 +826,11 @@ void singular_decomposition(NRMat<double> &a, NRMat<double> *u, NRVec<double> &s
extern "C" void FORNAME(zgesvd)(const char *JOBU, const char *JOBVT, const FINT *M,
const FINT *N, complex<double> *A, const FINT *LDA, double *S, complex<double> *U, const FINT *LDU,
complex<double> *VT, const FINT *LDVT, complex<double> *WORK, const FINT *LWORK, double *RWORK, FINT *INFO );
const FINT *N, std::complex<double> *A, const FINT *LDA, double *S, std::complex<double> *U, const FINT *LDU,
std::complex<double> *VT, const FINT *LDVT, std::complex<double> *WORK, const FINT *LWORK, double *RWORK, FINT *INFO );
void singular_decomposition(NRMat<complex<double> > &a, NRMat<complex<double> > *u, NRVec<double> &s,
NRMat<complex<double> > *v, const bool vnotdagger, int m, int n)
void singular_decomposition(NRMat<std::complex<double> > &a, NRMat<std::complex<double> > *u, NRVec<double> &s,
NRMat<std::complex<double> > *v, const bool vnotdagger, int m, int n)
{
FINT m0 = a.nrows();
FINT n0 = a.ncols();
@ -852,7 +854,7 @@ void singular_decomposition(NRMat<complex<double> > &a, NRMat<complex<double> >
// v should be transposed at the end
char jobu = u ? 'A' : 'N';
char jobv = v ? 'A' : 'N';
complex<double> work0;
std::complex<double> work0;
FINT lwork = -1;
FINT r;
double *rwork = new double[5*nmin];
@ -868,7 +870,7 @@ void singular_decomposition(NRMat<complex<double> > &a, NRMat<complex<double> >
#endif
lwork = (FINT) work0.real();
complex<double> *work = new complex<double>[lwork];
std::complex<double> *work = new std::complex<double>[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<double> *A, const FINT *LDA, complex<double> *W, complex<double> *VL, const FINT *LDVL,
complex<double> *VR, const FINT *LDVR, complex<double> *WORK, const FINT *LWORK,
std::complex<double> *A, const FINT *LDA, std::complex<double> *W, std::complex<double> *VL, const FINT *LDVL,
std::complex<double> *VR, const FINT *LDVR, std::complex<double> *WORK, const FINT *LWORK,
double *RWORK, FINT *INFO );
extern "C" void FORNAME(zggev)(const char *JOBVL, const char *JOBVR, const FINT *N,
complex<double> *A, const FINT *LDA, complex<double> *B, const FINT *LDB, complex<double> *W, complex<double> *WBETA,
complex<double> *VL, const FINT *LDVL, complex<double> *VR, const FINT *LDVR,
complex<double> *WORK, const FINT *LWORK, double *RWORK, FINT *INFO );
std::complex<double> *A, const FINT *LDA, std::complex<double> *B, const FINT *LDB, std::complex<double> *W, std::complex<double> *WBETA,
std::complex<double> *VL, const FINT *LDVL, std::complex<double> *VR, const FINT *LDVR,
std::complex<double> *WORK, const FINT *LWORK, double *RWORK, FINT *INFO );
@ -1124,10 +1126,10 @@ void gdiagonalize(NRMat<double> &a, NRVec<double> &wr, NRVec<double> &wi,
//most general complex routine
template<>
void gdiagonalize(NRMat<complex<double> > &a, NRVec< complex<double> > &w,
NRMat< complex<double> >*vl, NRMat< complex<double> > *vr,
void gdiagonalize(NRMat<std::complex<double> > &a, NRVec< std::complex<double> > &w,
NRMat< std::complex<double> >*vl, NRMat< std::complex<double> > *vr,
const bool corder, int n, const int sorttype, const int biorthonormalize,
NRMat<complex<double> > *b, NRVec<complex<double> > *beta)
NRMat<std::complex<double> > *b, NRVec<std::complex<double> > *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<complex<double> > &a, NRVec< complex<double> > &w,
char jobvl = LAPACK_FORTRANCASE(vl ? 'v' : 'n');
char jobvr = LAPACK_FORTRANCASE(vr ? 'v' : 'n');
complex<double> work0;
std::complex<double> work0;
FINT lwork = -1;
FINT r;
FINT lda=a.ncols();
@ -1165,30 +1167,30 @@ void gdiagonalize(NRMat<complex<double> > &a, NRVec< complex<double> > &w,
#ifdef FORINT
FINT ntmp = n;
if(b) FORNAME(zggev)(&jobvr, &jobvl, &ntmp, a, &lda, *b, &ldb, w, *beta, vr?vr[0]:(complex<double> *)0,
&ldvr, vl?vl[0]:(complex<double> *)0, &ldvl, &work0, &lwork, rwork, &r);
else FORNAME(zgeev)(&jobvr, &jobvl, &ntmp, a, &lda, w, vr?vr[0]:(complex<double> *)0,
&ldvr, vl?vl[0]:(complex<double> *)0, &ldvl, &work0, &lwork, rwork, &r);
if(b) FORNAME(zggev)(&jobvr, &jobvl, &ntmp, a, &lda, *b, &ldb, w, *beta, vr?vr[0]:(std::complex<double> *)0,
&ldvr, vl?vl[0]:(std::complex<double> *)0, &ldvl, &work0, &lwork, rwork, &r);
else FORNAME(zgeev)(&jobvr, &jobvl, &ntmp, a, &lda, w, vr?vr[0]:(std::complex<double> *)0,
&ldvr, vl?vl[0]:(std::complex<double> *)0, &ldvl, &work0, &lwork, rwork, &r);
#else
if(b) FORNAME(zggev)(&jobvr, &jobvl, &n, a, &lda, *b, &ldb, w, *beta, vr?vr[0]:(complex<double> *)0,
&ldvr, vl?vl[0]:(complex<double> *)0, &ldvl, &work0, &lwork, rwork, &r);
else FORNAME(zgeev)(&jobvr, &jobvl, &n, a, &lda, w, vr?vr[0]:(complex<double> *)0,
&ldvr, vl?vl[0]:(complex<double> *)0, &ldvl, &work0, &lwork, rwork, &r);
if(b) FORNAME(zggev)(&jobvr, &jobvl, &n, a, &lda, *b, &ldb, w, *beta, vr?vr[0]:(std::complex<double> *)0,
&ldvr, vl?vl[0]:(std::complex<double> *)0, &ldvl, &work0, &lwork, rwork, &r);
else FORNAME(zgeev)(&jobvr, &jobvl, &n, a, &lda, w, vr?vr[0]:(std::complex<double> *)0,
&ldvr, vl?vl[0]:(std::complex<double> *)0, &ldvl, &work0, &lwork, rwork, &r);
#endif
lwork = (FINT) work0.real();
complex<double> *work = new complex<double>[lwork];
std::complex<double> *work = new std::complex<double>[lwork];
#ifdef FORINT
if(b) FORNAME(zggev)(&jobvr, &jobvl, &ntmp, a, &lda, *b, &ldb, w, *beta, vr?vr[0]:(complex<double> *)0,
&ldvr, vl?vl[0]:(complex<double> *)0, &ldvl, work, &lwork, rwork, &r);
else FORNAME(zgeev)(&jobvr, &jobvl, &ntmp, a, &lda, w, vr?vr[0]:(complex<double> *)0,
&ldvr, vl?vl[0]:(complex<double> *)0, &ldvl, work, &lwork, rwork, &r);
if(b) FORNAME(zggev)(&jobvr, &jobvl, &ntmp, a, &lda, *b, &ldb, w, *beta, vr?vr[0]:(std::complex<double> *)0,
&ldvr, vl?vl[0]:(std::complex<double> *)0, &ldvl, work, &lwork, rwork, &r);
else FORNAME(zgeev)(&jobvr, &jobvl, &ntmp, a, &lda, w, vr?vr[0]:(std::complex<double> *)0,
&ldvr, vl?vl[0]:(std::complex<double> *)0, &ldvl, work, &lwork, rwork, &r);
#else
if(b) FORNAME(zggev)(&jobvr, &jobvl, &n, a, &lda, *b, &ldb, w, *beta, vr?vr[0]:(complex<double> *)0,
&ldvr, vl?vl[0]:(complex<double> *)0, &ldvl, work, &lwork, rwork, &r);
else FORNAME(zgeev)(&jobvr, &jobvl, &n, a, &lda, w, vr?vr[0]:(complex<double> *)0,
&ldvr, vl?vl[0]:(complex<double> *)0, &ldvl, work, &lwork, rwork, &r);
if(b) FORNAME(zggev)(&jobvr, &jobvl, &n, a, &lda, *b, &ldb, w, *beta, vr?vr[0]:(std::complex<double> *)0,
&ldvr, vl?vl[0]:(std::complex<double> *)0, &ldvl, work, &lwork, rwork, &r);
else FORNAME(zgeev)(&jobvr, &jobvl, &n, a, &lda, w, vr?vr[0]:(std::complex<double> *)0,
&ldvr, vl?vl[0]:(std::complex<double> *)0, &ldvl, work, &lwork, rwork, &r);
#endif
delete[] work;
@ -1205,7 +1207,7 @@ void gdiagonalize(NRMat<complex<double> > &a, NRVec< complex<double> > &w,
for(int i=0; i<n; ++i)
{
//calculate scaling paramter
complex<double> tmp;
std::complex<double> tmp;
cblas_zdotc_sub(n,(*vr)[i],1,(*vl)[i], 1, &tmp);
tmp = 1./tmp;
std::cout <<"scaling by "<<tmp<<"\n";
@ -1229,8 +1231,8 @@ void gdiagonalize(NRMat<complex<double> > &a, NRVec< complex<double> > &w,
template<>
void gdiagonalize(NRMat<double> &a, NRVec< complex<double> > &w,
NRMat< complex<double> >*vl, NRMat< complex<double> > *vr,
void gdiagonalize(NRMat<double> &a, NRVec< std::complex<double> > &w,
NRMat< std::complex<double> >*vl, NRMat< std::complex<double> > *vr,
const bool corder, int n, const int sorttype, const int biorthonormalize,
NRMat<double> *b, NRVec<double> *beta)
{
@ -1246,7 +1248,7 @@ void gdiagonalize(NRMat<double> &a, NRVec< complex<double> > &w,
//process the results into complex matrices
int i;
for (i=0; i<n; i++) w[i] = complex<double>(wr[i], wi[i]);
for (i=0; i<n; i++) w[i] = std::complex<double>(wr[i], wi[i]);
if (rvl || rvr) {
i = 0;
while (i < n) {
@ -1267,26 +1269,26 @@ void gdiagonalize(NRMat<double> &a, NRVec< complex<double> > &w,
for (int j=0; j<n; j++) {
if(corder)
{
(*vl)[j][i] = complex<double>((*rvl)[i][j], (*rvl)[i+1][j]);
(*vl)[j][i+1] = complex<double>((*rvl)[i][j], -(*rvl)[i+1][j]);
(*vl)[j][i] = std::complex<double>((*rvl)[i][j], (*rvl)[i+1][j]);
(*vl)[j][i+1] = std::complex<double>((*rvl)[i][j], -(*rvl)[i+1][j]);
}
else
{
(*vl)[i][j] = complex<double>((*rvl)[i][j], (*rvl)[i+1][j]);
(*vl)[i+1][j] = complex<double>((*rvl)[i][j], -(*rvl)[i+1][j]);
(*vl)[i][j] = std::complex<double>((*rvl)[i][j], (*rvl)[i+1][j]);
(*vl)[i+1][j] = std::complex<double>((*rvl)[i][j], -(*rvl)[i+1][j]);
}
}
if (vr)
for (int j=0; j<n; j++) {
if(corder)
{
(*vr)[j][i] = complex<double>((*rvr)[i][j], (*rvr)[i+1][j]);
(*vr)[j][i+1] = complex<double>((*rvr)[i][j], -(*rvr)[i+1][j]);
(*vr)[j][i] = std::complex<double>((*rvr)[i][j], (*rvr)[i+1][j]);
(*vr)[j][i+1] = std::complex<double>((*rvr)[i][j], -(*rvr)[i+1][j]);
}
else
{
(*vr)[i][j] = complex<double>((*rvr)[i][j], (*rvr)[i+1][j]);
(*vr)[i+1][j] = complex<double>((*rvr)[i][j], -(*rvr)[i+1][j]);
(*vr)[i][j] = std::complex<double>((*rvr)[i][j], (*rvr)[i+1][j]);
(*vr)[i+1][j] = std::complex<double>((*rvr)[i][j], -(*rvr)[i+1][j]);
}
}
i += 2;
@ -1299,7 +1301,7 @@ void gdiagonalize(NRMat<double> &a, NRVec< complex<double> > &w,
template<>
const NRMat<double> realpart<NRMat< complex<double> > >(const NRMat< complex<double> > &a)
const NRMat<double> realpart<NRMat< std::complex<double> > >(const NRMat< std::complex<double> > &a)
{
NRMat<double> result(a.nrows(), a.ncols());
@ -1317,7 +1319,7 @@ const NRMat<double> realpart<NRMat< complex<double> > >(const NRMat< complex<dou
}
template<>
const NRMat<double> imagpart<NRMat< complex<double> > >(const NRMat< complex<double> > &a)
const NRMat<double> imagpart<NRMat< std::complex<double> > >(const NRMat< std::complex<double> > &a)
{
NRMat<double> result(a.nrows(), a.ncols());
@ -1336,17 +1338,17 @@ const NRMat<double> imagpart<NRMat< complex<double> > >(const NRMat< complex<dou
}
template<>
const NRMat< complex<double> > realmatrix<NRMat<double> > (const NRMat<double> &a)
const NRMat< std::complex<double> > realmatrix<NRMat<double> > (const NRMat<double> &a)
{
NRMat <complex<double> > result(a.nrows(), a.ncols());
NRMat <std::complex<double> > result(a.nrows(), a.ncols());
#ifdef CUDALA
if(a.location == cpu){
#endif
// NRMat <complex<double> > result(a.nrows(), a.ncols());
// NRMat <std::complex<double> > 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<double> > realmatrix<NRMat<double> > (const NRMat<double> &
}
template<>
const NRMat< complex<double> > imagmatrix<NRMat<double> > (const NRMat<double> &a)
const NRMat< std::complex<double> > imagmatrix<NRMat<double> > (const NRMat<double> &a)
{
NRMat< complex<double> > result(a.nrows(), a.ncols());
NRMat< std::complex<double> > result(a.nrows(), a.ncols());
#ifdef CUDALA
if(a.location == cpu){
#endif
// NRMat< complex<double> > result(a.nrows(), a.ncols());
// NRMat< std::complex<double> > 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<double> > imagmatrix<NRMat<double> > (const NRMat<double> &
}
template<>
const NRMat< complex<double> > complexmatrix<NRMat<double> > (const NRMat<double> &re, const NRMat<double> &im)
const NRMat< std::complex<double> > complexmatrix<NRMat<double> > (const NRMat<double> &re, const NRMat<double> &im)
{
if(re.nrows()!=im.nrows() || re.ncols() != im.ncols()) laerror("incompatible sizes of real and imaginary parts");
NRMat< complex<double> > result(re.nrows(), re.ncols());
NRMat< std::complex<double> > 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<double> > complexmatrix<SparseSMat<double> >(const SparseSMat<double> &re, const SparseSMat<double> &im) {
const SparseSMat< std::complex<double> > complexmatrix<SparseSMat<double> >(const SparseSMat<double> &re, const SparseSMat<double> &im) {
if(re.nrows()!=im.nrows() || re.ncols() != im.ncols()) laerror("incompatible sizes of real and imaginary parts");
SparseSMat< complex<double> > result(re.nrows(),re.ncols());
complex<double> tmp;
SparseSMat< std::complex<double> > result(re.nrows(),re.ncols());
std::complex<double> tmp;
SparseSMat<double>::iterator pre(re);
for(; pre.notend(); ++pre) {
@ -1401,7 +1403,7 @@ const SparseSMat< complex<double> > complexmatrix<SparseSMat<double> >(const Spa
SparseSMat<double>::iterator pim(im);
for(; pim.notend(); ++pim) {
tmp = complex<double>(0,1)*(pim->elem);
tmp = std::complex<double>(0,1)*(pim->elem);
result.add(pim->row,pim->col,tmp,false);
}
@ -1409,9 +1411,9 @@ const SparseSMat< complex<double> > complexmatrix<SparseSMat<double> >(const Spa
}
template<>
const SparseSMat< complex<double> > realmatrix<SparseSMat<double> >(const SparseSMat<double> &re) {
SparseSMat< complex<double> > result(re.nrows(),re.ncols());
complex<double> tmp;
const SparseSMat< std::complex<double> > realmatrix<SparseSMat<double> >(const SparseSMat<double> &re) {
SparseSMat< std::complex<double> > result(re.nrows(),re.ncols());
std::complex<double> tmp;
SparseSMat<double>::iterator pre(re);
for(; pre.notend(); ++pre) {
@ -1423,14 +1425,14 @@ const SparseSMat< complex<double> > realmatrix<SparseSMat<double> >(const Sparse
}
template<>
const SparseSMat< complex<double> > imagmatrix<SparseSMat<double> >(const SparseSMat<double> &im) {
SparseSMat< complex<double> > result(im.nrows(),im.ncols());
complex<double> tmp;
const SparseSMat< std::complex<double> > imagmatrix<SparseSMat<double> >(const SparseSMat<double> &im) {
SparseSMat< std::complex<double> > result(im.nrows(),im.ncols());
std::complex<double> tmp;
SparseSMat<double>::iterator pim(im);
for(; pim.notend(); ++pim) {
tmp = complex<double>(0,1)*(pim->elem);
tmp = std::complex<double>(0,1)*(pim->elem);
result.add(pim->row,pim->col,tmp,false);
}
@ -1456,7 +1458,7 @@ NRMat<double> realmatrixfunction(NRMat<double> a, double (*f) (const double))
}
NRMat<complex<double> > complexmatrixfunction(NRMat<double> a, double (*fre) (const double), double (*fim) (const double))
NRMat<std::complex<double> > complexmatrixfunction(NRMat<double> a, double (*fre) (const double), double (*fim) (const double))
{
int n = a.nrows();
NRVec<double> wre(n),wim(n);
@ -1470,8 +1472,8 @@ NRMat<complex<double> > complexmatrixfunction(NRMat<double> a, double (*fre) (co
NRMat<double> 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<complex<double> > r(n, n);
for (int i=0; i<a.nrows(); i++) for(int j=0; j<a.ncols(); ++j) r(i,j)=complex<double>(t(i,j),tt(i,j));
NRMat<std::complex<double> > r(n, n);
for (int i=0; i<a.nrows(); i++) for(int j=0; j<a.ncols(); ++j) r(i,j)=std::complex<double>(t(i,j),tt(i,j));
return r;
}
@ -1480,7 +1482,7 @@ NRMat<complex<double> > complexmatrixfunction(NRMat<double> a, double (*fre) (co
// instantize template to an addresable function
complex<double> myccopy (const complex<double> &x)
std::complex<double> myccopy (const std::complex<double> &x)
{
return x;
}
@ -1490,18 +1492,18 @@ double mycopy (const double x)
return x;
}
complex<double> myclog (const complex<double> &x)
std::complex<double> myclog (const std::complex<double> &x)
{
return log(x);
}
complex<double> mycexp (const complex<double> &x)
std::complex<double> mycexp (const std::complex<double> &x)
{
return std::exp(x);
}
complex<double> sqrtinv (const complex<double> &x)
std::complex<double> sqrtinv (const std::complex<double> &x)
{
return 1./std::sqrt(x);
}
@ -1517,7 +1519,7 @@ NRMat<double> log(const NRMat<double> &a)
return matrixfunction(a, &myclog);
}
NRMat<complex<double> > log(const NRMat<complex<double> > &a)
NRMat<std::complex<double> > log(const NRMat<std::complex<double> > &a)
{
return matrixfunction(a, &myclog);
}
@ -1528,12 +1530,12 @@ NRMat<double> exp0(const NRMat<double> &a)
return matrixfunction(a, &mycexp);
}
NRMat<complex<double> > exp0(const NRMat<complex<double> > &a)
NRMat<std::complex<double> > exp0(const NRMat<std::complex<double> > &a)
{
return matrixfunction(a, &mycexp);
}
NRMat<complex<double> > copytest(const NRMat<complex<double> > &a)
NRMat<std::complex<double> > copytest(const NRMat<std::complex<double> > &a)
{
return matrixfunction(a, &myccopy);
}
@ -1565,13 +1567,13 @@ const NRVec<double> diagofproduct(const NRMat<double> &a, const NRMat<double> &b
}
const NRVec< complex<double> > diagofproduct(const NRMat< complex<double> > &a,
const NRMat< complex<double> > &b, bool trb, bool conjb)
const NRVec< std::complex<double> > diagofproduct(const NRMat< std::complex<double> > &a,
const NRMat< std::complex<double> > &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<complex>()");
NRVec< complex<double> > result(a.nrows());
NRVec< std::complex<double> > result(a.nrows());
if (trb) {
if (conjb) {
for(int i=0; i<a.nrows(); i++)
@ -1608,16 +1610,16 @@ double trace2(const NRMat<double> &a, const NRMat<double> &b, bool trb)
}
// LV
complex<double> trace2(const NRMat<complex<double> > &a, const NRMat<complex<double> > &b, bool adjb)
std::complex<double> trace2(const NRMat<std::complex<double> > &a, const NRMat<std::complex<double> > &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<double> dot;
std::complex<double> dot;
if (adjb) { cblas_zdotc_sub(a.nrows()*a.ncols(), b, 1, a, 1, &dot); return dot; }
complex<double> sum = complex<double>(0.,0.);
std::complex<double> sum = std::complex<double>(0.,0.);
for (int i=0; i<a.nrows(); i++) {
cblas_zdotu_sub(a.ncols(), a[i], 1, b[0]+i, b.ncols(), &dot);
sum += dot;
@ -1659,7 +1661,7 @@ return trace2(b,a,diagscaled);
//Cholesky interface
extern "C" void FORNAME(dpotrf)(const char *UPLO, const FINT *N, double *A, const FINT *LDA, FINT *INFO);
extern "C" void FORNAME(zpotrf)(const char *UPLO, const FINT *N, complex<double> *A, const FINT *LDA, FINT *INFO);
extern "C" void FORNAME(zpotrf)(const char *UPLO, const FINT *N, std::complex<double> *A, const FINT *LDA, FINT *INFO);
void cholesky(NRMat<double> &a, bool upper)
{
@ -1680,7 +1682,7 @@ else
}
void cholesky(NRMat<complex<double> > &a, bool upper)
void cholesky(NRMat<std::complex<double> > &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<double> *A, const FINT *LDA, double *WORK);
extern "C" double FORNAME(zlange)( const char *NORM, const FINT *M, const FINT *N, std::complex<double> *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<complex<double> > &A, const char norm)
double MatrixNorm(NRMat<std::complex<double> > &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<double > &A, const char norm)
//condition number
extern "C" void FORNAME(zgecon)( const char *norm, const FINT *n, complex<double> *A, const FINT *LDA, const double *anorm, double *rcond, complex<double> *work, double *rwork, FINT *info);
extern "C" void FORNAME(zgecon)( const char *norm, const FINT *n, std::complex<double> *A, const FINT *LDA, const double *anorm, double *rcond, std::complex<double> *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<complex<double> > &A, const char norm)
double CondNumber(NRMat<std::complex<double> > &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<double> *work;
std::complex<double> *work;
double *rwork;
if(N != A.ncols()){
laerror("nonsquare matrix in zgecon");
return 0.0;
}
work = new complex<double>[2*N];
work = new std::complex<double>[2*N];
rwork = new double[2*N];
Norma = MatrixNorm(A, norm);

34
nonclass.h
View File

@ -100,7 +100,7 @@ extern void singular_decomposition(NRMat<T> &a, NRMat<T> *u, NRVec<LA_traits<T>:
/*NOTE!!! all versions of diagonalize DESTROY A and generalized diagonalize also B matrix */
declare_la(double)
declare_la(complex<double>)
declare_la(std::complex<double>)
// Separate declarations
//general nonsymmetric matrix and generalized diagonalization
@ -110,8 +110,8 @@ extern void gdiagonalize(NRMat<double> &a, NRVec<double> &wr, NRVec<double> &wi,
NRMat<double> *b=NULL, NRVec<double> *beta=NULL); //this used real storage of eigenvectors like dgeev
template<typename T>
extern void gdiagonalize(NRMat<T> &a, NRVec< complex<double> > &w,
NRMat< complex<double> >*vl, NRMat< complex<double> > *vr,
extern void gdiagonalize(NRMat<T> &a, NRVec< std::complex<double> > &w,
NRMat< std::complex<double> >*vl, NRMat< std::complex<double> > *vr,
const bool corder=1, int n=0, const int sorttype=0, const int biorthonormalize=0,
NRMat<T> *b=NULL, NRVec<T> *beta=NULL); //eigenvectors are stored in complex matrices for T both double and complex
@ -129,7 +129,7 @@ extern const typename LA_traits<T>::complextype complexmatrix (const T&, const T
//Cholesky decomposition
extern void cholesky(NRMat<double> &a, bool upper=1);
extern void cholesky(NRMat<complex<double> > &a, bool upper=1);
extern void cholesky(NRMat<std::complex<double> > &a, bool upper=1);
//inverse by means of linear solve, preserving rhs intact
template<typename T>
@ -207,7 +207,7 @@ int linear_solve_x(NRMat<T> &A, T *B, const int rhsCount, const int eqCount, con
// the info parameter of dgesvx is returned (see man dgesvx)
//------------------------------------------------------------------------------
template<class T>
int multiply_by_inverse(NRMat<T> &A, NRMat<T> &B, bool useEq, double *rcond);
int multiply_by_inverse(NRMat<T> &A, NRMat<T> &B, bool useEq=false, double *rcond=NULL);
//general submatrix, INDEX will typically be NRVec<int> or even int*
@ -310,7 +310,7 @@ return r;
//matrix functions via diagonalization
extern NRMat<double> realmatrixfunction(NRMat<double> a, double (*f) (double)); //a has to by in fact symmetric
extern NRMat<complex<double> > complexmatrixfunction(NRMat<double> a, double (*fre) (double), double (*fim) (double)); //a has to by in fact symmetric
extern NRMat<std::complex<double> > complexmatrixfunction(NRMat<double> a, double (*fre) (double), double (*fim) (double)); //a has to by in fact symmetric
template<typename T>
NRMat<T> matrixfunction(NRSMat<T> a, double (*f) (double)) //of symmetric/hermitian matrix
@ -331,18 +331,18 @@ NRMat<T> matrixfunction(NRSMat<T> a, double (*f) (double)) //of symmetric/hermit
template<typename T>
extern NRMat<T> matrixfunction(NRMat<T> a, complex<double> (*f)(const complex<double> &)) //of a general real/complex matrix
extern NRMat<T> matrixfunction(NRMat<T> a, std::complex<double> (*f)(const std::complex<double> &)) //of a general real/complex matrix
{
int n = a.nrows();
NRVec<complex<double> > w(n);
NRMat<complex<double> > u(n,n),v(n,n);
NRVec<std::complex<double> > w(n);
NRMat<std::complex<double> > u(n,n),v(n,n);
#ifdef debugmf
NRMat<complex<double> > a0=a;
NRMat<std::complex<double> > a0=a;
#endif
gdiagonalize<T>(a, w, &u, &v, false,n,0,false,NULL,NULL);//a gets destroyed, eigenvectors are rows
NRVec< complex<double> > z = diagofproduct(u, v, 1, 1);
NRVec< std::complex<double> > z = diagofproduct(u, v, 1, 1);
#ifdef debugmf
std::cout <<"TEST matrixfunction\n"<<w<<u<<v<<z;
@ -351,7 +351,7 @@ std::cout <<"TEST matrixfunction2 "<< a0*v.transpose(1) - v.transpose(1)*diagona
std::cout <<"TEST matrixfunction3 "<< u*v.transpose(1)<<diagonalmatrix(z)<<std::endl;
#endif
NRVec< complex<double> > wz(n);
NRVec< std::complex<double> > wz(n);
for (int i=0; i<a.nrows(); i++) wz[i] = w[i]/z[i];
#ifdef debugmf
@ -361,7 +361,7 @@ std::cout <<"TEST matrixfunction4 "<< a0<< v.transpose(true)*diagonalmatrix(wz)*
for (int i=0; i<a.nrows(); i++) w[i] = (*f)(w[i])/z[i];
u.diagmultl(w);
NRMat< complex<double> > r(n, n);
NRMat< std::complex<double> > r(n, n);
r.gemm(0.0, v, 'c', u, 'n', 1.0);
return (NRMat<T>) 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<double> sqrtinv(const complex<double> &);
extern std::complex<double> sqrtinv(const std::complex<double> &);
extern double sqrtinv(const double);
//functions on matrices
@ -379,9 +379,9 @@ inline NRMat<double> realsqrt(const NRMat<double> &a) { return realmatrixfuncti
inline NRMat<double> realsqrtinv(const NRMat<double> &a) { return realmatrixfunction(a,&sqrtinv); }
inline NRMat<double> log(const NRSMat<double> &a) { return matrixfunction(a,&std::log); }
extern NRMat<double> log(const NRMat<double> &a);
extern NRMat<complex<double> > log(const NRMat<complex<double> > &a);
extern NRMat<complex<double> > exp0(const NRMat<complex<double> > &a);
extern NRMat<complex<double> > copytest(const NRMat<complex<double> > &a);
extern NRMat<std::complex<double> > log(const NRMat<std::complex<double> > &a);
extern NRMat<std::complex<double> > exp0(const NRMat<std::complex<double> > &a);
extern NRMat<std::complex<double> > copytest(const NRMat<std::complex<double> > &a);
extern NRMat<double> copytest(const NRMat<double> &a);
extern NRMat<double> exp0(const NRMat<double> &a);

200
quaternion.cc
View File

@ -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<typename T>
void Quaternion<T>::normquat2euler(T *e) const
void Quaternion<T>::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<typename T>
void Quaternion<T>::normquat2euler(T *eul, const char *type) const
{
Mat3<T> 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<typename T>
void Quaternion<T>::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<typename T>
void Quaternion<T>::axis2normquat(const T *axis, const T &angle)
{
@ -188,9 +324,69 @@ axis[2]= q[3]*s;
template<typename T>
Quaternion<T> LA_Quaternion::exp(const Quaternion<T> &x)
{
Quaternion<T> 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<typename T>
Quaternion<T> LA_Quaternion::log(const Quaternion<T> &x)
{
Quaternion<T> 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<typename T>
Quaternion<T> LA_Quaternion::pow(const Quaternion<T> &x, const T &y)
{
Quaternion<T> 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<T>; \
template Quaternion<T> LA_Quaternion::pow(const Quaternion<T> &x, const T &y); \
template Quaternion<T> LA_Quaternion::log(const Quaternion<T> &x); \
template Quaternion<T> LA_Quaternion::exp(const Quaternion<T> &x); \
INSTANTIZE(float)

130
quaternion.h
View File

@ -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<NRMat<double>>>
#ifndef AVOID_STDSTREAM
template <typename T>
std::istream& operator>>(std::istream &s, Quaternion<T> &x)
{
@ -110,8 +115,10 @@ s >> x.q[3];
return s;
}
template <typename T>
std::ostream& operator<<(std::ostream &s, const Quaternion<T> &x) {
std::ostream& operator<<(std::ostream &s, const Quaternion<T> &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<typename T, typename M>
void normquat2su2mat(const Quaternion<T> &q, M &a)
{
a[0][0] = std::complex<T>(q[0],q[1]);
a[0][1] = std::complex<T>(q[2],q[3]);
a[1][0] = std::complex<T>(-q[2],q[3]);
a[1][1] = std::complex<T>(q[0],-q[1]);
}
//use transpose option to match nasa paper definition
//conversion between quanternions and rotation matrices (+/- q yields the same rotation)
//
template<typename T, typename M>
void normquat2rotmat(const Quaternion<T> &q, M &a)
void normquat2rotmat(const Quaternion<T> &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<typename T, typename M>
void quat2rotmat(Quaternion<T> q, M &a, const bool already_normalized=false)
void quat2rotmat(Quaternion<T> 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<typename T, typename M>
void normquat2rotmatder(const Quaternion<T> &q, Quaternion<M> &a)
void normquat2rotmatder(const Quaternion<T> &q, Quaternion<M> &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<typename T, typename M>
void rotmat2normquat(const M &a, Quaternion<T> &q)
void rotmat2normquat(const M &a, Quaternion<T> &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<typename T>
Quaternion<T> exp(const Quaternion<T> &x)
{
Quaternion<T> 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<T> exp(const Quaternion<T> &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<typename T>
Quaternion<T> log(const Quaternion<T> &x);
template<typename T>
Quaternion<T> log(const Quaternion<T> &x)
{
Quaternion<T> 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<T> pow(const Quaternion<T> &x, const T &y);
template<typename T>
Quaternion<T> pow(const Quaternion<T> &x, const T &y)
{
Quaternion<T> 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

44
t.cc
View File

@ -1886,7 +1886,7 @@ cout<<r<<endl;
Mat3<double> rotmat;
quat2rotmat(r,rotmat);
cout << rotmat[0][1]<<endl;
r.normalize();
r.normalize(NULL,true);
NRMat<double> rotmat2(3,3),rotmat3(3,3);
Quaternion<NRMat<double> > rotmatder;
rotmatder[0].resize(3,3);
@ -1905,8 +1905,18 @@ cout <<"orig "<<r<<endl;
cout <<"reconstruct "<<rr<<endl;
cout <<"diff " <<r-rr<<endl;
Vec3<double> eul;
r.normquat2euler(eul);
cout<<eul[0]<<" " <<eul[1]<<" "<<eul[2]<<endl;
r.normalize(NULL,true);
r.normquat2eulerzyx(eul);
cout<< "euler "<<eul[0]<<" " <<eul[1]<<" "<<eul[2]<<endl;
Quaternion<double> rback;
rback.eulerzyx2normquat(eul);
cout <<"q from euler back "<<endl<<r<<endl<<rback<<endl;
Mat3<double> rm;
euler2rotmat((double *)eul,rm,"xyz",false,false,false);
cout <<rm;
Vec3<double> eulback;
rotmat2euler((double *)eulback,rm,"xyz",false,false,false);
cout <<"euler back from rm error "<<eulback-eul<<endl;
Vec3<double> axis={0,1/sqrt(2),1/sqrt(2)};
Quaternion<double> rrr;
rrr.axis2normquat(axis,0.5);
@ -1924,11 +1934,31 @@ Quaternion<double> rrvec = rvec.rotateby(rrr.conjugate());
cout <<rrvec<<endl;
Vec3<double> rotvec;
rrr.rotate(rotvec,vec);
Quaternion<double> qq={1.5,2,-3,2.123};
Quaternion<double> qq={1.5,2,-2,-1.123};
cout << " test "<<qq*qq<<endl;
cout <<"exp " <<exp(qq)<<endl;
cout <<"log " <<log(qq)<<endl;
cout <<"pow " <<pow(qq,0.5)<<endl;
cout <<"exp " <<exp(qq)<< " "<< log(exp(qq))-qq<<endl;
cout <<"log " <<log(qq)<<" "<< exp(log(qq))-qq<<endl;
cout <<"pow " <<pow(qq,0.5)<<" " <<pow(qq,0.5)*pow(qq,0.5)-qq<<endl;
Quaternion<double> qq2={-.5,1,2,3.23};
Mat3<double> m(1,2,3,4,5,6,7,8,-9);
NRMat<double> mm(m,3,3);
Vec3<double> v(10,20,30);
NRVec<double> vv(v,3);
cout << m*v<<mm*vv<<endl;
cout << v*m<<vv*mm<<endl;
Mat3<double> m2 = m.inverse();
cout<<m2<<m*m2<<m2*m<<endl;
NRMat<double> mm2(3,3); mm2=1.;
multiply_by_inverse(mm2,mm);
cout <<mm<<endl;
qq.normalize(NULL,true);
Vec3<double> xeul,yeul;
qq.normquat2euler((double *)xeul,"zyx");
qq.normquat2eulerzyx((double *)yeul);
cout <<xeul<< " "<<yeul<<endl;
Quaternion<double> xqq;
xqq.euler2normquat((double *)xeul,"zyx");
cout <<"normquat2euler test "<<endl<<qq<<endl<<xqq<<endl<<qq-xqq<<endl;
}

332
vecmat3.cc
View File

@ -17,13 +17,92 @@
*/
#include "vecmat3.h"
#include <ctype.h>
using namespace LA_Vecmat3;
//cf. https://en.wikipedia.org/wiki/Euler_angles
template<typename T>
void euler2rotmat(const Vec3<T> &eul, Mat3<T> a, char *type, bool transpose, bool direction, bool reverse)
const Vec3<T> Vec3<T>::operator*(const Mat3<T> &rhs) const
{
Vec3<T> 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<typename T>
Mat3<T>& Mat3<T>::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<typename T>
Mat3<T>& Mat3<T>::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<typename T>
const Mat3<T> Mat3<T>::operator*(const Mat3 &rhs) const
{
Mat3<T> 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<typename T>
T Mat3<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]);
}
template<typename T>
void Mat3<T>::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<typename T>
const Mat3<T> Mat3<T>::inverse() const
{
Mat3<T> 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<typename T>
const Vec3<T> Mat3<T>::operator*(const Vec3<T> &rhs) const
{
Vec3<T> 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<typename T>
void LA_Vecmat3::euler2rotmat(const T *eul, Mat3<T> &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<typename T>
void LA_Vecmat3::rotmat2euler(T *eul, const Mat3<T> &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 <typename T>
std::istream& LA_Vecmat3::operator>>(std::istream &s, Vec3<T> &x)
{
s >> x.q[0];
s >> x.q[1];
s >> x.q[2];
return s;
}
template <typename T>
std::ostream& LA_Vecmat3::operator<<(std::ostream &s, const Vec3<T> &x) {
s << x.q[0]<<" ";
s << x.q[1]<<" ";
s << x.q[2];
return s;
}
template <typename T>
std::istream& LA_Vecmat3::operator>>(std::istream &s, Mat3<T> &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 <typename T>
std::ostream& LA_Vecmat3::operator<<(std::ostream &s, const Mat3<T> &x) {
s << x.q[0][0]<<" "<< x.q[0][1]<<" " << x.q[0][2]<<std::endl;
s << x.q[1][0]<<" "<< x.q[1][1]<<" " << x.q[1][2]<<std::endl;
s << x.q[2][0]<<" "<< x.q[2][1]<<" " << x.q[2][2]<<std::endl;
return s;
}
#endif
//force instantization
#define INSTANTIZE(T) \
template class Vec3<T>; \
template class Mat3<T>; \
template void LA_Vecmat3::euler2rotmat(const T *eul, Mat3<T> &a, const char *type, bool transpose=0, bool direction=0, bool reverse=0); \
template void LA_Vecmat3::rotmat2euler(T *eul, const Mat3<T> &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<T> &x); \
template std::ostream& LA_Vecmat3::operator<<(std::ostream &s, const Vec3<T> &x); \
template std::istream& LA_Vecmat3::operator>>(std::istream &s, Mat3<T> &x); \
template std::ostream& LA_Vecmat3::operator<<(std::ostream &s, const Mat3<T> &x); \
#endif
INSTANTIZE(float)
#ifndef QUAT_NO_DOUBLE
INSTANTIZE(double)
#endif
#ifndef AVOID_STDSTREAM
INSTANTIZE2(float)
#ifndef QUAT_NO_DOUBLE
INSTANTIZE2(double)
#endif
#endif

109
vecmat3.h
View File

@ -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<T> &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<T> &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<T> operator*(const Vec3<T> &rhs) const
{
Vec3<T> 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<T> operator*(const Vec3<T> &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 <typename T>
std::istream& operator>>(std::istream &s, Vec3<T> &x)
{
s >> x.q[0];
s >> x.q[1];
s >> x.q[2];
return s;
}
std::istream& operator>>(std::istream &s, Vec3<T> &x);
template <typename T>
std::ostream& operator<<(std::ostream &s, const Vec3<T> &x) {
s << x.q[0]<<" ";
s << x.q[1]<<" ";
s << x.q[2];
return s;
}
std::ostream& operator<<(std::ostream &s, const Vec3<T> &x);
template <typename T>
std::istream& operator>>(std::istream &s, Mat3<T> &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<T> &x);
template <typename T>
std::ostream& operator<<(std::ostream &s, const Mat3<T> &x) {
s << x.q[0][0]<<" "<< x.q[0][1]<<" " << x.q[0][2]<<std::endl;
s << x.q[1][0]<<" "<< x.q[1][1]<<" " << x.q[1][2]<<std::endl;
s << x.q[2][0]<<" "<< x.q[2][1]<<" " << x.q[2][2]<<std::endl;
return s;
}
std::ostream& operator<<(std::ostream &s, const Mat3<T> &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<typename T>
void euler2rotmat(const T *eul, Mat3<T> &a, const char *type, bool transpose=0, bool direction=0, bool reverse=0);
template<typename T>
void rotmat2euler(T *eul, const Mat3<T> &a, const char *type, bool transpose=0, bool direction=0, bool reverse=0);
}//namespace
#endif /* _VECMAT3_H_ */