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;
}
/**
* 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);
}
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}});
}
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);
}
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());
}
}
}
}
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;
}
void Graph::addEdge(igraph_integer_t source, igraph_integer_t target) {
Vector edge(2);
edge[0] = source; edge[1] = target;
addEdges(edge);
}
/*
* 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;
}
}
