Ejemplo n.º 1
0
// 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);
    }
  }
}
Ejemplo n.º 2
0
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;
}
Ejemplo n.º 3
0
// 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];
	}
      }
    }
  }
}
Ejemplo n.º 4
0
 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]));
 }
Ejemplo n.º 5
0
	double getLengthSqr(){
		return distanceSqr(Coordinate(0., 0.));
	}