public static void Main(string[] args){
PhysObject physobject = new PhysObject(new Vector2(0,100),new Vector2(0,100));
foreach(PhysObject n in PhysStep(physobject)){
Console.WriteLine("Position: "+n.pos.x+":"+n.pos.y);
}
Console.ReadLine();
}
protected static int DoLabor(Place location, PhysObject target, Tool tool)
{
int result = 1;
//
return(result);
}
// returns true if robot senses food in +/- angle within distance
public bool senseFood(double maxangle, double maxdistance, Robot robot,
PhysObject obj)
{
//Console.WriteLine("object location: " + obj.Location.X + " " + obj.Location.Y);
if (obj.GetType() == typeof(Food))
{
//Console.WriteLine("THIS IS A FOOD OBJECT");
}
return (base.senseObject(maxangle, maxdistance, robot, obj) && (obj.GetType() == typeof(Food)));
}
public static IEnumerable<PhysObject> PhysStep(PhysObject phys){
Vector2 pos = phys.pos;
Vector2 vel= phys.vel;
int r=0;
Vector2 gravity=new Vector2(0,-10);
while(cancel ==false&&r<11){
vel+=gravity;
phys.pos=pos+vel;
pos=phys.pos;
phys.vel=vel;
r++;
yield return phys;
}
}
// returns true if robot senses another robot in +/- angle within distance
public bool senseRobot(double maxangle, double maxdistance, Robot robot,
PhysObject obj)
{
if (obj.GetType() == typeof(Robot))
{
Robot other = (Robot)obj;
if (Math.Abs(other.Location.X - robot.Location.X) < 5 &&
Math.Abs(other.Location.Y - robot.Location.Y) < 5)
{
return false;
}
}
return senseObject(maxangle, maxdistance, robot, obj) && (obj.GetType() == typeof(Robot));
}
// returns true if robot senses obstacle in +/- angle within distance
public bool senseObstacle(double maxangle, double maxdistance, Robot robot,
PhysObject obj)
{
return (base.senseObject(maxangle, maxdistance, robot, obj) && (obj.GetType() == typeof(Obstacle)));
}
// Given an object, determines whether that object is sensed by the robot
public bool senseObject(double maxangle, double maxdistance, Robot robot, PhysObject obj)
{
Coordinates objectLocation = obj.Location;
Coordinates robotLocation = robot.Location;
double distance = Math.Sqrt(
(objectLocation.X - robotLocation.X)*(objectLocation.X - robotLocation.X) +
(objectLocation.Y - robotLocation.Y)*(objectLocation.Y - robotLocation.Y));
//Console.WriteLine("==================Proximity Sensor===========================");
//Console.WriteLine("x obstacle = " + objectLocation.X);
//Console.WriteLine("y obstacle = " + objectLocation.Y);
//Console.WriteLine("x robot = " + robotLocation.X);
//Console.WriteLine("y robot = " + robotLocation.Y);
//Console.WriteLine("orientation robot = " + robot.Orientation);
//Console.WriteLine("DISTANCE = " + distance);
//Console.WriteLine("maxdistance =");
//System.Console.WriteLine(maxdistance);
if (distance > maxdistance)
{
//Console.WriteLine("Detectable distance:" + maxdistance);
//Console.WriteLine("Actual Distance: " + distance);
//Console.WriteLine("Too far away, cannot sense it. ");
return false;
}
//Console.WriteLine("robot " + robot.Id + " detects an obstacle");
// get angle between
double radians = Math.Atan2(objectLocation.Y - robotLocation.Y,
objectLocation.X - robotLocation.X);// -0.5 * Math.PI; // -pi/2 factor shifts frame
// according to definition in
// vision
double angle = radians * (180/Math.PI); // convert to from -180 to 180
//Console.WriteLine("ROBOT-OBSTACLE ANGLE = " + angle);
// assume orientation is defined from -180 to 180
// TODO: Is the above true?
double angleDifference;
double orientation = robot.Orientation;
//Console.WriteLine("ROBOT DIRECTION = " + orientation);
if((angle >= 0 && orientation >= 0) || (angle <= 0 && orientation <= 0)){
//Console.WriteLine("ANGLES HAVE SAME SIGN");
angleDifference = Math.Abs(angle - orientation);
}else{ //find angle difference by adding their degree distance from 0, then
// finding shorter arc if it exists
//Console.WriteLine("ANGLES HAVE DIFFERENT SIGNS");
angleDifference = Math.Abs(angle) + Math.Abs(orientation);
if(angleDifference > (360 - angleDifference)){
angleDifference = 360 - angleDifference;
}
}
//Console.WriteLine("ANGLE DIFFERENCE = " + angleDifference);
//Console.WriteLine("=============================================");
if(angleDifference < maxangle){
//Console.WriteLine("max angle:" + maxangle);
//Console.WriteLine("Actual angle: " + angle);
//Console.WriteLine("Angle difference is less than max angle");
Console.WriteLine("Colliding with object");
return true;
}
//Console.WriteLine("before last false");
return false;
}
