21 #include <initializer_list> 26 #define cout_octave(x) octave((cout),(x),(#x)) 27 #define HERE std::cerr << __FILE__ << ':' << __LINE__ << std::endl; 33 const double PI = 3.1415926535897932384626433832795029L;
34 const double PI_2 = 1.5707963267948966192313216916397514L;
42 const double REQ = 6378137.0;
43 const double rEQ = 6356752.3142;
44 const double f = 1.0/298.257223563;
45 const double ECC = 0.0818191908426;
47 const double WS = 7.292115E-05;
49 const double g = 9.78032534;
50 const double k = 0.00193185;
53 template <
typename tData,
size_t tRows,
size_t tCols >
57 void matalloc (
size_t iRowIndex,
size_t iColIndex);
62 tData storage[tRows*tCols];
88 Matrix (
const std::initializer_list<tData>& l);
108 void load(
const tData* tArray);
117 inline tData * operator [] (
size_t iRowIndex)
const {
118 assert(iRowIndex<iRows);
119 return ij[iRowIndex];
130 inline tData & operator () (
size_t iRowIndex,
size_t iColIndex)
const;
138 inline tData & operator () (
size_t iIndex)
const;
143 template <
class tData0,
size_t tRows0,
size_t tCols0 >
144 friend std::ostream& operator << (std::ostream& s,const Matrix< tData0, tRows0, tCols0 >& A);
154 template <
class tData0,
size_t tRows0,
size_t tCols0 >
163 template <
class tData0,
size_t tRows0,
size_t tCols0 >
164 friend std::ostream&
octave (std::ostream& s,
const Matrix< std::complex<tData0>, tRows0, tCols0 >& A,
const char* Aname);
171 inline tData *
pIJ (
void)
const {
178 inline size_t rows (
void)
const {
185 inline size_t cols (
void)
const {
227 template<
class tData0,
size_t tRows0,
size_t tCols0 >
238 template <
size_t tColsR >
243 for (
size_t iIndex=0; iIndex<iRows; iIndex++) {
244 for (
size_t jIndex=0; jIndex<R.
cols(); jIndex++) {
246 for (
size_t kIndex=0; kIndex<R.
rows(); kIndex++) {
247 x += ij[iIndex][kIndex] * R[kIndex][jIndex];
249 Result[iIndex][jIndex] = x;
270 template <
class tData0,
size_t tCols0,
size_t tRowsT,
size_t tRowsB>
277 template <
class tData0,
size_t tRows0,
size_t tColsL,
size_t tColsR >
303 template <
class tData0,
size_t tRows0,
size_t tCols0 >
306 template <
size_t tRows0,
size_t tCols0 >
309 template <
size_t tRows0,
size_t tCols0 >
316 template <
class tData0,
size_t tRows0 >
319 template <
class tData0,
size_t tRows0 >
322 template <
class tData0,
size_t tCols0 >
333 template <
class tData0 >
339 template <
class tData0 >
345 template <
class tData0,
size_t tRows0 >
351 tData n(
void )
const;
356 template <
class tData0,
size_t tRows0 >
367 template <
size_t tRows0 >
370 template <
size_t tRows0>
371 friend Matrix< std::complex<float>, tRows0, tRows0 >
inv(
const Matrix< std::complex<float>, tRows0, tRows0 >& R );
373 template <
size_t tRows0 >
376 template <
size_t tRows0 >
377 friend Matrix< std::complex<double>, tRows0, tRows0 >
inv(
const Matrix< std::complex<double>, tRows0, tRows0 >& R );
382 template <
size_t tRows0 >
385 template <
size_t tRows0 >
388 template <
size_t tRows0 >
389 friend std::complex<float>
det(
const Matrix< std::complex<float>, tRows0, tRows0 >& R );
391 template <
size_t tRows0 >
392 friend std::complex<double>
det(
const Matrix< std::complex<double>, tRows0, tRows0 >& R );
399 template <
class tData0 >
409 template <
class tData,
size_t tRows,
size_t tCols >
412 for (
size_t iIndex = 1; iIndex < iRowIndex; iIndex++)
413 ij[iIndex] = ij[iIndex - 1] + iColIndex;
415 std::memset (storage, 0x0,
sizeof(storage));
418 template <
class tData,
size_t tRows,
size_t tCols >
423 template <
class tData,
size_t tRows,
size_t tCols >
425 : iRows(tRows), iCols(tCols) {
429 template <
class tData,
size_t tRows,
size_t tCols >
436 template <
class tData,
size_t tRows,
size_t tCols >
443 template <
class tData,
size_t tRows,
size_t tCols >
449 ij[0][i] = *(l.begin() + i);
453 template <
class tData,
size_t tRows,
size_t tCols >
462 template <
class tData,
size_t tRows,
size_t tCols >
466 ij[0][i] = *(l.begin() + i);
471 template <
class tData,
size_t tRows,
size_t tCols >
476 template <
class tData,
size_t tRows,
size_t tCols >
480 assert(0<=iRowIndex && iRowIndex<
iRows);
481 assert(0<=iColIndex && iColIndex<
iCols);
482 return ij[iRowIndex][iColIndex];
485 template <
class tData,
size_t tRows,
size_t tCols >
490 assert(0<=iIndex && iIndex<
iRows);
491 return ij[iIndex][0];
493 assert(0<=iIndex && iIndex<
iCols);
494 return ij[0][iIndex];
498 template <
class tData,
size_t tRows,
size_t tCols >
499 std::ostream& operator << (std::ostream& s,const Matrix< tData, tRows, tCols >& A) {
502 std::streamsize old_precision = s.precision(8);
507 for (
size_t i=0; i<A.iRows; i++) {
508 for (
size_t j=0; j<A.iCols; j++) {
510 s << (A[i][j]) <<
'\t';
515 s.precision(old_precision);
519 template <
class tData0,
size_t tRows0,
size_t tCols0 >
520 std::ostream&
octave (std::ostream& s,
const Matrix< std::complex< tData0 >, tRows0, tCols0 >& A,
const char* Aname) {
521 s <<
"# name: " << Aname << std::endl;
522 s <<
"# type: complex matrix" << std::endl;
523 s <<
"# rows: " << A.iRows << std::endl;
524 s <<
"# columns: " << A.iCols << std::endl;
530 template <
class tData0,
size_t tRows0,
size_t tCols0 >
532 s <<
"# name: " << Aname << std::endl;
533 s <<
"# type: matrix" << std::endl;
534 s <<
"# rows: " << A.
iRows << std::endl;
535 s <<
"# columns: " << A.
iCols << std::endl;
541 template <
class tData,
size_t tRows,
size_t tCols >
543 tData * pLeft =
ij[0];
544 tData * pRight = R.
ij[0];
546 for (
size_t iIndex = 0; iIndex <
iRows *
iCols; iIndex++)
547 if((*pLeft++) != tData(*pRight++)) {
554 template <
class tData,
size_t tRows,
size_t tCols >
556 return !(*
this ==
R);
559 template <
class tData,
size_t tRows,
size_t tCols >
564 template <
class tData,
size_t tRows,
size_t tCols >
567 tData * pLeft =
ij[0];
568 tData * pResult = Result.
ij[0];
569 for (
size_t iIndex = 0; iIndex <
iRows *
iCols; iIndex++)
570 (*pResult++) = (-(*pLeft++));
574 template <
class tData,
size_t tRows,
size_t tCols >
577 tData * pLeft =
ij[0];
578 tData * pRight = R.
ij[0];
579 tData * pResult = Result.
ij[0];
581 for (
size_t iIndex = 0; iIndex <
iRows *
iCols; iIndex++)
582 (*pResult++) = (*pLeft++) + (*pRight++);
587 template <
class tData,
size_t tRows,
size_t tCols >
590 tData * pLeft =
ij[0];
591 tData * pRight = R.
ij[0];
592 tData * pResult = Result.
ij[0];
594 for (
size_t iIndex = 0; iIndex <
iRows *
iCols; iIndex++)
595 (*pResult++) = (*pLeft++) - (*pRight++);
599 template <
class tData,
size_t tRows,
size_t tCols >
602 tData * pResult = Result.
ij[0];
603 for (
size_t iIndex = 0; iIndex <
iRows *
iCols; iIndex++)
604 (*pResult++) *= scalar;
608 template <
class tData,
size_t tRows,
size_t tCols >
613 tData * pResult = Result.
ij[0];
614 for (
size_t iIndex = 0; iIndex < iRows *
iCols; iIndex++)
615 (*pResult++) *= scalar;
619 template <
class tData,
size_t tRows,
size_t tCols >
623 tData * pResult = Result.
ij[0];
624 for (
size_t iIndex = 0; iIndex <
iRows *
iCols; iIndex++)
625 (*pResult++) /= scalar;
629 template <
class tData,
size_t tRows,
size_t tCols >
632 tData * pLeft =
ij[0];
633 tData * pRight = R.
ij[0];
634 tData * pResult = Result.
ij[0];
635 for (
size_t iIndex = 0; iIndex <
iRows*
iCols; iIndex++)
636 (*pResult++) = (*pLeft++) * tData(*pRight++);
640 template <
class tData,
size_t tRows,
size_t tCols >
643 tData * pLeft =
ij[0];
644 tData * pRight = R.
ij[0];
645 tData * pResult = Result.
ij[0];
646 for (
size_t iIndex = 0; iIndex <
iRows *
iCols; iIndex++) {
647 assert(*pRight != 0.0);
648 (*pResult++) = (*pLeft++) / tData(*pRight++);
653 template <
class tData0,
size_t tCols0,
size_t tRowsT,
size_t tRowsB>
659 for(i=0; i<Top.
iRows; i++)
660 for(j=0; j<Top.
iCols; j++)
661 Result[i][j] = Top[i][j];
664 for(j=0; j<Top.
iCols; j++) {
665 Result[i][j] = Bottom[ii][j];
671 template <
class tData0,
size_t tRows0,
size_t tColsL,
size_t tColsR >
676 for(i=0; i<Left.
iRows; i++)
677 for(j=0; j<Left.
iCols; j++)
678 Result[i][j] = Left[i][j];
679 for(i=0; i<Left.
iRows; i++) {
681 Result[i][j] = Right[i][jj];
687 template <
class tData,
size_t tRows,
size_t tCols >
689 std::memset (
ij[0], 0x0,
sizeof(tData) *
iRows *
iCols);
693 template <
class tData,
size_t tRows,
size_t tCols >
695 tData * pThis =
ij[0];
696 for (
size_t iIndex = 0; iIndex <
iRows *
iCols; iIndex++)
697 (*pThis++) = tData(1);
701 template <
class tData,
size_t tRows,
size_t tCols >
703 assert(tRows==tCols);
704 std::memset (
ij[0], 0x0,
sizeof(tData) *
iRows *
iCols);
706 tData * pThis =
ij[0];
707 for (
size_t iIndex = 0; iIndex <
iRows; iIndex++, pThis+=
iCols)
708 (*pThis++) = tData(1);
712 template <
class tData,
size_t tRows,
size_t tCols >
714 std::default_random_engine generator;
715 std::normal_distribution<tData> distribution;
716 for (
size_t iIndex = 0; iIndex <
iRows*
iCols; iIndex++) {
717 ij[0][iIndex] = distribution(generator);
722 template <
class tData,
size_t tRows,
size_t tCols >
726 for (
size_t iIndex = 0; iIndex < tCols; iIndex++)
727 for (
size_t jIndex = 0; jIndex < tRows; jIndex++)
728 Result[iIndex][jIndex] = R[jIndex][iIndex];
733 template <
size_t tRows,
size_t tCols >
737 for (
size_t iIndex = 0; iIndex < tCols; iIndex++)
738 for (
size_t jIndex = 0; jIndex < tRows; jIndex++)
739 Result[iIndex][jIndex] = std::conj(
R[jIndex][iIndex]);
744 template <
size_t tRows,
size_t tCols >
748 for (
size_t iIndex = 0; iIndex < tCols; iIndex++)
749 for (
size_t jIndex = 0; jIndex < tRows; jIndex++)
750 Result[iIndex][jIndex] = std::conj(
R[jIndex][iIndex]);
755 template <
class tData,
size_t tRows >
758 for (
size_t iIndex = 0; iIndex < tRows; iIndex++ ) {
759 Result[iIndex][0] = R[iIndex][iIndex];
764 template <
class tData,
size_t tRows >
767 for (
size_t iIndex = 0; iIndex < tRows; iIndex++ ) {
768 Result[iIndex][iIndex] = R[iIndex][0];
773 template <
class tData,
size_t tCols >
776 for (
size_t iIndex = 0; iIndex < tCols; iIndex++ ) {
777 Result[iIndex][iIndex] = R[0][iIndex];
782 template <
class tData >
784 tData * pR = R.
pIJ();
795 template <
class tData >
800 template <
class tData,
size_t tRows >
802 tData Result = tData(0);
804 tData * pR = R.
pIJ();
805 tData * pL = L.
pIJ();
807 for (
size_t iIndex = 0; iIndex < tRows; iIndex++) {
808 Result += (*pR++)*(*pL++);
813 template <
class tData,
size_t tRows,
size_t tCols >
815 return std::sqrt(
dot(*
this,*
this));
818 template <
class tData,
size_t tRows >
820 return std::sqrt(
dot(R,R));
823 template <
class tData,
size_t tRows,
size_t tCols >
828 return Result*(1.0/den);
831 extern "C" void sgesv_(
const int &
n,
const int &nrhs,
float *A,
832 const int &lda,
int* ipiv,
float *B,
const int &ldb,
int *info);
833 extern "C" void cgesv_(
const int &n,
const int &nrhs, std::complex<float> *A,
834 const int &lda,
int* ipiv, std::complex<float> *B,
const int &ldb,
int *info);
835 extern "C" void dgesv_(
const int &n,
const int &nrhs,
double *A,
836 const int &lda,
int* ipiv,
double *B,
const int &ldb,
int *info);
837 extern "C" void zgesv_(
const int &n,
const int &nrhs, std::complex<double> *A,
838 const int &lda,
int* ipiv, std::complex<double> *B,
const int &ldb,
int *info);
840 template <
size_t tRows >
844 int ipiv[tRows] = {0};
852 std::cerr <<
"?gesv returned error: " << info << std::endl;
859 template <
size_t tRows >
863 int ipiv[tRows] = {0};
871 std::cerr <<
"?gesv returned error: " << info << std::endl;
878 template <
size_t tRows >
882 int ipiv[tRows] = {0};
890 std::cerr <<
"?gesv returned error: " << info << std::endl;
897 template <
size_t tRows >
901 int ipiv[tRows] = {0};
909 std::cerr <<
"?gesv returned error: " << info << std::endl;
916 extern "C" void sgetrf_(
const int &m,
const int &n,
float *A,
917 const int &lda,
int *ipiv,
int *info);
918 extern "C" void cgetrf_(
const int &m,
const int &n, std::complex<float> *A,
919 const int &lda,
int *ipiv,
int *info);
920 extern "C" void dgetrf_(
const int &m,
const int &n,
double *A,
921 const int &lda,
int *ipiv,
int *info);
922 extern "C" void zgetrf_(
const int &m,
const int &n, std::complex<double> *A,
923 const int &lda,
int *ipiv,
int *info);
925 template <
size_t tRows >
929 int ipiv[tRows] = {0};
936 for(
int i=0; i<
n; i++) {
937 if(ipiv[i] != (i+1)) result *= -a[i][i];
938 else result *= +a[i][i];
941 std::cerr <<
"?getrf returned error: " << info << std::endl;
947 template <
size_t tRows >
948 std::complex<float>
det(
const Matrix< std::complex<float>, tRows, tRows >&
R ) {
951 int ipiv[tRows] = {0};
953 std::complex<float> result(1.0,0.0);
958 for(
int i=0; i<
n; i++) {
959 if(ipiv[i] != (i+1)) result *= -a[i][i];
960 else result *= +a[i][i];
963 std::cerr <<
"?getrf returned error: " << info << std::endl;
969 template <
size_t tRows >
973 int ipiv[tRows] = {0};
980 for(
int i=0; i<
n; i++) {
981 if(ipiv[i] != (i+1)) result *= -a[i][i];
982 else result *= +a[i][i];
985 std::cerr <<
"?getrf returned error: " << info << std::endl;
991 template <
size_t tRows >
992 std::complex<double>
det(
const Matrix< std::complex<double>, tRows, tRows >&
R ) {
995 int ipiv[tRows] = {0};
997 std::complex<double> result(1.0,0.0);
1002 for(
int i=0; i<
n; i++) {
1003 if(ipiv[i] != (i+1)) result *= -a[i][i];
1004 else result *= +a[i][i];
1007 std::cerr <<
"?getrf returned error: " << info << std::endl;
1013 template <
class tData >
1015 assert(axis ==
'x' || axis ==
'y' || axis ==
'z');
1019 result = {1.0,0.0,0.0,0.0,cos(angle),-sin(angle),0.0,sin(angle), cos(angle)};
1020 }
else if(axis==
'y') {
1021 result = {cos(angle),0.0,sin(angle),0.0,1.0,0.0,-sin(angle),0.0,cos(angle)};
1022 }
else if(axis==
'z') {
1023 result = {cos(angle),-sin(angle),0.0,sin(angle),cos(angle),0.0,0.0,0.0,1.0};
1030 template <
class tData >
1035 template <
class tData >
1040 template <
class tData >
1042 tData a_r_psi[] = {cos(psi),-sin(psi), 0,
1043 sin(psi), cos(psi), 0,
1048 tData a_r_the[] = {cos(the), 0, sin(the),
1050 -sin(the), 0, cos(the)
1054 tData a_r_phi[] = { 1, 0, 0,
1055 0, cos(phi),-sin(phi),
1056 0, sin(phi), cos(phi)
1060 return r_psi*r_the*r_phi;
1063 template <
class tData >
1065 if(
norm(u) != 1.0) u=u*(1.0/
norm(u));
1066 *az=atan2(
u(2),
u(1));
1073 #endif // from _EMATRIX_H void uv_to_ae(tData *az, tData *el, Matrix< tData, 3, 1 > &u)
Matrix< tData, tRows, tCols > ones(void)
friend Matrix< tData0, 3, 3 > R(tData0 angle, char axis)
std::ostream & octave(std::ostream &s, const Matrix< std::complex< tData0 >, tRows0, tCols0 > &A, const char *Aname)
bool operator==(Matrix< tData, tRows, tCols > &R)
Matrix< tData, tRows, tCols > operator*(const tData &scalar, const Matrix< tData, tRows, tCols > &R)
tData dot(const Matrix< tData, tRows, 1 > &L, const Matrix< tData, tRows, 1 > &R)
Matrix< tData, tRows, tCols > operator*=(const Matrix< tData, tRows, tCols > &R)
bool operator!=(Matrix< tData, tRows, tCols > &R)
void zgesv_(const int &n, const int &nrhs, std::complex< double > *A, const int &lda, int *ipiv, std::complex< double > *B, const int &ldb, int *info)
void cgesv_(const int &n, const int &nrhs, std::complex< float > *A, const int &lda, int *ipiv, std::complex< float > *B, const int &ldb, int *info)
Matrix< tData, 3, 3 > TIE(tData t)
Matrix< tData, 3, 3 > R(tData angle, char axis)
Matrix< tData, tRows, tCols > operator+()
void dgetrf_(const int &m, const int &n, double *A, const int &lda, int *ipiv, int *info)
Matrix< tData, tRows, tCols > operator/(const tData &scalar)
Matrix< tData, tRows, tCols > eye(void)
Matrix< tData, 3, 1 > cross(const Matrix< tData, 3, 1 > &L, const Matrix< tData, 3, 1 > &R)
Matrix< tData, 3, 3 > TEL(tData lat_rad, tData lon_rad)
Matrix< tData, tRows, 1 > u(void) const
friend tData0 dot(const Matrix< tData0, tRows0, 1 > &L, const Matrix< tData0, tRows0, 1 > &R)
float det(const Matrix< float, tRows, tRows > &R)
Matrix< tData, tRows, tCols > zeros(void)
friend Matrix< tData0, 3, 1 > cross(const Matrix< tData0, 3, 1 > &L, const Matrix< tData0, 3, 1 > &R)
friend Matrix< tData0, tCols0, tRows0 > trans(const Matrix< tData0, tRows0, tCols0 > &R)
Matrix< float, tRows, tRows > inv(const Matrix< float, tRows, tRows > &R)
friend Matrix< float, tRows0, tRows0 > inv(const Matrix< float, tRows0, tRows0 > &R)
void zgetrf_(const int &m, const int &n, std::complex< double > *A, const int &lda, int *ipiv, int *info)
Matrix< tData, tRows, 1 > diag(const Matrix< tData, tRows, tRows > &R)
Matrix< tData, 3, 3 > skew(const Matrix< tData, 3, 1 > &R)
Matrix< tData, 3, 3 > ang_to_tib(tData psi, tData the, tData phi)
tData & operator()(size_t iRowIndex, size_t iColIndex) const
Matrix< tData, tRows, tCols > randn(void)
Matrix< tData0, tRowsT+tRowsB, tCols0 > operator&(const Matrix< tData0, tRowsT, tCols0 > &Top, const Matrix< tData0, tRowsB, tCols0 > &Bottom)
friend Matrix< tData0, 3, 3 > skew(const Matrix< tData0, 3, 1 > &R)
void sgesv_(const int &n, const int &nrhs, float *A, const int &lda, int *ipiv, float *B, const int &ldb, int *info)
const Matrix< tData, tRows, tCols > & operator=(const Matrix< tData, tRows, tCols > &R)
tData storage[tRows *tCols]
friend std::ostream & octave(std::ostream &s, const Matrix< tData0, tRows0, tCols0 > &A, const char *Aname)
friend Matrix< tData0, tRows0, 1 > diag(const Matrix< tData0, tRows0, tRows0 > &R)
Matrix< tData, tRows, tCols > operator*(const tData &scalar)
void dgesv_(const int &n, const int &nrhs, double *A, const int &lda, int *ipiv, double *B, const int &ldb, int *info)
void load(const tData *tArray)
Matrix< tData, tRows, tCols > operator-()
Matrix< tData0, tRows0, tColsL+tColsR > operator|(const Matrix< tData0, tRows0, tColsL > &Left, const Matrix< tData0, tRows0, tColsR > &Right)
Matrix< tData, tRows, tCols > operator/=(const Matrix< tData, tRows, tCols > &R)
friend Matrix< tData0, tRows0, tColsL+tColsR > operator|(const Matrix< tData0, tRows0, tColsL > &Left, const Matrix< tData0, tRows0, tColsR > &Right)
void cgetrf_(const int &m, const int &n, std::complex< float > *A, const int &lda, int *ipiv, int *info)
void matalloc(size_t iRowIndex, size_t iColIndex)
friend tData0 norm(const Matrix< tData0, tRows0, 1 > &R)
virtual ~Matrix()
Virtual destructor, no need though.
Matrix< tData, tCols, tRows > trans(const Matrix< tData, tRows, tCols > &R)
friend float det(const Matrix< float, tRows0, tRows0 > &R)
void sgetrf_(const int &m, const int &n, float *A, const int &lda, int *ipiv, int *info)
tData norm(const Matrix< tData, tRows, 1 > &R)
friend Matrix< tData0, tRowsT+tRowsB, tCols0 > operator&(const Matrix< tData0, tRowsT, tCols0 > &Top, const Matrix< tData0, tRowsB, tCols0 > &Bottom)