int main(int argc, char** argv)
{
arma_rng::set_seed_random();
// Set up 2D space
cout << "Generating initial mesh..." << endl;
TriMesh mesh = generate_initial_mesh();
Points points = mesh.get_spatial_nodes();
uint N = points.n_rows;
assert(N == mesh.number_of_spatial_nodes());
assert(N > 0);
DoubleIntegratorSimulator di = build_di_simulator();
uint A = di.num_actions();
assert(A >= 2);
// Reference blocks
cout << "Building LCP blocks..." << endl;
vector<sp_mat> blocks = di.lcp_blocks(&mesh,GAMMA);
assert(A == blocks.size());
assert(size(N,N) == size(blocks.at(0)));
// Build smoother
cout << "Building smoother matrix..." << endl;
sp_mat smoother = gaussian_smoother(points,SMOOTH_BW,SMOOTH_THRESH);
assert(size(N,N) == size(smoother));
// Build and pertrub the q
cout << "Building RHS Q..." << endl;
mat Q = di.q_mat(&mesh);
bvec free_vars = zeros<bvec>((A+1)*N);
free_vars.head(N).fill(1);
cout << "Making value basis..." << endl;
sp_mat value_basis = make_value_basis(points);
cout << "Building reference approximate PLCP..." << endl;
PLCP ref_plcp = approx_lcp(value_basis,smoother,blocks,Q,free_vars);
ProjectiveSolver psolver = ProjectiveSolver();
psolver.comp_thresh = 1e-8;
psolver.initial_sigma = 0.25;
psolver.verbose = false;
psolver.iter_verbose = false;
cout << "Starting reference augmented PLCP solve..." << endl;
SolverResult ref_sol = psolver.aug_solve(ref_plcp);
mat ref_P = reshape(ref_sol.p,N,A+1);
vec ref_V = ref_P.col(0);
vec ref_res = bellman_residual_at_nodes(&mesh,&di,ref_V,GAMMA);
vec ref_res_norm = vec{norm(ref_res,1),norm(ref_res,2),norm(ref_res,"inf")};
//////////////////////////
// SETUP FOR EXPERIMENT //
//////////////////////////
vec bandwidths = logspace<vec>(-3,1,NUM_BW); // 0.01 to 1
uint num_points = bandwidths.n_elem;
mat res_diff = mat(num_points,3);
for(uint i = 0; i < num_points; i++){
cout << "Running " << i << "/" << num_points << endl;
mat new_vects = mat(N,2);
new_vects.col(0) = gaussian(points,NEW_CENTER,bandwidths(i));
new_vects.col(1) = gaussian(points,-NEW_CENTER,bandwidths(i));
sp_mat new_basis = sp_mat(orth(join_horiz(mat(value_basis),
new_vects)));
PLCP plcp = approx_lcp(new_basis,smoother,
blocks,Q,free_vars);
SolverResult sol = psolver.aug_solve(plcp);
// Interpret results
mat P = reshape(sol.p,N,A+1);
vec V = P.col(0);
vec res = bellman_residual_at_nodes(&mesh,&di,V,GAMMA);
vec res_norm = vec{norm(res,1),norm(res,2),norm(res,"inf")};
res_diff.row(i) = (res_norm - ref_res_norm).t();
}
mesh.write_cgal("test.mesh");
Archiver arch = Archiver();
arch.add_vec("ref_res",ref_res);
arch.add_mat("res_diff",res_diff);
arch.add_mat("bandwidths",bandwidths);
arch.write("test.data");
}
////////////////////////////////////////////////////////////
// MAIN FUNCTION ///////////////////////////////////////////
////////////////////////////////////////////////////////////
int main(int argc, char** argv)
{
po::variables_map var_map = read_command_line(argc,argv);
arma_rng::set_seed_random();
// Read in the CGAL mesh
TriMesh mesh;
string file_base = var_map["outfile_base"].as<string>();
double edge_length = var_map["edge_length"].as<double>();
generate_minop_mesh(mesh,file_base,edge_length);
mesh.freeze();
// Stats
uint N = mesh.number_of_vertices();
uint F = mesh.number_of_faces();
cout << "Mesh stats:"
<< "\n\tNumber of vertices: " << N
<< "\n\tNumber of faces: " << F
<< endl;
// Build value basis
uint num_value_basis = 25;
uint num_flow_basis = 10;
Points points = mesh.get_spatial_nodes();
cout << "Generating Radial Fourier basis for value..." << endl;
mat value_basis = make_radial_fourier_basis(points,
num_value_basis,
(double)num_value_basis);
cout << "\tOrthogonalizing..." << endl;
value_basis = orth(value_basis);
cout << "Generating Voronoi basis for flow..." << endl;
sp_mat sp_value_basis = sp_mat(value_basis);
Points centers = 2 * randu(10,2) - 1;
mat flow_basis = make_voronoi_basis(points,
centers);
cout << "\tOrthogonalizing..." << endl;
flow_basis = orth(flow_basis);
sp_mat sp_flow_basis = sp_mat(flow_basis);
cout << "Building LCP..." << endl;
vec ref_weights = ones<vec>(N) / (double)N;
LCP ref_lcp;
vec ans;
build_minop_lcp(mesh,ref_weights,ref_lcp,ans);
assert(N == ans.n_elem);
cout << "Building PLCP..." << endl;
block_sp_vec D = {sp_value_basis,
sp_flow_basis,
sp_flow_basis};
sp_mat P = block_diag(D);
double regularizer = 1e-12;
sp_mat U = P.t() * (ref_lcp.M + regularizer*speye(size(ref_lcp.M)));
vec q = P *(P.t() * ref_lcp.q);
assert(3*N == P.n_rows);
assert(3*N == q.n_rows);
bvec free_vars = zeros<bvec>(3*N);
free_vars.head(N).fill(1);
PLCP ref_plcp = PLCP(P,U,q,free_vars);
ref_plcp.write(file_base + ".plcp");
ProjectiveSolver psolver;
psolver.comp_thresh = 1e-12;
psolver.max_iter = 250;
psolver.aug_rel_scale = 5;
psolver.verbose = false;
psolver.initial_sigma = 0.3;
cout << "Starting reference solve..." << endl;
SolverResult ref_sol = psolver.aug_solve(ref_plcp);
cout << "\tDone." << endl;
cout << "Reference solution error: "
<< norm(ans - ref_sol.p.head(N)) << endl;
assert(ALMOST_ZERO > norm(ref_sol.d.head(N))); // Essentially zero
ref_sol.write(file_base + ".sol");
psolver.comp_thresh = 1e-8;
// Exactish
vec twiddle = vec(N);
for(uint i = 0; i < N; i++){
cout << "Component: " << i << endl;
LCP twiddle_lcp;
vec et = zeros<vec>(N);
et(i) += 1.0 / (double) N;
assert(size(ref_weights) == size(et));
build_minop_lcp(mesh,ref_weights + et,twiddle_lcp,ans);
vec twiddle_q = P *(P.t() * twiddle_lcp.q);
PLCP twiddle_plcp = PLCP(P,U,twiddle_q,free_vars);
SolverResult twiddle_sol = psolver.aug_solve(twiddle_plcp);
twiddle(i) = twiddle_sol.p(i) - ref_sol.p(i);
}
uint R = 75;
mat jitter = mat(N,R);
mat noise = mat(N,R);
for(uint i = 0; i < R; i++){
cout << "Jitter round: " << i << endl;
LCP jitter_lcp;
noise.col(i) = max(1e-4*ones<vec>(N), 0.075 * randn<vec>(N));
build_minop_lcp(mesh,ref_weights + noise.col(i),jitter_lcp,ans);
vec jitter_q = P *(P.t() * jitter_lcp.q);
PLCP jitter_plcp = PLCP(P,U,jitter_q,free_vars);
SolverResult jitter_sol = psolver.aug_solve(jitter_plcp);
jitter.col(i) = (jitter_sol.p.head(N) - ref_sol.p.head(N));
}
Archiver arch;
arch.add_vec("p",ref_sol.p.head(N));
arch.add_vec("twiddle",twiddle);
arch.add_mat("jitter",jitter);
arch.add_mat("noise",noise);
arch.add_sp_mat("flow_basis",sp_flow_basis);
arch.write(file_base + ".exp_res");
}
