public static void correctOverlap(Agent agent, List<Flockable> neighbors)
{
double distSq;
double overlap;
double newX, newY;
foreach (Flockable other in neighbors)
{
if (other is Agent == true)
{
Vector2 toOther = new Vector2((float)(other.getLocation().X - agent.getLocation().X), (float)(other.getLocation().Y - agent.getLocation().Y));
distSq = toOther.LengthSquared();
//distSq = (Math.Pow((agent.getLocation().X - other.getLocation().X), 2) + Math.Pow((agent.getLocation().Y - other.getLocation().Y), 2));
overlap = agent.getAgentRadiusSq() + other.getAgentRadiusSq() - distSq;
if (agent.Equals(other) == false && overlap >= 0)
{
double scale = overlap / Math.Sqrt(distSq);
Vector2 move = new Vector2((float)(toOther.X * scale), (float)(toOther.Y * scale));
//newX = agent.getLocation().X + (((other.getLocation().X - agent.getLocation().X) / Math.Sqrt(distSq)) * overlap);
//newY = agent.getLocation().Y + (((other.getLocation().Y - agent.getLocation().Y) / Math.Sqrt(distSq)) * overlap);
newX = agent.getLocation().X + move.X;
newY = agent.getLocation().Y + move.Y;
agent.setLocation(new DotNET.Point(newX, newY));
}
}
} // end foreach
}
