void ompl::geometric::LBKPIECE1::removeMotion(Discretization<Motion> &disc, Motion *motion) { /* remove from grid */ Discretization<Motion>::Coord coord; projectionEvaluator_->computeCoordinates(motion->state, coord); disc.removeMotion(motion, coord); /* remove self from parent list */ if (motion->parent) { for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i) if (motion->parent->children[i] == motion) { motion->parent->children.erase(motion->parent->children.begin() + i); break; } } /* remove children */ for (unsigned int i = 0 ; i < motion->children.size() ; ++i) { motion->children[i]->parent = NULL; removeMotion(disc, motion->children[i]); } freeMotion(motion); }
void ompl::control::KPIECE1::freeCellData(CellData *cdata) { for (unsigned int i = 0 ; i < cdata->motions.size() ; ++i) freeMotion(cdata->motions[i]); delete cdata; }
void CudaMasterContactSolver<real>::step(const core::ExecParams* params /* PARAMS FIRST */, double dt) { sofa::helper::AdvancedTimer::stepBegin("AnimationStep"); context = dynamic_cast<simulation::Node *>(this->getContext()); // access to current node #ifdef DISPLAY_TIME CTime *timer; double timeScale = 1.0 / (double)CTime::getRefTicksPerSec(); timer = new CTime(); double time_Free_Motion = (double) timer->getTime(); #endif // Update the BehaviorModels // Required to allow the RayPickInteractor interaction simulation::BehaviorUpdatePositionVisitor updatePos(params /* PARAMS FIRST */, dt); context->execute(&updatePos); simulation::MechanicalBeginIntegrationVisitor beginVisitor(params /* PARAMS FIRST */, dt); context->execute(&beginVisitor); // Free Motion simulation::SolveVisitor freeMotion(params /* PARAMS FIRST */, dt, true); context->execute(&freeMotion); //simulation::MechanicalPropagateFreePositionVisitor().execute(context); { sofa::core::MechanicalParams mparams(*params); sofa::core::MultiVecCoordId xfree = sofa::core::VecCoordId::freePosition(); mparams.x() = xfree; simulation::MechanicalPropagatePositionVisitor(&mparams /* PARAMS FIRST */, 0, xfree, true).execute(context); } //core::VecId dx_id = (VecId)core::VecDerivId::dx(); //simulation::MechanicalVOpVisitor(params /* PARAMS FIRST */, dx_id).execute( context); //simulation::MechanicalPropagateDxVisitor(params /* PARAMS FIRST */, dx_id, true).execute( context); //ignore the masks (is it necessary?) //simulation::MechanicalVOpVisitor(params /* PARAMS FIRST */, dx_id).execute( context); #ifdef DISPLAY_TIME time_Free_Motion = ((double) timer->getTime() - time_Free_Motion)*timeScale; double time_computeCollision = (double) timer->getTime(); #endif computeCollision(); #ifdef DISPLAY_TIME time_computeCollision = ((double) timer->getTime() - time_computeCollision)*timeScale; double time_build_LCP = (double) timer->getTime(); #endif //MechanicalResetContactForceVisitor().execute(context); for (unsigned int i=0;i<constraintCorrections.size();i++) { core::behavior::BaseConstraintCorrection* cc = constraintCorrections[i]; cc->resetContactForce(); } build_LCP(); #ifdef DISPLAY_TIME time_build_LCP = ((double) timer->getTime() - time_build_LCP)*timeScale; double time_solve_LCP = (double) timer->getTime(); #endif double _tol = tol.getValue(); int _maxIt = maxIt.getValue(); if (! initial_guess.getValue()) _f.clear(); double error = 0.0; #ifdef CHECK real t1,t2; if (_mu > 0.0) { f_check.resize(_numConstraints,MBSIZE); for (unsigned i=0;i<_numConstraints;i++) f_check[i] = _f[i]; real toln = ((int) (_realNumConstraints/3) + 1) * (real)_tol; t2 = sofa::gpu::cuda::CudaLCP<real>::CudaNlcp_gaussseidel(useGPU_d.getValue(),_numConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), f_check.getCudaVector(), _mu,_toln, _maxIt); t1 = sofa::gpu::cuda::CudaLCP<real>::CudaNlcp_gaussseidel(0,_numConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), _f.getCudaVector(), _mu,_toln, _maxIt); } else { f_check.resize(_numConstraints); for (unsigned i=0;i<_numConstraints;i++) f_check[i] = _f[i]; t2 = sofa::gpu::cuda::CudaLCP<real>::CudaGaussSeidelLCP1(useGPU_d.getValue(),_numConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), f_check.getCudaVector(), _tol, _maxIt); t1 = sofa::gpu::cuda::CudaLCP<real>::CudaGaussSeidelLCP1(0,_numConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), _f.getCudaVector(), _tol, _maxIt); } for (unsigned i=0;i<f_check.size();i++) { if ((f_check.element(i)-_f.element(i)>CHECK) || (f_check.element(i)-_f.element(i)<-CHECK)) { std::cerr << "Error(" << useGPU_d.getValue() << ") dim(" << _numConstraints << ") realDim(" << _realNumConstraints << ") elmt(" << i << ") : (cpu," << f_check.element(i) << ") (gpu,(" << _f.element(i) << ")" << std::endl; } } #else sout << "numcontacts = " << _realNumConstraints << " RealNumContacts =" << _numConstraints << sendl; if (_mu > 0.0) { real toln = ((int) (_realNumConstraints/3) + 1) * (real)_tol; error = sofa::gpu::cuda::CudaLCP<real>::CudaNlcp_gaussseidel(useGPU_d.getValue(),_realNumConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), _f.getCudaVector(), _mu,toln, _maxIt); // printf("\nFE = ["); // for (unsigned j=0;j<_numConstraints;j++) { // printf("%f ",_f.element(j)); // } // printf("]\n"); // real toln = ((int) (_numConstraints/3) + 1) * (real)_tol; // error = sofa::gpu::cuda::CudaLCP<real>::CudaNlcp_gaussseidel(useGPU_d.getValue(),_realNumConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), _f.getCudaVector(), _mu,toln, _maxIt); if (this->f_printLog.getValue()) { printf("M = [\n"); for (unsigned j=0;j<_numConstraints;j++) { for (unsigned i=0;i<_numConstraints;i++) { printf("%f\t",_W.element(i,j)); } printf("\n"); } printf("]\n"); printf("q = ["); for (unsigned j=0;j<_numConstraints;j++) { printf("%f\t",_dFree.element(j)); } printf("]\n"); printf("FS = ["); for (unsigned j=0;j<_numConstraints;j++) { printf("%f ",_f.element(j)); } printf("]\n"); } } else { error = sofa::gpu::cuda::CudaLCP<real>::CudaGaussSeidelLCP1(useGPU_d.getValue(),_realNumConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), _f.getCudaVector(), (real)_tol, _maxIt); } #endif if (error > _tol) sout << "No convergence in gaussSeidelLCP1 : error = " << error << sendl; #ifdef DISPLAY_TIME time_solve_LCP = ((double) timer->getTime() - time_solve_LCP)*timeScale; double time_contactCorrections = (double) timer->getTime(); #endif if (initial_guess.getValue()) keepContactForcesValue(); // MechanicalApplyContactForceVisitor(_result).execute(context); for (unsigned int i=0;i<constraintCorrections.size();i++) { core::behavior::BaseConstraintCorrection* cc = constraintCorrections[i]; cc->applyContactForce(&_f); } core::MechanicalParams mparams(*params); simulation::MechanicalPropagateAndAddDxVisitor(&mparams).execute(context); //simulation::MechanicalPropagatePositionAndVelocityVisitor().execute(context); //simulation::MechanicalPropagateAndAddDxVisitor().execute( context); //MechanicalResetContactForceVisitor().execute(context); for (unsigned int i=0;i<constraintCorrections.size();i++) { core::behavior::BaseConstraintCorrection* cc = constraintCorrections[i]; cc->resetContactForce(); } #ifdef DISPLAY_TIME time_contactCorrections = ((double) timer->getTime() - time_contactCorrections)*timeScale; #endif #ifdef DISPLAY_TIME double total = time_Free_Motion + time_computeCollision + time_build_LCP + time_solve_LCP + time_contactCorrections; if (this->print_info.getValue()) { sout<<"********* Start Iteration : " << _numConstraints << " contacts *********" <<sendl; sout<<"Free Motion\t" << time_Free_Motion <<" s \t| " << time_Free_Motion*100.0/total << "%" <<sendl; sout<<"ComputeCollision\t" << time_computeCollision <<" s \t| " << time_computeCollision*100.0/total << "%" <<sendl; sout<<"Build_LCP\t" << time_build_LCP <<" s \t| " << time_build_LCP*100.0/total << "%" <<sendl; sout<<"Solve_LCP\t" << time_solve_LCP <<" s \t| " << time_solve_LCP*100.0/total << "%" <<sendl; sout<<"ContactCorrections\t" << time_contactCorrections <<" s \t| " << time_contactCorrections*100.0/total << "%" <<sendl; unsigned nbNnul = 0; unsigned sz = _realNumConstraints * _realNumConstraints; for (unsigned j=0;j<sz;j++) if (_W.getCudaMatrix().hostRead()[j]==0.0) nbNnul++; sout<<"Sparsity = " << ((double) (((double) nbNnul * 100.0) / ((double)sz))) <<"%" <<sendl; } #endif simulation::MechanicalEndIntegrationVisitor endVisitor(params /* PARAMS FIRST */, dt); context->execute(&endVisitor); sofa::helper::AdvancedTimer::stepEnd("AnimationStep"); }