void SourceEditor::showLine(int line)
{
if (!foldingEnabled())
return;
if (!static_cast<SourceFile &>(file()).structureUpdated())
return;
for (const SourceFile::StructureNode *node =
static_cast<SourceFile &>(file()).structureNodeAt(line);
node;
node = node->parent())
{
if (m_foldsData[node->beginLine()].collapsed &&
line > node->beginLine() && line < node->endLine())
expandLine(node->beginLine());
}
}
bool VoronoiExpansion::calculatePotentials(unsigned char *costs, double start_x, double start_y, double end_x,
double end_y, int cycles, float *potential)
{
if (!gotMap_)
{
ROS_ERROR("Global Planner: no map received!!!");
return false;
}
float *distance_field = distance_field_.get();
int8_t *voronoi_graph = voronoi_graph_.get();
int8_t *global_map = global_map_.get();
getMaps(distance_field, voronoi_graph, global_map);
Index startPoint = Index(start_x, start_y, nx_, 0, 0, 0);
Index endPoint = Index(end_x, end_y, nx_, 0, 0, 0);
// EndPoint candidates //Dikjstra instead of going along the distance_field because sometimes the voronoi map is a
// little bit of the distance field (if so gradient mehtod would fail)
std::fill(potential, potential + ns_, POT_HIGH);
Index endPointGraph =
expandDijkstraToVoronoi(endPoint, startPoint, cycles, global_map, voronoi_graph, distance_field, potential);
if (endPointGraph.i < 0)
return false;
int endPointImprovementSteps = (int)distance_field[endPointGraph.i];
endpoints_ = findVoronoiCandidates(endPointGraph, global_map, voronoi_graph, distance_field, potential,
endPointImprovementSteps);
// StartPoint candidates //Reset map for Start calculation (in case Start is next to end)
std::fill(potential, potential + ns_, POT_HIGH);
Index startPointGraph =
expandDijkstraToVoronoi(startPoint, endPoint, cycles, global_map, voronoi_graph, distance_field, potential);
if (startPointGraph.i < 0)
return false;
int startPointImprovementSteps = (int)distance_field[startPointGraph.i];
std::list<Index> startpoints = findVoronoiCandidates(startPointGraph, global_map, voronoi_graph, distance_field,
potential, startPointImprovementSteps);
startpoints.push_back(startPointGraph);
// Start the Algorigthm by expanding on the Graph
std::fill(potential, potential + ns_, POT_HIGH); // For having a high startpotential
potential[startPointGraph.i] = startPointGraph.potential;
Index endPointGraphReal =
expandVoronoi(startPointGraph, endPointGraph, cycles, global_map, voronoi_graph, distance_field, potential);
if (endPointGraphReal.i < 0)
return false;
// Draw lines to all startpoints and the endpoint (there is allways a straight line between voronoi Graph and
// endPoint)
expandLine(endPointGraphReal, endPoint, endPointGraphReal.potential, -1, potential);
for (std::list<Index>::iterator it = startpoints.begin(); it != startpoints.end(); ++it)
{
expandLine(startPoint, *it, 0, startPointGraph.potential, potential);
}
return true;
}