Position
PositionVector::transformToVectorCoordinates(const Position& p, bool extend) const {
// XXX this duplicates most of the code in nearest_offset_to_point2D. It should be refactored
if (extend) {
PositionVector extended = *this;
const SUMOReal dist = 2 * distance(p);
extended.extrapolate(dist);
return extended.transformToVectorCoordinates(p) - Position(dist, 0);
}
SUMOReal minDist = std::numeric_limits<SUMOReal>::max();
SUMOReal nearestPos = -1;
SUMOReal seen = 0;
int sign = 1;
for (const_iterator i = begin(); i != end() - 1; i++) {
const SUMOReal pos =
GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, true);
const SUMOReal dist = pos < 0 ? minDist : p.distanceTo2D(Line(*i, *(i + 1)).getPositionAtDistance(pos));
if (dist < minDist) {
nearestPos = pos + seen;
minDist = dist;
sign = isLeft(*i, *(i + 1), p) >= 0 ? -1 : 1;
}
if (i != begin() && pos == GeomHelper::INVALID_OFFSET) {
// even if perpendicular is set we still need to check the distance to the inner points
const SUMOReal cornerDist = p.distanceTo2D(*i);
if (cornerDist < minDist) {
const SUMOReal pos1 =
GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
const SUMOReal pos2 =
GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
nearestPos = seen;
minDist = cornerDist;
sign = isLeft(*(i - 1), *i, p) >= 0 ? -1 : 1;
}
}
}
seen += (*i).distanceTo2D(*(i + 1));
}
if (nearestPos != -1) {
return Position(nearestPos, sign * minDist);
} else {
return Position::INVALID;
}
}
