C++ NBEdge::getAngleAtNode方法代码示例

void
NBTurningDirectionsComputer::computeTurnDirectionsForNode(NBNode* node) {
    const std::vector<NBEdge*>& incoming = node->getIncomingEdges();
    const std::vector<NBEdge*>& outgoing = node->getOutgoingEdges();
    std::vector<Combination> combinations;
    for (std::vector<NBEdge*>::const_iterator j = outgoing.begin(); j != outgoing.end(); ++j) {
        NBEdge* outedge = *j;
        for (std::vector<NBEdge*>::const_iterator k = incoming.begin(); k != incoming.end(); ++k) {
            NBEdge* e = *k;
            if (e->getConnections().size() != 0 && !e->isConnectedTo(outedge)) {
                // has connections, but not to outedge; outedge will not be the turn direction
                //
                // @todo: this seems to be needed due to legacy issues; actually, we could regard
                //  such pairs, too, and it probably would increase the accuracy. But there is
                //  no mechanism implemented, yet, which would avoid adding them as turnarounds though
                //  no connection is specified.
                continue;
            }

            // @todo: check whether NBHelpers::relAngle is properly defined and whether it should really be used, here
            SUMOReal angle = fabs(NBHelpers::relAngle(e->getAngleAtNode(node), outedge->getAngleAtNode(node)));
            if (angle < 160) {
                continue;
            }
            if (e->getFromNode() == outedge->getToNode()) {
                // they connect the same nodes; should be the turnaround direction
                // we'll assign a maximum number
                //
                // @todo: indeed, we have observed some pathological intersections
                //  see "294831560" in OSM/adlershof. Here, several edges are connecting
                //  same nodes. We have to do the angle check before...
                //
                // @todo: and well, there are some other as well, see plain import
                //  of delphi_muenchen (elmar), intersection "59534191". Not that it would
                //  be realistic in any means; we will warn, here.
                angle += 360;
            }
            Combination c;
            c.from = e;
            c.to = outedge;
            c.angle = angle;
            combinations.push_back(c);
        }
    }
    // sort combinations so that the ones with the highest angle are at the begin
    std::sort(combinations.begin(), combinations.end(), combination_by_angle_sorter());
    std::set<NBEdge*> seen;
    bool haveWarned = false;
    for (std::vector<Combination>::const_iterator j = combinations.begin(); j != combinations.end(); ++j) {
        if (seen.find((*j).from) != seen.end() || seen.find((*j).to) != seen.end()) {
            // do not regard already set edges
            if ((*j).angle > 360 && !haveWarned) {
                WRITE_WARNING("Ambiguity in turnarounds computation at node '" + node->getID() + "'.");
                haveWarned = true;
            }
            continue;
        }
        // mark as seen
        seen.insert((*j).from);
        seen.insert((*j).to);
        // set turnaround information
        (*j).from->setTurningDestination((*j).to);
    }
}

