WayLocation LocationOfPoint::locateAfter(const Coordinate& inputPt, const WayLocation& minLocation)
const
{
if (minLocation.isValid() == false)
{
return locate(inputPt);
}
assert(minLocation.getWay() == _way);
double minDistance = std::numeric_limits<double>().max();
WayLocation nextClosestLocation = minLocation;
LineSegment seg;
size_t startIndex = 0;
startIndex = (size_t)minLocation.getSegmentIndex();
for (size_t i = startIndex; i < _way->getNodeCount() - 1; i++)
{
seg.p0 = _map->getNode(_way->getNodeId(i))->toCoordinate();
seg.p1 = _map->getNode(_way->getNodeId(i + 1))->toCoordinate();
if (i == startIndex)
{
seg.p0 = minLocation.getCoordinate();
}
double segDistance = seg.distance(inputPt);
double segFrac = segmentFraction(seg, inputPt);
if (segDistance < minDistance)
{
// if this is the first case (a partial line segment)
if (i == startIndex)
{
// recalculate the segFrac in terms of the whole line segment.
segFrac = minLocation.getSegmentFraction() +
(1 - minLocation.getSegmentFraction()) * segFrac;
}
nextClosestLocation = WayLocation(_map, _way, i, segFrac);
minDistance = segDistance;
}
}
// Return the minDistanceLocation found.
// This will not be null, since it was initialized to minLocation
Assert::isFalse(nextClosestLocation >= minLocation,
"computed location is before specified minimum location");
return nextClosestLocation;
}
cv::Mat GraphComparator::_calculateCostDistance(shared_ptr<OsmMap> map, Coordinate c)
{
// make a copy of the map so we can manipulate it.
map.reset(new OsmMap(map));
// find the nearest feature
long wId = map->getIndex().findNearestWay(c);
shared_ptr<Way> w = map->getWay(wId);
// split way at c
WayLocation wl = LocationOfPoint::locate(map, w, c);
vector< shared_ptr<Way> > v = WaySplitter::split(map, w, wl);
wl = LocationOfPoint::locate(map, v[0], c);
if (wl.isNode() == false)
{
// I haven't been able to recreate the case when this happens.
LOG_WARN("Internal Error: Expected wl to be on a node, but it was this: " << wl);
}
assert(wl.isNode() == true);
// populate graph
shared_ptr<DirectedGraph> graph(new DirectedGraph());
graph->deriveEdges(map);
ShortestPath sp(graph);
// set cost at c to zero.
long sourceId = v[0]->getNodeId(wl.getSegmentIndex());
sp.setNodeCost(sourceId, 0.0);
// calculate cost
sp.calculateCost();
// paint graph onto raster
//_exportGraphImage(map, *graph, sp, "/home/jason.surratt/tmp/graph.png");
cv::Mat mat = _paintGraph(map, *graph, sp);
// calculate cost distance raster
_calculateRasterCost(mat);
return mat;
}
void GraphComparator::drawCostDistance(shared_ptr<OsmMap> map, vector<Coordinate>& c,
QString output)
{
_updateBounds();
// make a copy of the map so we can manipulate it.
map.reset(new OsmMap(map));
for (size_t i = 0; i < c.size(); i++)
{
cout << c[i].x << " " << c[i].y << endl;
c[i] = _findNearestPointOnFeature(map, c[i]);
cout << c[i].x << " " << c[i].y << endl;
// find the nearest feature
long wId = map->getIndex().findNearestWay(c[i]);
shared_ptr<Way> w = map->getWay(wId);
// split way at c
WayLocation wl = LocationOfPoint::locate(map, w, c[i]);
vector< shared_ptr<Way> > v = WaySplitter::split(map, w, wl);
wl = LocationOfPoint::locate(map, v[0], c[i]);
assert(wl.isNode() == true);
}
// populate graph
shared_ptr<DirectedGraph> graph(new DirectedGraph());
graph->deriveEdges(map);
LOG_WARN("Running cost");
ShortestPath sp(graph);
for (size_t i = 0; i < c.size(); i++)
{
long wId = map->getIndex().findNearestWay(c[i]);
shared_ptr<Way> w = map->getWay(wId);
WayLocation wl = LocationOfPoint::locate(map, w, c[i]);
// set cost at c to zero.
long sourceId = w->getNodeId(wl.getSegmentIndex());
sp.setNodeCost(sourceId, 0.0);
}
// calculate cost
sp.calculateCost();
LOG_WARN("Cost done");
cv::Mat mat = _paintGraph(map, *graph, sp);
_saveImage(mat, output, -1.0, false);
_saveImage(mat, output.replace(".png", "2.png"), -1.0, true);
shared_ptr<OGRSpatialReference> srs(new OGRSpatialReference());
srs->importFromEPSG(900913);
Coordinate c1 = MapProjector::project(Coordinate(_projectedBounds.MinX, _projectedBounds.MinY), map->getProjection(), srs);
cout << "coord " << c1.x << ", " << c1.y << endl;
Coordinate c2 = MapProjector::project(Coordinate(_projectedBounds.MaxX, _projectedBounds.MaxY), map->getProjection(), srs);
cout << "coord2 " << c2.x << ", " << c2.y << endl;
printf("POSITION_Y=%f\n", (c1.y + c2.y) / 2.0);
//cout << "POSITION_Y=" << (c1.y + c2.y) / 2.0 << endl;
printf("POSITION_X=%f\n", (c1.x + c2.x) / 2.0);
//cout << "POSITION_X=" << (c1.x + c2.x) / 2.0 << endl;
cout << "M12=0.0" << endl;
cout << "M11=1.0" << endl;
cout << "M10=0.0" << endl;
cout << "M02=0.0" << endl;
cout << "INITIAL_SCALE=" << (c2.x - c1.x) / (double)_width * 100.0 << endl;
cout << "M01=0.0" << endl;
cout << "M00=1.0" << endl;
// paint graph onto raster
//_exportGraphImage(map, *graph, sp, output);
}
vector<ElementPtr> PertyWaySplitVisitor::_split(ElementPtr element)
{
//randomly select elements and split them into two parts
boost::uniform_real<> randomSplitDistribution(0.0, 1.0);
const double randomSplitNum = randomSplitDistribution(*_rng);
if (randomSplitNum <= _waySplitProbability)
{
LOG_TRACE("element " << element->getElementId() << " *will* be split based on a split " <<
"probability of: " << _waySplitProbability << " and a randomly generated number: " <<
randomSplitNum << "\n");
_splitRecursionLevel++;
LOG_VART(_splitRecursionLevel);
const int numNodesBeforeSplit = _map->getNodes().size();
LOG_VART(numNodesBeforeSplit);
const int numWaysBeforeSplit = _map->getWays().size();
LOG_VART(numWaysBeforeSplit);
WayLocation waySplitPoint;
MultiLineStringLocation multiLineSplitPoint;
QList<long> nodeIdsBeforeSplit;
int segmentIndex = -1;
ElementId wayId;
//determine where to split the element
if (element->getElementType() == ElementType::Way)
{
WayPtr way = boost::dynamic_pointer_cast<Way>(element);
LOG_VART(way->getNodeCount());
nodeIdsBeforeSplit = QVector<long>::fromStdVector(way->getNodeIds()).toList();
LOG_VART(nodeIdsBeforeSplit);
waySplitPoint = _calcSplitPoint(way);
}
else
{
multiLineSplitPoint = _calcSplitPoint(boost::dynamic_pointer_cast<Relation>(element), wayId);
waySplitPoint = multiLineSplitPoint.getWayLocation();
}
const QString distanceMsgStrEnd =
QString("a minimum node spacing of ")
.append(QString::number(_minNodeSpacing))
.append(" meters");
if (!waySplitPoint.isValid())
{
_splitRecursionLevel--;
LOG_VART(_splitRecursionLevel);
LOG_TRACE("split point *will not* be used because *it violates* " << distanceMsgStrEnd);
//if it violates the min node spacing, return an empty element collection, which will end the
//recursive splitting on the current way
return vector<ElementPtr>();
}
else
{
LOG_TRACE("split point *will* be used because it *does not* violate " << distanceMsgStrEnd);
segmentIndex = waySplitPoint.getSegmentIndex();
LOG_VART(segmentIndex);
}
//split the element
vector<ElementPtr> newElementsAfterSplit;
if (element->getElementType() == ElementType::Way)
{
vector<WayPtr> newWaysAfterSplit =
WaySplitter::split(_map->shared_from_this(),
boost::dynamic_pointer_cast<Way>(element), waySplitPoint);
for (size_t i = 0; i < newWaysAfterSplit.size(); i++)
{
newElementsAfterSplit.push_back(newWaysAfterSplit.at(i));
}
}
else
{
ElementPtr match;
MultiLineStringSplitter().split(_map->shared_from_this(), multiLineSplitPoint, match);
newElementsAfterSplit.push_back(match);
}
const int numNodesAfterSplit = _map->getNodes().size();
LOG_VART(numNodesAfterSplit);
const int numNewNodesCreatedBySplit = numNodesAfterSplit - numNodesBeforeSplit;
LOG_VART(numNewNodesCreatedBySplit);
LOG_VART(_map->getWays().size());
if (numNewNodesCreatedBySplit > 0)
{
WayPtr way = boost::dynamic_pointer_cast<Way>(element);
//Its possible that the splitting of a relation could generate a new node. In that case,
//_updateNewNodeProperties does not need to be called b/c the MultiLineStringSplitter has
//already properly updated the new node's properties. when a way is split, however, the
//new node's properties must be updated by the call to _updateNewNodeProperties.
if (way != 0)
{
assert(nodeIdsBeforeSplit.size() > 0);
//update properties on any nodes added as a result of the way splitting (new ways created as a
//result of the splitting will already have had their parent's tags added by WaySplitter)
_updateNewNodeProperties(
_getNodeAddedBySplit(nodeIdsBeforeSplit, newElementsAfterSplit),
_map->getNode(way->getNodeId(segmentIndex)),
_map->getNode(way->getNodeId(segmentIndex + 1)));
}
}
//recursive call
for (vector<ElementPtr>::const_iterator it = newElementsAfterSplit.begin();
it != newElementsAfterSplit.end(); ++it)
{
_split(*it);
}
return newElementsAfterSplit;
}
else
{
LOG_TRACE("element " << element->getElementId() << " *will not* be split based on a split " <<
"probability of: " << _waySplitProbability << " and a randomly generated number: " <<
randomSplitNum << "\n");
}
//end the recursive splitting on the current way
return vector<ElementPtr>();
}
bool LocationOfPoint::isGreater(int i, double segFrac, const WayLocation& loc) const
{
return WayLocation::compareLocationValues(i, segFrac,
loc.getSegmentIndex(), loc.getSegmentFraction()) > 0;
}
