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