public static bool collide(Wall wall, Robot robot) { Point2D a1 = new Point2D(wall.line.p1); Point2D a2 = new Point2D(wall.line.p2); Point2D b = new Point2D(robot.location.x, robot.location.y); if (!wall.visible) { return(false); } double rad = robot.radius; double r = ((b.x - a1.x) * (a2.x - a1.x) + (b.y - a1.y) * (a2.y - a1.y)) / wall.line.length_sq(); double px = a1.x + r * (a2.x - a1.x); double py = a1.y + r * (a2.y - a1.y); Point2D np = new Point2D(px, py); double rad_sq = rad * rad; if (r >= 0.0f && r <= 1.0f) { if (np.distance_sq(b) < rad_sq) { return(true); } else { return(false); } } double d1 = b.distance_sq(a1); double d2 = b.distance_sq(a2); if (d1 < rad_sq || d2 < rad_sq) { return(true); } else { return(false); } }
//what is the squared distance from this line to a point public double distance_sq(Point2D n) { double utop = (n.x - p1.x) * (p2.x - p1.x) + (n.y - p1.y) * (p2.y - p1.y); double ubot = p1.distance_sq(p2); double u = utop / ubot; if (u < 0 || u > 1) { double d1 = p1.distance_sq(n); double d2 = p2.distance_sq(n); if (d1 < d2) { return(d1); } return(d2); } Point2D p = new Point2D(0.0, 0.0); p.x = p1.x + u * (p2.x - p1.x); p.y = p1.y + u * (p2.y - p1.y); return(p.distance_sq(n)); }
public void updatePosition() { //record old coordinates temp_dist = (float)old_location.distance_sq(location); dist_trav += temp_dist; old_location.x = location.x; old_location.y = location.y; //update current coordinates (may be revoked if new position forces collision) if (!stopped) { //Console.WriteLine("In updatePosition() before noisyHeading(): heading = " + heading); double tempHeading = noisyHeading(); //Console.WriteLine("After noisyHeading(): tempHeading = " + tempHeading + ",heading = " + heading); heading = tempHeading; //Console.WriteLine(velocity + " " + timeStep); double dx = Math.Cos(tempHeading) * velocity * timeStep; double dy = Math.Sin(tempHeading) * velocity * timeStep; location.x += dx; location.y += dy; //Console.WriteLine(dx); } history.Add(new Point2D(location)); // Schrum: I think the heading should always be from -PI to PI. This check assures that. while (heading > Math.PI) { heading -= Math.PI * 2; /* * Console.WriteLine("Heading out of range (updatePosition): " + heading); * Console.WriteLine(this.GetType().ToString()); * System.Windows.Forms.Application.Exit(); * System.Environment.Exit(1); */ } while (heading < -Math.PI) { heading += Math.PI * 2; /* * Console.WriteLine("Heading out of range (updatePosition): " + heading); * Console.WriteLine(this.GetType().ToString()); * System.Windows.Forms.Application.Exit(); * System.Environment.Exit(1); */ } }
public void updatePosition() { //record old coordinates temp_dist = (float)old_location.distance_sq(location); dist_trav += temp_dist; old_location.x = location.x; old_location.y = location.y; //update current coordinates (may be revoked if new position forces collision) if (!stopped) { double tempHeading = noisyHeading(); heading = tempHeading; //Console.WriteLine(velocity + " " + timeStep); double dx = Math.Cos(tempHeading) * velocity * timeStep; double dy = Math.Sin(tempHeading) * velocity * timeStep; location.x += dx; location.y += dy; //Console.WriteLine(dx); } history.Add(new Point2D(location)); }
public static bool collide(Wall wall, Robot robot) { Point2D a1 = new Point2D(wall.line.p1); Point2D a2 = new Point2D(wall.line.p2); Point2D b = new Point2D(robot.location.x, robot.location.y); if (!wall.visible) return false; double rad = robot.radius; double r = ((b.x - a1.x) * (a2.x - a1.x) + (b.y - a1.y) * (a2.y - a1.y)) / wall.line.length_sq(); double px = a1.x + r * (a2.x - a1.x); double py = a1.y + r * (a2.y - a1.y); Point2D np = new Point2D(px, py); double rad_sq = rad * rad; if (r >= 0.0f && r <= 1.0f) { if (np.distance_sq(b) < rad_sq) return true; else return false; } double d1 = b.distance_sq(a1); double d2 = b.distance_sq(a2); if (d1 < rad_sq || d2 < rad_sq) return true; else return false; }
//what is the squared distance from this line to a point public double distance_sq(Point2D n) { double utop = (n.x-p1.x)*(p2.x-p1.x)+(n.y-p1.y)*(p2.y-p1.y); double ubot = p1.distance_sq(p2); double u = utop/ubot; if(u<0 || u> 1) { double d1 = p1.distance_sq(n); double d2 = p2.distance_sq(n); if(d1<d2) return d1; return d2; } Point2D p=new Point2D(0.0,0.0); p.x=p1.x+u*(p2.x-p1.x); p.y=p1.y+u*(p2.y-p1.y); return p.distance_sq(n); }