Vector2D Road::getLaneCenter(const int &l, const double &s) { if (l != Lane::NOLANE) { LaneSection *section = (--laneSectionMap.upper_bound(s))->second; //std::cout << "getLaneCenter: Road: " << id << ", section: " << section << std::endl; return Vector2D(section->getDistanceToLane(s, l) + 0.5 * section->getLaneWidth(s, l), atan(0.5 * section->getLaneWidthSlope(s, l))); } else { return Vector2D(0.0, 0.0); } }