C++ solver函数代码示例

	void SimulatorParameters::loadSolverParameters(){
		ifstream fid;
		string str;
		// read solver parameters
		string solver(parametersPath + "/solver.dat");
		fid.open(solver.c_str());
		if ( !fid.is_open() ) throw Exception(__LINE__,__FILE__,"solver.dat could not be opened or it doesn't exist.\n");

		// start reading file
		setPositionToRead(fid,"absolute convergence tolerance");
		fid >> _abstol;

		setPositionToRead(fid,"relative convergence tolerance for KSP solver");
		fid >> _rtol;

		setPositionToRead(fid,"relative convergence tolerance for external iteration");
		fid >> _rtol2;

		setPositionToRead(fid,"convergence tolerance in terms of the norm of the change in the solution between steps");
		fid >> _dtol;

		setPositionToRead(fid,"maximum number of iterations");
		fid >> _maxit;

		setPositionToRead(fid,"Sets number of iterations at which GMRES, FGMRES and LGMRES restarts:");
		fid >> _Krylov_restart;


		// Load a 'database' to set a preconditioner chosen by user. <private>
		setPreconditionerDataBase();

		// Read solver.dat and check which preconditioner user chose.
		string::size_type pos;
		int numpc = (int)mapPC.size(); // number of preconditioners presented on data base.
		setPositionToRead(fid,"Define a preconditioner (Default: none)");
		for (int i=0; i<numpc; i++) {
			getline(fid,str,'\n');

			// search for chosen option
			pos = str.find("(x)",0);

			// if marked with 'x', search in 'str' for pre-conditioner available
			if (pos != string::npos){
				std::map<std::string,PCType>::iterator miter = mapPC.begin();
				while (miter != mapPC.end()){
					pos = str.find(miter->first,0);
					if (pos != string::npos){				// miter->first: string name
						pctype = miter->second;				// miter->second preconditioner
					}
					miter++;
				}
			}
		}

		// read pressure solver scheme for EBFV1 formulation
		str.clear();
		setPositionToRead(fid,"Use <defect-correction> scheme for EBFV1 pressure solver?: (default: matrix-free scheme)");
		getline(fid,str);
		pos = str.find("(x)",0);
		if (pos != string::npos) {
			setUseDefectCorrection();
			if (!P_pid()) printf("Using defect-correction scheme for EBFV1 pressure solver.\n");
		}

		fid.close();
	}

