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