void AdjMatrix::makePlanes()
{
    for(int i = 1; i <= r; i++) // planes
    {
        int tempPlane = i;
        int start = i;
        int beg = r+1, end = 2*r;

        while(start <= nr)           // connections
        {
            addEdges(tempPlane, start, beg, end);
            start += 2*r;
            /*if(n%2 == 0 && start > nr)
            {
                start -= 1;
                beg -= 1;
                end -= 1;
            }*/

            if(start <= nr || n%2 == 0)
                addEdges(tempPlane, start, beg, end);
            beg += 2*r;
            end += 2*r;
        }
    }
}
istream& BipartiteGraph<VT>::getEdges(istream& in)
{
    LiBEVector<LiBEPair<POSITTYPE, POSITTYPE> > edges;
    in >> edges;
    addEdges(edges);
    return in;
}
Example #3
0
/**
 * Convert an Image
 * into a Graph
 *
 * @param graph			The graph pointer
 * @param img			The Source image
 * @param neighboursWeight	Neighbours influence
 * @param windowSize	Neighbours range
 *
 *
 * @return Updates the graph
 */
void GRAPHCUT::img2graph(GCGraph<float>& graph, Mat& img, float neighboursWeight, int windowSize)
{
	cvtColor(img, img, CV_BGR2GRAY);
	normalize(img, img, 0, 1, NORM_MINMAX, CV_32F);

	addVertices(graph, img);
	addEdges(graph, img, neighboursWeight, windowSize);
}
Example #4
0
void GraphBuilder::generateTestWeb() {
    delete constructed;
    constructed = new Graph(6);

    addVertex(0);   // A
    addVertex(1);   // B
    addVertex(2);   // C
    addVertex(3);   // D
    addVertex(4);   // E
    addVertex(5);   // F

    // TODO: add throwughputs
    addEdges(0, {{1, 9}, {2, 8}});
    addEdges(1, {{3, 6}, {4, 3}});
    addEdges(2, {{4, 4}});
    addEdges(3, {{5, 10}});
    addEdges(4, {{3, 4}, {5, 7}});
}
Example #5
0
void GraphBuilder::generateFlowTestGraph() {
    delete constructed;
    constructed = new Graph(6);

    addVertex(0);
    addVertex(1);
    addVertex(2);
    addVertex(3);
    addVertex(4);
    addVertex(5);

    // TODO: add throwughputs
    addEdges(0, {{1, 3}, {2, 4}});
    addEdges(1, {{3, 3}, {4, 1}});
    addEdges(2, {{3, 1}, {4, 2}});
    addEdges(3, {{5, 3}});
    addEdges(4, {{5, 5}});
}
void ompl::LTLVis::VRVPlanner::sampleAndConnect(const ompl::base::PlannerTerminationCondition &ptc)
{
  bool extended = false;
  while((!extended) && (false == ptc()))
  {
    ompl::base::State* newState = si_->allocState();
ompl::tools::Profiler::Begin("sampling");
    while((!validSampler_->sample(newState)) && (false == ptc()))
    {
    }
ompl::tools::Profiler::End("sampling");
    if(false == ptc())
    {
      //Valid state found. Create a temp vertex for it
      ompl::LTLVis::tIndex newIndex = vertices_.size();
      ompl::LTLVis::VRVVertex dummy(si_, newState);
      vertices_.push_back(dummy);
      //Find neighbors and their CCs
      //TODO: add some variable radius function;
      double r = si_->getMaximumExtent()/std::pow(1.0 + vertices_.size(), 1.0/(1.0*si_->getStateDimension()));
      std::vector<ompl::LTLVis::tIndex> neighbors;
      getValidRNeighborhood(newIndex, r, neighbors);
      std::set<ompl::LTLVis::tIndex> neighborCCs;
      getNeighborCCs(neighbors, neighborCCs);
      if(0 == neighbors.size())
      {
ompl::tools::Profiler::Event("new CC");
        extended = true;
        //No connection to vertices in the roadmap: node improves coverage. Add to roadmap.
        vertices_[newIndex].setIndex(newIndex);
        nearFinder_.add(newIndex);
        vertices_[newIndex].setCCIndex(CCs_.makeNewComponent());
      }
      else if((1 == neighbors.size()) || (1 < neighborCCs.size()))
      {
ompl::tools::Profiler::Event("new Connection");
        extended = true;
        //Either extends in a hard to reach area or improves connectivity. Add to roadmap.
        nearFinder_.add(newIndex);
        vertices_[newIndex].setCCIndex(CCs_.makeNewComponent());
        vertices_[newIndex].setIndex(newIndex);
        addEdges(newIndex, neighbors, true, true);
      }
      //TODO: add useful loop/path shortening heuristic
      else
      {
        //Remove vertex from the roadmap, it does not appear useful.
ompl::tools::Profiler::Event("rejection");
        vertices_.pop_back();
      }
    }
    si_->freeState(newState);
  }
}
void mapQuadTreeAbstraction::buildAbstraction()
{
	//inefficient for the moment
	abstractions.push_back(getMapGraph(getMap()));
	while (abstractions.back()->getNumEdges() > 0)
	{
		graph *g = new graph();
		addNodes(g);
		addEdges(g);
		abstractions.push_back(g);
	}
}
    void build(Inst& inst, const Liveness<Tmp>::LocalCalc& localCalc)
    {
        inst.forEachDefAndExtraClobberedTmp(type, [&] (Tmp& arg) {
            // All the Def()s interfere with each other and with all the extra clobbered Tmps.
            // We should not use forEachDefAndExtraClobberedTmp() here since colored Tmps
            // do not need interference edges in our implementation.
            inst.forEachTmp([&] (Tmp& otherArg, Arg::Role role, Arg::Type otherArgType) {
                if (otherArgType != type)
                    return;

                if (Arg::isDef(role))
                    addEdge(arg, otherArg);
            });
        });

        if (MoveInstHelper<type>::mayBeCoalescable(inst)) {
            // We do not want the Use() of this move to interfere with the Def(), even if it is live
            // after the Move. If we were to add the interference edge, it would be impossible to
            // coalesce the Move even if the two Tmp never interfere anywhere.
            Tmp defTmp;
            Tmp useTmp;
            inst.forEachTmp([&defTmp, &useTmp] (Tmp& argTmp, Arg::Role role, Arg::Type) {
                if (Arg::isDef(role))
                    defTmp = argTmp;
                else {
                    ASSERT(Arg::isUse(role));
                    useTmp = argTmp;
                }
            });
            ASSERT(defTmp);
            ASSERT(useTmp);

            unsigned nextMoveIndex = m_coalescingCandidates.size();
            m_coalescingCandidates.append({ useTmp, defTmp });

            unsigned newIndexInWorklist = m_worklistMoves.addMove();
            ASSERT_UNUSED(newIndexInWorklist, newIndexInWorklist == nextMoveIndex);

            ASSERT(nextMoveIndex <= m_activeMoves.size());
            m_activeMoves.ensureSize(nextMoveIndex + 1);

            for (const Arg& arg : inst.args) {
                auto& list = m_moveList[AbsoluteTmpHelper<type>::absoluteIndex(arg.tmp())];
                list.add(nextMoveIndex);
            }

            for (const Tmp& liveTmp : localCalc.live()) {
                if (liveTmp != useTmp && liveTmp.isGP() == (type == Arg::GP))
                    addEdge(defTmp, liveTmp);
            }
        } else
            addEdges(inst, localCalc.live());
    }
