void propagateGridValues(Grid3D<float>* grid, const glm::vec3& voxelSize) {
int WIDTH = grid->getWidth();
int HEIGHT = grid->getHeight();
int DEPTH = grid->getDepth();
// first pass
for (int i = 0; i < WIDTH; i++) {
for (int j = 0; j < HEIGHT; j++) {
for (int k = 0; k < DEPTH; k++) {
float dist = grid->get(i, j, k);
dist = minDist(grid, dist, i, j, k, -1, -1, -1, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, -1, -1, 0, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, -1, -1, 1, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, -1, 0, -1, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, -1, 0, 0, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, -1, 0, 1, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, -1, 1, -1, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, -1, 1, 0, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, -1, 1, 1, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, 0, -1, -1, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, 0, -1, 0, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, 0, -1, 1, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, 0, 0, -1, WIDTH, HEIGHT, DEPTH, voxelSize);
grid->set(i, j, k, dist);
}
}
}
// second pass
for (int i = WIDTH - 1; i >= 0; i--) {
for (int j = HEIGHT - 1; j >= 0; j--) {
for (int k = DEPTH - 1; k >= 0; k--) {
float dist = grid->get(i, j, k);
dist = minDist(grid, dist, i, j, k, 1, 1, 1, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, 1, 1, 0, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, 1, 1, -1, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, 1, 0, 1, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, 1, 0, 0, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, 1, 0, -1, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, 1, -1, 1, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, 1, -1, 0, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, 1, -1, -1, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, 0, 1, 1, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, 0, 1, 0, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, 0, 1, -1, WIDTH, HEIGHT, DEPTH, voxelSize);
dist = minDist(grid, dist, i, j, k, 0, 0, 1, WIDTH, HEIGHT, DEPTH, voxelSize);
grid->set(i, j, k, dist);
}
}
}
}
int Marker::getMarkerId(cv::Mat &markerImage,int &nRotations)
{
// assert(markerImage.rows == markerImage.cols);
// assert(markerImage.type() == CV_8UC1);
cv::Mat grey = markerImage;
// Threshold image
cv::threshold(grey, grey, 125, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);
#ifdef SHOW_DEBUG_IMAGES
cv::showAndSave("Binary marker", grey);
#endif
//Markers are divided in 7x7 regions, of which the inner 5x5 belongs to marker info
//the external border should be entirely black
int cellSize = markerImage.rows / 7;
for (int y=0;y<7;y++)
{
int inc=6;
if (y==0 || y==6) inc=1; //for first and last row, check the whole border
for (int x=0;x<7;x+=inc)
{
int cellX = x * cellSize;
int cellY = y * cellSize;
cv::Mat cell = grey(cv::Rect(cellX,cellY,cellSize,cellSize));
int nZ = cv::countNonZero(cell);
if (nZ > (cellSize*cellSize) / 2)
{
return -1;//can not be a marker because the border element is not black!
}
}
}
cv::Mat bitMatrix = cv::Mat::zeros(5,5,CV_8UC1);
//get information(for each inner square, determine if it is black or white)
for (int y=0;y<5;y++)
{
for (int x=0;x<5;x++)
{
int cellX = (x+1)*cellSize;
int cellY = (y+1)*cellSize;
cv::Mat cell = grey(cv::Rect(cellX,cellY,cellSize,cellSize));
int nZ = cv::countNonZero(cell);
if (nZ> (cellSize*cellSize) /2)
bitMatrix.at<uchar>(y,x) = 1;
}
}
//check all possible rotations
cv::Mat rotations[4];
int distances[4];
rotations[0] = bitMatrix;
distances[0] = hammDistMarker(rotations[0]);
std::pair<int,int> minDist(distances[0],0);
for (int i=1; i<4; i++)
{
//get the hamming distance to the nearest possible word
rotations[i] = rotate(rotations[i-1]);
distances[i] = hammDistMarker(rotations[i]);
if (distances[i] < minDist.first)
{
minDist.first = distances[i];
minDist.second = i;
}
}
nRotations = minDist.second;
if (minDist.first == 0)
{
return mat2id(rotations[minDist.second]);
}
return -1;
}
// ---------------------------------------------------------------------------
// SimulatorCL::getObstacleDistance
// ---------------------------------------------------------------------------
Millimeter SimulatorCL::getObstacleDistance(SimulatorRobot* robot,
Millimeter rPosCaptor,
Radian dirPosCaptor,
Millimeter zPosCaptor,
Radian dirCaptor)
{
if (!robot) return INFINITE_DIST;
Point captor(rPosCaptor, dirPosCaptor);
Position robotPos;
robot->getRobotRealPosition(robotPos.center, robotPos.direction);
Geometry2D::convertToOrthogonalCoord(robotPos.center,
robotPos.direction,
captor);
Point captorEndPoint=captor;
captorEndPoint.x+=SIMU_MAX_DIST_DETECT_OBSTACLE*cos(dirCaptor+robotPos.direction);
captorEndPoint.y+=SIMU_MAX_DIST_DETECT_OBSTACLE*sin(dirCaptor+robotPos.direction);
Segment captorVision(captor, captorEndPoint);
Millimeter distance=INFINITE_DIST;
Point intersectionPt;
// detection des murs
if (zPosCaptor < TERRAIN_BORDURE_HAUTEUR) {
Point* wallPts=getWallPts();
for(unsigned int i=0;i!=SIMU_WALL_BORDER_PTS_NBR;i++) {
Segment wallBorder(wallPts[i], wallPts[((i+1)%SIMU_WALL_BORDER_PTS_NBR)]);
if(Geometry2D::getSegmentsIntersection(wallBorder, captorVision,
intersectionPt)){
distance = minDist(distance, dist(intersectionPt, captor));
}
}
}
// bordure des ponts
if (zPosCaptor < TERRAIN_BORDURE_PONT_HAUTEUR) {
Point* bridgePts=SimulatorCL::instance()->getBridgePts();
for(unsigned int i=0;i+1<SIMU_BRIDGE_BORDER_PTS_NBR;i+=2) {
Segment bridgeBorder(bridgePts[i], bridgePts[i+1]);
if(Geometry2D::getSegmentsIntersection(bridgeBorder, captorVision,
intersectionPt)){
distance = minDist(distance, dist(intersectionPt, captor));
}
}
}
// robots
for(unsigned int i=0; i<SIMU_PORT_MAX_CONNECTIONS; i++) {
SimulatorRobot* robotIter = server_->getRobot(i);
if (robotIter && robotIter != robot) {
if (robotIter->getIntersection(captor, captorVision, zPosCaptor, intersectionPt)) {
distance = minDist(distance, dist(intersectionPt, captor));
}
}
}
// balles
for(unsigned int i=0;i!=BALLE_GRS_NBR;i++) {
if (sgBalls_[i]->getIntersection(captor, captorVision, zPosCaptor, intersectionPt)) {
distance = minDist(distance, dist(intersectionPt, captor));
}
}
// quilles
for(unsigned int i=0;i!=QUILLE_NBR;i++) {
if (sskittles_[i]->getIntersection(captor, captorVision, zPosCaptor, intersectionPt)) {
distance = minDist(distance, dist(intersectionPt, captor));
}
}
return distance;
}