std::vector<QPolygonF> ObstacleBitmap::drawCSpace(const Robot& robot, const Configuration& config){ std::vector<QPolygonF> ret; for (size_t i = 0; i < obstacles.size(); i++){ for(size_t j = 0; j < obstacles.at(i).NPolygons(); j++){ QPolygonF transObstacle(CoordinateTransform::toPlannerPoints(obstacles.at(i).getPolygonF(j), obstacles.at(i).getInitialConfiguration(), Constant::SWP)); for(size_t k = 0; k < robot.NPolygons(); k++){ QPolygonF transRobot(minusPolygon(QPolygonF(CoordinateTransform::toPlannerPoints(robot.getPolygonF(k), config, Constant::SWP)))); //qDebug() << "robot: " << transRobot; //qDebug() << "Obstacle: " << transObstacle; QPolygonF result = minkowskiSums(transObstacle, transRobot); //qDebug() << "result: " << result; ret.push_back(result); drawObstacle(result); } } } QPolygonF top; top.push_back(QPointF(0, -1)); top.push_back(QPointF(0, -2)); top.push_back(QPointF(Constant::BITMAPSIZE, -2)); top.push_back(QPointF(Constant::BITMAPSIZE, -1)); QPolygonF bottom; bottom.push_back(QPointF(0, Constant::BITMAPSIZE+1)); bottom.push_back(QPointF(0, Constant::BITMAPSIZE)); bottom.push_back(QPointF(Constant::BITMAPSIZE, Constant::BITMAPSIZE)); bottom.push_back(QPointF(Constant::BITMAPSIZE, Constant::BITMAPSIZE+1)); QPolygonF left; left.push_back(QPointF(-2, 0)); left.push_back(QPointF(-1, 0)); left.push_back(QPointF(-1, Constant::BITMAPSIZE)); left.push_back(QPointF(-2, Constant::BITMAPSIZE)); QPolygonF right; right.push_back(QPointF(Constant::BITMAPSIZE, 0)); right.push_back(QPointF(Constant::BITMAPSIZE+1, 0)); right.push_back(QPointF(Constant::BITMAPSIZE+1, Constant::BITMAPSIZE)); right.push_back(QPointF(Constant::BITMAPSIZE, Constant::BITMAPSIZE)); for(size_t k = 0; k < robot.NPolygons(); k++){ QPolygonF transRobot(CoordinateTransform::toPlannerPoints(robot.getPolygonF(k), config, Constant::SWP)); QPolygonF result1 = minkowskiSums(top, transRobot); QPolygonF result2 = minkowskiSums(bottom, transRobot); QPolygonF result3 = minkowskiSums(left, transRobot); QPolygonF result4 = minkowskiSums(right, transRobot); ret.push_back(result1); drawObstacle(result1); ret.push_back(result2); drawObstacle(result2); ret.push_back(result3); drawObstacle(result3); ret.push_back(result4); drawObstacle(result4); } return ret; }