public static bool collide(Wall wall, Robot robot) { Point2D a1 = new Point2D(wall.Line.Endpoint1); Point2D a2 = new Point2D(wall.Line.Endpoint2); 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.squaredLength(); 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.squaredDistance(b) < rad_sq) { return(true); } else { return(false); } } double d1 = b.squaredDistance(a1); double d2 = b.squaredDistance(a2); if (d1 < rad_sq || d2 < rad_sq) { return(true); } else { return(false); } }
/// <summary> /// Calculate the squared Distance from this line to a point. /// </summary> public double squaredDistance(Point2D point) { double utop = (point.X - Endpoint1.X) * (Endpoint2.X - Endpoint1.X) + (point.Y - Endpoint1.Y) * (Endpoint2.Y - Endpoint1.Y); double ubot = Endpoint1.squaredDistance(Endpoint2); double u = utop / ubot; if (u < 0 || u > 1) { double d1 = Endpoint1.squaredDistance(point); double d2 = Endpoint2.squaredDistance(point); if (d1 < d2) { return(d1); } return(d2); } Point2D p = new Point2D(0.0, 0.0); p.X = Endpoint1.X + u * (Endpoint2.X - Endpoint1.X); p.Y = Endpoint1.Y + u * (Endpoint2.Y - Endpoint1.Y); return(p.squaredDistance(point)); }
/// <summary> /// Finds the robot's new position given its Heading and Velocity. /// </summary> public void updatePosition() { //record old coordinates TempDistance = (float)OldLocation.squaredDistance(Location); DistanceTraveled += TempDistance; OldLocation.X = Location.X; OldLocation.Y = Location.Y; //update current coordinates (may be revoked if new position forces collision) if (!Stopped) { double tempHeading = noisyHeading(); Heading = tempHeading; double dx = Math.Cos(tempHeading) * Velocity * Timestep; double dy = Math.Sin(tempHeading) * Velocity * Timestep; Location.X += dx; Location.Y += dy; } }
public static bool collide(Wall wall, Robot robot) { Point2D a1 = new Point2D(wall.Line.Endpoint1); Point2D a2 = new Point2D(wall.Line.Endpoint2); 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.squaredLength(); 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.squaredDistance(b) < rad_sq) return true; else return false; } double d1 = b.squaredDistance(a1); double d2 = b.squaredDistance(a2); if (d1 < rad_sq || d2 < rad_sq) return true; else return false; }
/// <summary> /// Calculate the squared Distance from this line to a point. /// </summary> public double squaredDistance(Point2D point) { double utop = (point.X-Endpoint1.X)*(Endpoint2.X-Endpoint1.X)+(point.Y-Endpoint1.Y)*(Endpoint2.Y-Endpoint1.Y); double ubot = Endpoint1.squaredDistance(Endpoint2); double u = utop/ubot; if(u<0 || u> 1) { double d1 = Endpoint1.squaredDistance(point); double d2 = Endpoint2.squaredDistance(point); if(d1<d2) return d1; return d2; } Point2D p=new Point2D(0.0,0.0); p.X=Endpoint1.X+u*(Endpoint2.X-Endpoint1.X); p.Y=Endpoint1.Y+u*(Endpoint2.Y-Endpoint1.Y); return p.squaredDistance(point); }