void
NBEdgeCont::guessRoundabouts(std::vector<EdgeVector>& marked) {
    // step 1: keep only those edges which have no turnarounds
    std::set<NBEdge*> candidates;
    for (EdgeCont::const_iterator i = myEdges.begin(); i != myEdges.end(); ++i) {
        NBEdge* e = (*i).second;
        NBNode* const to = e->getToNode();
        if (e->getTurnDestination() == 0 && to->getConnectionTo(e->getFromNode()) == 0) {
            candidates.insert(e);
        }
    }

    // step 2:
    std::set<NBEdge*> visited;
    for (std::set<NBEdge*>::const_iterator i = candidates.begin(); i != candidates.end(); ++i) {
        EdgeVector loopEdges;
        // start with a random edge (this doesn't have to be a roundabout edge)
        // loop over connected edges (using always the leftmost one)
        // and keep the list in loopEdges
        // continue until we loop back onto a loopEdges and extract the loop
        NBEdge* e = (*i);
        if (visited.count(e) > 0) {
            // already seen
            continue;
        }
        loopEdges.push_back(e);
        bool doLoop = true;
        do {
            visited.insert(e);
            const EdgeVector& edges = e->getToNode()->getEdges();
            if (edges.size() < 2) {
                doLoop = false;
                break;
            }
            if (e->getTurnDestination() != 0 || e->getToNode()->getConnectionTo(e->getFromNode()) != 0) {
                // do not follow turn-arounds while in a (tentative) loop
                doLoop = false;
                break;
            }
            EdgeVector::const_iterator me = find(edges.begin(), edges.end(), e);
            NBContHelper::nextCW(edges, me);
            NBEdge* left = *me;
            SUMOReal angle = fabs(NBHelpers::relAngle(e->getAngleAtNode(e->getToNode()), left->getAngleAtNode(e->getToNode())));
            if (angle >= 90) {
                // roundabouts do not have sharp turns (or they wouldn't be called 'round')
                doLoop = false;
                break;
            }
            EdgeVector::const_iterator loopClosed = find(loopEdges.begin(), loopEdges.end(), left);
            const size_t loopSize = loopEdges.end() - loopClosed;
            if (loopSize > 0) {
                // loop found
                if (loopSize < 3) {
                    doLoop = false; // need at least 3 edges for a roundabout
                } else if (loopSize < loopEdges.size()) {
                    // remove initial edges not belonging to the loop
                    EdgeVector(loopEdges.begin() + (loopEdges.size() - loopSize), loopEdges.end()).swap(loopEdges);
                }
                // count attachments to the outside. need at least 3 or a roundabout doesn't make much sense
                int attachments = 0;
                for (EdgeVector::const_iterator j = loopEdges.begin(); j != loopEdges.end(); ++j) {
                    if ((*j)->getToNode()->getEdges().size() > 2) {
                        attachments++;
                    }
                }
                if (attachments < 3) {
                    doLoop = false;
                }
                break;
            }
            if (visited.count(left) > 0) {
                doLoop = false;
            } else {
                // keep going
                loopEdges.push_back(left);
                e = left;
            }
        } while (doLoop);
        // mark collected edges in the case a loop (roundabout) was found
        if (doLoop) {
            std::set<NBEdge*> loopEdgesSet(loopEdges.begin(), loopEdges.end());
            for (std::set<NBEdge*>::const_iterator j = loopEdgesSet.begin(); j != loopEdgesSet.end(); ++j) {
                // disable turnarounds on incoming edges
                NBNode* node = (*j)->getToNode();
                const EdgeVector& incoming = node->getIncomingEdges();
                for (EdgeVector::const_iterator k = incoming.begin(); k != incoming.end(); ++k) {
                    NBEdge* inEdge = *k;
                    if (loopEdgesSet.count(inEdge) > 0) {
                        continue;
                    }
                    if ((inEdge)->getStep() >= NBEdge::LANES2LANES_USER) {
                        continue;
                    }
                    inEdge->removeFromConnections(inEdge->getTurnDestination(), -1);
                }
                // let the connections to succeeding roundabout edge have a higher priority
                (*j)->setJunctionPriority(node, 1000);
            }
            marked.push_back(loopEdges);
        }
//.........这里部分代码省略.........

void
NBEdgePriorityComputer::setPriorityJunctionPriorities(NBNode& n) {
    if (n.myIncomingEdges.size() == 0 || n.myOutgoingEdges.size() == 0) {
        return;
    }
    EdgeVector incoming = n.myIncomingEdges;
    EdgeVector outgoing = n.myOutgoingEdges;
    // what we do want to have is to extract the pair of roads that are
    //  the major roads for this junction
    // let's get the list of incoming edges with the highest priority
    std::sort(incoming.begin(), incoming.end(), NBContHelper::edge_by_priority_sorter());
    EdgeVector bestIncoming;
    NBEdge* best = incoming[0];
    while (incoming.size() > 0 && samePriority(best, incoming[0])) {
        bestIncoming.push_back(*incoming.begin());
        incoming.erase(incoming.begin());
    }
    // now, let's get the list of best outgoing
    assert(outgoing.size() != 0);
    sort(outgoing.begin(), outgoing.end(), NBContHelper::edge_by_priority_sorter());
    EdgeVector bestOutgoing;
    best = outgoing[0];
    while (outgoing.size() > 0 && samePriority(best, outgoing[0])) { //->getPriority()==best->getPriority()) {
        bestOutgoing.push_back(*outgoing.begin());
        outgoing.erase(outgoing.begin());
    }
    // now, let's compute for each of the best incoming edges
    //  the incoming which is most opposite
    //  the outgoing which is most opposite
    EdgeVector::iterator i;
    std::map<NBEdge*, NBEdge*> counterIncomingEdges;
    std::map<NBEdge*, NBEdge*> counterOutgoingEdges;
    incoming = n.myIncomingEdges;
    outgoing = n.myOutgoingEdges;
    for (i = bestIncoming.begin(); i != bestIncoming.end(); ++i) {
        std::sort(incoming.begin(), incoming.end(), NBContHelper::edge_opposite_direction_sorter(*i, &n));
        counterIncomingEdges[*i] = *incoming.begin();
        std::sort(outgoing.begin(), outgoing.end(), NBContHelper::edge_opposite_direction_sorter(*i, &n));
        counterOutgoingEdges[*i] = *outgoing.begin();
    }
    // ok, let's try
    // 1) there is one best incoming road
    if (bestIncoming.size() == 1) {
        // let's mark this road as the best
        NBEdge* best1 = extractAndMarkFirst(n, bestIncoming);
        if (counterIncomingEdges.find(best1) != counterIncomingEdges.end()) {
            // ok, look, what we want is the opposit of the straight continuation edge
            // but, what if such an edge does not exist? By now, we'll determine it
            // geometrically
            NBEdge* s = counterIncomingEdges.find(best1)->second;
            if (GeomHelper::getMinAngleDiff(best1->getAngleAtNode(&n), s->getAngleAtNode(&n)) > 180 - 45) {
                s->setJunctionPriority(&n, 1);
            }
        }
        if (bestOutgoing.size() != 0) {
            // mark the best outgoing as the continuation
            sort(bestOutgoing.begin(), bestOutgoing.end(), NBContHelper::edge_similar_direction_sorter(best1));
            best1 = extractAndMarkFirst(n, bestOutgoing);
            if (counterOutgoingEdges.find(best1) != counterOutgoingEdges.end()) {
                NBEdge* s = counterOutgoingEdges.find(best1)->second;
                if (GeomHelper::getMinAngleDiff(best1->getAngleAtNode(&n), s->getAngleAtNode(&n)) > 180 - 45) {
                    s->setJunctionPriority(&n, 1);
                }
            }
        }
        return;
    }

    // ok, what we want to do in this case is to determine which incoming
    //  has the best continuation...
    // This means, when several incoming roads have the same priority,
    //  we want a (any) straight connection to be more priorised than a turning
    SUMOReal bestAngle = 0;
    NBEdge* bestFirst = 0;
    NBEdge* bestSecond = 0;
    bool hadBest = false;
    for (i = bestIncoming.begin(); i != bestIncoming.end(); ++i) {
        EdgeVector::iterator j;
        NBEdge* t1 = *i;
        SUMOReal angle1 = t1->getTotalAngle() + 180;
        if (angle1 >= 360) {
            angle1 -= 360;
        }
        for (j = i + 1; j != bestIncoming.end(); ++j) {
            NBEdge* t2 = *j;
            SUMOReal angle2 = t2->getTotalAngle() + 180;
            if (angle2 >= 360) {
                angle2 -= 360;
            }
            SUMOReal angle = GeomHelper::getMinAngleDiff(angle1, angle2);
            if (!hadBest || angle > bestAngle) {
                bestAngle = angle;
                bestFirst = *i;
                bestSecond = *j;
                hadBest = true;
            }
        }
    }
    bestFirst->setJunctionPriority(&n, 1);
    sort(bestOutgoing.begin(), bestOutgoing.end(), NBContHelper::edge_similar_direction_sorter(bestFirst));
//.........这里部分代码省略.........