Beispiel #1
0
//Compile using g++ -std=c++14 ./test.cpp -o ./test
int main(int argc, char* argv[])
{
  std::vector<int64_t> arr1={-9,-3,0,1,2,3,5,7,18};
  std::vector<int64_t> arr2={-8,-3,1,3,4,6,7,8,9,10,11,18};
  std::vector<int64_t> res={-3,1,3,7,18};
  std::vector<int64_t> dup;
  std::vector<int64_t> dup2;

  //Print inputs
  print(arr1);
  print(arr2);
  print(res);

  //Solution 1
  findDup(arr1,arr2,dup);
  print(dup);
  checkSolution(dup,res);

  //Solution 2
  findDup2(arr1,arr2,dup2);
  print(dup2);
  checkSolution(dup2,res);

  return EXIT_SUCCESS;
}
Beispiel #2
0
FaToDFA::FaToDFA(modes _mode, AlgorithmWidget* _algorithm_widget, FA_widget* _not_dfa_widget, FA_widget* _dfa_widget, QLabel* _var_widget, QObject* parrent)
 : Algorithm(parrent),  mode(_mode),  algorithm_widget(_algorithm_widget), not_dfa_widget(_not_dfa_widget), dfa_widget(_dfa_widget), var_widget(_var_widget)
{
    actInstruction = HEADER;
    prewInstruction = HEADER;
    instruction_count = WHILE_NEW+1;
    initInstructions();
    initBreakpoints(instruction_count);

    this->setColumnCount(1);
    this->setRowCount(instructions.count());

    var_widget->setText("");

    for(int i = 0; i < instructions.count();i++)
    {
        QModelIndex index = this->index(i,0,QModelIndex());
        setData(index,instructions[i],Qt::EditRole);
        setData(index,true,Algorithm::HasBreakpoint_Role);
        setData(index,false,Algorithm::Breakpoint_Role);
        switch(i)
        {
            case HEADER:
                setData(index,false,Algorithm::HasBreakpoint_Role);
                break;
        }
    }


    //
    // Connect algorithm buttons.
    //
    connect(this->algorithm_widget,SIGNAL(playPressed(int)),this,SLOT(runAlgorithm(int)));
    connect(this->algorithm_widget,SIGNAL(stopPressed()),this,SLOT(stop()));
    connect(this->algorithm_widget,SIGNAL(prewPressed()),this,SLOT(prevStep()));
    connect(this->algorithm_widget,SIGNAL(nextPressed()),this,SLOT(nextStep()));
    connect(this->algorithm_widget, SIGNAL(checkSolutionPressed()), this, SLOT(checkSolution()));
    connect(this->algorithm_widget, SIGNAL(showCorrectSolutionPressed()), this, SLOT(showCorrectSolution()));
    connect(this->algorithm_widget, SIGNAL(showUserSolutionPressed()), this, SLOT(showUserSolution()));
    connect(this->algorithm_widget, SIGNAL(beginPressed()), this, SLOT(toBegin()));
    connect(this->algorithm_widget, SIGNAL(endPressed()), this, SLOT(toEnd()));

    //
    // Connect timers.
    //
    connect(play_timer, SIGNAL(timeout()), this, SLOT(nextStep()));
    connect(check_step_timer, SIGNAL(timeout()), this, SLOT(checkSolution()));

    // Connect Finite Automata widgets
    connect(not_dfa_widget,SIGNAL(FA_changed(FiniteAutomata*)),this,SLOT(setFA(FiniteAutomata*)));
    connect(dfa_widget,SIGNAL(FA_changed(FiniteAutomata*)),this,SLOT(setDFA(FiniteAutomata*)));

    not_dfa_widget->setFA(new FiniteAutomata());

    algorithm_widget->enableShowButton();
}
Beispiel #3
0
//Runs the genetic algorithm
void Solver::runAStar()
{
    for(int i=0;i<gateLimit;++i)
    {
        circuits.push_back(Circuit());
    }
    bool go = true;
    int loc = -1;
    unsigned long long int generation=0;
    do
    {
        //algorithm adds new random gate every generation, and reverts if the fitness value goes lower
        ++generation;
        addGatesAstar();
        calcFitness();
        revert();
        runSortFitness();
        cull();
        loc = checkSolution();
        if(generation%1000 == 0)
        {
            cout<<"Generation: "<<generation<<"\n";
            cout<<"Num Gates:  "<<circuits[0].getGateNum()<<"\n";
        }
        if(loc != -1)
        {
            solutionLoc = loc;
            cout<<"Generation: "<<generation<<"\n";
            go = false;
        }
    }while(go);
}
Beispiel #4
0
void dfs(int nowv, int cost)
{
	static int w = 0;
	//printf("Dfs: the %d times\n", w++);
	//printf("node: %d cost: %d\n", nowv, cost);

	if(cost > min_cost) return;
	
	visited[nowv] = true;
	visitnum++;

	if(nowv == end){
		//printf("\nFind one sulution!  nowv: %d visited: %d  and cost: %d\n", nowv, visitnum, cost);
		bool flag = checkSolution(cost);
		if(true == flag){
			printf("\nFind one sulution!  nowv: %d visited: %d  and cost: %d\n", nowv, visitnum, cost);
			updateResult(visitnum-1, cost); //  visitnum个点被访问,产生visitnum-1条边
		}
		visited[nowv] = false;
		visitnum--;
		return ; 
	}

	for(int nextv = 0; nextv < MAXV; nextv++){
		if(isValidE(nowv, nextv, cost)){
			possible_result[visitnum-1] = netMap[nowv][nextv].num;
			dfs(nextv, cost + netMap[nowv][nextv].cost);
		}
	}
	
	visitnum--;
	visited[nowv] = false;
}
Beispiel #5
0
void runPolDecompTest(float tol)
{
	using scalar_t = float;
	using real_t = WideScalar;
	using matrix3_t = Eigen::Matrix<real_t, 3, 3>;
	using vector3_t = Eigen::Matrix<real_t, 3, 1>;

	size_t nr_problems = 128;
	Vcl::Core::InterleavedArray<scalar_t, 3, 3, -1> resR(nr_problems);
	Vcl::Core::InterleavedArray<scalar_t, 3, 3, -1> resS(nr_problems);

	Vcl::Core::InterleavedArray<scalar_t, 3, 3, -1> refR(nr_problems);
	Vcl::Core::InterleavedArray<scalar_t, 3, 3, -1> refS(nr_problems);

	auto F = createProblems<scalar_t>(nr_problems);
	computeReferenceSolution(nr_problems, F, refR, refS);

	// Strides
	size_t stride = nr_problems;
	size_t width = sizeof(real_t) / sizeof(scalar_t);

	for (int i = 0; i < static_cast<int>(stride / width); i++)
	{
		matrix3_t A = F.at<real_t>(i);
		matrix3_t R, S;

		Vcl::Mathematics::PolarDecomposition(A, R, &S);

		resR.at<real_t>(i) = R;
		resS.at<real_t>(i) = S;
	}

	// Check against reference solution
	checkSolution(nr_problems, tol, refR, refS, resR, resS);
}
Beispiel #6
0
void FlowersGui::updateAllButtons()
{
    for (int y = 0; y < puzzle->getRows(); y++) {
        for (int x = 0; x < puzzle->getCols(); x++) {
            char symbol = puzzle->getView(y, x);
            int i = y * puzzle->getCols() + x;
            buttons->button(i)->setText(QString(symbol));
        }
    }
    checkSolution();
}
Beispiel #7
0
int ParallelAlgorithm::communicationLoop(void)
{
	for(int processId = 0; processId < countOfProcesses; processId++)
	{
//		if(processId == myRank) 
//			continue;

		int flag = 0;
    		MPI_Iprobe(processId, MPI_ANY_TAG, MPI_COMM_WORLD, &flag, &status);

    		if (flag)
    		{
      		//v promenne status je tag (status.MPI_TAG), cislo odesilatele (status.MPI_SOURCE)
      		//a pripadne cislo chyby (status.MPI_ERROR)
      		switch (status.MPI_TAG)
      		{
         			case MSG_WORK_REQUEST : // zadost o praci, prijmout a dopovedet
					checkWorkRequest();
					break;

         			case MSG_WORK_TRANSMIT : // prisel rozdeleny zasobnik, prijmout
					checkIncommingWork();
                              break;

         			case MSG_WORK_NOWORK : // odmitnuti zadosti o praci
					std::cout << "Process #" << myRank << " recieved MSG_NO_WORK from #" << status.MPI_SOURCE << std::endl;
                              // zkusit jiny proces
//					requestMoreWork();
					break;

         			case MSG_TOKEN : //ukoncovaci token, prijmout a nasledne preposlat
					checkToken();					
                          	break;

				case MSG_SOLUTION_TRANSMIT: //prijmeme reseni od jineho procesu
					checkSolution();
					break;

         			case MSG_END_WORK : //konec vypoctu - proces 0 pomoci tokenu zjistil, ze j*z nikdo nema praci
					std::cout << "Process #" << myRank << " recieved MSG_END_WORK from " << status.MPI_SOURCE << std::endl;
					checkWorkEnd();
					return 2;
                           	break;

				default :
					throw new Exception("ERROR: Unknown message.");
      		}
    		}
	}
	return 0;
}
Beispiel #8
0
//Runs the genetic algorithm
//Note: Longer than 24 lines due to extensive commenting needed
void Solver::runGenetic()
{
    //Uses top 20% of children
    int keep = gateLimit/5;
    for(int i=0;i<keep;++i)
    {
        circuits.push_back(Circuit());
    }
    addNewGates(keep);
    bool go = true;
    int loc = -1;
    unsigned long long int generation=0;
    do
    {
        ++generation;
        //Randomly deletes a gate every 100 generations
        if(generation%100==0)
        {
            randomDelete();
        }
        //Gets all crosses for top 20% of children every 20 generations
        if(generation%20==0)
        {
            crossover(keep);
        }
        //Mutates a Random Gate every 10 generations
        if(generation%10==0)
        {
            mutate();
        }
        //Adds a new random Gate every generation
        addNewGates(keep);
        calcFitness();
        runSortFitness();
        shrink();
        cull();
        loc = checkSolution();
        if(generation%1000 == 0)
        {
            cout<<"Generation: "<<generation<<"\n";
            cout<<"Num Gates:  "<<circuits[0].getGateNum()<<"\n";
        }
        if(loc != -1)
        {
            solutionLoc = loc;
            cout<<"Generation: "<<generation<<"\n";
            go = false;
        }
    }while(go);
}
Beispiel #9
0
int march_solve_rec()
{
	int branch_literal = 0, _result, _percentage_forced;
	int skip_left = 0, skip_right = 0, top_flag = 0;
#ifdef DISTRIBUTION
	int record_index = current_record;

	top_flag = TOP_OF_TREE;

	if( fix_recorded_literals(record_index) == UNSAT )
	    return UNSAT;

	if( record_index == 0 ) record_index = init_new_record( );
#endif
#ifdef SUPER_LINEAR
	if( depth < sl_depth ) subtree_size = 0;
	else if( subtree_size == SL_MAX ) return UNSAT;
	else	subtree_size++;
#endif
#ifdef TIMEOUT
	if( (int) clock() > CLOCKS_PER_SEC * TIMEOUT )
	    return UNKNOWN;
#endif
#ifdef SUBTREE_SIZE
	path_length++;
#endif
#ifdef CUT_OFF
	if( depth <= CUT_OFF ) last_SAT_bin = -1;

        if( solution_bin == last_SAT_bin )
	{
#ifdef DISTRIBUTION
	    records[ record_index ].UNSAT_flag = 1;
#endif
            return UNSAT;
	}
#endif
#ifdef DETECT_COMPONENTS
	determine_components();
#endif
#ifdef DISTRIBUTION
	branch_literal = records[record_index].branch_literal;

	if( branch_literal != 0 ) dist_acc_flag = 1;
	else
//	if( branch_literal == 0 )
#endif
	do
	{
#ifdef LOCAL_AUTARKY
	    int _depth;
	
	    _depth = analyze_autarky();
	    if( _depth == 0 )
	        printf("c global autarky found at depth %i\n", depth );
	    else if( _depth != depth )
	        printf("c autarky found at depth %i (from %i)\n", depth, _depth );
#endif
	    nodeCount++;

//	    printf("node %i @ depth %i\n", nodeCount, depth );

	    if( ConstructCandidatesSet() == 0 )
	    { 
	        if( depth > 0 )
	        {
		    if( checkSolution() == SAT ) return verifySolution();
	        }
	    	if( PreselectAll() == 0 )
		    return verifySolution();
	    }
	    else	ConstructPreselectedSet();

	    if( lookahead() == UNSAT )
	    {
	    	lookDead++;
	    	return UNSAT;
	    }

	    if( propagate_forced_literals() == UNSAT )
		return UNSAT;

	    branch_literal = get_signedBranchVariable();

//	    printf("c branch literal %i", branch_literal );
	}
	while( (percentage_forced > 50.0) || (branch_literal == 0) );
#ifdef PLOT
	if( plotCount++ >= 10000 )
	    return UNSAT;

	printf("\t%i\t%.3f\t%i\n", plotCount, sum_plot / count_plot, depth );
#endif
	_percentage_forced = percentage_forced;

#ifdef PARALLEL
	if( depth < para_depth )
	    if( para_bin & (1 << (para_depth - depth - 1) ) )
	    	branch_literal *= -1;
#endif
//	printf("branch_literal = %i at depth %i\n", branch_literal, depth );
#ifdef GLOBAL_AUTARKY
	if( depth == 0 )
	{
	    int i;
	    for( i = 1; i <= nrofvars; i++ )
	    {
		TernaryImpReduction[  i ] = 0;
		TernaryImpReduction[ -i ] = 0;
		
		if( kSAT_flag )
		{
		    btb_size[  i ] = 0;
		    btb_size[ -i ] = 0;
		}
	    }

	    if( kSAT_flag )
		for( i = 0; i < nrofbigclauses; ++i )
		    clause_reduction[ i ] = 0;

//	    branch_literal = 10;
	}
#endif
	NODE_START();
#ifdef BLOCK_PRESELECT
	set_block_stamps( branch_literal );
#endif

#ifdef DISTRIBUTION
	if( top_flag )
	{	
	    branch_literal *= -1;
	    current_rights++;
	    UPDATE_BIN();
	}
	skip_left = skip_node();
#endif
	if( (skip_left==0) && IFIUP( branch_literal, FIX_BRANCH_VARIABLE ) )
	{ 
#ifdef DISTRIBUTION
	    current_record = LEFT_CHILD;
#endif
	    _result = recursive_solve();
#ifdef DISTRIBUTION
	    LEFT_CHILD = current_record;
#endif
	    if( _result == SAT || _result == UNKNOWN ) return _result; }
	else {  
#ifdef DISTRIBUTION
		if( (LEFT_CHILD != 0)  && records[ LEFT_CHILD ].UNSAT_flag == 0 )
		{
		    records[ LEFT_CHILD ].UNSAT_flag = 1; 
//		    printf("c left child %i UNSAT by parent!\n", LEFT_CHILD );
		}
#endif
		PUSH( r, STACK_BLOCK );}

	NODE_END();

#ifdef BACKJUMP
	if( backjump_literal != 0 )
	  if( timeAssignments[ backjump_literal ] >= NARY_MAX )
	  {
//		printf("backjumping at depth %i due to literal %i\n", depth, backjump_literal );
		return UNSAT;
	  }
	backjump_literal = 0;
#endif

#ifdef DISTRIBUTION
	if( top_flag )
	{
	    current_rights--;
	    UPDATE_BIN();
	}
#endif
	percentage_forced = _percentage_forced;

#ifdef PARALLEL
	if( depth < para_depth ) goto parallel_skip_node;
#endif

#ifdef GLOBAL_AUTARKY
	if( depth == 0 )
	  if( kSAT_flag )
	  {
	    int i;
	    for( i = 1; i <= nrofvars; ++i )
	    {
		assert( TernaryImpReduction[  i ] == 0 );
		assert( TernaryImpReduction[ -i ] == 0 );

		assert( btb_size[  i ] == 0 );
		assert( btb_size[ -i ] == 0 );
	    }

	    for( i = 0; i < nrofbigclauses; ++i )
	    {
		clause_red_depth[ i ] = nrofvars;
		clause_SAT_flag[ i ]  = 0;
	    }
	  }

	  if( kSAT_flag )
	  {
	    int i;
	    for( i = 1; i <= nrofvars; ++i )
	    {
		assert( TernaryImpReduction[  i ] >= 0 );
		assert( TernaryImpReduction[ -i ] >= 0 );

		assert( btb_size[  i ] >= 0 );
		assert( btb_size[ -i ] >= 0 );
	    }
	  }

#endif
	NODE_START();
#ifdef BLOCK_PRESELECT
	set_block_stamps( branch_literal );
#endif
        if( top_flag == 0 )
	{
#ifdef DISTRIBUTION
	    current_rights++;
#endif
	    UPDATE_BIN();
	}
#ifdef DISTRIBUTION
	skip_right = skip_node();
#endif
	if( (skip_right == 0) && IFIUP( -branch_literal, FIX_BRANCH_VARIABLE ) )
	{
#ifdef DISTRIBUTION
	    current_record = RIGHT_CHILD;
#endif
	    _result = recursive_solve();
#ifdef DISTRIBUTION
	    RIGHT_CHILD = current_record;
#endif
	    if( _result == SAT || _result == UNKNOWN ) return _result;}
	else {  
#ifdef DISTRIBUTION
		if( (RIGHT_CHILD != 0) && records[ RIGHT_CHILD ].UNSAT_flag == 0 )
		{
		    records[ RIGHT_CHILD ].UNSAT_flag = 1; 
//		    printf("c right child %i UNSAT by parent!\n", RIGHT_CHILD );
		}
#endif
		PUSH( r, STACK_BLOCK );}
	NODE_END();

	if( top_flag == 0 )
	{	
#ifdef DISTRIBUTION
	    current_rights--;
#endif
	    UPDATE_BIN();
	}

#ifdef PARALLEL
	parallel_skip_node:;
#endif
//	printf("ended node %i at depth %i\n", nodeCount, depth );

	

#ifdef SUBTREE_SIZE
	if( (skip_flag == 0) && (jump_depth == 0)  && (current_rights == 0) )
	{
	    int subtree = path_length - depth;

//	    float cost = nodeCount * (CLOCKS_PER_SEC /  ((float)(clock() + 10 - first_time)));
//	    printf("c nodes per second = %.3f at level %i\n", cost, depth );

	    if( jump_depth >= 30 ) jump_depth = 999;

	    if( subtree >     SUBTREE_SIZE )
	    {
	        jump_depth = depth;

	        while( subtree > 2*SUBTREE_SIZE )
	        {
		   jump_depth++; 
		   subtree = subtree / 2;
	        }

	        if( jump_depth >= 20 ) jump_depth = 999;

	        skip_flag = 1;
	    }
	}
#endif
#ifdef DISTRIBUTION
	record_node( record_index, branch_literal, skip_left, skip_right );

	current_record = record_index;
#endif

#ifdef BACKJUMP
	if( kSAT_flag )
	{
	    int *_rstackp = rstackp, nrval;
	
	    while( --_rstackp > rstack )
	    {
		nrval = *_rstackp;
		if( (TernaryImpReduction[ nrval ] + TernaryImpReduction[ -nrval ]) != 0 )
		{
		    backjump_literal = nrval;
		    break;
		}
	    }
	}
#endif
#ifdef ADD_CONFLICT
	printConflict();
#endif
	return UNSAT;
}
Beispiel #10
0
Datei: SBL.cpp Projekt: ompl/ompl
ompl::base::PlannerStatus ompl::geometric::SBL::solve(const base::PlannerTerminationCondition &ptc)
{
    checkValidity();
    auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());

    if (!goal)
    {
        OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
        return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
    }

    while (const base::State *st = pis_.nextStart())
    {
        auto *motion = new Motion(si_);
        si_->copyState(motion->state, st);
        motion->valid = true;
        motion->root = motion->state;
        addMotion(tStart_, motion);
    }

    if (tStart_.size == 0)
    {
        OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
        return base::PlannerStatus::INVALID_START;
    }

    if (!goal->couldSample())
    {
        OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
        return base::PlannerStatus::INVALID_GOAL;
    }

    if (!sampler_)
        sampler_ = si_->allocValidStateSampler();

    OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),
                (int)(tStart_.size + tGoal_.size));

    std::vector<Motion *> solution;
    base::State *xstate = si_->allocState();

    bool startTree = true;
    bool solved = false;

    while (ptc == false)
    {
        TreeData &tree = startTree ? tStart_ : tGoal_;
        startTree = !startTree;
        TreeData &otherTree = startTree ? tStart_ : tGoal_;

        // if we have not sampled too many goals already
        if (tGoal_.size == 0 || pis_.getSampledGoalsCount() < tGoal_.size / 2)
        {
            const base::State *st = tGoal_.size == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
            if (st)
            {
                auto *motion = new Motion(si_);
                si_->copyState(motion->state, st);
                motion->root = motion->state;
                motion->valid = true;
                addMotion(tGoal_, motion);
            }
            if (tGoal_.size == 0)
            {
                OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
                break;
            }
        }

        Motion *existing = selectMotion(tree);
        assert(existing);
        if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
            continue;

        /* create a motion */
        auto *motion = new Motion(si_);
        si_->copyState(motion->state, xstate);
        motion->parent = existing;
        motion->root = existing->root;
        existing->children.push_back(motion);

        addMotion(tree, motion);

        if (checkSolution(!startTree, tree, otherTree, motion, solution))
        {
            auto path(std::make_shared<PathGeometric>(si_));
            for (auto &i : solution)
                path->append(i->state);

            pdef_->addSolutionPath(path, false, 0.0, getName());
            solved = true;
            break;
        }
    }

    si_->freeState(xstate);

    OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", getName().c_str(),
                tStart_.size + tGoal_.size, tStart_.size, tGoal_.size, tStart_.grid.size() + tGoal_.grid.size(),
                tStart_.grid.size(), tGoal_.grid.size());

    return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}
