void ompl::control::Syclop::initEdge(Adjacency& adj, const Region* source, const Region* target)
{
adj.source = source;
adj.target = target;
updateEdge(adj);
regionsToEdge_[std::pair<int,int>(source->index, target->index)] = &adj;
}
bool ompl::control::Syclop::updateConnectionEstimate(const Region& c, const Region& d, const base::State *s)
{
Adjacency& adj = *regionsToEdge_[std::pair<int,int>(c.index,d.index)];
const int covCell = covGrid_.locateRegion(s);
if (adj.covGridCells.count(covCell) == 1)
return false;
adj.covGridCells.insert(covCell);
updateEdge(adj);
return true;
}
void resetIR(void) {
necState = inactive;
ir.decoded = 0;
ir.repeated = 0;
capture.edge = 0;
capture.bitcount = 0;
capture.data = 1;
updateEdge();
WriteTimer1(0);
}
void ompl::control::Syclop::setupEdgeEstimates(void)
{
EdgeIter ei, eend;
for (boost::tie(ei,eend) = boost::edges(graph_); ei != eend; ++ei)
{
Adjacency& adj = graph_[*ei];
adj.empty = true;
adj.numLeadInclusions = 0;
adj.numSelections = 0;
updateEdge(adj);
}
}
void ompl::control::Syclop::setupEdgeEstimates()
{
OMPL_PUSH_DISABLE_GCC_WARNING(-Wmaybe-uninitialized)
EdgeIter ei, eend;
OMPL_POP_GCC
for (boost::tie(ei, eend) = boost::edges(graph_); ei != eend; ++ei)
{
Adjacency &adj = graph_[*ei];
adj.empty = true;
adj.numLeadInclusions = 0;
adj.numSelections = 0;
updateEdge(adj);
}
}
void ompl::control::Syclop::computeLead(int startRegion, int goalRegion)
{
lead_.clear();
if (startRegion == goalRegion)
{
lead_.push_back(startRegion);
return;
}
else if (rng_.uniform01() < probShortestPath_)
{
std::vector<RegionGraph::vertex_descriptor> parents(decomp_->getNumRegions());
std::vector<double> distances(decomp_->getNumRegions());
try
{
boost::astar_search(graph_, boost::vertex(startRegion, graph_), DecompositionHeuristic(this, getRegionFromIndex(goalRegion)),
boost::weight_map(get(&Adjacency::cost, graph_)).distance_map(
boost::make_iterator_property_map(distances.begin(), get(boost::vertex_index, graph_)
)).predecessor_map(
boost::make_iterator_property_map(parents.begin(), get(boost::vertex_index, graph_))
).visitor(GoalVisitor(goalRegion))
);
}
catch (found_goal fg)
{
int region = goalRegion;
int leadLength = 1;
while (region != startRegion)
{
region = parents[region];
++leadLength;
}
lead_.resize(leadLength);
region = goalRegion;
for (int i = leadLength-1; i >= 0; --i)
{
lead_[i] = region;
region = parents[region];
}
}
}
else
{
/* Run a random-DFS over the decomposition graph from the start region to the goal region.
There may be a way to do this using boost::depth_first_search. */
VertexIndexMap index = get(boost::vertex_index, graph_);
std::stack<int> nodesToProcess;
std::vector<int> parents(decomp_->getNumRegions(), -1);
parents[startRegion] = startRegion;
nodesToProcess.push(startRegion);
bool goalFound = false;
while (!goalFound && !nodesToProcess.empty())
{
const int v = nodesToProcess.top();
nodesToProcess.pop();
std::vector<int> neighbors;
boost::graph_traits<RegionGraph>::adjacency_iterator ai, aend;
for (boost::tie(ai,aend) = adjacent_vertices(boost::vertex(v,graph_),graph_); ai != aend; ++ai)
{
if (parents[index[*ai]] < 0)
{
neighbors.push_back(index[*ai]);
parents[index[*ai]] = v;
}
}
for (std::size_t i = 0; i < neighbors.size(); ++i)
{
const int choice = rng_.uniformInt(i, neighbors.size()-1);
if (neighbors[choice] == goalRegion)
{
int region = goalRegion;
int leadLength = 1;
while (region != startRegion)
{
region = parents[region];
++leadLength;
}
lead_.resize(leadLength);
region = goalRegion;
for (int j = leadLength-1; j >= 0; --j)
{
lead_[j] = region;
region = parents[region];
}
goalFound = true;
break;
}
nodesToProcess.push(neighbors[choice]);
std::swap(neighbors[i], neighbors[choice]);
}
}
}
//Now that we have a lead, update the edge weights.
for (std::size_t i = 0; i < lead_.size()-1; ++i)
{
Adjacency& adj = *regionsToEdge_[std::pair<int,int>(lead_[i], lead_[i+1])];
if (adj.empty)
{
++adj.numLeadInclusions;
updateEdge(adj);
}
}
}
// *********************************************************
void CPrimRender::updatePos()
{
//H_AUTO(R2_CPrimRender_updatePos)
// world map
if (_AddedToWorldMap)
{
setWorldMapNumVertices(_Look.VertexLook.WorldMapTexture.empty() ? 0 : (uint)_Vertices.size());
setWorldMapNumEdges(_Look.EdgeLook.WorldMapTexture.empty() ? 0 : _NumEdges);
}
//
if (!_Vertices.empty())
{
// edges update
switch(_Look.Shape)
{
case CPrimLook::Star:
{
CVector centerPos = _Vertices[0];
for(uint k = 1; k < _Vertices.size(); ++k)
{
//
if (!_EdgeShapeInstances.empty())
{
updateEdge(_EdgeShapeInstances[k - 1], centerPos, _Vertices[k]);
}
if (!_EdgeDecals.empty())
{
updateEdgeDecal(*(_EdgeDecals[k - 1]), centerPos, _Vertices[k], _Look.FirstVertexLook.DecalDistToEdgeDecal, _Look.VertexLook.DecalDistToEdgeDecal);
}
}
}
break;
case CPrimLook::PolyLine:
case CPrimLook::ClosedPolyLine:
{
for(sint k = 0; k < _NumEdges; ++k)
{
//
if (!_EdgeShapeInstances.empty())
{
updateEdge(_EdgeShapeInstances[k], _Vertices[k], _Vertices[(k + 1) % _Vertices.size()]);
}
if (!_EdgeDecals.empty())
{
updateEdgeDecal(*(_EdgeDecals[k]), _Vertices[k], _Vertices[(k + 1) % _Vertices.size()],
k == 0 ? _Look.VertexLook.DecalDistToEdgeDecal : _Look.FirstVertexLook.DecalDistToEdgeDecal,
_Look.VertexLook.DecalDistToEdgeDecal
);
}
}
}
break;
default:
nlassert(0);
break;
}
}
// update vertices
for(uint k = 0; k < _Vertices.size(); ++k)
{
if (!_Look.VertexShapeName.empty())
{
if (!_VertexShapeInstances[k].empty())
{
CMatrix vertexMat;
vertexMat.setScale(_Look.VertexShapeScale);
vertexMat.setPos(_Vertices[k]);
_VertexShapeInstances[k].setTransformMode(UTransform::DirectMatrix);
_VertexShapeInstances[k].setMatrix(vertexMat);
}
}
if (!_VertexDecals.empty())
{
_VertexDecals[k]->setWorldMatrixForSpot(CVector2f(_Vertices[k].x, _Vertices[k].y), k == 0 ? _Look.FirstVertexLook.DecalSize : _Look.VertexLook.DecalSize);
}
}
//
if (_AddedToWorldMap)
{
updateWorldMapDisplay();
}
}
void invertEdge(void) {
capture.edge ^= 1;
updateEdge();
}