const Pose2D Pose2D::interpolate(const Pose2D& startPose,
const Pose2D& endPose,
const double& progress)
{
if (progress <= 0) {
return startPose;
} else if (progress >= 1) {
return endPose;
}
Vector2d diff = endPose.getPosition() - startPose.getPosition();
Angle angle = Angle::interpolate(startPose.getAngle(), endPose.getAngle(), progress);
return Pose2D(startPose.getPosition() + (diff * progress), angle);
}
void MovableObject::update(const Pose2D& pose)
{
Pose2D previousPose = _pose;
// Call update of visible object
VisibleObject::update(pose);
// Calculate object speed
_speed = _pose.getPosition() - previousPose.getPosition();
}
const Angle Pose2D::getAngleTo(const Pose2D& other) const
{
return getAngleTo(other.getPosition());
}
const Pose2D Pose2D::operator+(const Pose2D& other) const
{
return Pose2D(getPosition() + other.getPosition(), getAngle() + other.getAngle());
}
void StreetRenderer::drawSegment(QPainter& painter,
A2O::Segment::Ptr segment,
const PanelTransformation& panelTransform)
{
Pose2D beginPose = segment->getBeginLink()->getPose();
Pose2D endPose = Geometry::calculateArcPose(segment->getLength(), segment->getBendingAngle());
Angle bendingAngle = segment->getBendingAngle();
bool straightSegment = segment->getBendingAngle().rad() == 0;
double radius = 0;
// Set the current painting system to the begin of the segment
panelTransform.setTransform(beginPose, painter);
QPointF begin(0, 0);
QPointF end(endPose.getPosition()(0), endPose.getPosition()(1));
// Draw segment background
if (straightSegment) {
// Draw street background
if (segment->consumesDriveInstruction()) {
painter.setPen(_instructionStreetPen);
} else {
painter.setPen(_streetPen);
}
painter.drawLine(begin, end);
} else {
radius = double(segment->getLength() / bendingAngle.rad());
// Draw street background
if (segment->consumesDriveInstruction()) {
painter.setPen(_instructionStreetPen);
} else {
painter.setPen(_streetPen);
}
RenderUtil::drawArc(painter, begin, radius, bendingAngle.rad());
painter.setPen(_thinStreetPen);
RenderUtil::drawArc(painter, QPointF(0, -0.465), radius + 0.465, bendingAngle.rad());
RenderUtil::drawArc(painter, QPointF(0, 0.465), radius - 0.465, bendingAngle.rad());
}
// Draw Signs belonging to segment
std::vector<RoadSign::Ptr> signs = segment->getRoadSigns();
for(unsigned int i=0; i < signs.size(); i++) {
RoadSign::Ptr sign = signs[i];
std::stringstream fileName;
fileName << ":/res/" << sign->getName() << ".png";
std::string file = fileName.str();
QImage image(file.c_str());
Angle signAngle = sign->getPose().getAngle();
signAngle -= Angle::deg(90);
image = image.transformed(QTransform().rotate(signAngle.deg()));
image = image.mirrored(false, true);
QRectF rect(0.0, -0.25, 0.5, 0.5);
painter.drawImage(rect, image);
}
// Draw begin link and sub segments
drawSubSegments(painter, segment, panelTransform);
panelTransform.setTransform(beginPose, painter);
// Draw segment lines
if (straightSegment) {
// Draw segment lines
painter.setPen(_streetLinePen);
if (segment->isXCrossing()) {
painter.drawLine(QPointF(0, 0.465), QPointF(0.035, 0.5));
painter.drawLine(QPointF(0, -0.465), QPointF(0.035, -0.5));
painter.drawLine(QPointF(1, 0.465), QPointF(0.965, 0.5));
painter.drawLine(QPointF(1, -0.465), QPointF(0.965, -0.5));
} else if(segment->isTCrossingLS()) {
painter.drawLine(QPointF(0, 0.465), QPointF(0.035, 0.5));
painter.drawLine(QPointF(1, 0.465), QPointF(0.965, 0.5));
painter.drawLine(QPointF(0, -0.465), QPointF(1, -0.465));
} else if(segment->isTCrossingLR()) {
painter.drawLine(QPointF(0, 0.465), QPointF(0.035, 0.5));
painter.drawLine(QPointF(0, -0.465), QPointF(0.035, -0.5));
painter.drawLine(QPointF(0.965, 0.5), QPointF(0.965, -0.5));
} else if(segment->isTCrossingSR()) {
painter.drawLine(QPointF(0, 0.465), QPointF(1, 0.465));
painter.drawLine(QPointF(0, -0.465), QPointF(0.035, -0.5));
painter.drawLine(QPointF(1, -0.465), QPointF(0.965, -0.5));
} else {
painter.drawLine(QPointF(0, 0.465), QPointF(segment->getLength(), 0.465));
painter.drawLine(QPointF(0, -0.465), QPointF(segment->getLength(), -0.465));
// Draw middle line
painter.setPen(_middleLinePen);
painter.drawLine(begin, end);
}
} else {
// Draw left and right line
painter.setPen(_streetLinePen);
if (radius > 0) {
RenderUtil::drawArc(painter, QPointF(0, -0.465), radius + 0.465, bendingAngle.rad());
RenderUtil::drawArc(painter, QPointF(0, 0.465), radius - 0.465, bendingAngle.rad(), _helperLinePen);
} else {
RenderUtil::drawArc(painter, QPointF(0, -0.465), radius + 0.465, bendingAngle.rad(), _helperLinePen);
RenderUtil::drawArc(painter, QPointF(0, 0.465), radius - 0.465, bendingAngle.rad());
}
// Draw middle line
painter.setPen(_middleLinePen);
RenderUtil::drawArc(painter, begin, radius, bendingAngle.rad());
}
}
