void LaneVisitor::visit(const hesperia::data::scenario::PointModel *pm) { if (pm != NULL) { stringstream name; name << pm->getLane()->getRoad()->getLayer()->getID() << "." << pm->getLane()->getRoad()->getID() << "." << pm->getLane()->getID(); cerr << "Found PointModel Lane: " << name.str() << endl; const vector<IDVertex3> &listOfWaypoints = pm->getListOfIdentifiableVertices(); const uint32_t SIZE = listOfWaypoints.size(); for(uint32_t i = 0; i < (SIZE-1); i++) { IDVertex3 vt1 = listOfWaypoints.at(i); IDVertex3 vt2 = listOfWaypoints.at(i+1); // Construct a line between to consecutive points. hesperia::data::environment::NamedLine line(name.str(), vt1, vt2); m_listOfLines.push_back(line); WaypointVertex *v1 = new WaypointVertex(); v1->setLayerID(pm->getLane()->getRoad()->getLayer()->getID()); v1->setRoadID(pm->getLane()->getRoad()->getID()); v1->setLaneID(pm->getLane()->getID()); v1->setWaypointID(vt1.getID()); v1->setPosition(vt1); cerr << "Constructed node: " << pm->getLane()->getRoad()->getLayer()->getID() << "." << pm->getLane()->getRoad()->getID() << "." << pm->getLane()->getID() << "." << vt1.getID() << endl; WaypointVertex *v2 = new WaypointVertex(); v2->setLayerID(pm->getLane()->getRoad()->getLayer()->getID()); v2->setRoadID(pm->getLane()->getRoad()->getID()); v2->setLaneID(pm->getLane()->getID()); v2->setWaypointID(vt2.getID()); v2->setPosition(vt2); cerr << "Constructed node: " << pm->getLane()->getRoad()->getLayer()->getID() << "." << pm->getLane()->getRoad()->getID() << "." << pm->getLane()->getID() << "." << vt2.getID() << endl; WaypointsEdge *edge = new WaypointsEdge(); edge->setCosts(vt1.getXYDistanceTo(vt2)); // Add vertices and edge to graph. m_graph.updateEdge(v1, v2, edge); } // Add edges for the connectors. const vector<Connector> &listOfConnectors = pm->getListOfConnectors(); vector<Connector>::const_iterator mt = listOfConnectors.begin(); while (mt != listOfConnectors.end()) { Connector c = (*mt++); cerr << "Found connector: " << c.toString() << " to add." << endl; try { // Find start vertex. FindNodeByPointIDVisitor startVertexFinder(c.getSource()); m_scenario.accept(startVertexFinder); const PointModel *startPointModel = dynamic_cast<const PointModel*>(startVertexFinder.getLaneModel()); FindNodeByPointIDVisitor endVertexFinder(c.getTarget()); m_scenario.accept(endVertexFinder); const PointModel *endPointModel = dynamic_cast<const PointModel*>(endVertexFinder.getLaneModel()); if ( (startPointModel != NULL) && (endPointModel != NULL) ) { const IDVertex3 startV = startPointModel->getIDVertex3(c.getSource().getPointID()); const IDVertex3 endV = endPointModel->getIDVertex3(c.getTarget().getPointID()); WaypointVertex *v1 = new WaypointVertex(); v1->setLayerID(c.getSource().getLayerID()); v1->setRoadID(c.getSource().getRoadID()); v1->setLaneID(c.getSource().getLaneID()); v1->setWaypointID(c.getSource().getPointID()); v1->setPosition(startV); WaypointVertex *v2 = new WaypointVertex(); v2->setLayerID(c.getTarget().getLayerID()); v2->setRoadID(c.getTarget().getRoadID()); v2->setLaneID(c.getTarget().getLaneID()); v2->setWaypointID(c.getTarget().getPointID()); v2->setPosition(endV); WaypointsEdge *edge = new WaypointsEdge(); edge->setCosts(startV.getXYDistanceTo(endV)); m_graph.updateEdge(v1, v2, edge); cerr << "Adding edge for connector between " << v1->toString() << " and " << v2->toString() << endl; } } catch(...) {} } } }
void LaneVisitor::visit(const hesperia::data::scenario::Arc *arc) { if (arc != NULL) { stringstream name; name << arc->getLane()->getRoad()->getLayer()->getID() << "." << arc->getLane()->getRoad()->getID() << "." << arc->getLane()->getID(); cerr << "Found Arc Lane: " << name.str() << endl; IDVertex3 vt1 = arc->getStart(); IDVertex3 vt2 = arc->getEnd(); // Approximate arc by a set of lines. { // ScUI graphical modeling tool starts Arc at 6pm but we use 3pm. Thus, we have to correct the Z-rotation. double correctRotation = hesperia::data::Constants::PI / 2.0; // Transposition of the arc. hesperia::data::environment::Point3 t(0, 0, 0); t.setX(vt1.getX() - arc->getRadius() * cos(arc->getRotationZ() - correctRotation)); t.setY(vt1.getY() - arc->getRadius() * sin(arc->getRotationZ() - correctRotation)); t.setZ(0.05); double beginInterval = arc->getBeginInterval(); double endInterval = arc->getEndInterval(); double stepSize = 5.0 * hesperia::data::Constants::PI / 180.0; // 5° to rad. uint32_t steps = (unsigned int) round( (endInterval - beginInterval) / stepSize ); hesperia::data::environment::Point3 centerOld = vt1; for(uint32_t i = 0; i < steps; i++) { // Calculate the skeleton approximation. hesperia::data::environment::Point3 center(0, 0, 0); center.setX(arc->getRadius() * cos(arc->getRotationZ() - correctRotation + i * stepSize)); center.setY(arc->getRadius() * sin(arc->getRotationZ() - correctRotation + i * stepSize)); // Transpose the points. center += t; // Construct a line between to consecutive points. hesperia::data::environment::NamedLine line(name.str(), centerOld, center); m_listOfLines.push_back(line); // Keep old point. centerOld = center; } } WaypointVertex *v1 = new WaypointVertex(); v1->setLayerID(arc->getLane()->getRoad()->getLayer()->getID()); v1->setRoadID(arc->getLane()->getRoad()->getID()); v1->setLaneID(arc->getLane()->getID()); v1->setWaypointID(vt1.getID()); v1->setPosition(vt1); cerr << "Constructed node: " << arc->getLane()->getRoad()->getLayer()->getID() << "." << arc->getLane()->getRoad()->getID() << "." << arc->getLane()->getID() << "." << vt1.getID() << endl; WaypointVertex *v2 = new WaypointVertex(); v2->setLayerID(arc->getLane()->getRoad()->getLayer()->getID()); v2->setRoadID(arc->getLane()->getRoad()->getID()); v2->setLaneID(arc->getLane()->getID()); v2->setWaypointID(vt2.getID()); v2->setPosition(vt2); cerr << "Constructed node: " << arc->getLane()->getRoad()->getLayer()->getID() << "." << arc->getLane()->getRoad()->getID() << "." << arc->getLane()->getID() << "." << vt2.getID() << endl; WaypointsEdge *edge = new WaypointsEdge(); edge->setCosts(vt1.getXYDistanceTo(vt2)); // Add vertices and edge to graph. m_graph.updateEdge(v1, v2, edge); // Add edges for the connectors. const vector<Connector> &listOfConnectors = arc->getListOfConnectors(); vector<Connector>::const_iterator mt = listOfConnectors.begin(); while (mt != listOfConnectors.end()) { Connector c = (*mt++); cerr << "Found connector: " << c.toString() << " to add." << endl; try { // Find start vertex. FindNodeByPointIDVisitor startVertexFinder(c.getSource()); m_scenario.accept(startVertexFinder); const StraightLine *startStraightLine = dynamic_cast<const StraightLine*>(startVertexFinder.getLaneModel()); FindNodeByPointIDVisitor endVertexFinder(c.getTarget()); m_scenario.accept(endVertexFinder); const StraightLine *endStraightLine = dynamic_cast<const StraightLine*>(endVertexFinder.getLaneModel()); if ( (startStraightLine != NULL) && (endStraightLine != NULL) ) { const IDVertex3 startV = startVertexFinder.getIDVertex3(); const IDVertex3 endV = endVertexFinder.getIDVertex3(); v1 = new WaypointVertex(); v1->setLayerID(c.getSource().getLayerID()); v1->setRoadID(c.getSource().getRoadID()); v1->setLaneID(c.getSource().getLaneID()); v1->setWaypointID(c.getSource().getPointID()); v1->setPosition(startV); v2 = new WaypointVertex(); v2->setLayerID(c.getTarget().getLayerID()); v2->setRoadID(c.getTarget().getRoadID()); v2->setLaneID(c.getTarget().getLaneID()); v2->setWaypointID(c.getTarget().getPointID()); v2->setPosition(endV); edge = new WaypointsEdge(); edge->setCosts(startV.getXYDistanceTo(endV)); m_graph.updateEdge(v1, v2, edge); cerr << "Adding edge for connector between " << v1->toString() << " and " << v2->toString() << endl; } } catch(...) {} } } }