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; }