diff --git a/ConSol/ConSol/Utility/vvraEigen.cpp b/ConSol/ConSol/Utility/vvraEigen.cpp new file mode 100644 index 0000000..1a19c41 --- /dev/null +++ b/ConSol/ConSol/Utility/vvraEigen.cpp @@ -0,0 +1,238 @@ +// +// vvraEigen.cpp +// ConSol +// +// Created by Fabian Moenkeberg on 06.09.17. +// Copyright © 2017 EPFL. All rights reserved. +// + +#include "vvraEigen.hpp" +#include +#include +#include "lapacke.h" +#include "cblas.h" + +std::vector vvraEigen(std::function>(std::complex)> myfun, double epsilon, double rad, int K, int n){ + K = K + (K % 2); + Eigen::VectorXd ang(K/2); + Eigen::VectorXcd ei(K/2); + Eigen::VectorXcd e(K/2); + + for (int i = 0; i < K/2; i++){ + ang(i) = (M_PI_2*(double)(2*i+1))/(K); + ei(i) = rad*std::exp(std::complex(0.0, ang(i))); + } + + int m = K - n; + std::vector> W = myfun(ei(0)); + int M = (int) W.size(); + + Eigen::MatrixXcd fv(M,K/2); + std::vector>> E(m, std::vector>(K/2)); + Eigen::MatrixXd ER(m,K); + std::vector>> f(n+1, std::vector>(K/2)); + std::vector>>> F(M, std::vector>>(n, std::vector>(K/2))); + + double* FR = new double[M*n*K]; + double* gr = new double[M*K]; + double* FTb = new double[2*m]; + + Eigen::VectorXd fmax(K/2); + fmax(0) = 0; + + // std::complex one(1.0,0); + for (int j = 0; j < M; j++){ + fv(j,0) = W[j]; + // if (abs(W[j] - one)<1e-14){ + // fv[j][0] = 1; + // } + if (fmax(0) < abs(W[j])){ + fmax(0) = abs(W[j]); + } + } + // fv[0][0] = std::complex(0.093599272232731,0.000050862801134); + // fv[0][1] = std::complex(0.093495734459771,0.000052704090035); + // fv[1][0] = std::complex(0.946639802697000,-0.000000000070552); + // fv[1][1] = std::complex(0.946639802734853,-0.000000000022321); + // fmax[0] = 0.946639802697000; + // fmax[1] = 0.946639802734853; + e(0) = ei(0)*ei(0); + E[0][0] = 1/fmax(0); + + for (int i = 1; i < K/2; i++){ + W = myfun(ei(i)); + fmax(i) = 0; + for (int j = 0; j < M; j++){ + fv(j,i) = W[j]; + // if (abs(W[j] - one)<1e-14){ + // fv[j][i] = 1; + // } + if (fmax(i) < abs(W[j])){ + fmax(i) =abs(W[j]); + } + } + e(i) = ei(i)*ei(i); + + E[0][i] = 1/fmax(i); + } + + for (int j = 1; j < m; j++){ + for (int i = 0; i < K/2; i++){ + E[j][i] = E[j-1][i]*e(i); + } + } + + for( int j = 0; j < n+1; j++){ + f[j] = E[j]; + } + + for (int k = 0; k < M; k++){ + for (int j = 0 ; j < n; j++){ + for ( int i = 0; i < K/2; i++){ + F[k][j][i] = -f[j+1][i]*fv(k,i); + } + } + for ( int i = 0; i < K/2; i++){ + gr[i + k*K] = std::real(f[0][i]*fv(k,i)); + gr[i+ K/2 + k*K] = std::imag(f[0][i]*fv(k,i)); + } + } + + for (int j = 0; j < m; j++){ + for (int i = 0; i < K/2; i++){ + ER(i + j*K) = std::real(E[j][i]); + ER(i + K/2 + j*K) = std::imag(E[j][i]); + } + } + + // std::vector testER(ER, ER + m*K); + + for (int k = 0; k < M; k++){ + for (int j = 0; j < n; j++){ + for(int i = 0; i< K/2; i++){ + FR[i + j*K + k *K*n] = std::real(F[k][j][i]); + FR[i + K/2 + j*K + k *K*n] = std::imag(F[k][j][i]); + } + } + } + // std::vector testFR(FR, FR + M*K*n); + // std::vector testGR0(gr, gr + M*K); + lapack_int lda, mL, nL, info, rank, ldb; + lda = K; + mL = K; + nL = m; + rank = std::min(K,m); + ldb = mL; + double* tau = new double[rank]; + Eigen::ColPivHouseholderQR dec(ER); + // std::vector testER1(ER, ER + m*K); + + // Copy the upper triangular Matrix R (rank x _n) into position + Eigen::MatrixXd R = dec.matrixQR().template triangularView(); + Eigen::MatrixXd Q = dec.matrixQ(); + double * Qd = Q.data(); + // std::vector testR(_R, _R + nL*nL); + + double *ptr = nullptr; + for (int k = 0; k < M; k++){ + ptr = &FR[k*K*n]; + LAPACKE_dormqr(LAPACK_COL_MAJOR, 'L', 'T', mL, n, m, Qd, lda, tau, ptr, ldb); + ptr = &gr[k*K]; + LAPACKE_dormqr(LAPACK_COL_MAJOR, 'L', 'T', mL, 1, m, Qd, lda, tau, ptr, ldb); + } + + double* FT = new double[M*m*n]; + double* FB = new double[M*n*n]; + double* gt = new double[M*m]; + double* gb = new double[M*n]; + + for (int i = 0; i < n; i++){ + for( int j = 0; j < m; j++){ + for (int k = 0; k < M; k++){ + FT[j + i*M*m + k*m] = FR[j + i*K + k*n*K]; + } + } + for( int j = 0; j < n; j++){ + for (int k = 0; k < M; k++){ + FB[j + i*M*n + k*n] = FR[j + m + i*K + k*n*K]; + } + } + } + + for( int j = 0; j < m; j++){ + for (int k = 0; k < M; k++){ + gt[j + k*m] = gr[j + k*K]; + } + } + for( int j = 0; j < n; j++){ + for (int k = 0; k < M; k++){ + gb[j + k*n] = gr[j + m + k*K]; + } + } + + + + // std::vector testgb(gb, gb+ M*n); + // std::vector testgt(gt, gt+ M*m); + // std::vector testFT(FT, FT+ M*n*m); + // std::vector testFB(FB, FB+ M*n*n); + if (fmax.minCoeff()>0){ + lda = 2*n; + ldb = 2*n; + lapack_int nrhs = 1; + + info = LAPACKE_dgels(LAPACK_COL_MAJOR,'N',2*n,n,nrhs,FB,lda,gb,ldb); + + // std::vector testgb2(gb, gb+ n); + lda = 2*m; + ldb = 2*m; + nrhs = 1; + + cblas_dgemv(CblasColMajor, CblasNoTrans, 2*m, n, 1.0, FT, 2*m, gb, 1, 0.0, FTb, 1); + + // std::vector testFTb(FTb, FTb + 2*m); + for (int i = 0; i < 2*m; i++){ + gt[i] -= FTb[i]; + } + + double* _R = R.data(); + info = LAPACKE_dtrtrs(LAPACK_COL_MAJOR, 'U', 'N', 'N', m, M, _R, m, gt, m); + // std::vector testgt2(gt, gt+ M*K); + // int test = testgt2.size(); + } + else{ + for (int i = 0; i < n; i++){ + gb[i] = 1; + } + for ( int i = 0; i < M*m; i++){ + gt[i] = 0; + } + } + + + double denomval = 1; + for (int i = 0; i < n; i++){ + denomval += gb[i]*std::pow(epsilon, i+1); + } + + + std::vector res(M); + for (int i = 0; i < M; i++){ + res[i] = 0; + for (int j = 0; j < m; j++){ + res[i] += gt[j + i*m]*pow(epsilon,j); + } + res[i] /= denomval; + } + + delete [] gb; + delete [] tau; + delete [] FB; + delete [] FT; + delete [] FR; + delete [] gr; + delete [] gt; + delete [] FTb; + + return res; +} diff --git a/ConSol/ConSol/Utility/vvraEigen.hpp b/ConSol/ConSol/Utility/vvraEigen.hpp new file mode 100644 index 0000000..4ed013e --- /dev/null +++ b/ConSol/ConSol/Utility/vvraEigen.hpp @@ -0,0 +1,19 @@ +// +// vvraEigen.hpp +// ConSol +// +// Created by Fabian Moenkeberg on 06.09.17. +// Copyright © 2017 EPFL. All rights reserved. +// + +#ifndef vvraEigen_hpp +#define vvraEigen_hpp + +#include +#include +#include "AbstractFunct.hpp" +#include + +std::vector vvraEigen(std::function>(std::complex)> myfun, double epsilon, double rad, int K, int n); + +#endif /* vvraEigen_hpp */