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(...) {}
}
}
}
