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