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; }