void CellBasedPdeHandler<DIM>::SolvePdeAndWriteResultsToFile(unsigned samplingTimestepMultiple)
{
    // Record whether we are solving PDEs on a coarse mesh
    bool using_coarse_pde_mesh = (mpCoarsePdeMesh != NULL);

    // If solving PDEs on a coarse mesh, each PDE should have an averaged source term; otherwise none should
    assert(!mPdeAndBcCollection.empty());
    for (unsigned pde_index=0; pde_index<mPdeAndBcCollection.size(); pde_index++)
    {
        assert(mPdeAndBcCollection[pde_index]);
        assert(mPdeAndBcCollection[pde_index]->HasAveragedSourcePde() == using_coarse_pde_mesh || dynamic_cast<MultipleCaBasedCellPopulation<DIM>*>(mpCellPopulation));
    }

    // Make sure the cell population is in a nice state
    mpCellPopulation->Update();

    // Store a pointer to the (population-level or coarse) mesh
    TetrahedralMesh<DIM,DIM>* p_mesh;
    if (using_coarse_pde_mesh)
    {
        p_mesh = mpCoarsePdeMesh;
    }
    else
    {
        // If not using a coarse PDE mesh, we must be using a MeshBasedCellPopulation
        p_mesh = &(static_cast<MeshBasedCellPopulation<DIM>*>(mpCellPopulation)->rGetMesh());
    }

    // Loop over elements of mPdeAndBcCollection
    for (unsigned pde_index=0; pde_index<mPdeAndBcCollection.size(); pde_index++)
    {
        // Get pointer to this PdeAndBoundaryConditions object
        PdeAndBoundaryConditions<DIM>* p_pde_and_bc = mPdeAndBcCollection[pde_index];

        // Set up boundary conditions
        std::auto_ptr<BoundaryConditionsContainer<DIM,DIM,1> > p_bcc = ConstructBoundaryConditionsContainer(p_pde_and_bc, p_mesh);

        // If the solution at the previous timestep exists...
        PetscInt previous_solution_size = 0;
        if (p_pde_and_bc->GetSolution())
        {
            VecGetSize(p_pde_and_bc->GetSolution(), &previous_solution_size);
        }

        // ...then record whether it is the correct size...
        bool is_previous_solution_size_correct = (previous_solution_size == (int)p_mesh->GetNumNodes());

        // ...and if it is, store it as an initial guess for the PDE solver
        Vec initial_guess;
        if (is_previous_solution_size_correct)
        {
            // This Vec is copied by the solver's Solve() method, so must be deleted here too
            VecDuplicate(p_pde_and_bc->GetSolution(), &initial_guess);
            VecCopy(p_pde_and_bc->GetSolution(), initial_guess);
            p_pde_and_bc->DestroySolution();
        }
        else
        {
            ///\todo enable the coarse PDE mesh to change size, e.g. for a growing domain (#630/#1891)
            if (!using_coarse_pde_mesh && p_pde_and_bc->GetSolution())
            {
                assert(previous_solution_size != 0);
                p_pde_and_bc->DestroySolution();
            }
        }

        // Create a PDE solver and solve the PDE on the (population-level or coarse) mesh
        if (p_pde_and_bc->HasAveragedSourcePde())
        {
            // When using a coarse PDE mesh, we must set up the source terms before solving the PDE.
            // Pass in mCellPdeElementMap to speed up finding cells.
            this->UpdateCellPdeElementMap();
            p_pde_and_bc->SetUpSourceTermsForAveragedSourcePde(p_mesh, &mCellPdeElementMap);


            SimpleLinearEllipticSolver<DIM,DIM> solver(p_mesh, p_pde_and_bc->GetPde(), p_bcc.get());

            // If we have an initial guess, use this when solving the system...
            if (is_previous_solution_size_correct)
            {
                p_pde_and_bc->SetSolution(solver.Solve(initial_guess));
                PetscTools::Destroy(initial_guess);
            }
            else // ...otherwise do not supply one
            {
                p_pde_and_bc->SetSolution(solver.Solve());
            }
        }
        else
        {
            CellBasedPdeSolver<DIM> solver(p_mesh, p_pde_and_bc->GetPde(), p_bcc.get());

            // If we have an initial guess, use this...
            if (is_previous_solution_size_correct)
            {
                p_pde_and_bc->SetSolution(solver.Solve(initial_guess));
                PetscTools::Destroy(initial_guess);
            }
            else // ...otherwise do not supply one
            {
//.........这里部分代码省略.........

void VDBLinearFEMSolverModule<LatticeType>::solve() {

    Eigen::PardisoLDLT<SparseMatrix,Eigen::Lower> solver(M);
    x = solver.compute(M).solve(rhs);
    //std::cout << "#iterations : " << solver.iterations() << std::endl;
    //std::cout << "error: " << solver.error() << std::endl;
    //std::cout << x.transpose() << std::endl;
    std::cout << "L2 difference: " << (rhs - M*x).norm()/rhs.norm() << std::endl;
    switch(solver.info()) {
        case Eigen::NumericalIssue:
            std::cout << "Numerical Issue!" << std::endl;
            //return false;
            break;
        case Eigen::NoConvergence:
            std::cout << "No Convergence!" << std::endl;
            //return false;
            break;
        case Eigen::InvalidInput:
            std::cout << "Invalid Input!" << std::endl;
            //return false;
            break;
        case Eigen::Success:
            std::cout << "Success!" << std::endl;
            break;
    }

    rms::VectorGridPtr velocityFieldPtr = m_obj->getVectorField();
    rms::ScalarGridPtr rigidFieldPtr = m_obj->getRigidField();
    rms::ScalarGridPtr heatFieldPtr = m_obj->getHeatField();
    rms::RGBAGridPtr materialFieldPtr = m_obj->getMaterialField();
    rms::ScalarGrid::ConstAccessor heatAccessor = heatFieldPtr->getConstAccessor();
    rms::ScalarGrid::ConstAccessor rigidAccessor = rigidFieldPtr->getConstAccessor();
    rms::RGBAGrid::ConstAccessor materialAccessor = materialFieldPtr->getConstAccessor();
    rms::VectorGrid::ConstAccessor velocityAccessor = velocityFieldPtr->getConstAccessor();
    int count=0;

    std::cout << "About to look at heat field" << std::endl;
    Scalar scale = heatFieldPtr->transform().voxelVolume();
    StiffnessEntryStorage store(scale,integrator);//TODO: need to fix this
    std::cout << "Mincoord: " << m_minCoord << std::endl;
    vdb::CoordBBox bbox = m_obj->getDistanceField()->evalActiveVoxelBoundingBox();
    std::cout << bbox.min() << " " << bbox.max() << std::endl;
    m_minCoord = bbox.min();
    for(rms::ScalarGrid::ValueOnCIter it =  m_obj->getDistanceField()->cbeginValueOn(); it; ++it) {
        if(it.getValue() >= 0) continue;
        Index3 idx = VDBtoLatticeCoord(it.getCoord());
        //std::cout << "True index: " << idx << " " << it.getCoord() << std::endl;
        /*
           if(m_obj->getVertex(idx.i,idx.j,idx.k) == -1) {
           std::cout << "bottomleftinner lattice index doesn't exist, clearly lattice just isn't populated"<< std::endl;
           }
         */
        const vdb::Coord vdbcoord = m_minCoord + vdb::Coord(idx.i,idx.j,idx.k);

        for(VDBVoxelNodeIterator<LatticeType> alpha(m_obj->getDOFs(),idx); alpha; ++alpha) {
            const Index3 & alpha_index = alpha.index();
            const vdb::Coord alpha_vdbcoord = m_minCoord + vdb::Coord(alpha_index.i,alpha_index.j,alpha_index.k);
            const int32_t alpha_value = alpha.value();
            const int32_t alphaind = mat_ind(alpha_value,0);
            //const LinearElasticVertexProperties & alpha_props = m_vertex_properties[alpha_value];
            //alpha sets the matrix, betas set the rhs values for alpha
            if(rigidAccessor.getValue(alpha_vdbcoord) < 0) {
                std::cout << alpha_vdbcoord << ": ";
                std::cout << "Rigid!" << std::endl;
                std::cout << rhs(alphaind+0) << " ";
                std::cout << rhs(alphaind+1) << " ";
                std::cout << rhs(alphaind+2) << std::endl;
                std::cout << x(alphaind+0) << " ";
                std::cout << x(alphaind+1) << " ";
                std::cout << x(alphaind+2) << std::endl;
            }
        }
    }
}

void summarizer_bwt::do_summary(const function_namet &function_name, 
				local_SSAt &SSA,
				const summaryt &old_summary,
				summaryt &summary,
				bool context_sensitive)
{
  bool sufficient = options.get_bool_option("sufficient");
  status() << "Computing preconditions" << eom;

  // solver
  incremental_solvert &solver = ssa_db.get_solver(function_name);
  solver.set_message_handler(get_message_handler());

  template_generator_summaryt template_generator(
    options,ssa_db,ssa_unwinder.get(function_name));
  template_generator.set_message_handler(get_message_handler());
  template_generator(solver.next_domain_number(),SSA,false);

  exprt::operandst c;
  c.push_back(old_summary.fw_precondition);
  c.push_back(old_summary.fw_invariant);
  c.push_back(ssa_inliner.get_summaries(SSA)); //forward summaries
  exprt::operandst postcond;
  ssa_inliner.get_summaries(SSA,false,postcond,c); //backward summaries
  collect_postconditions(function_name, SSA, summary, postcond,sufficient);
  if(!sufficient)
  {
    c.push_back(conjunction(postcond)); 
  }
  else //sufficient
  {
    c.push_back(not_exprt(conjunction(postcond))); 
  }

  if(!template_generator.out_vars().empty())
  {
    ssa_analyzert analyzer;
    analyzer.set_message_handler(get_message_handler());
    analyzer(solver,SSA,conjunction(c),template_generator);
    analyzer.get_result(summary.bw_transformer,template_generator.inout_vars());
    analyzer.get_result(summary.bw_invariant,template_generator.loop_vars());
    analyzer.get_result(summary.bw_precondition,template_generator.out_vars());

    //statistics
    solver_instances += analyzer.get_number_of_solver_instances();
    solver_calls += analyzer.get_number_of_solver_calls();
  }
  else // TODO: yet another workaround for ssa_analyzer not being able to handle empty templates properly
  {
    solver << SSA;
    solver.new_context();
    solver << SSA.get_enabling_exprs();
    solver << conjunction(c);
    exprt result = true_exprt();
    if(solver()==decision_proceduret::D_UNSATISFIABLE) result = false_exprt();
    solver.pop_context();
    summary.bw_transformer = result;
    summary.bw_invariant = result;
    summary.bw_precondition = result;
  }

  if(sufficient)
  {
    summary.bw_transformer = not_exprt(summary.bw_transformer);
    summary.bw_invariant = not_exprt(summary.bw_invariant);
    summary.bw_precondition = not_exprt(summary.bw_precondition);
  }

  if(context_sensitive && !summary.bw_postcondition.is_true())
  {
    summary.bw_transformer = 
      implies_exprt(summary.bw_postcondition,summary.bw_transformer);
    summary.bw_invariant = 
      implies_exprt(summary.bw_postcondition,summary.bw_invariant);
    summary.bw_precondition = 
      implies_exprt(summary.bw_postcondition,summary.bw_precondition);
  }
}

    void FdBlackScholesAsianEngine::calculate() const {

        QL_REQUIRE(arguments_.exercise->type() == Exercise::European,
                   "European exercise supported only");
        QL_REQUIRE(arguments_.averageType == Average::Arithmetic,
                   "Arithmetic averaging supported only");
        QL_REQUIRE(   arguments_.runningAccumulator == 0
                   || arguments_.pastFixings > 0,
                   "Running average requires at least one past fixing");

        // 1. Mesher
        const ext::shared_ptr<StrikedTypePayoff> payoff =
            ext::dynamic_pointer_cast<StrikedTypePayoff>(arguments_.payoff);
        const Time maturity = process_->time(arguments_.exercise->lastDate());
        const ext::shared_ptr<Fdm1dMesher> equityMesher(
            new FdmBlackScholesMesher(xGrid_, process_, maturity,
                                      payoff->strike()));

        const Real spot = process_->x0();
        QL_REQUIRE(spot > 0.0, "negative or null underlying given");

        const Real avg = (arguments_.runningAccumulator == 0)
                 ? spot : arguments_.runningAccumulator/arguments_.pastFixings;

        const Real normInvEps = InverseCumulativeNormal()(1-0.0001);
        const Real sigmaSqrtT 
            = process_->blackVolatility()->blackVol(maturity, payoff->strike())
                                                        *std::sqrt(maturity);
        const Real r = sigmaSqrtT*normInvEps;

        Real xMin = std::min(std::log(avg)  - 0.25*r, std::log(spot) - 1.5*r);
        Real xMax = std::max(std::log(avg)  + 0.25*r, std::log(spot) + 1.5*r);

        const ext::shared_ptr<Fdm1dMesher> averageMesher(
            new FdmBlackScholesMesher(aGrid_, process_, maturity,
                                      payoff->strike(), xMin, xMax));

        const ext::shared_ptr<FdmMesher> mesher (
            new FdmMesherComposite(equityMesher, averageMesher));

        // 2. Calculator
        ext::shared_ptr<FdmInnerValueCalculator> calculator(
                                new FdmLogInnerValue(payoff, mesher, 1));

        // 3. Step conditions
        std::list<ext::shared_ptr<StepCondition<Array> > > stepConditions;
        std::list<std::vector<Time> > stoppingTimes;

        // 3.1 Arithmetic average step conditions
        std::vector<Time> averageTimes;
        for (Size i=0; i<arguments_.fixingDates.size(); ++i) {
            Time t = process_->time(arguments_.fixingDates[i]);
            QL_REQUIRE(t >= 0, "Fixing dates must not contain past date");
            averageTimes.push_back(t);
        }
        stoppingTimes.push_back(std::vector<Time>(averageTimes));
        stepConditions.push_back(ext::shared_ptr<StepCondition<Array> >(
                new FdmArithmeticAverageCondition(
                        averageTimes, arguments_.runningAccumulator,
                        arguments_.pastFixings, mesher, 0)));

        ext::shared_ptr<FdmStepConditionComposite> conditions(
                new FdmStepConditionComposite(stoppingTimes, stepConditions));

        // 4. Boundary conditions
        const FdmBoundaryConditionSet boundaries;

        // 5. Solver
        FdmSolverDesc solverDesc = { mesher, boundaries, conditions,
                                     calculator, maturity, tGrid_, 0 };
        ext::shared_ptr<FdmSimple2dBSSolver> solver(
              new FdmSimple2dBSSolver(
                              Handle<GeneralizedBlackScholesProcess>(process_),
                              payoff->strike(), solverDesc, schemeDesc_));

        results_.value = solver->valueAt(spot, avg);
        results_.delta = solver->deltaAt(spot, avg, spot*0.01);
        results_.gamma = solver->gammaAt(spot, avg, spot*0.01);
    }

int main(int argc, char **argv)
{
    bool dump_to_stdout = false;
    ifstream inFile;
    ofstream fs_out;
    string test_name = "";

    //-----------------------------------------------------------
    // initialize
    //-----------------------------------------------------------


    if ((argc == 2) && (strcmp (argv[1], "stdout") == 0))
    {
        dump_to_stdout = true;
        cout.precision (numeric_limits<double>::digits10);
    }
    else
    {
        // reference states generated using thr implementation of
        // the algorithm in Octave/MATLAB
        inFile.open ("./data/ip_states_inv.dat");
        //inFile.open ("./data/states_chol_downdate.dat");
        test_name = "test_05";
    }
    init_01 test_05 (test_name);



    smpc::solver_ip solver(
            test_05.wmg->N, 
            2000, 
            150, 
            0.02, 
            1, 
            1e-3,
            1e-2,
            100, 
            15, 
            0.01, 
            0.5,
            0,
            smpc::SMPC_IP_BS_LOGBAR,
            true);


    double err = 0;
    double max_err = 0;
    double max_err_first_state = 0;


    fs_out.open(test_05.fs_out_filename.c_str(), fstream::app);
    fs_out.precision (numeric_limits<double>::digits10);
    fs_out << endl << endl;
    fs_out << "CoM_ZMP = [";
   


    for(;;)
    {
        //------------------------------------------------------
        if (test_05.wmg->formPreviewWindow(*test_05.par) == WMG_HALT)
        {
            if (dump_to_stdout == false)
            {
                cout << "EXIT (halt = 1)" << endl;
            }
            break;
        }
        //------------------------------------------------------

        //------------------------------------------------------
        solver.set_parameters (test_05.par->T, test_05.par->h, test_05.par->h0, test_05.par->angle, test_05.par->fp_x, test_05.par->fp_y, test_05.par->lb, test_05.par->ub);
        solver.form_init_fp (test_05.par->fp_x, test_05.par->fp_y, test_05.par->init_state, test_05.par->X);
        solver.solve();
        solver.get_next_state(test_05.par->init_state);
        //------------------------------------------------------

        solver.get_next_state(test_05.X_tilde);
        fs_out << endl << test_05.par->init_state.x() << " " << test_05.par->init_state.y() << " " << test_05.X_tilde.x() << " " << test_05.X_tilde.y() << ";";


        if (dump_to_stdout)
        {
            for (unsigned int i = 0; i < test_05.wmg->N*SMPC_NUM_VAR; i++)
            {
                cout << test_05.par->X[i] << endl;
            }
        }
        else
        {
            printf ("-------------------------------------\nOBJ: ");
            for (unsigned int i = 0; i < solver.objective_log.size(); ++i)
            {   
                printf ("% 8e ", solver.objective_log[i]);
            }
            printf ("\n-------------------------------------\n");


            //------------------------------------------------------
//.........这里部分代码省略.........

bool test_lin_indep(Shapeset *shapeset) {
	_F_
	printf("I. linear independency\n");

	UMFPackMatrix mat;
	UMFPackVector rhs;
	UMFPackLinearSolver solver(&mat, &rhs);

	ShapeFunction fu(shapeset), fv(shapeset);

	int n =
		Hex::NUM_VERTICES * 1 +			// 1 vertex fn
		Hex::NUM_EDGES * shapeset->get_num_edge_fns(H3D_MAX_ELEMENT_ORDER) +
		Hex::NUM_FACES * shapeset->get_num_face_fns(order2_t(H3D_MAX_ELEMENT_ORDER, H3D_MAX_ELEMENT_ORDER)) +
		shapeset->get_num_bubble_fns(Ord3(H3D_MAX_ELEMENT_ORDER, H3D_MAX_ELEMENT_ORDER, H3D_MAX_ELEMENT_ORDER));

	printf("number of functions = %d\n", n);

	int *fn_idx = new int [n];
	int m = 0;
	// vertex fns
	for (int i = 0; i < Hex::NUM_VERTICES; i++, m++)
		fn_idx[m] = shapeset->get_vertex_index(i);
	// edge fns
	for (int i = 0; i < Hex::NUM_EDGES; i++) {
		int order = H3D_MAX_ELEMENT_ORDER;
		int *edge_idx = shapeset->get_edge_indices(i, 0, order);
		for (int j = 0; j < shapeset->get_num_edge_fns(order); j++, m++)
			fn_idx[m] = edge_idx[j];
	}
	// face fns
	for (int i = 0; i < Hex::NUM_FACES; i++) {
		order2_t order(H3D_MAX_ELEMENT_ORDER, H3D_MAX_ELEMENT_ORDER);
		int *face_idx = shapeset->get_face_indices(i, 0, order);
		for (int j = 0; j < shapeset->get_num_face_fns(order); j++, m++)
			fn_idx[m] = face_idx[j];
	}
	// bubble
	Ord3 order(H3D_MAX_ELEMENT_ORDER, H3D_MAX_ELEMENT_ORDER, H3D_MAX_ELEMENT_ORDER);
	int *bubble_idx = shapeset->get_bubble_indices(order);
	for (int j = 0; j < shapeset->get_num_bubble_fns(order); j++, m++)
		fn_idx[m] = bubble_idx[j];


	// precalc structure
	mat.prealloc(n);
	for (int i = 0; i < n; i++)
		for (int j = 0; j < n; j++)
			mat.pre_add_ij(i, j);
	mat.alloc();
	rhs.alloc(n);

	printf("assembling matrix ");

	for (int i = 0; i < n; i++) {
		fu.set_active_shape(fn_idx[i]);

		printf(".");
		fflush(stdout);			// prevent caching of output (to see that it did not freeze)

		for (int j = 0; j < n; j++) {
			fv.set_active_shape(fn_idx[j]);

			double value = l2_product(&fu, &fv);

			mat.add(i, j, value);
		}
	}
	printf("\n");

	for (int i = 0; i < n; i++)
		rhs.add(i, 0.0);

	printf("solving matrix\n");

	// solve the system
	if (solver.solve()) {
		double *sln = solver.get_solution();
		bool indep = true;
		for (int i = 1; i < n + 1; i++) {
			if (sln[i] >= EPS) {
				indep = false;
				break;
			}
		}

		if (indep)
			printf("ok\n");
		else
			printf("Shape functions are not linearly independent\n");
	}
	else {
		printf("Shape functions are not linearly independent\n");
	}

	delete [] fn_idx;

	return true;
}

//.........这里部分代码省略.........

			++count;
		}

		++iter;
	} while (found && count > 1 && iter < 50);

	if (toiContact == NULL)
	{
		body->Advance(1.0f);
		return;
	}

	b2Sweep backup = body->m_sweep;
	body->Advance(toi);
	toiContact->Update(m_contactManager.m_contactListener);
	if (toiContact->IsEnabled() == false)
	{
		// Contact disabled. Backup and recurse.
		body->m_sweep = backup;
		SolveTOI(body);
	}

	++toiContact->m_toiCount;

	// Update all the valid contacts on this body and build a contact island.
	b2Contact* contacts[b2_maxTOIContacts];
	count = 0;
	for (b2ContactEdge* ce = body->m_contactList; ce && count < b2_maxTOIContacts; ce = ce->next)
	{
		b2Body* other = ce->other;
		b2BodyType type = other->GetType();

		// Only perform correction with static bodies, so the
		// body won't get pushed out of the world.
		if (type == b2_dynamicBody)
		{
			continue;
		}

		// Check for a disabled contact.
		b2Contact* contact = ce->contact;
		if (contact->IsEnabled() == false)
		{
			continue;
		}

		b2Fixture* fixtureA = contact->m_fixtureA;
		b2Fixture* fixtureB = contact->m_fixtureB;

		// Cull sensors.
		if (fixtureA->IsSensor() || fixtureB->IsSensor())
		{
			continue;
		}

		// The contact likely has some new contact points. The listener
		// gives the user a chance to disable the contact.
		if (contact != toiContact)
		{
			contact->Update(m_contactManager.m_contactListener);
		}

		// Did the user disable the contact?
		if (contact->IsEnabled() == false)
		{
			// Skip this contact.
			continue;
		}

		if (contact->IsTouching() == false)
		{
			continue;
		}

		contacts[count] = contact;
		++count;
	}

	// Reduce the TOI body's overlap with the contact island.
	b2TOISolver solver(&m_stackAllocator);
	solver.Initialize(contacts, count, body);

	const float32 k_toiBaumgarte = 0.75f;
	bool solved = false;
	for (int32 i = 0; i < 20; ++i)
	{
		bool contactsOkay = solver.Solve(k_toiBaumgarte);
		if (contactsOkay)
		{
			solved = true;
			break;
		}
	}

	if (toiOther->GetType() != b2_staticBody)
	{
			toiContact->m_flags |= b2Contact::e_bulletHitFlag;
	}
}

//.........这里部分代码省略.........
        publishText(pub_text_,
                    "Failed to project goal",
                    ERROR);
        return;
      }
    }
    // set parameters
    if (use_transition_limit_) {
      graph_->setTransitionLimit(
        TransitionLimitXYZRPY::Ptr(new TransitionLimitXYZRPY(
                                     transition_limit_x_,
                                     transition_limit_y_,
                                     transition_limit_z_,
                                     transition_limit_roll_,
                                     transition_limit_pitch_,
                                     transition_limit_yaw_)));
    }
    else {
      graph_->setTransitionLimit(TransitionLimitXYZRPY::Ptr());
    }
    graph_->setLocalXMovement(local_move_x_);
    graph_->setLocalYMovement(local_move_y_);
    graph_->setLocalThetaMovement(local_move_theta_);
    graph_->setLocalXMovementNum(local_move_x_num_);
    graph_->setLocalYMovementNum(local_move_y_num_);
    graph_->setLocalThetaMovementNum(local_move_theta_num_);
    graph_->setPlaneEstimationMaxIterations(plane_estimation_max_iterations_);
    graph_->setPlaneEstimationMinInliers(plane_estimation_min_inliers_);
    graph_->setPlaneEstimationOutlierThreshold(plane_estimation_outlier_threshold_);
    graph_->setSupportCheckXSampling(support_check_x_sampling_);
    graph_->setSupportCheckYSampling(support_check_y_sampling_);
    graph_->setSupportCheckVertexNeighborThreshold(support_check_vertex_neighbor_threshold_);
    // Solver setup
    FootstepAStarSolver<FootstepGraph> solver(graph_,
                                              close_list_x_num_,
                                              close_list_y_num_,
                                              close_list_theta_num_,
                                              profile_period_,
                                              cost_weight_,
                                              heuristic_weight_);
    if (heuristic_ == "step_cost") {
      solver.setHeuristic(boost::bind(&FootstepPlanner::stepCostHeuristic, this, _1, _2));
    }
    else if (heuristic_ == "zero") {
      solver.setHeuristic(boost::bind(&FootstepPlanner::zeroHeuristic, this, _1, _2));
    }
    else if (heuristic_ == "straight") {
      solver.setHeuristic(boost::bind(&FootstepPlanner::straightHeuristic, this, _1, _2));
    }
    else if (heuristic_ == "straight_rotation") {
      solver.setHeuristic(boost::bind(&FootstepPlanner::straightRotationHeuristic, this, _1, _2));
    }
    else {
      JSK_ROS_ERROR("Unknown heuristics");
      as_.setPreempted();
      return;
    }
    solver.setProfileFunction(boost::bind(&FootstepPlanner::profile, this, _1, _2));
    ros::WallTime start_time = ros::WallTime::now();
    std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr> path = solver.solve(timeout);
    ros::WallTime end_time = ros::WallTime::now();
    double planning_duration = (end_time - start_time).toSec();
    JSK_ROS_INFO_STREAM("took " << planning_duration << " sec");
    JSK_ROS_INFO_STREAM("path: " << path.size());
    if (path.size() == 0) {
      JSK_ROS_ERROR("Failed to plan path");

    /* In the first test we use INCOMPRESSIBLE nonlinear elasticity. For incompressible elasticity, there
     * is a constraint on the deformation, which results in a pressure field (a Lagrange multiplier)
     * which is solved for together with the deformation.
     *
     * All the mechanics solvers solve for the deformation using the finite element method with QUADRATIC
     * basis functions for the deformation. This necessitates the use of a `QuadraticMesh` - such meshes have
     * extra nodes that aren't vertices of elements, in this case midway along each edge. (The displacement
     * is solved for at ''each node'' in the mesh (including internal [non-vertex] nodes), whereas the pressure
     * is only solved for at each vertex - in FEM terms, quadratic interpolation for displacement, linear
     * interpolation for pressure, which is required for stability. The pressure at internal nodes is computed
     * by linear interpolation).
     *
     */
    void TestSimpleIncompressibleProblem() throw(Exception)
    {
        /* First, define the geometry. This should be specified using the `QuadraticMesh` class, which inherits from `TetrahedralMesh`
         * and has mostly the same interface. Here we define a 0.8 by 1 rectangle, with elements 0.1 wide.
         * (`QuadraticMesh`s can also be read in using `TrianglesMeshReader`; see next tutorial/rest of code base for examples of this).
         */
        QuadraticMesh<2> mesh;
        mesh.ConstructRegularSlabMesh(0.1 /*stepsize*/, 0.8 /*width*/, 1.0 /*height*/);

        /* We use a Mooney-Rivlin material law, which applies to isotropic materials and has two parameters.
         * Restricted to 2D however, it only has one parameter, which can be thought of as the total
         * stiffness. We declare a Mooney-Rivlin law, setting the parameter to 1.
         */
        MooneyRivlinMaterialLaw<2> law(1.0);

        /* Next, the body force density. In realistic problems this will either be
         * acceleration due to gravity (ie b=(0,-9.81)) or zero if the effect of gravity can be neglected.
         * In this problem we apply a gravity-like downward force.
         */
        c_vector<double,2> body_force;
        body_force(0) =  0.0;
        body_force(1) = -2.0;


        /* Two types of boundary condition are required: displacement and traction. As with the other PDE solvers,
         * the displacement (Dirichlet) boundary conditions are specified at nodes, whereas traction (Neumann) boundary
         * conditions are specified on boundary elements.
         *
         * In this test we apply displacement boundary conditions on one surface of the mesh, the upper (Y=1.0) surface.
         * We are going to specify zero-displacement at these nodes.
         * We do not specify any traction boundary conditions, which means that (effectively) zero-traction boundary
         * conditions (ie zero pressures) are applied on the three other surfaces.
         *
         * We need to get a `std::vector` of all the node indices that we want to fix. The `NonlinearElasticityTools`
         * has a static method for helping do this: the following gets all the nodes for which Y=1.0. The second
         * argument (the '1') indicates Y . (So, for example, `GetNodesByComponentValue(mesh, 0, 10)` would get the nodes on X=10).
         */
        std::vector<unsigned> fixed_nodes = NonlinearElasticityTools<2>::GetNodesByComponentValue(mesh, 1, 1.0);

        /*
         * Before creating the solver we create a `SolidMechanicsProblemDefinition` object,  which contains
         * everything that defines the problem: mesh, material law, body force,
         * the fixed nodes and their locations, any traction boundary conditions, and the density
         * (which multiplies the body force, otherwise isn't used).
         */
        SolidMechanicsProblemDefinition<2> problem_defn(mesh);

        /* Set the material problem on the problem definition object, saying that the problem, and
         * the material law, is incompressible. All material law files can be found in
         * `continuum_mechanics/src/problem/material_laws`. */
        problem_defn.SetMaterialLaw(INCOMPRESSIBLE,&law);

        /* Set the fixed nodes, choosing zero displacement for these nodes (see later for how
         * to provide locations for the fixed nodes). */
        problem_defn.SetZeroDisplacementNodes(fixed_nodes);
        /* Set the body force and the density. (Note that the second line isn't technically
         * needed, as internally the density is initialised to 1)
         */
        problem_defn.SetBodyForce(body_force);
        problem_defn.SetDensity(1.0);

        /* Now we create the (incompressible) solver, passing in the mesh, problem definition
         * and output directory
         */
        IncompressibleNonlinearElasticitySolver<2> solver(mesh,
                                                          problem_defn,
                                                          "SimpleIncompressibleElasticityTutorial");

        /* .. and to compute the solution, just call `Solve()` */
        solver.Solve();


        /* '''Visualisation'''. Go to the folder `SimpleIncompressibleElasticityTutorial` in your test-output directory.
         * There should be 2 files, initial.nodes and solution.nodes. These are the original nodal positions and the deformed
         * positions. Each file has two columns, the x and y locations of each node. To visualise the solution in say
         * Matlab or Octave, you could do: `x=load('solution.nodes'); plot(x(:,1),x(:,2),'k*')`. For Cmgui output, see below.
         *
         * To get the actual solution from the solver, use these two methods. Note that the first
         * gets the deformed position (ie the new location, not the displacement). They are both of size
         * num_total_nodes.
         */
        std::vector<c_vector<double,2> >& r_deformed_positions = solver.rGetDeformedPosition();
        std::vector<double>& r_pressures = solver.rGetPressures();
        /* Let us obtain the values of the new position, and the pressure, at the bottom right corner node. */
        unsigned node_index = 8;
        assert( fabs(mesh.GetNode(node_index)->rGetLocation()[0] - 0.8) < 1e-6); // check that X=0.8, ie that we have the correct node,
        assert( fabs(mesh.GetNode(node_index)->rGetLocation()[1] - 0.0) < 1e-6); // check that Y=0.0, ie that we have the correct node,
//.........这里部分代码省略.........

    /* == Incompressible deformation: 2D shape hanging under gravity with a balancing traction ==
     *
     * We now repeat the above test but include a traction on the bottom surface (Y=0). We apply this
     * in the inward direction so that is counters (somewhat) the effect of gravity. We also show how stresses
     * and strains can be written to file.
     */
    void TestIncompressibleProblemWithTractions() throw(Exception)
    {
        /* All of this is exactly as above */
        QuadraticMesh<2> mesh;
        mesh.ConstructRegularSlabMesh(0.1 /*stepsize*/, 0.8 /*width*/, 1.0 /*height*/);

        MooneyRivlinMaterialLaw<2> law(1.0);

        c_vector<double,2> body_force;
        body_force(0) =  0.0;
        body_force(1) = -2.0;

        std::vector<unsigned> fixed_nodes = NonlinearElasticityTools<2>::GetNodesByComponentValue(mesh, 1, 1.0);

        /* Now the traction boundary conditions. We need to collect all the boundary elements on the surface which we want to
         * apply non-zero tractions, put them in a `std::vector`, and create a corresponding `std::vector` of the tractions
         * for each of the boundary elements. Note that the each traction is a 2D vector with dimensions of pressure.
         *
         * First, declare the data structures:
         */
        std::vector<BoundaryElement<1,2>*> boundary_elems;
        std::vector<c_vector<double,2> > tractions;
        /* Create a constant traction */
        c_vector<double,2> traction;
        traction(0) = 0;
        traction(1) = 1.0; // this choice of sign corresponds to an inward force (if applied to the bottom surface)
        /* Loop over boundary elements */
        for (TetrahedralMesh<2,2>::BoundaryElementIterator iter = mesh.GetBoundaryElementIteratorBegin();
             iter != mesh.GetBoundaryElementIteratorEnd();
             ++iter)
        {
            /* If the centre of the element has Y value of 0.0, it is on the surface we need */
            if (fabs((*iter)->CalculateCentroid()[1] - 0.0) < 1e-6)
            {
                /* Put the boundary element and the constant traction into the stores. */
                BoundaryElement<1,2>* p_element = *iter;
                boundary_elems.push_back(p_element);
                tractions.push_back(traction);
            }
        }
        /* A quick check */
        assert(boundary_elems.size() == 8u);

        /* Now create the problem definition object, setting the material law, fixed nodes and body force as
         * before (this time not calling `SetDensity()`, so using the default density of 1.0,
         * and also calling a method for setting tractions, which takes in the boundary elements
         * and tractions for each of those elements.
         */
        SolidMechanicsProblemDefinition<2> problem_defn(mesh);
        problem_defn.SetMaterialLaw(INCOMPRESSIBLE,&law);
        problem_defn.SetZeroDisplacementNodes(fixed_nodes);
        problem_defn.SetBodyForce(body_force);
        problem_defn.SetTractionBoundaryConditions(boundary_elems, tractions);

        /* Create solver as before */
        IncompressibleNonlinearElasticitySolver<2> solver(mesh,
                                                          problem_defn,
                                                          "IncompressibleElasticityWithTractionsTutorial");

        /* In this test we also output the stress and strain. For the former, we have to tell the solver to store
         * the stresses that are computed during the solve.
         */
        solver.SetComputeAverageStressPerElementDuringSolve();


        /* Call `Solve()` */
        solver.Solve();

        /* If VTK output is written (discussed above) strains can be visualised. Alternatively, we can create text files
         * for strains and stresses by doing the following.
         *
         * Write the final deformation gradients to file. The i-th line of this file provides the deformation gradient F,
         * written as 'F(0,0) F(0,1) F(1,0) F(1,1)', evaluated at the centroid of the i-th element. The first variable
         * can also be DEFORMATION_TENSOR_C or LAGRANGE_STRAIN_E to write C or E. The second parameter is the file name.
         */
        solver.WriteCurrentStrains(DEFORMATION_GRADIENT_F,"deformation_grad");

        /* Since we called `SetComputeAverageStressPerElementDuringSolve`, we can write the stresses to file too. However,
         * note that for each element this is not the stress evaluated at the centroid, but the mean average of the stresses
         * evaluated at the quadrature points - for technical cardiac electromechanics reasons, it is difficult to
         * define the stress at non-quadrature points.
         */
        solver.WriteCurrentAverageElementStresses("2nd_PK_stress");


        /* Another quick check */
        TS_ASSERT_EQUALS(solver.GetNumNewtonIterations(), 4u);

        /* Visualise as before by going to the output directory and doing
         * `x=load('solution.nodes'); plot(x(:,1),x(:,2),'m*')` in Matlab/octave, or by using Cmgui.
         * The effect of the traction should be clear (especially when compared to
         * the results of the first test).
         *
         * Create Cmgui output
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    // create table for writing the convergence behaviour of the nonlinear solves
    base::io::Table<4>::WidthArray widths = {{ 2, 5, 5, 15 }};
    base::io::Table<4> table( widths );
    table % "Step" % "Iter" % "|F|"  % "|x|";
    std::cout << "#" << table;

    // write a vtk file
    ref06::writeVTKFile( baseName, 0, mesh, field, material );


    //--------------------------------------------------------------------------
    // Loop over load steps
    //--------------------------------------------------------------------------
    for ( unsigned step = 0; step < loadSteps; step++ ) {

        // rescale constraints in every load step: (newValue / oldValue)
        const double  pullFactor =
            (step == 0 ?
             static_cast<double>( step+1 ) :
             static_cast<double>( step+1 )/ static_cast<double>(step) );

        // scale constraints
        base::dof::scaleConstraints( field, pullFactor );

        //----------------------------------------------------------------------
        // Nonlinear iterations
        //----------------------------------------------------------------------
        unsigned iter = 0;
        while ( iter < maxIter ) {

            table % step % iter;
    
            // Create a solver object
            typedef base::solver::Eigen3           Solver;
            Solver solver( numDofs );

            // apply traction boundary condition, if problem is not disp controlled
            if ( not dispControlled ) {
                
                // value of applied traction
                const double tractionFactor =
                    traction * 
                    static_cast<double>(step+1) / static_cast<double>( loadSteps );

                // apply traction load
                base::asmb::neumannForceComputation<SFTB>(
                    surfaceQuadrature, solver,
                    surfaceFieldBinder,
                    boost::bind( &ref06::PulledSheet<dim>::neumannBC,
                                 _1, _2, tractionFactor ) );
            }

            // residual forces
            base::asmb::computeResidualForces<FTB>( quadrature, solver,
                                                    fieldBinder,
                                                    hyperElastic );
            
            // Compute element stiffness matrices and assemble them
            base::asmb::stiffnessMatrixComputation<FTB>( quadrature, solver,
                                                         fieldBinder,
                                                         hyperElastic );

            // Finalise assembly
            solver.finishAssembly();

            // norm of residual 

const Vector<T> PDESolver<T,lFunc,rFunc,bFunc,tFunc,force>::solve() const
{
  U solver;
  return solver(m_A,m_B);
}

int main(int argc, char **args) 
{
  // Test variable.
  int success_test = 1;

  if (argc < 2) error("Not enough parameters.");

  // Load the mesh.
	Mesh mesh;
  H3DReader mloader;
  if (!mloader.load(args[1], &mesh)) error("Loading mesh file '%s'.", args[1]);

	// Initialize the space.
	int mx = 2;
	Ord3 order(mx, mx, mx);
	H1Space space(&mesh, bc_types, NULL, order);
#if defined LIN_DIRICHLET || defined NLN_DIRICHLET
	space.set_essential_bc_values(essential_bc_values);
#endif
	// Initialize the weak formulation.
	WeakForm wf;
	wf.add_vector_form(form_0<double, scalar>, form_0<Ord, Ord>);
#if defined LIN_NEUMANN || defined LIN_NEWTON
	wf.add_vector_form_surf(form_0_surf<double, scalar>, form_0_surf<Ord, Ord>);
#endif
#if defined LIN_DIRICHLET || defined NLN_DIRICHLET
	// preconditioner
	wf.add_matrix_form(precond_0_0<double, scalar>, precond_0_0<Ord, Ord>, HERMES_SYM);
#endif

	// Initialize the FE problem.
	DiscreteProblem fep(&wf, &space);

#if defined LIN_DIRICHLET || defined NLN_DIRICHLET
	// use ML preconditioner to speed-up things
	MlPrecond pc("sa");
	pc.set_param("max levels", 6);
	pc.set_param("increasing or decreasing", "decreasing");
	pc.set_param("aggregation: type", "MIS");
	pc.set_param("coarse: type", "Amesos-KLU");
#endif

	NoxSolver solver(&fep);
#if defined LIN_DIRICHLET || defined NLN_DIRICHLET
//	solver.set_precond(&pc);
#endif

	info("Solving.");
	Solution sln(&mesh);
	if(solver.solve()) Solution::vector_to_solution(solver.get_solution(), &space, &sln);
  else error ("Matrix solver failed.\n");
	

		Solution ex_sln(&mesh);
		ex_sln.set_exact(exact_solution);

		// Calculate exact error.
  info("Calculating exact error.");
  Adapt *adaptivity = new Adapt(&space, HERMES_H1_NORM);
  bool solutions_for_adapt = false;
  double err_exact = adaptivity->calc_err_exact(&sln, &ex_sln, solutions_for_adapt, HERMES_TOTAL_ERROR_ABS);

  if (err_exact > EPS)
		// Calculated solution is not precise enough.
		success_test = 0;

  if (success_test) {
    info("Success!");
    return ERR_SUCCESS;
  }
  else {
    info("Failure!");
    return ERR_FAILURE;
  }
}

//.........这里部分代码省略.........
#ifdef DEBUG
          std::cout << "A P before: " << from_expr(concrete_model.ns, "", predicate) << std::endl;
          std::cout << "Code:     " << from_expr(concrete_model.ns, "", tid_tmp_code) << std::endl;
#endif
          
          // compute weakest precondition
          if(tid_tmp_code.get_statement()==ID_assign)
            approximate_nondet(to_code_assign(tid_tmp_code).rhs());
          renaming_state.source.thread_nr=it->thread_nr;
          renaming_state.rename(tid_tmp_code, concrete_model.ns, goto_symex_statet::L0);
          exprt predicate_wp=wp(tid_tmp_code, predicate, concrete_model.ns);
      
          simplify(predicate_wp, concrete_model.ns);
          predicate=predicate_wp;

#ifdef DEBUG
          std::cout << "A P after:  " << from_expr(concrete_model.ns, "", predicate) << std::endl;
#endif
        }
        break;

      default:
        // ignore
        break;
      }

#ifdef DEBUG
          std::cout << "B P to-check:  " << from_expr(concrete_model.ns, "", predicate) << std::endl;
#endif
          
      if(pred_bak != predicate)
      {
        satcheckt satcheck;
        bv_pointerst solver(concrete_model.ns, satcheck);
        solver.unbounded_array=boolbvt::U_NONE;
        literalt li=make_pos(concrete_model.ns, solver, predicate);
        satcheck.set_assumptions(bvt(1, li));
        propt::resultt result=satcheck.prop_solve();
        assert(propt::P_SATISFIABLE==result || propt::P_UNSATISFIABLE==result);
        if(propt::P_UNSATISFIABLE==result)
          predicate.make_false();
        else
        {
          satcheck.set_assumptions(bvt(1, li.negation()));
          propt::resultt result=satcheck.prop_solve();
          assert(propt::P_SATISFIABLE==result || propt::P_UNSATISFIABLE==result);
          if(propt::P_UNSATISFIABLE==result)
          {
            predicate.make_true();
            if(it->pc->type==ASSIGN)
            {
              const codet &code=concrete_pc->code;
              if(code.get_statement()==ID_assign)
              {
                equal_exprt pred_new(to_code_assign(code).lhs(),
                    to_code_assign(code).rhs());
                simplify(pred_new, concrete_model.ns);
#ifdef DEBUG
                std::cout << "Adding new predicate as we arrived at TRUE: "
                  << from_expr(concrete_model.ns, "", pred_new) << std::endl;
#endif
                no_tid_predicate=pred_new;
                renaming_state.get_original_name(no_tid_predicate);
                add_predicates(abstract_pc, predicates, no_tid_predicate, found_new, FROM);
              }
            }