/// <summary>
/// Tests collision between the specified robot and all walls and other robots in the CurrentEnvironment.
/// </summary>
public override bool robotCollide(Robot robot)
{
foreach (Wall wall in Domain.walls)
{
if (EngineUtilities.collide(robot, wall))
{
return(true);
}
}
if (!AgentCollide)
{
return(false);
}
foreach (Robot otherRobot in Robots)
{
if (robot == otherRobot)
{
continue;
}
if (EngineUtilities.collide(robot, otherRobot))
{
return(true);
}
}
return(false);
}
/// <summary>
/// Updates the sensor based on the current state of the environment.
/// </summary>
public void update(Environment env, List <Robot> robots, CollisionManager cm)
{
Activation = 0;
Point2D temp;
double dist;
double angle;
if (Type == "poi")
{
foreach (Point p in env.POIPosition)
{
temp = new Point2D(p.X, p.Y);
dist = EngineUtilities.euclideanDistance(temp, new Point2D(Owner.AreaOfImpact.Position.X, Owner.AreaOfImpact.Position.Y));
temp.X -= (float)Owner.AreaOfImpact.Position.X;
temp.Y -= (float)Owner.AreaOfImpact.Position.Y;
angle = (float)temp.angle();
angle -= Owner.Heading;
angle *= 57.297f;
while (angle > 360)
{
angle -= 360;
}
while (angle < 0)
{
angle += 360;
}
if (StartAngle < 0 && EndAngle > 0) // sensor spans the 0 line
{
if ((angle >= StartAngle + 360.0 && angle <= 360) || (angle >= 0 && angle <= EndAngle))
{
Activation = Math.Max(1.0 - (dist > MaxRange ? 1.0 : dist / MaxRange), Activation);
}
}
else
{
if (angle >= StartAngle && angle < EndAngle)
{
Activation = Math.Max(1.0 - (dist > MaxRange ? 1.0 : dist / MaxRange), Activation);
}
else if (angle + 360.0 >= StartAngle && angle + 360.0 < EndAngle)
{
Activation = Math.Max(1.0 - (dist > MaxRange ? 1.0 : dist / MaxRange), Activation);
}
}
}
return;
}
if (Type == "goal")
{
temp = new Point2D(env.goal_point.X, env.goal_point.Y);
}
else
{
temp = new Point2D(env.start_point.X, env.start_point.Y);
}
dist = EngineUtilities.euclideanDistance(temp, new Point2D(Owner.AreaOfImpact.Position.X, Owner.AreaOfImpact.Position.Y));
//translate with respect to location of navigator
temp.X -= (float)Owner.AreaOfImpact.Position.X;
temp.Y -= (float)Owner.AreaOfImpact.Position.Y;
angle = (float)temp.angle();
angle *= 57.297f;
angle -= (float)Owner.Heading * 57.297f;
while (angle > 360)
{
angle -= 360;
}
while (angle < 0)
{
angle += 360;
}
if (angle >= StartAngle && angle < EndAngle)
{
if (Type == "goal")
{
Activation = 1.0 - (dist > MaxRange ? 1.0 : dist / MaxRange);
}
else
{
Activation = 1.0;
}
}
else if (angle + 360.0 >= StartAngle && angle + 360.0 < EndAngle)
{
if (Type == "goal")
{
Activation = 1.0 - (dist > MaxRange ? 1.0 : dist / MaxRange);
}
else
{
Activation = 1.0;
}
}
}
public static bool collide(Robot a, Wall b)
{
return(EngineUtilities.collide(b, a));
}
