Real VisibilityGraphPlanner::Distance(const Config& a,const Config& b)
{
Assert(a.n == 2);
Assert(b.n == 2);
int vstart = tempGraph.NumNodes()-2;
int vgoal = tempGraph.NumNodes()-1;
tempGraph.nodes[vstart].q = a;
tempGraph.nodes[vgoal].q = b;
tempGraph.DeleteIncomingEdges(vstart);
tempGraph.DeleteIncomingEdges(vgoal);
//test straight line
EdgePlanner* e = IsVisible(space,tempGraph.nodes[vstart].q,tempGraph.nodes[vgoal].q);
if(e) {
return LengthWeightFunc(e,vstart,vgoal);
}
//connect vstart
for(int i=0;i<vstart;i++) {
SmartPointer<EdgePlanner> e = IsVisible(space,tempGraph.nodes[i].q,tempGraph.nodes[vstart].q);
if(e)
tempGraph.AddEdge(i,vstart,e);
}
//connect vgoal
for(int i=0;i<vstart;i++) {
SmartPointer<EdgePlanner> e = IsVisible(space,tempGraph.nodes[i].q,tempGraph.nodes[vgoal].q);
if(e)
tempGraph.AddEdge(i,vgoal,e);
}
//search
Graph::ShortestPathProblem<Vertex,Arc> spp(tempGraph);
spp.InitializeSource(vstart);
spp.FindPath_Undirected(vgoal,LengthWeightFunc);
return spp.d[vgoal];
}
bool VisibilityGraphPlanner::Plan(const Config& a,const Config& b,MilestonePath& path)
{
Assert(a.n == 2);
Assert(b.n == 2);
int vstart = tempGraph.NumNodes()-2;
int vgoal = tempGraph.NumNodes()-1;
tempGraph.nodes[vstart].q = a;
tempGraph.nodes[vgoal].q = b;
tempGraph.DeleteIncomingEdges(vstart);
tempGraph.DeleteIncomingEdges(vgoal);
//test straight line
EdgePlanner* e = IsVisible(space,tempGraph.nodes[vstart].q,tempGraph.nodes[vgoal].q);
if(e) {
path.edges.resize(1);
path.edges[0] = e;
return true;
}
//connect vstart
for(int i=0;i<vstart;i++) {
SmartPointer<EdgePlanner> e = IsVisible(space,tempGraph.nodes[i].q,tempGraph.nodes[vstart].q);
if(e)
tempGraph.AddEdge(i,vstart,e);
}
//connect vgoal
for(int i=0;i<vstart;i++) {
SmartPointer<EdgePlanner> e = IsVisible(space,tempGraph.nodes[i].q,tempGraph.nodes[vgoal].q);
if(e)
tempGraph.AddEdge(i,vgoal,e);
}
//search
Graph::ShortestPathProblem<Vertex,Arc> spp(tempGraph);
spp.InitializeSource(vstart);
spp.FindPath_Undirected(vgoal,LengthWeightFunc);
list<int> nodes;
bool res=Graph::GetAncestorPath(spp.p,vgoal,vstart,nodes);
if(!res) return false;
path.edges.clear();
for(list<int>::const_iterator i=nodes.begin();i!=--nodes.end();++i) {
list<int>::const_iterator n=i; ++n;
SmartPointer<EdgePlanner>* e=tempGraph.FindEdge(*i,*n);
Assert(e != NULL);
if(*i < *n) {
path.edges.push_back((*e)->Copy());
}
else {
path.edges.push_back((*e)->ReverseCopy());
}
}
return true;
}
void VisibilityGraphPlanner::AllDistances(const Config& a,vector<Real>& distances)
{
Assert(a.n == 2);
int vstart = tempGraph.NumNodes()-2;
int vgoal = tempGraph.NumNodes()-1;
tempGraph.nodes[vstart].q = a;
tempGraph.DeleteIncomingEdges(vstart);
tempGraph.DeleteIncomingEdges(vgoal);
//connect vstart
for(int i=0;i<vstart;i++) {
SmartPointer<EdgePlanner> e = IsVisible(space,tempGraph.nodes[i].q,tempGraph.nodes[vstart].q);
if(e)
tempGraph.AddEdge(i,vstart,e);
}
//search
Graph::ShortestPathProblem<Vertex,Arc> spp(tempGraph);
spp.InitializeSource(vstart);
spp.FindAllPaths_Undirected(LengthWeightFunc);
distances = spp.d;
distances.resize(vstart);
}
int main (int argc, char **argv)
{
Q330 q330;
Q330_ADDR addr;
init(argc, argv, &q330);
switch (q330.cmd.code) {
case Q330_CMD_REBOOT:
boot(&q330);
break;
case Q330_CMD_SAVE:
save(&q330);
break;
case Q330_CMD_SAVEBOOT:
save(&q330);
sleep(5);
boot(&q330);
break;
case Q330_CMD_RESYNC:
resync(&q330);
break;
case Q330_CMD_GPS_ON:
gpsON(&q330);
break;
case Q330_CMD_GPS_OFF:
gpsOFF(&q330);
break;
case Q330_CMD_GPS_COLD:
gpsColdStart(&q330);
break;
case Q330_CMD_GPS_CNF:
gpsCnf(&q330);
break;
case Q330_CMD_GPS_ID:
gpsId(&q330);
break;
case Q330_CMD_CAL_START:
calStart(&q330);
break;
case Q330_CMD_CAL_STOP:
calStop(&q330);
break;
case Q330_CMD_IFCONFIG:
ifconfig(&q330);
break;
case Q330_CMD_STATUS:
status(&q330);
break;
case Q330_CMD_FIX:
fix(&q330);
break;
case Q330_CMD_GLOB:
glob(&q330);
break;
case Q330_CMD_SC:
sc(&q330);
break;
case Q330_CMD_PULSE:
pulse(&q330);
break;
case Q330_CMD_AMASS:
amass(&q330);
break;
case Q330_CMD_DCP:
dcp(&q330);
break;
case Q330_CMD_SPP:
spp(&q330);
break;
case Q330_CMD_MAN:
man(&q330);
break;
case Q330_CMD_CO:
checkout(&q330);
break;
default:
fprintf(stderr, "ERROR: command '%s' is unsupported\n", q330.cmd.name);
break;
}
if (q330.qdp != NULL) qdpDeregister(q330.qdp, TRUE);
}
