public PhysObject(int id, String desc, Coordinates location, double orientation, double width, double height)
{
this.Id = id;
this.Description = desc;
this.Location = location;
this.Orientation = orientation;
this.Width = width;
this.Height = height;
}
/* Get the location on the grid (actually the CellData object at that
* location, which contains all the information)
*/
public CellData getGridLoc(Coordinates location)
{
double epsilon = 0.00001; // to avoid numeric issues with double equality comparison
int gridX;
int gridY;
// check for boundary conditions. If X location is at world boundaries, then grid should be
// at last square
if (location.X >= WorldWidth && location.X - WorldWidth <= epsilon) gridX = NumSquaresX - 1;
// to avoid numeric issues of a 0 location as being recognized as slightly off the grid
else if (location.X <= 0 && location.X >= -epsilon) gridX = 0;
// range from [0, NumSquaresX - 1] as long as location.X != WorldWidth (case handled above)
else gridX = (int)Math.Floor(NumSquaresX * location.X / WorldWidth);
// check for boundary conditions. If X location is at world boundaries, then grid should be
// at last square
if (location.Y >= WorldHeight && location.Y - WorldHeight <= epsilon) gridY = NumSquaresY - 1;
// to avoid numeric issues of a 0 location as being recognized as slightly off the grid
else if (location.Y <= 0 && location.Y >= -epsilon) gridY = 0;
// range from [0, NumSquaresY - 1] as long as location.Y != WorldHeight (case handled above)
else gridY = (int)Math.Floor(NumSquaresY * location.Y / WorldHeight);
return cellData[gridX, gridY];
// gridX = NumSquaresX - 1;
////else if (location.X < 0)
//// gridX = 0;
//else if (location.X < 0 && location.X > -50)
// gridX = 0;
//else
// gridX = (int)Math.Floor(NumSquaresX * location.X / WorldWidth);
//if (location.Y >= WorldHeight)
// gridY = NumSquaresY - 1;
////else if (location.Y < 0)
// // gridY = 0;
//else if (location.Y < 0 && location.Y > -50)
// gridY = 0;
//else
// gridY = (int) Math.Floor(NumSquaresY * location.Y / WorldHeight);
//Console.WriteLine(".......................................");
//Console.WriteLine("Location:" + location.X + " " + location.Y);
//Console.WriteLine("grid [x, y]:" + gridX + " " + gridY);
//Console.WriteLine(".......................................");
//return cellData[gridX, gridY];
}
public Obstacle(int id, String desc, Coordinates location, double orientation, double width, double height)
: base(id, desc, location, orientation, width, height)
{
}
public Food(int id, String desc, Coordinates location, double orientation, double width, double height)
: base(id, desc, location, orientation, width, height)
{
//TODO Add food-specific constructor parameters
}
public Robot(int id, String desc, Coordinates location, double orientation, double width, double height)
: base(id, desc, location, orientation, width, height)
{
Stop();
}
// Turn on continuous marking
public void TurnOnContinuousMarking(Robot robot)
{
// Make a copy of the location
Coordinates locCopy = new Coordinates(robot.Location.X, robot.Location.Y);
// Get location of robot and store in prevLocations
prevLocations.Add(robot, locCopy);
}
public void GridUpdate(Robot robot)
{
// Console.WriteLine(robot.Id);
Console.WriteLine("ROBOT's X in gridupdate : " + robot.Location.X);
Console.WriteLine("ROBOT's Y in gridupdate : " + robot.Location.Y);
// Take out robot from locations of where robot is in the grid
CellData location = findObj(robot);
if (location != null)
{
location.objectsInSquare.Remove(robot);
}
// Add robot
getGridLoc(robot.Location).objectsInSquare.Add(robot);
Coordinates newLoc = new Coordinates(robot.Location.X, robot.Location.Y);
CellData finalSpot = getGridLoc(newLoc);
// Update prevLocationsForMark so that they're synchronized. This assume that Mark is called AFTER GridUpdate
foreach (Robot r in prevLocations.Keys)
{
prevLocationsForMark[r] = new Coordinates(prevLocations[r].X, prevLocations[r].Y);
}
if(!prevLocations.ContainsKey(robot)){
prevLocations[robot] = newLoc;
finalSpot.numTimesVisited++;
return;
}
Coordinates prevLoc = new Coordinates(prevLocations[robot].X, prevLocations[robot].Y);
Coordinates curLoc = new Coordinates(prevLocations[robot].X, prevLocations[robot].Y);
CellData curSpot = getGridLoc(curLoc);
CellData startSpot = getGridLoc(curLoc);
if(finalSpot.row == curSpot.row && finalSpot.col == curSpot.col){
// don't mark anything
return;
}
double incr = (double)Math.Min((WorldHeight / NumSquaresY), (WorldWidth / NumSquaresX)) / 5.0;
double dX = robot.Location.X - prevLocations[robot].X;
double dY = robot.Location.Y - prevLocations[robot].Y;
double incrX = incr * dX / (double)Math.Sqrt(dX * dX + dY * dY);
double incrY = incr * dY / (double)Math.Sqrt(dX * dX + dY * dY);
// Console.WriteLine("Prev location is " + curLoc.X + ", " + curLoc.Y + " trying to reach " + newLoc.X + ", " + newLoc.Y);
do
{
CellData nextSpot;
// Increment X and Y
curLoc.X += incrX;
curLoc.Y += incrY;
// Get next grid spot
nextSpot = getGridLoc(curLoc);
// Console.WriteLine("Current spot: " + nextSpot.col + " " + nextSpot.row);
// If different mark it
if (nextSpot != curSpot)
{
nextSpot.numTimesVisited++;
curSpot = nextSpot;
}
//if (!inBounds(curSpot, startSpot, finalSpot))
//{
// break;
//}
//} while (curSpot != finalSpot);
} while (curSpot.row != finalSpot.row || curSpot.col != finalSpot.col);
prevLocations[robot] = curLoc;
return;
}
