/// <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;
}
}
}
/// <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;
}
}
/// <summary>
/// Updates all of the robot's GoalSensors. Called on each simulator tick.
/// </summary>
/// <param name="env">The simulator CurrentEnvironment.</param>
/// <param name="robots">List of other robots in the CurrentEnvironment. Not actually used in this function, only included for inheritance reasons.</param>
/// <param name="cm">The CurrentEnvironment's collision manager.</param>
public override void updateSensors(Environment env, List<Robot> robots, CollisionManager cm)
{
// Clear out GoalSensors from last time
InputValues.Clear();
foreach (Radar r in GoalSensors)
{
r.Activation = 0;
}
foreach (Radar r in CompassSensors)
{
r.Activation = 0;
}
// Update regular (target) GoalSensors
double angle = 0;
Point2D temp;
temp = new Point2D(env.goal_point.X, env.goal_point.Y);
temp.X -= (float)AreaOfImpact.Position.X;
temp.Y -= (float)AreaOfImpact.Position.Y;
angle = (float)temp.angle();
angle -= Heading;
angle *= 57.297f; // convert radians to degrees
while (angle > 360)
angle -= 360;
while (angle < 0)
angle += 360;
foreach (Radar r in GoalSensors)
{
// First, check if the Angle is in the wonky pie slice
if ((angle < 45 || angle >= 315) && r.StartAngle == 315)
{
r.Activation = 1;
break;
}
// Then check the other pie slices
else if (angle >= r.StartAngle && angle < r.EndAngle)
{
r.Activation = 1;
break;
}
}
// Update the compass/northstar GoalSensors
// Note: This is trivial compared to rangefinder updates, which check against all walls for collision. No need to gate it to save CPU.
double northstarangle = Heading;
northstarangle *= 57.297f; // convert radians to degrees
while (northstarangle > 360)
northstarangle -= 360;
while (northstarangle < 0)
northstarangle += 360;
foreach (Radar r in CompassSensors)
{
// First, check if the Angle is in the wonky pie slice
if ((northstarangle < 45 || northstarangle >= 315) && r.StartAngle == 315)
{
r.Activation = 1;
break;
}
// Then check the other pie slices
else if (northstarangle >= r.StartAngle && northstarangle < r.EndAngle)
{
r.Activation = 1;
break;
}
}
// Update the rangefinders
foreach (RangeFinder r in WallSensors)
{
r.update(env, robots, cm);
}
// Update wall sensor inputs
for (int j = 0; j < WallSensors.Count; j++)
{
Inputs[j] = (float)(1 - (WallSensors[j].getActivation()));
if (Inputs[j] > 1.0) Inputs[j] = 1.0f;
}
// Update pie slice sensor inputs
for (int j = WallSensors.Count; j < WallSensors.Count + GoalSensors.Count; j++)
{
Inputs[j] = (float)GoalSensors[j-WallSensors.Count].getActivation();
if (Inputs[j] > 1.0) Inputs[j] = 1.0f;
}
// Update NN inputs based on GoalSensors
Brain.setInputSignals(ID, Inputs);
// Make the sensor values accessible to the behavior characterization
for (int i = 0; i < Inputs.Length; i++)
InputValues.Add(Inputs[i]);
}
/// <summary>
/// Updates all of the robot's GoalSensors. Called on each simulator tick.
/// </summary>
/// <param name="env">The simulator CurrentEnvironment.</param>
/// <param name="robots">List of other robots in the CurrentEnvironment. Not actually used in this function, only included for inheritance reasons.</param>
/// <param name="cm">The CurrentEnvironment's collision manager.</param>
public override void updateSensors(Environment env, List <Robot> robots, CollisionManager cm)
{
// Clear out GoalSensors from last time
InputValues.Clear();
foreach (Radar r in GoalSensors)
{
r.Activation = 0;
}
foreach (Radar r in CompassSensors)
{
r.Activation = 0;
}
// Update regular (target) GoalSensors
double angle = 0;
Point2D temp;
temp = new Point2D(env.goal_point.X, env.goal_point.Y);
temp.X -= (float)AreaOfImpact.Position.X;
temp.Y -= (float)AreaOfImpact.Position.Y;
angle = (float)temp.angle();
angle -= Heading;
angle *= 57.297f; // convert radians to degrees
while (angle > 360)
{
angle -= 360;
}
while (angle < 0)
{
angle += 360;
}
foreach (Radar r in GoalSensors)
{
// First, check if the Angle is in the wonky pie slice
if ((angle < 45 || angle >= 315) && r.StartAngle == 315)
{
r.Activation = 1;
break;
}
// Then check the other pie slices
else if (angle >= r.StartAngle && angle < r.EndAngle)
{
r.Activation = 1;
break;
}
}
// Update the compass/northstar GoalSensors
// Note: This is trivial compared to rangefinder updates, which check against all walls for collision. No need to gate it to save CPU.
double northstarangle = Heading;
northstarangle *= 57.297f; // convert radians to degrees
while (northstarangle > 360)
{
northstarangle -= 360;
}
while (northstarangle < 0)
{
northstarangle += 360;
}
foreach (Radar r in CompassSensors)
{
// First, check if the Angle is in the wonky pie slice
if ((northstarangle < 45 || northstarangle >= 315) && r.StartAngle == 315)
{
r.Activation = 1;
break;
}
// Then check the other pie slices
else if (northstarangle >= r.StartAngle && northstarangle < r.EndAngle)
{
r.Activation = 1;
break;
}
}
// Update the rangefinders
foreach (RangeFinder r in WallSensors)
{
r.update(env, robots, cm);
}
// Update wall sensor inputs
for (int j = 0; j < WallSensors.Count; j++)
{
Inputs[j] = (float)(1 - (WallSensors[j].getActivation()));
if (Inputs[j] > 1.0)
{
Inputs[j] = 1.0f;
}
}
// Update pie slice sensor inputs
for (int j = WallSensors.Count; j < WallSensors.Count + GoalSensors.Count; j++)
{
Inputs[j] = (float)GoalSensors[j - WallSensors.Count].getActivation();
if (Inputs[j] > 1.0)
{
Inputs[j] = 1.0f;
}
}
// Update NN inputs based on GoalSensors
Brain.setInputSignals(ID, Inputs);
// Make the sensor values accessible to the behavior characterization
for (int i = 0; i < Inputs.Length; i++)
{
InputValues.Add(Inputs[i]);
}
}