// Alignment
// For every nearby boid in the system, calculate the average velocity
public static Vector2D Align(List<Vehicle> vehicles, ref Vector2D currentPosition, ref Vector2D Velocity, int max_speed)
{
float neighbordist = 40.0F;
Vector2D steer = new Vector2D();
int count = 0;
for (int i = 0; i < vehicles.Count; i++)
{
Vehicle other = vehicles[i];
Vector2D temp = new Vector2D();
temp.X = other.CurrentPosition.X - currentPosition.X;
temp.Y = other.CurrentPosition.Y - currentPosition.Y;
float d = (float)temp.Length();
if ((d > 0) && (d < neighbordist))
{
steer.X += other.Velocity.X;
steer.Y += other.Velocity.Y;
count++;
}
}
if (count > 0)
{
steer = steer / (float)count;
}
// As long as the vector is greater than 0
if (steer.Length() > 0)
{
// Implement Reynolds: Steering = Desired - Velocity
steer.Normalize();
float x = steer.X * max_speed;
float y = steer.Y * max_speed;
steer.X = x;
steer.Y = y;
steer = Vector2D.Subtract(steer, Velocity);
}
return steer;
}
// Cohesion
// For the average location (i.e. center) of all nearby boids, calculate steering vector towards that location
public static Vector2D Cohesion(List<Vehicle> vehicles, ref Vector2D currentPosition, ref Vector2D Velocity, int max_speed)
{
float neighbordist = 30.0F;
Vector2D sum = new Vector2D(); // Start with empty vector to accumulate all locations
int count = 0;
for (int i = 0; i < vehicles.Count; i++)
{
Vehicle other = vehicles[i];
Vector2D temp = new Vector2D();
temp.X = other.CurrentPosition.X - currentPosition.X;
temp.Y = other.CurrentPosition.Y - currentPosition.Y;
float d = (float)temp.Length();
if ((d > 0) && (d < neighbordist))
{
sum.X += other.CurrentPosition.X;
sum.Y += other.CurrentPosition.Y;
count++;
}
}
if (count > 0)
{
sum = sum / (float)count;
return Seek(sum, ref currentPosition, ref Velocity, max_speed);
}
return sum;
}
// Separation
// Method checks for nearby boids and steers away
public static Vector2D Separate(List<Vehicle> vehicles, ref Vector2D currentVehicle, ref Vector2D Velocity, int max_speed, int max_force)
{
float desiredseparation = 30.0F;
Vector2D steer = new Vector2D();
int count = 0;
// For every boid in the system, check if it's too close
for (int i = 0; i < vehicles.Count; i++)
{
Vehicle other = vehicles[i];
Vector2D temp = new Vector2D();
temp.X = other.CurrentPosition.X - currentVehicle.X;
temp.Y = other.CurrentPosition.Y - currentVehicle.Y;
float d = (float)temp.Length();
// If the distance is greater than 0 and less than an arbitrary amount (0 when you are yourself)
if ((d > 0) && (d < desiredseparation))
{
// Calculate vector pointing away from neighbor
Vector2D diff = Vector2D.Subtract(currentVehicle, other.CurrentPosition);
diff.Normalize();
diff /= d; // Weight by distance
steer += diff;
count++; // Keep track of how many
}
}
// Average -- divide by how many
if (count > 0)
{
steer/=((float)count);
}
// As long as the vector is greater than 0
if (steer.Length() > 0)
{
// Implement Reynolds: Steering = Desired - Velocity
steer.Normalize();
steer*=max_speed;
steer = Vector2D.Subtract(steer,Velocity);
steer = Vector2D.Truncate(steer, max_force);
}
return steer;
}
public static Vector2D Normalize(this Vector2D v) => v / (v.Length());
/**
* Returns the distance from current position to the given target
*/
public double Distance(Vector2D target)
{
Vector2D ToTarget = target.Clone().Sub(this);
return(ToTarget.Length());
}
internal static Vector2D Truncate(Vector2D vec, float max_value)
{
if (vec.Length() > max_value)
{
return Vector2D.Multiply(Vector2D.Normalize(vec), max_value);
}
return vec;
}