// Gets sensor information: either wall detection/no detection or ball detection.
void sensorUpdate(int type, bool detect, RealPosition &worldPos, RealPosition &robotPos) {
Position gridPos = realToGrid(worldPos);
Position robotGridPos = realToGrid(robotPos);
int l,c;
double minDist, d;
Position **neighbors;
int nNeighbors;
// Ball detection
if(type == RED_BALL || type == GREEN_BALL) {
// BFS to mark spots in the BALL_RADIUS.
bfsMark(type, detect, gridPos, BALL_RADIUS);
}
// Robot clearing
else if(type == ROBOT_BODY) {
bfsMark(type, detect, robotGridPos, ROBOT_RADIUS);
}
// Wall detection/no detection
else {
// Do a best first search to find a line from robot to wall and clear it:
minDist = distanceSqr(gridPos, robotGridPos);
while(robotGridPos.l != gridPos.l || robotGridPos.c != gridPos.c) {
l = robotGridPos.l;
c = robotGridPos.c;
theMap[l][c].pWall = bayesianUpdate(theMap[l][c].pWall, P_DETECT_GIVEN_WALL, P_DETECT_GIVEN_NWALL, false);
theMap[l][c].visited = true;
nNeighbors = allnNeighbors[l][c];
neighbors = allNeighbors[l][c];
for(int i = 0; i < nNeighbors; ++i) {
d = distanceSqr(gridPos, *neighbors[i]);
if(d < minDist) {
minDist = d;
robotGridPos.l = neighbors[i]->l;
robotGridPos.c = neighbors[i]->c;
}
}
}
// Update detected/undetected spot
l = gridPos.l;
c = gridPos.c;
if(isValid(gridPos)) {
theMap[l][c].visited = true;
if(detect) {
theMap[l][c].pBall = bayesianUpdate(theMap[l][c].pBall, P_DETECT_GIVEN_BALL, P_DETECT_GIVEN_NBALL, false);
}
theMap[l][c].pWall = bayesianUpdate(theMap[l][c].pWall, P_DETECT_GIVEN_WALL, P_DETECT_GIVEN_NWALL, detect);
}
}
}
bool setWallType(int type, RealPosition &orientation, RealPosition &robotPos) {
double minDist,d;
Position gridOri = realToGrid(orientation);
Position robotGridPos = realToGrid(robotPos);
if(!isValid(robotGridPos)) {
return false;
}
Position **neighbors;
int nNeighbors;
// Do a best first search to "draw" a line in the provided orientation and see if any walls intersect
int l=-1,c=-1;
minDist = distanceSqr(gridOri, robotGridPos);
do {
l = robotGridPos.l;
c = robotGridPos.c;
if(isWall(theMap[l][c])) {
theMap[l][c].wallType = type;
return true;
}
neighbors = allNeighbors[l][c];
nNeighbors = allnNeighbors[l][c];
for(int i = 0; i < nNeighbors; ++i) {
d = distanceSqr(gridOri, *neighbors[i]);
if(d < minDist) {
minDist = d;
robotGridPos.l = neighbors[i]->l;
robotGridPos.c = neighbors[i]->c;
}
}
} while((l != gridOri.l || c != gridOri.c) && minDist < FIELD_DIAMETER);
return false;
}
// BFS to mark every square around in a radius.
void bfsMark(int type, bool detect, Position &start, int radius) {
int nNeighbors;
Position **neighbors;
Position *cur;
++operationID;
visited[start.l][start.c] = operationID;
queueFront = queueBack = 0;
queue[queueBack++] = &start;
while(queueBack > queueFront) {
cur = queue[queueFront++];
nNeighbors = allnNeighbors[cur->l][cur->c];
neighbors = allNeighbors[cur->l][cur->c];
theMap[cur->l][cur->c].visited = true;
switch(type) {
case ROBOT_BODY:
theMap[cur->l][cur->c].pWall = bayesianUpdate(theMap[cur->l][cur->c].pWall, P_DETECT_GIVEN_WALL, P_DETECT_GIVEN_NWALL, detect);
default:
theMap[cur->l][cur->c].pBall = bayesianUpdate(theMap[cur->l][cur->c].pBall, P_DETECT_GIVEN_BALL, P_DETECT_GIVEN_NBALL, detect);
}
if(type == RED_BALL || type == GREEN_BALL) {
theMap[cur->l][cur->c].ballType = type;
}
for(int i = 0; i < nNeighbors; ++i) {
if(visited[neighbors[i]->l][neighbors[i]->c] != operationID) {
visited[neighbors[i]->l][neighbors[i]->c] = operationID;
if(distanceSqr(start, *neighbors[i]) <= radius*radius) {
queue[queueBack++] = neighbors[i];
}
}
}
}
}
bool operator()(GLushort i1, GLushort i2)
{
// Is this so horribly naive that it's completely unrealistic?
return distanceSqr(m_eye, truncate(m_positions[i1])) > distanceSqr(m_eye, truncate(m_positions[i2]));
}
double getLengthSqr(){
return distanceSqr(Coordinate(0., 0.));
}