public static double Dot(this Vector2D v, Vector2D w) { var vNorm = v.Normalize(); var wNorm = w.Normalize(); var cosAlpha = vNorm.X * wNorm.X + vNorm.Y * wNorm.Y; return(cosAlpha); }
public static Vector2D Truncate(this Vector2D v, double max) { if (v.Length() > max) { var normalized = v.Normalize(); return(normalized * max); } return(v.Clone()); }
// 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; }
// 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; }