public static void Update(Boid currentBoid)
{
Vector2 alignmentVector = Vector2.Zero;
int alignmentNeighbors = 0;
Vector2 cohesionVector = Vector2.Zero;
int cohesionNeighbors = 0;
Vector2 separationVector = Vector2.Zero;
int separationNeighbors = 0;
for (int i = 0; i < boids.Length; i++)
{
if (boids[i] != currentBoid)
{
Vector2 differenceVec = currentBoid.Pos - boids[i].Pos;
float distance = differenceVec.Length();
float angle = float.MaxValue;
if (distance < alignmentRange)
{
if (angle == float.MaxValue)
{
angle = AngleToNeighbour(currentBoid, boids[i]);
}
if (angle < perceptionAngle / 2)
{
alignmentVector += boids[i].Velocity;
alignmentNeighbors++;
}
}
if (distance < cohesionRange)
{
if (angle == float.MaxValue)
{
angle = AngleToNeighbour(currentBoid, boids[i]);
}
if (angle < perceptionAngle / 2)
{
cohesionVector += boids[i].Pos;
cohesionNeighbors++;
}
}
if (distance < separationRange)
{
if (angle == float.MaxValue)
{
angle = AngleToNeighbour(currentBoid, boids[i]);
}
if (angle < perceptionAngle / 2)
{
float weightedMagnitude = separationWeight / (distance * distance);
differenceVec = Vector2.Multiply(differenceVec, new Vector2(weightedMagnitude, weightedMagnitude));
separationVector += differenceVec;
separationNeighbors++;
}
}
}
}
if (alignmentNeighbors == 0)
{
currentBoid.SetAlignment(alignmentVector);
}
else
{
alignmentVector /= alignmentNeighbors;
currentBoid.SetAlignment(Vector2.Normalize(alignmentVector));
}
if (cohesionNeighbors == 0)
{
currentBoid.SetCohesion(cohesionVector);
}
else
{
cohesionVector /= cohesionNeighbors;
currentBoid.SetCohesion(Vector2.Normalize(cohesionVector));
}
if (separationNeighbors == 0)
{
currentBoid.SetSeperation(separationVector);
}
else
{
separationVector /= separationNeighbors;
currentBoid.SetSeperation(Vector2.Normalize(separationVector));
}
}