vec build_q_vec(const Simulator * sim, const Discretizer * disc, double gamma, bool include_oob){ uint N = disc->number_of_all_nodes(); uint n = disc->number_of_spatial_nodes(); // One oob nodes Points points = disc->get_spatial_nodes(); mat costs = sim->get_costs(points); uint A = sim->num_actions(); assert(size(n,A) == size(costs)); if(include_oob){ double tail = 1.0 / (1.0 - gamma); costs = join_vert(costs,tail*ones<rowvec>(A)); assert(size(N,A) == size(costs)); } vec weights = sim->get_state_weights(points); assert(n == weights.n_elem); if(include_oob){ weights = join_vert(weights,zeros<vec>(1)); assert(N == weights.n_elem); } vec q = join_vert(-weights,vectorise(costs)); if(include_oob) assert((A+1)*N == q.n_elem); else assert((A+1)*n == q.n_elem); return q; }
/****************************************************************** * 函数功能:几何校正需要调用的函数 * * */ arma::mat centering(arma::mat &x){ arma::mat tmp = -mean(x.rows(0, 1), 1); arma::mat tmp2(2,2); tmp2.eye(); arma::mat tmp1 = join_horiz(tmp2, tmp); arma::mat v; v << 0 << 0 << 1 << arma::endr; arma::mat T = join_vert(tmp1, v); //T.print("T ="); arma::mat xm = T * x; //xm.print("xm ="); //at least one pixel apart to avoid numerical problems //xm.print("xm ="); double std11 = arma::stddev(xm.row(0)); //cout << "std11:" << std11 << endl; double std22 = stddev(xm.row(1)); //cout << "std22:" << std22 << endl; double std1 = std::max(std11, 1.0); double std2 = std::max(std22, 1.0); arma::mat S; S << 1/std1 << 0 << 0 << arma::endr << 0 << 1/std2 << 0 << arma::endr << 0 << 0 << 1 << arma::endr; arma::mat C = S * T ; //C.print("C ="); return C; }
vec GNR::g_(vec y_){ switch(caso){ case 1: return -solve(J_(y_), f_(x0_) ); case 2: R->DoitKin( join_vert(R->P->q0_, y_), zeros(R->nq) ); return -solve(R->Ao1_ , R->_q_ ); } }
_5R_::_5R_(uint dof, Mecanismo *P, Serial **R_, uint nR_, Col<uint> u_nzi_, mat D_, mat E_, mat F_, vec f_, mat P_, mat Q_, mat S_):Mecanismo(dof){ this->Construct(dof, P, R_, nR_, u_nzi_, D_, E_, F_, f_); this->P_ = P_; this->Q_ = Q_; this->S_ = S_; Ah_ = join_vert(D_, P_); caso = 2; }
/******************************************************************* * 函数功能:几何校正需要调用的函数 * * */ arma::mat toAffinity(arma::mat &f){ arma::mat A; arma::mat v; v << 0 << 0 << 1 << arma::endr; int flag = f.n_rows; switch(flag){ case 6:{ // oriented ellipses arma::mat T = f.rows(0, 1); arma::mat tmp = join_horiz(f.rows(2, 3), f.rows(4, 5)); arma::mat tmp1 = join_horiz(tmp, T); A = join_vert(tmp1, v); break;} case 4:{ // oriented discs arma::mat T = f.rows(0, 1); double s = f.at(2,0); double th = f.at(3,0); arma::mat S = arma::randu<arma::mat>(2,2); /*S.at(0, 0) = s*cos(th); S.at(0, 1) = -s*sin(th); S.at(1, 0) = s*sin(th); S.at(1, 1) = s*cos(th);*/ S << s*cos(th) << -s*sin(th) << arma::endr << s*sin(th) << s*cos(th) << arma::endr; arma::mat tmp1 = join_horiz(S, T); A = join_vert(tmp1, v); //A.print("A ="); break;} /*case 3:{ // discs mat T = f.rows(0, 1); mat s = f.row(2); int th = 0 ; A = [s*[cos(th) -sin(th) ; sin(th) cos(th)], T ; 0 0 1] ; } case 5:{ // ellipses mat T = f.rows(0, 1); A = [mapFromS(f(3:5)), T ; 0 0 1] ; }*/ default: std::cout << "出错啦!" << std::endl; break; } return A; }
/// /// \brief TestSession::GenerateMockData /// \param spectra /// \param abscissa /// \param x /// \param y /// Generate mock spectra /// This is a matrix containing 100 spectra in columns. /// The abscissa will consist of 100 to 4000 cm-1 with 4096 elements /// 50 spectra will have Gaussian peaks /// 50 spectra will have Lorentzian peaks /// For each peak type, there will be 5 unique spectra, each with 5 different baselines /// Peak centers will be uniformly distributed from 200 to 3800 cm-1 /// Peak widths will be uniformly distributed from 5 to 100 cm-1 /// Peak intensities will be uniformly distributed from 1000 to 50000 a.u. /// Basline types: /// * None /// * Typical Raman baseline (generated from linear regression of a quadratic /// function to 100 Si Raman signals) /// * Fluorescent Raman baseline (generated from linear regression of a quadratic /// function to 100 highly-fluorescent Rhodamine-6G Raman signals) /// * Average of typical and fluorescent baseline /// * Linear baseline from 0 to 25000 a.u. /// /// Mock spectra for IR and MS signals will be created in the future void TestSession::GenerateMockData(mat &spectra, vec &abscissa, vec &x, vec &y, mat &peak_data, mat &baselines) { double raman_a0 = 1; double raman_a1 = 1; double raman_a2 = 1; double fluor_a0 = 1; double fluor_a1 = 1; double fluor_a2 = 1; abscissa = linspace(100, 4000, 4096); spectra = zeros(4096, 100); vec coords = linspace(0, 9, 10); y = coords; while (y.n_rows < 100) y = join_vert(y, coords); x = zeros(10); uword x_row = 0; while (x.n_rows < 100) x = join_vert(x, (x_row++) * ones(10)); mat unique_baselines(4096, 5); unique_baselines.col(0) = zeros(4096); unique_baselines.col(1) = GenerateRamanBaseline(raman_a0, raman_a1, raman_a2, abscissa); unique_baselines.col(2) = GenerateRamanBaseline(fluor_a0, fluor_a1, fluor_a2, abscissa); unique_baselines.col(3) = 0.5 * (unique_baselines.col(1), unique_baselines.col(2)); unique_baselines.col(4) = linspace(0, 25000, 4096); vec peak_centers = (3600 * randu<vec>(4096)) + (200 * ones(4096)); vec peak_intensities = (49000 * randu<vec>(4096)) + (1000 * ones(4096)); vec peak_fwhm = (95 * randu<vec>(4096)) + (5 * ones(4096)); for (uword i = 0; i < 50; ++i){ for (uword j = 0; j < 5; ++j){ vec peak_center = peak_centers.col(j); vec peak_intensity = peak_intensities.col(j); vec peak_width = peak_fwhm.col(j); for (uword k = 0; k < 5; ++j){ vec baseline = baselines.col(k); baselines.col(i) = baseline; } } } }
Dy* _5R_::Doit(vec q0_, vec q1_){ this->q0_ = q0_; this->q1_ = q1_; P->Doit(q0_(span(0,P->dof-1)), q1_(span(0,P->dof-1))); uint soma = P->dof; for(uint i=0; i<nR_; i++){ soma += R_[i]->dof; R_[i]->Doit(q0_(span(soma-R_[i]->dof,soma-1)), q1_(span(soma-R_[i]->dof,soma-1)) ); } //M_, v_ e g_ M_ = join_diag(M__, nR_+1); v_ = join_vert(v__, nR_+1); g_ = join_vert(g__, nR_+1); // _q_, Ah_, Ao_, A_, b_, C_ e Z_ _q_ = D_*P->q0_ - E_*join_vert(o__, nR_) - F_*join_vert(q0__, nR_) - f_; switch(caso){ case 1: Ao_ = -E_*join_diag(Jv_n__, nR_) - F_; b_ = E_*join_vert(a_co_n__, nR_); break; case 2: Ao_ = join_vert(-E_*join_diag(Jv_n__, nR_) - F_, -Q_*join_diag(Jw_n__, nR_) - S_); b_ = join_vert(E_*join_vert(a_co_n__, nR_), F_*join_vert(dw_co_n__, nR_)); break; } A_ = join_horiz(Ah_, Ao_); C_ = join_vert( eye(dof,dof), -solve(Ao_,Ah_) ); mat Aux_ = C_.row(u_nzi_(0)); for(uint i=1; i<u_nzi_.n_rows; i++) Aux_ = join_vert(Aux_, C_.row(u_nzi_(i))); Z_ = Aux_; //Z_ = join_vert(C_.row(2), C_.row(4)); // Mh_, vh_ e gh_ dy->Mh_ = solve(Z_.t(), C_.t()*M_*C_); dy->vh_ = solve(Z_.t(), C_.t()*( M_*join_vert(zeros(dof), solve(Ao_, b_) ) + v_ )); dy->gh_ = solve(Z_.t(), C_.t()*g_); return dy; }
/****************************************************************** * 函数功能:几何校正 * * 待写:H_final的值也应该返回去 */ arma::uvec geometricVerification(const arma::mat &frames1, const arma::mat &frames2, const arma::mat &matches, const superluOpts &opts){ // 测试载入是否准确 /*std::cout<< "element测试: " << " x: " << frames1(0,1) << " y: " << frames1(1,1) << std::endl; std::cout << " 行数: " << frames1.n_rows << " 列数:" << frames1.n_cols << std::endl; std::cout << "==========================================================" << std::endl;*/ int numMatches = matches.n_cols; // 测试匹配数目是否准确 /*std::cout << "没有RANSAC前匹配数目: " << numMatches << std::endl; std::cout << "==========================================================" << std::endl;*/ arma::field<arma::uvec> inliers(1, numMatches); arma::field<arma::mat> H(1, numMatches); arma::uvec v = arma::linspace<arma::uvec>(0,1,2); arma::mat onesVector = arma::ones(1, matches.n_cols); arma::uvec matchedIndex_Query = arma::conv_to<arma::uvec>::from(matches.row(0)-1); arma::uvec matchedIndex_Object = arma::conv_to<arma::uvec>::from(matches.row(1)-1); arma::mat x1 = frames1(v, matchedIndex_Query) ; arma::mat x2 = frames2(v, matchedIndex_Object); /*std::cout << " x1查询图像匹配行数: " << x1.n_rows << " 查询图像匹配列数:" << x1.n_cols << std::endl; std::cout << " x2目标图像匹配行数: " << x2.n_rows << " 目标图像匹配列数:" << x2.n_cols << std::endl; std::cout<< "x1 element测试: " << " x: " << x1(0,1) << " y: " << x1(1,1) << std::endl; std::cout<< "x2 element测试: " << " x: " << x2(0,1) << " y: " << x2(1,1) << std::endl; std::cout << "==========================================================" << std::endl;*/ arma::mat x1hom = arma::join_cols(x1, arma::ones<arma::mat>(1, numMatches)); //在下面添加一行,注意和join_rows的区别 arma::mat x2hom = arma::join_cols(x2, arma::ones<arma::mat>(1, numMatches)); /*std::cout << " x1hom查询图像匹配行数: " << x1hom.n_rows << " 查询图像匹配列数:" << x1hom.n_cols << std::endl; std::cout<< "x1hom element测试: " << " x: " << x1hom(0,1) << " y: " << x1hom(1,1) << " z: " << x1hom(2,1) << std::endl; std::cout << "==========================================================" << std::endl;*/ arma::mat x1p, H21; //作用域 double tol; for(int m = 0; m < numMatches; ++m){ //cout << "m: " << m << endl; for(unsigned int t = 0; t < opts.numRefinementIterations; ++t){ //cout << "t: " << t << endl; if (t == 0){ arma::mat tmp1 = frames1.col(matches(0, m)-1); arma::mat A1 = toAffinity(tmp1); //A1.print("A1 ="); arma::mat tmp2 = frames2.col(matches(1, m)-1); arma::mat A2 = toAffinity(tmp2); //A2.print("A2 ="); H21 = A2 * inv(A1); //H21.print("H21 ="); x1p = H21.rows(0, 1) * x1hom ; //x1p.print("x1p ="); tol = opts.tolerance1; }else if(t !=0 && t <= 3){ arma::mat A1 = x1hom.cols(inliers(0, m)); arma::mat A2 = x2.cols(inliers(0, m)); //A1.print("A1 ="); //A2.print("A2 ="); H21 = A2*pinv(A1); //H21.print("H21 ="); x1p = H21.rows(0, 1) * x1hom ; //x1p.print("x1p ="); arma::mat v; v << 0 << 0 << 1 << arma::endr; H21 = join_vert(H21, v); //H21.print("H21 ="); //x1p.print("x1p ="); tol = opts.tolerance2; }else{ arma::mat x1in = x1hom.cols(inliers(0, m)); arma::mat x2in = x2hom.cols(inliers(0, m)); arma::mat S1 = centering(x1in); arma::mat S2 = centering(x2in); arma::mat x1c = S1 * x1in; //x1c.print("x1c ="); arma::mat x2c = S2 * x2in; //x2c.print("x2c ="); arma::mat A1 = arma::randu<arma::mat>(x1c.n_rows ,x1c.n_cols); A1.zeros(); arma::mat A2 = arma::randu<arma::mat>(x1c.n_rows ,x1c.n_cols); A2.zeros(); arma::mat A3 = arma::randu<arma::mat>(x1c.n_rows ,x1c.n_cols); A3.zeros(); for(unsigned int i = 0; i < x1c.n_cols; ++i){ A2.col(i) = x1c.col(i)*(-x2c.row(0).col(i)); A3.col(i) = x1c.col(i)*(-x2c.row(1).col(i)); } arma::mat T1 = join_cols(join_horiz(x1c, A1), join_horiz(A1, x1c)); arma::mat T2 = join_cols(T1, join_horiz(A2, A3)); //T2.print("T2 ="); arma::mat U; arma::vec s; arma::mat V; svd_econ(U, s, V, T2); //U.print("U ="); //V.print("V ="); arma::vec tmm = U.col(U.n_cols-1); H21 = reshape(tmm, 3, 3).t(); H21 = inv(S2) * H21 * S1; H21 = H21 / H21(H21.n_rows-1, H21.n_cols-1) ; //H21.print("H21 ="); arma::mat x1phom = H21 * x1hom ; arma::mat cc1 = x1phom.row(0) / x1phom.row(2); arma::mat cc2 = x1phom.row(1) / x1phom.row(2); arma::mat x1p = join_cols(cc1, cc2); //x1p.print("x1p ="); tol = opts.tolerance3; } arma::mat tmp = arma::square(x2 - x1p); //精度跟matlab相比更高? //tmp.print("tmp ="); arma::mat dist2 = tmp.row(0) + tmp.row(1); //dist2.print("dist2 ="); inliers(0, m) = arma::find(dist2 < pow(tol, 2)); H(0, m) = H21; //H(0, m).print("H(0, m) ="); //inliers(0, m).print("inliers(0, m) ="); //cout << inliers(0, m).size() << endl; //cout << "==========================================================" << endl; if (inliers(0, m).size() < opts.minInliers) break; if (inliers(0, m).size() > 0.7 * numMatches) break; } } arma::uvec scores(numMatches); for(int i = 0; i < numMatches; ++i){ scores.at(i) = inliers(0, i).n_rows; } //scores.print("scores = "); arma::uword index; scores.max(index); //cout << index << endl; arma::mat H_final = inv(H(0, index)); //H_final.print("H_final = "); arma::uvec inliers_final = inliers(0, index); //inliers_final.print("inliers_final = "); return inliers_final; }
SolverResult KojimaSolver::solve(const LCP & lcp, vec & x, vec & y) const{ superlu_opts opts; opts.equilibrate = true; opts.permutation = superlu_opts::COLAMD; opts.refine = superlu_opts::REF_SINGLE; vec q = lcp.q; sp_mat M = lcp.M + regularizer * speye(size(lcp.M)); uint N = q.n_elem; assert(N == x.n_elem); assert(N == y.n_elem); // Figure out what is free (-inf,inf) // and what is bound to be non-negative [0,inf) bvec free_vars = lcp.free_vars; uvec bound_idx = find(0 == free_vars); uvec free_idx = find(1 == free_vars); assert(N == bound_idx.n_elem + free_idx.n_elem); uint NB = bound_idx.n_elem; // number of bound vars uint NF = free_idx.n_elem; // number of free vars /* In what follows, the primal variables x are partitioned into free variables and bound variables x = [f;b]' Likewise, the dual variables are partitioned into y = [0,s]' */ /* The Newton system: [M_ff M_fb 0][df] [M_f x + q_f] [M_bf M_bb -I][db] + [M_b x + q_b - s] [0 S B][dv] [u1 + VBe] Where "M_ff" is the free-free block Overwrite the S,B diagonals every iteration*/ // Split M matrix into blocks based on free and bound indicies block_sp_mat M_part = sp_partition(M,free_idx,bound_idx); sp_mat M_recon = block_mat(M_part); assert(PRETTY_SMALL > norm(M_recon - M)); vec qf = q(free_idx); vec qb = q(bound_idx); vec b = x(bound_idx); vec f = x(free_idx); vec s = y(bound_idx); // Build the Newton matrix vector<vector<sp_mat>> block_G; block_G.push_back(block_sp_vec{sp_mat(),sp_mat(),sp_mat(NB,NB)}); block_G.push_back(block_sp_vec{-M_part[0][0],-M_part[0][1],sp_mat()}); block_G.push_back(block_sp_vec{-M_part[1][0],-M_part[1][1],speye(NB,NB)}); // Start iteration double mean_comp, steplen; double sigma = initial_sigma; uint iter; for(iter = 0; iter < max_iter; iter++){ if(verbose or iter_verbose) cout << "---Iteration " << iter << "---" << endl; assert(all(0 == y(free_idx))); // Mean complementarity mean_comp = dot(b,s) / (double) NB; if(mean_comp < comp_thresh) break; block_G[0][1] = spdiag(s); block_G[0][2] = spdiag(b); sp_mat G = block_mat(block_G); assert(size(N + NB,N + NB) == size(G)); // Form RHS from residual and complementarity vec h = vec(N + NB); vec res_f = M_part[0][0]*f + M_part[0][1]*b + qf; vec res_b = M_part[1][0]*f + M_part[1][1]*b + qb - s; h.head(NB) = sigma * mean_comp - b % s; h.subvec(NB,size(res_f)) = res_f; h.tail(NB) = res_b; //Archiver arch; //arch.add_sp_mat("G",G); //arch.add_vec("h",h); //arch.write("test.sys"); // Solve and extract directions vec dir = spsolve(G,h,"superlu",opts); assert((N+NB) == dir.n_elem); vec df = dir.head(NF); vec db = dir.subvec(NF,N-1); assert(NB == db.n_elem); vec ds = dir.tail(NB); vec dir_recon = join_vert(df,join_vert(db,ds)); assert(PRETTY_SMALL > norm(dir_recon-dir)); steplen = steplen_heuristic(b,s,db,ds,0.9); sigma = sigma_heuristic(sigma,steplen); f += steplen * df; b += steplen * db; s += steplen * ds; if(verbose){ double res = norm(join_vert(res_f,res_b)); cout <<"\t Mean complementarity: " << mean_comp <<"\n\t Residual norm: " << res <<"\n\t |df|: " << norm(df) <<"\n\t |db|: " << norm(db) <<"\n\t |ds|: " << norm(ds) <<"\n\t Step length: " << steplen <<"\n\t Centering sigma: " << sigma << endl; } } if(verbose){ cout << "Finished" <<"\n\t Final mean complementarity: " << mean_comp << endl; } x(free_idx) = f; x(bound_idx) = b; y(free_idx).fill(0); y(bound_idx) = s; return SolverResult(x,y,iter); }
SolverResult ProjectiveSolver::solve(const PLCP & plcp, vec & x, vec & y, vec & w) const{ sp_mat P = plcp.P; sp_mat U = plcp.U; vec q = plcp.q; uint N = P.n_rows; uint K = P.n_cols; assert(size(P.t()) == size(U)); bvec free_vars = plcp.free_vars; uvec bound_idx = find(0 == free_vars); uvec free_idx = find(1 == free_vars); assert(N == bound_idx.n_elem + free_idx.n_elem); uint NB = bound_idx.n_elem; // number of bound vars uint NF = free_idx.n_elem; // number of free vars assert(NF == accu(conv_to<uvec>::from(free_vars))); assert(N == x.n_elem); assert(N == y.n_elem); assert(K == w.n_elem); assert(ALMOST_ZERO > norm(y(free_idx))); if(verbose) cout << "Free variables: \t" << NF << endl << "Non-neg variables:\t" << NB << endl; sp_mat J = join_vert(sp_mat(NF,NB),speye(NB,NB)); assert(size(N,NB) == size(J)); assert(PRETTY_SMALL > norm(y(free_idx))); if(verbose) cout << "Forming pre-computed products..." << endl; mat PtP = mat(P.t() * P); assert(size(K,K) == size(PtP)); mat PtPU = PtP*U; assert(size(K,N) == size(PtPU)); mat Pt_PtPU = P.t() - PtPU; assert(size(K,N) == size(Pt_PtPU)); mat PtPUP = PtPU * P; assert(size(K,K) == size(PtPUP)); vec Ptq = P.t() * q; if(verbose) cout << "Done..." << endl; double sigma = initial_sigma; uint iter; double mean_comp; for(iter = 0; iter < max_iter; iter++){ if(verbose or iter_verbose) cout << "---Iteration " << iter << "---" << endl; // Mean complementarity vec s = y(bound_idx); vec b = x(bound_idx); mean_comp = dot(s,b) / (double) NB; if(mean_comp < comp_thresh) break; // Generate reduced Netwon system mat C = s+b; vec g = sigma * mean_comp - s % b; assert(NB == g.n_elem); // NB: A,G,and h have opposite sign from python version mat A = Pt_PtPU * J * spdiag(1.0 / C); assert(size(K,NB) == size(A)); mat G = PtPUP + (A * spdiag(s)) * J.t() * P; assert(size(K,K) == size(G)); vec Ptr = P.t() * (J * s) - PtPU*x - Ptq; vec h = Ptr + A*g; assert(K == h.n_elem); // Options don't make much difference vec dw = arma::solve(G+1e-15*eye(K,K),h, solve_opts::equilibrate); assert(K == dw.n_elem); // Recover dy vec Pdw = P * dw; vec JtPdw = J.t() * Pdw; assert(NB == JtPdw.n_elem); vec ds = (g - s % JtPdw) / C; assert(NB == ds.n_elem); // Recover dx vec dx = (J * ds) + (Pdw); assert(N == dx.n_elem); double steplen = steplen_heuristic(x(bound_idx),s,dx(bound_idx),ds,0.9); sigma = sigma_heuristic(sigma,steplen); x += steplen * dx; s += steplen * ds; y(bound_idx) = s; w += steplen * dw; if(verbose){ cout <<"\tMean complementarity: " << mean_comp <<"\n\tStep length: " << steplen <<"\n\tCentering sigma: " << sigma << endl; } } if(verbose){ cout << "Finished" <<"\n\t Final mean complementarity: " << mean_comp << endl; } return SolverResult(x,y,iter); }
int main(){ Mecanismo P = Mecanismo(2); cube I1__; I1__.zeros(3,3,2); I1__.slice(0) << 0 << 0 << 0 << endr << 0 << 107.307e-6 + 146.869e-6 << 0 << endr << 0 << 0 << 107.307e-6 + 146.869e-6 << endr; I1__.slice(1) << 0 << 0 << 0 << endr << 0 << 438.0e-6 << 0 << endr << 0 << 0 << 438.0e-6 << endr; cube I2__; I2__.zeros(3,3,2); I2__.slice(0) << 0 << 0 << 0 << endr << 0 << 107.307e-6 + 188.738e-6 << 0 << endr << 0 << 0 << 107.307e-6 + 188.738e-6 << endr; I2__.slice(1) << 0 << 0 << 0 << endr << 0 << 301.679e-6 << 0 << endr << 0 << 0 << 301.679e-6 << endr; Serial RR1 = Serial(2, {0.12, 0.16}, {0.06, 0.078},{0.062, 0.124}, I1__ , {0, 0, 9.8}, &fDH_RR); Serial RR2 = Serial(2, {0.12, 0.16}, {0.06, 0.058},{0.062, 0.097}, I2__ , {0, 0, 9.8}, &fDH_RR); Serial **RR_ = new Serial* [2]; RR_[0] = &RR1; RR_[1] = &RR2; //Matrizes que descrevem a arquitetura do mecanismo double l0 = 0.05; mat D_ = join_vert((mat)eye(2,2),2); mat E_ = join_diag( Roty(0)(span(0,1),span(0,2)), Roty(PI)(span(0,1),span(0,2)) ); mat F_ = zeros(4,4); vec f_ = {l0,0,-l0,0}; Parallel Robot = Parallel(2, &P, RR_, 2, {2,4}, D_, E_, F_, f_); Reference RefObj = Reference(0.12, {0.08, 0.16}, {-0.08, 0.4}); //Plotar área de trabalho uint nx = 96.0; uint ny = 56.0; double lx = 0.24; double ly = 0.28; double xi = -lx; double xf = lx; double yi = 0.0; double yf = ly; double dx = (xf-xi)/(nx-1); double dy = (yf-yi)/(ny-1); double dl = 0.5*(dx+dy); Mat<int> M; M.zeros(nx,ny); field<mat> fZ_(nx,ny); field<mat> fMh_(nx,ny); field<vec> fgh_(nx,ny); field<vec> fa1_(nx,ny); field<vec> fa2_(nx,ny); field<vec> fa12_(nx,ny); for(uint i=0; i<nx; i++){ for(uint j=0; j<ny; j++){ fZ_(i,j).zeros(2,2); fMh_(i,j).zeros(2,2); fgh_(i,j).zeros(2); fa1_(i,j).zeros(2); fa2_(i,j).zeros(2); fa12_(i,j).zeros(2); } } vec v1_ = {1,0}; vec v2_ = {0,1}; vec v12_ = {1,1}; double r = 0.07; double x0 = 0.0; double y0 = 0.17; uint rows = nx; uint cols = ny; vec q0_ = {0.823167, 1.81774, 0.823167, 1.81774}; GNR2 gnr2 = GNR2("RK6", &Robot, 1e-6, 30); //gnr2.Doit(q0_, {0.05,0.08}); //cout << gnr2.convergiu << endl; //cout << gnr2.x_ << endl; //cout << gnr2.res_ << endl; //cout << gnr2.n << endl; double x; double y; mat A2_; for(uint i=0; i<rows; i++){ for(uint j=0; j<cols; j++){ x = xi + i*dx; y = yi + j*dy; gnr2.Doit(q0_, {x, y}); if(gnr2.convergiu){ q0_ = gnr2.x_; A2_ = join_horiz(Robot.Ah_, join_horiz(Robot.Ao_.col(1), Robot.Ao_.col(3)) ); if(abs(det(Robot.Ao_)) < 1.6*1e-6 || abs(det(A2_)) < 1e-11 ) M(i,j) = 2; else{ M(i,j) = 1; Robot.Doit(Robot.q0_, join_vert(v1_, -solve(Robot.Ao_, Robot.Ah_*v1_)) ); fZ_(i,j) = Robot.Z_; fMh_(i,j) = Robot.dy->Mh_; fgh_(i,j) = Robot.dy->gh_; fa1_(i,j) = Robot.dy->vh_; Robot.Doit(Robot.q0_, Robot.C_*v2_); fa2_(i,j) = Robot.dy->vh_; Robot.Doit(Robot.q0_, Robot.C_*v12_); fa12_(i,j) = Robot.dy->vh_ - fa1_(i,j) - fa2_(i,j); } } gnr2.convergiu = false; if( ((x - x0)*(x - x0) + (y - y0)*(y - y0) <= (r+dl)*(r+dl) ) && ((x - x0)*(x - x0) + (y - y0)*(y - y0) >= (r-dl)*(r-dl) ) && (M(i,j) != 2) ) M(i,j) = 3; } } for(uint i=0; i<rows; i++){ for(uint j=0; j<cols; j++){ cout << M(i,j) << ";" ; if(j==cols-1) cout << endl; } } fZ_.save("fZ_field"); fMh_.save("fMh_field"); fgh_.save("fgh_field"); fa1_.save("fa1_field"); fa2_.save("fa2_field"); fa12_.save("fa12_field"); return 0; }
int dtq::compProp(void) { // need myh to proceed if (! haveMyh) return 1; // fundamental size ylen = 2*bigm+1; // start computing the propagator! prop = arma::sp_mat(ylen,ylen); // create yvec yvec = arma::zeros(ylen); for (int i=-bigm; i<=bigm; i++) yvec(bigm+i) = i*myk; // apply f and g to yvec fy = arma::zeros(ylen); gy = arma::zeros(ylen); for (int i=-bigm; i<=bigm; i++) { fy(bigm+i) = (*f)(yvec(bigm+i),curtheta); gy(bigm+i) = (*g)(yvec(bigm+i),curtheta); } // normalization "constant" arma::vec c0mod = 1.0/(sqrt(2.0*(arma::datum::pi)*myh)*gy); // variance arma::vec gy2 = gy % gy; arma::vec myvar = gy2*myh; // compute and set main diagonal // prop.diag() = maindiag; arma::vec propvals = arma::exp(-(myh/2.0)*(fy%fy)/gy2) % c0mod; arma::umat proploc(2, ylen); proploc.row(0) = arma::regspace<arma::urowvec>(0, (ylen-1)); proploc.row(1) = arma::regspace<arma::urowvec>(0, (ylen-1)); // superdiagonals bool done = false; int curdiag = 1; double mytol = 2.0e-16; double refsum = arma::sum(arma::abs(propvals))*myk; while (! done) { arma::vec mymean = curdiag*myk + fy*myh; arma::vec thisdiag = arma::exp(-mymean%mymean/(2.0*myvar))%c0mod; thisdiag = thisdiag.tail(ylen - curdiag); double thissum = arma::sum(arma::abs(thisdiag)); if ((curdiag == 1) || (thissum > mytol*refsum)) { // prop.diag(curdiag) = thisdiag; arma::umat newloc(2, ylen-curdiag); newloc.row(0) = arma::regspace<arma::urowvec>(0, (ylen-curdiag-1)); newloc.row(1) = newloc.row(0) + curdiag; proploc = join_horiz(proploc, newloc); propvals = join_vert(propvals, thisdiag); curdiag++; if (curdiag == ylen) done = true; } else done = true; } int maxdiag = curdiag; for (curdiag=1; curdiag<=maxdiag; curdiag++) { arma::vec mymean = -curdiag*myk + fy*myh; arma::vec thisdiag = arma::exp(-mymean%mymean/(2.0*myvar))%c0mod; thisdiag = thisdiag.head(ylen - curdiag); // prop.diag(-curdiag) = thisdiag; arma::umat newloc(2, ylen-curdiag); newloc.row(1) = arma::regspace<arma::urowvec>(0, (ylen-curdiag-1)); newloc.row(0) = newloc.row(1) + curdiag; proploc = join_horiz(proploc, newloc); propvals = join_vert(propvals, thisdiag); } prop = arma::sp_mat(proploc, propvals, ylen, ylen); // check normalization, should get all 1's // std::cout << myk*sum(prop,0) << '\n'; haveProp = true; return 0; }