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