/*private*/
void
ConnectedSubgraphFinder::addReachable(Node* startNode,
		Subgraph* subgraph)
{
	stack<Node *> nodeStack;
	nodeStack.push(startNode);
	while ( !nodeStack.empty() )
	{
		Node* node = nodeStack.top();
		nodeStack.pop();
		addEdges(node, nodeStack, subgraph);
	}
}
void DependenceAnalysis::analyze(ir::IRKernel& kernel)
{
	report("Running dependence analysis on kernel " << kernel.name);
	
	auto controlDependenceAnalysis = static_cast<ControlDependenceAnalysis*>(
		getAnalysis("ControlDependenceAnalysis")); 
	auto dataDependenceAnalysis = static_cast<DataDependenceAnalysis*>(
		getAnalysis("DataDependenceAnalysis")); 
	auto memoryDependenceAnalysis = static_cast<MemoryDependenceAnalysis*>(
		getAnalysis("MemoryDependenceAnalysis")); 
		
	for(auto& node : *controlDependenceAnalysis)
	{
		auto newNode = _nodes.insert(_nodes.end(), Node(node.instruction));
	
		_instructionToNodes.insert(std::make_pair(node.instruction, newNode));
	}
	
	for(auto& node : *this)
	{
		auto controlDependenceNode = controlDependenceAnalysis->getNode(
			node.instruction);
	
		addEdges(node, *controlDependenceNode, _instructionToNodes);

		auto dataDependenceNode = dataDependenceAnalysis->getNode(
			node.instruction);
	
		addEdges(node, *dataDependenceNode, _instructionToNodes);
		
		auto memoryDependenceNode = memoryDependenceAnalysis->getNode(
			node.instruction);
		
		if(memoryDependenceNode != memoryDependenceAnalysis->end())
		{
			addEdges(node, *memoryDependenceNode, _instructionToNodes);
		}
	}
}
void ompl::LTLVis::VRVPlanner::addVertex(const ompl::base::State *newState)
{
  ompl::LTLVis::tIndex newIndex = vertices_.size();
  ompl::LTLVis::VRVVertex dummy(si_, newState);
  vertices_.push_back(dummy);
  vertices_[newIndex].setIndex(newIndex);
  vertices_[newIndex].setCCIndex(CCs_.makeNewComponent());
  std::vector<ompl::LTLVis::tIndex> neighbors;
  getValidRNeighborhood(newIndex, 1e9, neighbors);
  //std::cout << "!!! " << vertices_.size() << ": " << neighbors.size() << std::endl;
  nearFinder_.add(newIndex);
  addEdges(newIndex, neighbors, true, true);
}
Example #12
0
void ACIS_Internals::loadSAT(std::string fileName, GModel *gm)
{
  FILE *f = Fopen (fileName.c_str(), "r");
  if (!f){
    return;
  }
  outcome prout = api_restore_entity_list(f,1,entities);
  if (!prout.ok()){
    Msg::Error("Unable to load ACIS FILE %d",fileName.c_str());
    fclose(f);
    return;
  }
  Msg::Info("ACIS FILE %d Loaded",fileName.c_str());

  ENTITY *e;
  entities.init();
  while((e = entities.next())){
    //    printf("an entity\n");
    if (is_VERTEX(e)){
      //      printf("VERTEX FOUND\n");
    }
    if (is_BODY(e)){
      api_split_periodic_faces(e);
      {
	ENTITY_LIST vertex_list;
	outcome prout = api_get_vertices (e,vertex_list);
	addVertices (gm,vertex_list);
	printf("BODY COUNT %d !\n",vertex_list.count());
      }
      {
	ENTITY_LIST edge_list;
	outcome prout = api_get_edges (e,edge_list);
	addEdges (gm,edge_list);
	printf("BODY COUNT %d !\n",edge_list.count());
      }
      {
	ENTITY_LIST face_list;
	outcome prout = api_get_faces(e,face_list);
	addFaces (gm,face_list);
	printf("BODY COUNT %d !\n",face_list.count());
      }
    }
  }
}
Example #13
0
bool AddAction::apply(Lattice &lattice, std::string langCode,
        int matchedStartIndex, RuleTokenSizes &ruleTokenSizes,
        std::list<Lattice::EdgeSequence>&) {
    int count = ruleTokenSizes[tokenIndex - 1];
    if (count == 0) {
//        std::cout << "Nothing matched to " << tokenIndex << " in ...." << std::endl;
        return true;
    }
    int before = util::getAddActionParams(ruleTokenSizes, tokenIndex);

    Lattice::VertexDescriptor startVertex = lattice::getVertex(lattice,
            langCode, before, matchedStartIndex);
    Lattice::VertexDescriptor endVertex = lattice::getVertex(lattice,
            langCode, before + count - 1, matchedStartIndex);

    addEdges(lattice, langCode, startVertex, endVertex);

    return true;
}
int parseGraphFile(FILE * file) {
	char cNodeA, cNodeB;
	int weight;
	int n = 0;
	node * ptrA;
	node * ptrB;
	
	while((n = fscanf(file,"%c;%d;%c\n",&cNodeA,&weight,&cNodeB)) != EOF) {
		if(n != 3) {
			PRINT(DEBUG,"Ignoring line starting with #\n");
			while(getc(file) != '\n');
			continue; //Ignore lines starting with #
		}
		PRINT(DEBUG,"Pr Add: %c -> %d -> %c\n",cNodeA,weight,cNodeB);
		ptrA = getNode(cNodeA);
		ptrB = getNode(cNodeB);
		addEdges(ptrA, ptrB, weight);
	}
	
	return 0;
}
Example #15
0
void Graph::addEdge(igraph_integer_t source, igraph_integer_t target) {
    Vector edge(2);
    edge[0] = source; edge[1] = target;
    addEdges(edge);
}
Example #16
0
/*
 * Takes input till we see an EOF. Input lines may be:
 * 1. V int -- specifies a (new) set of vertices. We assume that the
 *               identities of the vertices are 0..<int>-1.
 * 2. E {<int,int>,<int,int>...} -- specifies edges in undirected graph
 * 3. s int int --specifies the origin and end point of a shortest path that user wants to know
 */