Beispiel #11
0
static void activatePeg()
{
	int val;
	Entity *e;

	e = self->target;

	while (e != NULL)
	{
		if (e->x == self->endX && e->y == self->endY)
		{
			if (e->mental == 0)
			{
				e->health++;

				if (e->health > self->health)
				{
					e->health = 1;
				}

				setEntityAnimationByID(e, e->health);
			}

			else
			{
				val = checkSolution();

				if (val == TRUE)
				{
					self->mental = 2;

					e = self->target;

					while (e != NULL)
					{
						e->flags &= ~DO_NOT_PERSIST;

						e = e->target;
					}
				}

				else if (val == FALSE)
				{
					self->endY -= TILE_SIZE;
					self->endX = self->x;

					if (self->endY < self->y)
					{
						self->endY = self->y;

						self->mental = 1;
					}
				}
			}

			break;
		}

		e = e->target;
	}
}
Beispiel #12
0
void ompl::geometric::pSBL::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol)
{
    RNG rng;

    std::vector<Motion*> solution;
    base::State *xstate = si_->allocState();
    bool      startTree = rng.uniformBool();

    while (!sol->found && ptc == false)
    {
        bool retry = true;
        while (retry && !sol->found && ptc == false)
        {
            removeList_.lock.lock();
            if (!removeList_.motions.empty())
            {
                if (loopLock_.try_lock())
                {
                    retry = false;
                    std::map<Motion*, bool> seen;
                    for (unsigned int i = 0 ; i < removeList_.motions.size() ; ++i)
                        if (seen.find(removeList_.motions[i].motion) == seen.end())
                            removeMotion(*removeList_.motions[i].tree, removeList_.motions[i].motion, seen);
                    removeList_.motions.clear();
                    loopLock_.unlock();
                }
            }
            else
                retry = false;
            removeList_.lock.unlock();
        }

        if (sol->found || ptc)
            break;

        loopLockCounter_.lock();
        if (loopCounter_ == 0)
            loopLock_.lock();
        loopCounter_++;
        loopLockCounter_.unlock();


        TreeData &tree      = startTree ? tStart_ : tGoal_;
        startTree = !startTree;
        TreeData &otherTree = startTree ? tStart_ : tGoal_;

        Motion *existing = selectMotion(rng, tree);
        if (!samplerArray_[tid]->sampleNear(xstate, existing->state, maxDistance_))
            continue;

        /* create a motion */
        Motion *motion = new Motion(si_);
        si_->copyState(motion->state, xstate);
        motion->parent = existing;
        motion->root = existing->root;

        existing->lock.lock();
        existing->children.push_back(motion);
        existing->lock.unlock();

        addMotion(tree, motion);

        if (checkSolution(rng, !startTree, tree, otherTree, motion, solution))
        {
            sol->lock.lock();
            if (!sol->found)
            {
                sol->found = true;
                PathGeometric *path = new PathGeometric(si_);
                for (unsigned int i = 0 ; i < solution.size() ; ++i)
                    path->append(solution[i]->state);
                pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName());
            }
            sol->lock.unlock();
        }


        loopLockCounter_.lock();
        loopCounter_--;
        if (loopCounter_ == 0)
            loopLock_.unlock();
        loopLockCounter_.unlock();
    }

    si_->freeState(xstate);
}
Beispiel #13
0
void H1_heuristic (Problem * pb, Solution * s)
{
  initSolution (s);
  checkSolution (s, pb);

  /* build one route for each request */
  double distance = 0.0;
  for (uint16 req = 0; req < pb->nb_requests; ++req)
  {
    /* the four nodes involved */
    uint16 out  = s->id_out + req;
    uint16 pic  = pb->requests[req].orig->id;
    uint16 del  = pb->requests[req].dest->id;
    uint16 in   = s->id_in + req;

    /* set the next - prev chain */
    s->next[out] = pic;
    s->next[pic] = del;
    s->next[del] = in;
    s->prev[pic] = out;
    s->prev[del] = pic;
    s->prev[in]  = del;

    /* set the travel time */
    double arrival = 0.0;
    if (arrival < pb->nodes[0].open)
      arrival = pb->nodes[0].open;
    s->arrival[out] = arrival;
    arrival += pb->nodes[0].service;
    arrival += pb->duration[0][pic];
    if (arrival < pb->nodes[pic].open)
      arrival = pb->nodes[pic].open;
    s->arrival[pic] = arrival;
    arrival += pb->nodes[pic].service;
    arrival += pb->duration[pic][del];
    if (arrival < pb->nodes[del].open)
      arrival = pb->nodes[del].open;
    s->arrival[del] = arrival;
    arrival += pb->nodes[del].service;
    arrival += pb->duration[del][0];
    if (arrival < pb->nodes[0].open)
      arrival = pb->nodes[0].open;
    s->arrival[in] = arrival;

    /* compute the latest arrival time */
    double latest = pb->nodes[0].close;
    s->latest[in] = latest;
    latest -= pb->duration[del][0] + pb->nodes[del].service;
    if (latest > pb->nodes[del].close)
      latest = pb->nodes[del].close;
    s->latest[del] = latest;
    latest -= pb->duration[pic][del] + pb->nodes[pic].service;
    if (latest > pb->nodes[pic].close)
      latest = pb->nodes[pic].close;
    s->latest[pic] = latest;
    latest -= pb->duration[0][pic] + pb->nodes[0].service;
    if (latest > pb->nodes[0].close)
      latest = pb->nodes[0].close;
    s->latest[out] = latest;

    /* set the capacity */
    uint16 amount = pb->requests[req].amount;
    s->load[out] = 0;
    s->load[pic] = 0;
    s->load[del] = amount;
    s->load[in]  = 0;

    /* update the number of vehicles used */
    ++s->nb_vehicles;

    /* update the total distance */
    distance += pb->duration[0][pic] +
                pb->duration[pic][del] +
                pb->duration[del][0];
  }

  s->distance = distance;
}