int main(int argc, char* argv[]) {
//omp_set_num_threads(8);
//=========================================================================================================
ChSystemParallelDVI * system_gpu = new ChSystemParallelDVI;
system_gpu->SetIntegrationType(ChSystem::INT_ANITESCU);
//=========================================================================================================
//ReadInputFile("convergence_config.txt",system_gpu);
//system_gpu->SetMaxiter(max_iter);
//system_gpu->SetIterLCPmaxItersSpeed(max_iter);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationNormal(15);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationSliding(0);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationSpinning(0);
system_gpu->SetTol(tolerance);
system_gpu->SetTolSpeeds(tolerance);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetTolerance(tolerance);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetCompliance( 0);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetContactRecoverySpeed(1000);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetSolverType(APGDRS);
//((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->solver.SetAPGDParams(10, .9, 2.0);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->DoStabilization(false);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->DoCollision(false);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetWarmStart(false);
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->SetCollisionEnvelope(particle_radius * .01);
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->setBinsPerAxis(I3(20, 20, 20));
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->setBodyPerBin(100, 50);
//=========================================================================================================
system_gpu->Set_G_acc(ChVector<>(0, gravity, 0));
system_gpu->SetStep(timestep);
ReadAllObjectsWithGeometryChrono(system_gpu, "dump_sor_heavy_.4.txt");
system_gpu->Get_bodylist()->at(5)->SetMass(100000);
/*
//=========================================================================================================
ChSharedBodyPtr L = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr R = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr F = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr B = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr BTM = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
BLOCK = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedPtr<ChMaterialSurface> material;
material = ChSharedPtr<ChMaterialSurface>(new ChMaterialSurface);
material->SetFriction(plate_friction);
material->SetCompliance(0);
InitObject(L, 100000, Vector(-container_width + container_thickness, plate_height, 0), quat, material, true, true, -20, -20);
InitObject(R, 100000, Vector(container_width - container_thickness, plate_height, 0), quat, material, true, true, -20, -20);
InitObject(F, 100000, Vector(0, plate_height, -container_width + container_thickness), quat, material, true, true, -20, -20);
InitObject(B, 100000, Vector(0, plate_height, container_width - container_thickness), quat, material, true, true, -20, -20);
InitObject(BTM, 100000, Vector(0, plate_height - container_height, 0), quat, material, true, true, -20, -20);
InitObject(BLOCK, 10000, Vector(0, 5-.5, 0), quat, material, true, false, -1, -20);
AddCollisionGeometry(L, BOX, Vector(container_thickness, container_height, container_width), lpos, quat);
AddCollisionGeometry(R, BOX, Vector(container_thickness, container_height, container_width), lpos, quat);
AddCollisionGeometry(F, BOX, Vector(container_width * wscale, container_height, container_thickness), lpos, quat);
AddCollisionGeometry(B, BOX, Vector(container_width * wscale, container_height, container_thickness), lpos, quat);
AddCollisionGeometry(BTM, BOX, Vector(container_width * wscale, container_thickness, container_width), lpos, quat);
AddCollisionGeometry(BLOCK, BOX, Vector(container_width, container_thickness, container_width), lpos, quat);
FinalizeObject(L, (ChSystemParallel *) system_gpu);
FinalizeObject(R, (ChSystemParallel *) system_gpu);
FinalizeObject(F, (ChSystemParallel *) system_gpu);
FinalizeObject(B, (ChSystemParallel *) system_gpu);
FinalizeObject(BTM, (ChSystemParallel *) system_gpu);
FinalizeObject(BLOCK, (ChSystemParallel *) system_gpu);
BLOCK->SetPos_dt(ChVector<>(0,-4,0));
ParticleGenerator<ChSystemParallel> layer_gen(system_gpu);
layer_gen.SetDensity(1000);
layer_gen.SetRadius(R3(particle_radius));
layer_gen.material->SetFriction(1);
layer_gen.material->SetRollingFriction(0);
layer_gen.material->SetSpinningFriction(0);
layer_gen.material->SetCohesion(0);
layer_gen.material->SetCompliance(1e-5);
int3 num_per_dir = I3(2.0 / particle_radius, 4.0 / particle_radius, 2.0 / particle_radius);
layer_gen.AddMixtureType(MIX_SPHERE);
//layer_gen.AddMixtureType(MIX_ELLIPSOID);
//layer_gen.AddMixtureType(MIX_CONE);
//layer_gen.AddMixtureType(MIX_CUBE);
//layer_gen.AddMixtureType(MIX_CYLINDER);
//layer_gen.SetNormalDistribution(rad.x, rad.x/4.0);
//layer_gen->UseNormalCohesion(particle_cohesion, 1);
layer_gen.addPerturbedVolumeMixture(R3(0, -.8, 0), num_per_dir, R3(0.1, .1, 0.1), R3(0, -4, 0));
*/
//=========================================================================================================
//////Rendering specific stuff:
ChOpenGLManager * window_manager = new ChOpenGLManager();
ChOpenGL openGLView(window_manager, system_gpu, 800, 600, 0, 0, "Test_Solvers");
//openGLView.render_camera->camera_position = glm::vec3(0, -5, -40);
//openGLView.render_camera->camera_look_at = glm::vec3(0, -5, 0);
//openGLView.render_camera->camera_scale = .1;
openGLView.SetCustomCallback(RunTimeStep);
openGLView.StartSpinning(window_manager);
window_manager->CallGlutMainLoop();
//=========================================================================================================
//timestep
//iterations
//warm start
//mass
ofstream ofile("convergence.txt");
ChTimer<double> timer;
timer.start();
int file = 0;
for (int i = 0; i < num_steps; i++) {
cout << "step " << i << endl;
system_gpu->DoStepDynamics(timestep);
RunTimeStep(system_gpu, i);
//if (i % save_every == 0) {
ofile << ((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->GetResidual() << endl;
// stringstream ss;
// cout << "Frame: " << file << endl;
// DumpObjects(system_gpu, ss.str());
// //output.ExportData(ss.str());
// file++;
//}
timer.stop();
}
//DumpAllObjectsWithGeometryChrono(system_gpu, "dump.txt");
cout << "TIME: " << timer() << endl;
ofile.close();
return 0;
}
int main(int argc,
char* argv[]) {
omp_set_num_threads(8);
ChSystemParallelDVI * system_gpu = new ChSystemParallelDVI;
system_gpu->SetIntegrationType(ChSystem::INT_ANITESCU);
std::stringstream ss;
ss << "container_checkpoint_50_settled.txt";
ChCollisionSystemBulletParallel * bullet_coll = new ChCollisionSystemBulletParallel();
system_gpu->ChangeCollisionSystem(bullet_coll);
system_gpu->SetStep(timestep);
system_gpu->SetMaxPenetrationRecoverySpeed(10000);
utils::ReadCheckpoint(system_gpu, ss.str());
system_gpu->AssembleSystem();
ChContactContainer* container = (ChContactContainer *) system_gpu->GetContactContainer();
std::vector<ChLcpConstraint*>& mconstraints = system_gpu->GetLcpSystemDescriptor()->GetConstraintsList();
std::vector<ChLcpVariables*>& mvariables = system_gpu->GetLcpSystemDescriptor()->GetVariablesList();
size_t nOfVars = mvariables.size();
size_t nOfConstraints = mconstraints.size();
ChMatrixDynamic<> mb(nOfConstraints, 1);
DynamicVector<double> rhs_vector;
//#########
for (unsigned int ic = 0; ic < nOfConstraints; ic++) {
mconstraints[ic]->Update_auxiliary();
}
// Average all g_i for the triplet of contact constraints n,u,v.
// Can be used for the fixed point phase and/or by preconditioner.
int j_friction_comp = 0;
double gi_values[3];
for (unsigned int ic = 0; ic < nOfConstraints; ic++) {
if (mconstraints[ic]->GetMode() == CONSTRAINT_FRIC) {
gi_values[j_friction_comp] = mconstraints[ic]->Get_g_i();
j_friction_comp++;
if (j_friction_comp == 3) {
double average_g_i = (gi_values[0] + gi_values[1] + gi_values[2]) / 3.0;
mconstraints[ic - 2]->Set_g_i(average_g_i);
mconstraints[ic - 1]->Set_g_i(average_g_i);
mconstraints[ic - 0]->Set_g_i(average_g_i);
j_friction_comp = 0;
}
}
}
for (unsigned int iv = 0; iv < nOfVars; iv++) {
if (mvariables[iv]->IsActive()) {
mvariables[iv]->Compute_invMb_v(mvariables[iv]->Get_qb(), mvariables[iv]->Get_fb()); // q = [M]'*fb
}
}
ChMatrixDynamic<> mb_tmp(nOfConstraints, 1);
int s_i = 0;
for (unsigned int ic = 0; ic < nOfConstraints; ic++)
if (mconstraints[ic]->IsActive()) {
mb(s_i, 0) = -mconstraints[ic]->Compute_Cq_q();
++s_i;
}
system_gpu->GetLcpSystemDescriptor()->BuildBiVector(mb_tmp); // b_i = -c = phi/h
mb.MatrDec(mb_tmp);
ChLcpSystemDescriptor* sysd = system_gpu->GetLcpSystemDescriptor();
ChTimer<double> timer;
timer.start();
//#########
double max_iterations = 212;
double SIZE = nOfConstraints;
double lastgoodres = 10e30;
double theta_k = 1.0;
double theta_k1 = theta_k;
double beta_k1 = 0.0;
double L_k = 0.0;
double t_k = 0.0;
ChMatrixDynamic<> ml(nOfConstraints, 1);
ChMatrixDynamic<> ml_candidate(nOfConstraints, 1);
ChMatrixDynamic<> mg(nOfConstraints, 1);
ChMatrixDynamic<> mg_tmp(nOfConstraints, 1);
ChMatrixDynamic<> my(nOfConstraints, 1);
ChMatrixDynamic<> mx(nOfConstraints, 1);
ChMatrixDynamic<> mg_tmp1(nOfConstraints, 1);
ChMatrixDynamic<> mg_tmp2(nOfConstraints, 1);
ChMatrixDynamic<> ms(nOfConstraints, 1);
ml.FillElem(0);
sysd->ConstraintsProject(ml);
ml_candidate = ml;
sysd->ShurComplementProduct(mg, &ml, 0);
mg = mg - mb;
mb_tmp.FillElem(-1.0);
mb_tmp += ml;
sysd->ShurComplementProduct(mg_tmp, &mb_tmp, 0); // 1) g = N*l ... #### MATR.MULTIPLICATION!!!###
if (mb_tmp.NormTwo() == 0) {
L_k = 1;
} else {
L_k = mg_tmp.NormTwo() / mb_tmp.NormTwo();
}
t_k = 1.0 / L_k;
double obj1 = 0;
double obj2 = 0;
my = ml;
mx = ml;
for (int iter = 0; iter < max_iterations; iter++) {
sysd->ShurComplementProduct(mg_tmp1, &my, 0);
mg = mg_tmp1 - mb;
mx = mg * -t_k + my;
sysd->ConstraintsProject(mx);
sysd->ShurComplementProduct(mg_tmp, &mx, 0);
mg_tmp2 = mg_tmp - mb;
ms = mg_tmp * 0.5 - mb;
obj1 = mx.MatrDot(&mx, &ms);
ms = mg_tmp1 * 0.5 - mb;
obj2 = my.MatrDot(&my, &ms);
ms = mx - my;
while (obj1 > obj2 + mg.MatrDot(&mg, &ms) + 0.5 * L_k * pow(ms.NormTwo(), 2.0)) {
L_k = 2.0 * L_k;
t_k = 1.0 / L_k;
mx = mg * -t_k + my;
sysd->ConstraintsProject(mx);
sysd->ShurComplementProduct(mg_tmp, &mx, 0);
mg_tmp2 = mg_tmp - mb;
ms = mg_tmp * 0.5 - mb;
obj1 = mx.MatrDot(&mx, &ms);
ms = mx - my;
}
theta_k1 = (-pow(theta_k, 2) + theta_k * sqrt(pow(theta_k, 2) + 4)) / 2.0;
beta_k1 = theta_k * (1.0 - theta_k) / (pow(theta_k, 2) + theta_k1);
ms = (mx - ml);
my = ms * beta_k1 + mx;
if (mg.MatrDot(&mg, &ms) > 0) {
my = mx;
theta_k1 = 1.0;
}
L_k = 0.9 * L_k;
t_k = 1.0 / L_k;
ml = mx;
theta_k = theta_k1;
//========
ChMatrixDynamic<> testres(nOfConstraints, 1);
sysd->ShurComplementProduct(testres, &ml, 0);
testres = testres - mb;
double gdiff = .1;
ChMatrixDynamic<> inside = ml - testres * gdiff;
sysd->ConstraintsProject(inside);
ChMatrixDynamic<> resid = (ml - inside) * (1.0 / nOfConstraints * gdiff);
double g_proj_norm = resid.NormTwo();
//========
if (g_proj_norm < lastgoodres) {
lastgoodres = g_proj_norm;
ml_candidate = ml;
}
//========
// f_p = 0.5*l_candidate'*N*l_candidate - l_candidate'*b = l_candidate'*(0.5*Nl_candidate - b);
sysd->ShurComplementProduct(mg_tmp, &ml_candidate, 0); // 1) g_tmp = N*l_candidate ... #### MATR.MULTIPLICATION!!!###
mg_tmp = mg_tmp * 0.5 - mb; // 2) g_tmp = 0.5*N*l_candidate // 3) g_tmp = 0.5*N*l_candidate-b_shur
double m_objective = ml_candidate.MatrDot(&ml_candidate, &mg_tmp); // 4) mf_p = l_candidate'*(0.5*N*l_candidate-b_shur)
//========
double maxdeltalambda = ms.NormInf();
double maxd = lastgoodres;
//cout << " iter=" << iter << " f=" << m_objective << " |d|=" << maxd << " |s|=" << maxdeltalambda << "\n";
}
ml = ml_candidate;
timer.stop();
std::cout << timer() << std::endl;
//#########
std::cout << system_gpu->GetNbodies() << " " << container->GetNcontacts() << std::endl;
return 0;
}