int main() {
    struct listType **adjList = NULL; 
	/* v is an array. Each entry in the array contains a list of
	 * type struct listType.
	 */

    int state = 0;
	/* To keep track of the current state of our state-machine.
	 * 0 -- means we are ready to read the 1st char of a line.
	 * 1 -- means we have read V, and are looking for # of vertices.
	 * 2 -- means we have read E, and are looking for 1st vert. in edge.
	 * 3 -- means we have read 1st vert of edge, and are looking for 2nd.
	 * 4 -- means we have read s, and looking for origin point the shortest path user wants to know
	 * 5 -- means we have read origin point and looking for the end point
	 */

    char c = '\0'; /* Input character */
    int nVerts = 0; /* To store # of vertices */
    int nEdges = 0; /* to store number of edges in graph*/
    int v[4] = {-1}; /* v[0] and v[1] to store two vertices that are to be an edge */
			/*v[3] and v[4] to store two vertices that are to be origin and end point of route*/
    int in_brace=0; //flag to show if we are in a pair of { }
    int in_bracket=0; //flag to show if we are in a pair fo < >
    int init_done=0; //flag to show if the initialization of graph is finished
    while(scanf("%c", &c) >= 1) 
	{
		if(state == 0) 
		{
			if(c == 'V') 
			{
				freeAllLists(adjList, nVerts);
				init_done = 0;
				if(adjList != NULL) 
				{
			    	free(adjList);
			    	adjList = NULL;
				}
				state = 1;
				nVerts = 0;
				continue;
		    }
		    
			else if(c == 'E') 
			{
				/* Have we read vertices yet? */
				if(adjList == NULL) 
				{
					printf("Error: need \'V\' specification first.\n"); fflush(stdout);
					skipToNextLineOfInput();
					continue;
				}
				init_done = 0;
				bzero(adjList,nVerts*sizeof(struct listType *));
				state = 2; continue;
			}
		    
			else if(c == 's') 
			{
				//have we read vertices yet?
				if(adjList == NULL)
				{
					printf("Error: need \'V\' specification first.\n"); fflush(stdout);
					skipToNextLineOfInput();
					continue;
				}
				//judge whether vertex and edges have been initiated				
				if(init_done!=1)
				{
					printf("Error: specifying edge first.\n"); fflush(stdout);
					state=2;
					continue;
				}
				state = 4;
				continue;
			}
			else 
			{
				/* Error! */
				printf("Error: Unrecognized command. Rejecting line.\n"); fflush(stdout);
				skipToNextLineOfInput();
				continue;
			}
		}
		else if(state == 1) 
		{
			if(readInteger(c, &nVerts, 0) < 0) 
			{
				printf("Error: Erroneous line. Rejecting it.\n"); fflush(stdout);
				skipToNextLineOfInput();
				state = 0;
				continue;
		    	}
	
			/* Allocate an nVerts sized array of (struct listType *) */
		 	adjList = (struct listType **)malloc(nVerts * sizeof(struct listType *));
		 	if(adjList == NULL) 
			{
				printf("Error: malloc() returned null.\n"); fflush(stdout);
				break;
		 	}
	
		 	bzero(adjList, nVerts*sizeof(struct listType *));
	
		 	state = 0; 
		 	continue;
		}
		else if(state == 2 || state == 3) 
		{
			//the ',' in '>,<',ignore it,do nothing until next loop	
			if(c==' '||c=='\t')
				continue;
			//when '{' appears, in_brace=1 to suggest we are in a { }
			if(c=='{')
			{
				in_brace=1;
				continue;
			}
			//when '}' appears, in_brace=0 to suggest we are out of the { }
			if(c=='}')
			{
				in_brace=0;
				continue;
			}
			//if in_brace=0 and c='\n', we arrive at the end of E command, 
			//so, transfer to state 0 and wait for new command
			if(in_brace==0&&c=='\n')
			{
				init_done=1; //the initialization of graph finishd
				state=0;
				continue;
			}
			if(in_brace&&c=='<')
			{
				in_bracket=1;
				continue;
			}
			if(in_bracket)
			{
				//if(readInteger(c, &(v[state - 2]), !(state - 2)) < 0) 
				int t=0;

				t=readInteger(c,&(v[state-2]), !(state-2));
				if(t<0)
				{
					printf("Error: Erroneous line. Rejecting it.\n"); fflush(stdout);
					skipToNextLineOfInput();
					state = 0;
					continue;
				}
				if(state == 2) 
				{
					state = 3; 
					continue;
				}
	
				/* Else -- state == 3 */
				if(v[0] == v[1]) 
				{
					printf("Error: Cannot add self-edge.\n"); fflush(stdout);
					state = 0;
					continue;
				}
		
				if(addEdges(v, adjList, nVerts) < 0) 
				{
					printf("Error: could not add edges.\n"); fflush(stdout);
				}
				
				if(t==1)
				{
					in_bracket=0;
					state=2;
					continue;
				}
			}
		}
		else if(state == 4 || state == 5) 
		{
			
			int t = readInteger(c, &(v[state - 2]), !(state - 4));
			if(t==-1)
			{
				printf("Error: command s needs two integers as begin and end point\n"); fflush(stdout);
				state = 0;
				continue;
			}
			if(t<0) 
			{
				printf("Error: Erroneous line. Rejecting it.\n"); fflush(stdout);
				skipToNextLineOfInput();
				state = 0;
				continue;
		    	}
	
			if(state == 4) 
			{
				state = 5; continue;
			}

			/* Else -- state == 5 */
			if(v[2] < 0 || v[3] < 0 || v[2] >= nVerts || v[3] >= nVerts) 
			{
				/* Error! */
				printf("Error: specifying invalid vertex\n"); fflush(stdout);
				state=0;
				continue;
			}
			//if begin and end are same vertice, obvious result
			if(v[2] == v[3]) 
			{
				printf("%d\n",v[2]);fflush(stdout);
				state = 0;
				continue;
			}
			
			
			NodeType *nodes=NULL;
			nodes=(NodeType *)malloc(sizeof(NodeType)*nVerts);

		    if(nodes == NULL) 
			{
				printf("Error: malloc() returned NULL.\n"); fflush(stdout);
				continue;
			}	
			
			EdgeType *edges=NULL;
			edges=(EdgeType *)malloc(sizeof(EdgeType)*(nVerts*(nVerts-1)/2));

			if(edges == NULL) 
			{
				printf("Error: malloc() returned NULL.\n"); fflush(stdout);
				continue;
			}
			//nEdges is the number of edges in the graph
			nEdges = GenEdge(adjList, nVerts, edges);
			
			//tempflag is a flag to show if the begin and end point are exchanged
			int tempflag=-1;
			if(v[2]>v[3])
			{	
				tempflag=v[2];
				v[2]=v[3];
				v[3]=tempflag;
			}
			
			//implementing Bellman-Ford algorithm
			BellmanFord(v[2], nVerts, nEdges, nodes, edges);
			//finding out the shortest path, if it exists
			GenPath(nodes, nVerts, v[2], v[3], tempflag);
			
			//release this area of memory
			free(nodes);
			nodes=NULL;
			free(edges);
			edges=NULL;
				
	    	state = 0;
		    continue;
		}
		else 
		{
		    /* Weird -- state should never be anything else */
		    /* Indicates a serious bug in the program */
	
		    printf("Error: state == %d unexpected.\n", state); fflush(stdout);
		    break;
		}
    }

    /* We read EOF, or there was some other input that made scanf() fail.
     * Exit.
     */

    //printf("Exiting...\n");
    return 0;
}
	CWorkFlowGraph::CWorkFlowGraph( const std::list<const CBaseInstruction*>& asmFunction ) : CGraph( asmFunction.size() )
	{
		buildLabelMap( asmFunction );
		addEdges( asmFunction );
	}
