QHash<QString, QLine> Controller::coordinateAxes(Length length){
QLine x;
QLine y;
QLine z;
int size = length;
//z
z.setP1(QPoint(0, size));
z.setP2(QPoint(0, -size));
//x
x.setP1(QPoint( -size, 0));
x.setP2(QPoint(size, 0));
//z
y.setP1(QPoint(size * cos(_alpha * M_PI/180), -size * sin(_alpha * M_PI/180)));
y.setP2(QPoint(-size * cos(_alpha * M_PI/180), size * sin(_alpha * M_PI/180)));
QHash<QString, QLine> axes = {
{"X", x},
{"Y", y},
{"Z", z}
};
return axes;
}
QLine extractBlackGuide(QImage input,QPoint start,bool hor) {
QLine line;
QPoint step(1,0);
if (!hor) {
// vertical movement
step = QPoint(0,1);
}
QPoint pt = start;
while (pt.x() < input.width() && pt.y() < input.height()) {
if (input.pixel(pt) == 0xFF000000) {
line.setP1(pt);
break;
}
pt += step;
}
while (pt.x() < input.width() && pt.y() < input.height()) {
if (input.pixel(pt) != 0xFF000000) {
line.setP2(pt);
break;
}
pt += step;
}
return line;
}
void map_widget::drawRobotPose(QPainter *painter , QPaintEvent *event)
{
QPoint Robot_Pose = this->convertToActFrame( sensor_connection->act_pose.x_pos,
sensor_connection->act_pose.y_pos,
event );
painter->setPen( QPen( Qt::red, 5, Qt::SolidLine, Qt::RoundCap) );
painter->drawPoint( Robot_Pose );
// Draw the orientatian;
// Berechne Steigung
const double lenght_of_orientation_line = 2.0;
double theta = sensor_connection->act_pose.theta;
while( theta >= (2* M_PI) )
{
theta -= (2* M_PI);
}
double skew_y = tan( theta );
double skew_x = 1.0;
if( theta == 0.5 * M_PI )
{
skew_y = 1.0;
skew_x = 0.0;
}
else if( theta == (3.0*M_PI)/2.0 )
{
skew_y = -1.0;
skew_x = 0.0;
}
else if( theta >= 0.5 * M_PI && theta < (3.0*M_PI)/2.0)
{
skew_y *= -1.0;
skew_x *= -1.0;
}
double distance = sqrt( (skew_x*skew_x) + (skew_y*skew_y) );
skew_x = (skew_x * lenght_of_orientation_line) / distance;
skew_y = (skew_y * lenght_of_orientation_line) / distance;
skew_x *= ((double) event->rect().width() / (double) this->visible_rect.width() );
skew_y *= ((double) event->rect().height() / (double) this->visible_rect.height() );
QLine line;
line.setP1( Robot_Pose );
line.setP2( QPoint(Robot_Pose.x() + skew_x, Robot_Pose.y() - skew_y) );
painter->setPen( QPen( Qt::red, 3, Qt::SolidLine, Qt::RoundCap) );
painter->drawLine( line );
}
void tst_QLine::testSet()
{
{
QLine l;
l.setP1(QPoint(1, 2));
l.setP2(QPoint(3, 4));
QCOMPARE(l.x1(), 1);
QCOMPARE(l.y1(), 2);
QCOMPARE(l.x2(), 3);
QCOMPARE(l.y2(), 4);
l.setPoints(QPoint(5, 6), QPoint(7, 8));
QCOMPARE(l.x1(), 5);
QCOMPARE(l.y1(), 6);
QCOMPARE(l.x2(), 7);
QCOMPARE(l.y2(), 8);
l.setLine(9, 10, 11, 12);
QCOMPARE(l.x1(), 9);
QCOMPARE(l.y1(), 10);
QCOMPARE(l.x2(), 11);
QCOMPARE(l.y2(), 12);
}
{
QLineF l;
l.setP1(QPointF(1, 2));
l.setP2(QPointF(3, 4));
QCOMPARE(l.x1(), 1.0);
QCOMPARE(l.y1(), 2.0);
QCOMPARE(l.x2(), 3.0);
QCOMPARE(l.y2(), 4.0);
l.setPoints(QPointF(5, 6), QPointF(7, 8));
QCOMPARE(l.x1(), 5.0);
QCOMPARE(l.y1(), 6.0);
QCOMPARE(l.x2(), 7.0);
QCOMPARE(l.y2(), 8.0);
l.setLine(9.0, 10.0, 11.0, 12.0);
QCOMPARE(l.x1(), 9.0);
QCOMPARE(l.y1(), 10.0);
QCOMPARE(l.x2(), 11.0);
QCOMPARE(l.y2(), 12.0);
}
}