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()); } }