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
}
public static Vector2 Seek(Agent agent, Vector2 goal)
{
Vector2 agentLocation = new Vector2((float) agent.getLocation().X, (float) agent.getLocation().Y);
Vector2 desired = Vector2.Subtract(goal, agentLocation);
desired.Normalize();
return desired;
}
/* Seperation
* Steer away from our neighbors to distance given agent from peers.
*
* We assume that neighbors represents all of the visible neighbors of the
* provided agent.
*/
private static Vector2 ComputeSeperation(Agent agent, List<Flockable> neighbors)
{
Vector2 steeringForce = new Vector2();
int numNeighbors = 0;
foreach (Flockable otherAgent in neighbors)
{
if (otherAgent.Equals(agent) == false)
{
Vector stupidTemp = DotNET.Point.Subtract(agent.getLocation(), otherAgent.getLocation());
Vector2 diffBetweenAgents = new Vector2((float) stupidTemp.X, (float) stupidTemp.Y);
diffBetweenAgents.Normalize();
diffBetweenAgents = Vector2.Divide(diffBetweenAgents, diffBetweenAgents.Length());
steeringForce = Vector2.Add(steeringForce, diffBetweenAgents);
numNeighbors += 1;
}
}
return steeringForce;
}
public static List<Flockable> getNeigbors(Agent agent, List<Flockable> candidates)
{
List<Flockable> neighbors = new List<Flockable>();
double distSq;
foreach (Flockable other in candidates)
{
distSq = (Math.Pow((agent.getLocation().X - other.getLocation().X), 2) + Math.Pow((agent.getLocation().Y - other.getLocation().Y), 2));
if (other is Hand)
{
if (distSq < other.getNeighborhoodRadiusSq())
{
neighbors.Add(other);
}
}
else if (distSq < other.getNeighborhoodRadiusSq() && agent.Equals(other) == false)
{
neighbors.Add(other);
}
}
return neighbors;
}