void Road::getLaneRoadPoints(double s, int i, RoadPoint &pointIn, RoadPoint &pointOut, double &disIn, double &disOut)
{
LaneSection *section = (--laneSectionMap.upper_bound(s))->second;
disIn = section->getDistanceToLane(s, i);
disOut = disIn + section->getLaneWidth(s, i);
//std::cerr << "s: " << s << ", i: " << i << ", distance: " << disIn << ", width: " << disOut-disIn << std::endl;
Vector3D xyPoint = ((--xyMap.upper_bound(s))->second)->getPoint(s);
Vector2D zPoint = ((--zMap.upper_bound(s))->second)->getPoint(s);
double alpha = ((--lateralProfileMap.upper_bound(s))->second)->getAngle(s, (disOut + disIn) * 0.5);
double beta = zPoint[1];
double gamma = xyPoint[2];
double Tx = (sin(alpha) * sin(beta) * cos(gamma) - cos(alpha) * sin(gamma));
double Ty = (sin(alpha) * sin(beta) * sin(gamma) + cos(alpha) * cos(gamma));
double Tz = (sin(alpha) * cos(beta));
//std::cout << "s: " << s << ", i: " << i << ", alpha: " << alpha << ", beta: " << beta << ", gamma: " << gamma << std::endl;
double nx = sin(alpha) * sin(gamma) + cos(alpha) * sin(beta) * cos(gamma);
double ny = cos(alpha) * sin(beta) * sin(gamma) - sin(alpha) * cos(gamma);
double nz = cos(alpha) * cos(beta);
pointIn = RoadPoint(xyPoint.x() + Tx * disIn, xyPoint.y() + Ty * disIn, zPoint[0] + Tz * disIn, nx, ny, nz);
pointOut = RoadPoint(xyPoint.x() + Tx * disOut, xyPoint.y() + Ty * disOut, zPoint[0] + Tz * disOut, nx, ny, nz);
//std::cout << "s: " << s << ", i: " << i << ", inner: x: " << pointIn.x() << ", y: " << pointIn.y() << ", z: " << pointIn.z() << std::endl;
//std::cout << "s: " << s << ", i: " << i << ", outer: x: " << pointOut.x() << ", y: " << pointOut.y() << ", z: " << pointOut.z() << std::endl << std::endl;
}
RoadPoint Road::getRoadPoint(double s, double t)
{
//RoadPoint center = getChordLinePoint(s);
Vector3D xyPoint = ((--xyMap.upper_bound(s))->second)->getPoint(s);
Vector2D zPoint = ((--zMap.upper_bound(s))->second)->getPoint(s);
//std::cerr << "Center: x: " << center.x() << ", y: " << center.y() << ", z: " << center.z() << std::endl;
double alpha = ((--lateralProfileMap.upper_bound(s))->second)->getAngle(s, t);
double beta = zPoint[1];
double gamma = xyPoint[2];
//std::cerr << "s: " << s << "Alpha: " << alpha << ", Beta: " << beta << ", Gamma: " << gamma << std::endl;
double sinalpha = sin(alpha);
double cosalpha = cos(alpha);
double sinbeta = sin(beta);
double cosbeta = cos(beta);
double singamma = sin(gamma);
double cosgamma = cos(gamma);
return RoadPoint(xyPoint.x() + (sinalpha * sinbeta * cosgamma - cosalpha * singamma) * t,
xyPoint.y() + (sinalpha * sinbeta * singamma + cosalpha * cosgamma) * t,
zPoint[0] + (sinalpha * cosbeta) * t,
sinalpha * singamma + cosalpha * sinbeta * cosgamma,
cosalpha * sinbeta * singamma - sinalpha * cosgamma,
cosalpha * cosbeta);
}
RoadPoint GetRoadPoint(bool front) const { return RoadPoint(m_featureId, GetPointId(front)); }
RoadPoint Road::getChordLinePoint(double s)
{
Vector3D xyPoint(((--xyMap.upper_bound(s))->second)->getPoint(s));
double zValue = (((--zMap.upper_bound(s))->second)->getValue(s));
return RoadPoint(xyPoint.x(), xyPoint.y(), zValue);
}