ompl::base::PlannerStatus ompl::LTLVis::VRVPlanner::solve(const ompl::base::PlannerTerminationCondition &ptc)
{

  loadVerticesWithMetaData();
  std::cout << "Vertices loaded. " << std::endl;

  //prevent nasty overflows of the crVisitation_ index (unlikely though they are)
  if(ULONG_MAX - vertices_.size() < crVisitation_)
  {
    resetVisitation();
  }

  // make sure the planner is configured correctly; ompl::base::Planner::checkValidity
  // ensures that there is at least one input state and a ompl::base::Goal object specified
  checkValidity();
  std::cout << "Planner validity checked. " << std::endl;
  // get a handle to the Goal from the ompl::base::ProblemDefinition member, pdef_
  ompl::base::Goal *goal = pdef_->getGoal().get();
  std::cout << "Obtained goal. " << std::endl;
  ompl::geometric::PathGeometric solution(si_);
  bool pathFound = false;

ompl::tools::Profiler::Clear();
ompl::tools::Profiler::Start();

  std::cout << "VRV planner starting ... " << std::endl;
  std::cout << "Roadmap currently has " << vertices_.size() << " vertices." << std::endl;

  //Obtain one start and one goal state.
  //TODO: possibly make this more flexible to allow several start/goal states
  const ompl::base::State *startState = pis_.nextStart();
  const ompl::base::State *goalState = pis_.nextGoal(ptc);

  if(!startState)
  {
    return ompl::base::PlannerStatus::INVALID_START;
  }
  if(!goalState)
  {
    return ompl::base::PlannerStatus::INVALID_GOAL;
  }

  //Create temp vertices for the start and goal states
  ompl::LTLVis::tIndex startIndex = vertices_.size();
  ompl::LTLVis::tIndex goalIndex = startIndex + 1;

  vertices_.push_back(VRVVertex(si_, startState));
  vertices_.push_back(VRVVertex(si_, goalState));
  vertices_[startIndex].setIsStart();
  vertices_[startIndex].setIndex(startIndex);
  vertices_[goalIndex].setIsGoal();
  vertices_[goalIndex].setIndex(goalIndex);

  //Look for neighbors of the start/goal states in the roadmap
  //  do motion validation here. Keep only reachable neighbors. Lazy check not implemented.
  std::vector<ompl::LTLVis::tIndex> startNeighbors;
  std::vector<ompl::LTLVis::tIndex> goalNeighbors;
  getValidKNeighborhood(startIndex, 40, startNeighbors);
  getValidKNeighborhood(goalIndex, 40, goalNeighbors);


  //Check if the there is some path between a start and goal state: one pair of start/goal states
  //connects to the same connected component
  std::set<ompl::LTLVis::tIndex> startNeighborCCs;
  std::set<ompl::LTLVis::tIndex> goalNeighborCCs;
  std::set<ompl::LTLVis::tIndex> commonCCs;
  getNeighborCCs(startNeighbors, startNeighborCCs);
  getNeighborCCs(goalNeighbors, goalNeighborCCs);
  getCommonCCs(startNeighborCCs, goalNeighborCCs, commonCCs);

  if(commonCCs.size() && (1 == goalNeighborCCs.size()) && (1 == startNeighborCCs.size()))
  {
  std::cout << "Start and goal connectable to " << *(commonCCs.begin()) << std::endl;
    //If path between start and goal exists through a component C:
    //  add connectable start/goal to roadmap as temporary vertices
    //    {current implementation uses only one start and one goal state, and they were already added above}
    //  add temporary edges start->{neighbor vertices in C}
    addEdges(startIndex, startNeighbors, true, false);
    //  add temporary edges {neighbor vertices in C}->goal
    addEdges(goalIndex, goalNeighbors, false, true);
    //  check whether the problem is trivially solvable
    if(si_->checkMotion(vertices_[startIndex].getStateData(), vertices_[goalIndex].getStateData()))
    {
      std::vector<ompl::LTLVis::tIndex> dummy;
      std::cout << "Start and goal trivially connectable." << std::endl;
      dummy.clear(); dummy.push_back(goalIndex);
      addEdges(startIndex, dummy, true, false);
    }
    //  graph search, extract path
    pathFound = GraphSearch(ptc, solution, false);
    //  remove temporary edges and vertices
//  unsigned int kMax = vertices_.size() - 2;
//  for(unsigned int k = 0; k < kMax; k++)
//  {
//    std::cout << "CC " << vertices_[k].getCCIndex() << ": " << (verifyCC(k)?"consistent":"SET FAIL") << std::endl;
//  }
    ompl::LTLVis::tIndex maxK = goalNeighbors.size();
    for(ompl::LTLVis::tIndex k = 0; k < maxK; k++)
    {
      vertices_[goalNeighbors[k]].popEdge();
    }
    //if(pathFound)
    //  storeVerticesWithMetaData();
    vertices_.pop_back();
    vertices_.pop_back();
    //  storeVerticesWithMetaData();
    ///vertices_[goalIndex].setIsGoal(false);
    //  flag clearing: nothing much to do in this branch for now.
  }
  else
  {
  std::cout << "Start and goal in disconnected components." << std::endl;
    //If no path exists between start and goal through current roadmap:
    //  add the start/goal vertices to the roadmap as permanent vertices
    //    {current implementation uses only one start and one goal state; to fully add them to the roadmap:
    //       - add them to the nearFinder_ too
    //       - provide a CCindex
    //    }
    nearFinder_.add(startIndex);
    nearFinder_.add(goalIndex);
    vertices_[startIndex].setCCIndex(CCs_.makeNewComponent());
    vertices_[goalIndex].setCCIndex(CCs_.makeNewComponent());
    //  add edges to neighbors, merge connected components as appropriate
    addEdges(startIndex, startNeighbors, true, true);
    addEdges(goalIndex, goalNeighbors, true, true);
    //  check whether the problem is trivially solvable
    if(si_->checkMotion(vertices_[startIndex].getStateData(), vertices_[goalIndex].getStateData()))
    {
      std::vector<ompl::LTLVis::tIndex> dummy;
      dummy.clear(); dummy.push_back(goalIndex);
      std::cout << "Start and goal trivially connectable." << std::endl;
      addEdges(startIndex, dummy, true, true);
    }
    //  test whether start/goal get in same CC
  unsigned int kMax = vertices_.size();
  for(unsigned int k = 0; k < kMax; k++)
  {
    std::cout << "CC " << vertices_[k].getCCIndex() << ": " << (verifyCC(k)?"consistent":"SET FAIL") << std::endl;
  }
  //  pathFound = false;

    while((!areInSameCC(startIndex, goalIndex)) && (false == ptc()))
    {
      //  grow roadmap if not
ompl::tools::Profiler::Begin("sampleAndConnect");
      sampleAndConnect(ptc);
ompl::tools::Profiler::End("sampleAndConnect");
    }
    //  if start/goal in same CC
    if(areInSameCC(startIndex, goalIndex))
    {
      //  search for and extract path
ompl::tools::Profiler::Begin("graphSearch");
      pathFound = GraphSearch(ptc, solution, false);
ompl::tools::Profiler::End("graphSearch");
    }

    //  flag clearing
    vertices_[startIndex].setIsStart(false);
    vertices_[goalIndex].setIsGoal(false);
    //if(pathFound)
      storeVerticesWithMetaData();
  }
  // When a solution path is computed, save it here
  if(pathFound)
  {
    solution.interpolate();
    ompl::base::PathPtr solutionPtr(new ompl::geometric::PathGeometric(solution));
    pdef_->addSolutionPath(solutionPtr);
  }
  std::cout << "Roadmap currently has " << vertices_.size() << " vertices." << std::endl;
ompl::tools::Profiler::Stop();
ompl::tools::Profiler::Console();
ompl::tools::Profiler::Clear();
  // Return a value from the PlannerStatus enumeration.
  if(pathFound)
  {
    return ompl::base::PlannerStatus::EXACT_SOLUTION;
  }
  else
  {
    return ompl::base::PlannerStatus::TIMEOUT;
  }
}