// 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; }