/***
* Flee is the opposite of seek. Instead of producing a steering force to steer the agent toward a target position, flee creates a force that steers the agent away.
* */
public static Vector2D Flee(Vector2D targetPosition, ref Vector2D currentPosition, ref Vector2D Velocity, int max_speed)
{
int FOV = 100; // Field Of View
if (Vector2D.Length(Vector2D.Subtract(targetPosition, currentPosition)) > FOV)
{
return Vector2D.None();
}
else
{
Vector2D desired_V = Vector2D.Normalize(Vector2D.Subtract(currentPosition, targetPosition)) * max_speed;
return Vector2D.Subtract(desired_V, Velocity);
}
}
/***
* Seek is useful for getting an agent moving in the right direction, but often you'll want your agents
* to come to a gentle halt at the target position, and as you've seen, seek is not too great at stopping gracefully.
* Arrive is a behavior that steers the agent in such a way it decelerates onto the target position.
* */
public static Vector2D Arrive(Vector2D targetPosition, ref Vector2D currentPosition, ref Vector2D Velocity, int max_speed)
{
int arriveRadius = 100;
Vector2D toTarget = Vector2D.Subtract(targetPosition, currentPosition);
double distance = toTarget.Length();
if (distance > 0)
{
double speed = max_speed * (distance / arriveRadius);
speed = Math.Min(speed, max_speed);
Vector2D desired_V = toTarget * (float)(speed / distance);
return Vector2D.Subtract(desired_V, Velocity);
}
return Vector2D.None();
}
public Vehicle(Vehicle target, SB sb)
{
this.sb = sb;
mass = World.Instance.random.Next(20, 80);
max_force = 20;
max_speed = 10;
velocity = new Vector2D(5, 5);
acceleration = Vector2D.None();
heading = Vector2D.Normalize(velocity);
CurrentPosition = new Vector2D(World.Instance.random.Next(World.Instance.ClientSize.Width), World.Instance.random.Next(World.Instance.ClientSize.Height));
steerForce = Vector2D.None();
brushVehicle = Brushes.Red;
brushTarget = Brushes.Blue;
whitePen = Pens.White;
this.target = target;
}
/***
* You'll often find wander a useful ingredient when creating an agent's behavior.
* It's designed to produce a steering force that will give the impression of a random walk through the agent's environment.
* */
public static Vector2D Wander(ref Vector2D wanderTarget, ref Vector2D currentPosition, ref Vector2D Velocity, ref Vector2D heading, ref float wandertheta, int max_speed)
{
// see: http://www.shiffman.net/teaching/nature/steering/
float wanderR = 80.0f; // Radius for our "wander circle"
float wanderD = 40.0f; // Distance for our "wander circle"
float change = 0.5f;
wandertheta += (float)(World.Instance.random.NextDouble() - World.Instance.random.NextDouble()) * change; // Randomly change wander theta -1 .. +1
// Now we have to calculate the new location to steer towards on the wander circle
Vector2D circleloc = Velocity.Copy(); // Start with velocity
circleloc.Normalize(); // Normalize to get heading
circleloc *= wanderD; // Multiply by distance
circleloc += currentPosition; // Make it relative to boid's location
Vector2D circleOffSet = new Vector2D(wanderR * Math.Cos(wandertheta), wanderR * Math.Sin(wandertheta));
Vector2D target = Vector2D.Add(circleloc, circleOffSet);
wanderTarget = target;
// do a seek to target ...
return Seek(target, ref currentPosition, ref Velocity, max_speed);
}
// 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;
}
// We accumulate a new acceleration each time based on three rules
public static Vector2D Flocking(ref Vector2D Velocity, int max_speed, ref Vector2D currentPosition, int max_force)
{
Vector2D sep = Separate(World.Instance.agents, ref currentPosition, ref Velocity, max_speed, max_force); // Separation
Vector2D ali = Align(World.Instance.agents, ref currentPosition, ref Velocity, max_speed); // Alignment
Vector2D coh = Cohesion(World.Instance.agents, ref currentPosition, ref Velocity, max_speed); // Cohesion
// Arbitrarily weight these forces
sep = sep * 4.0F;
ali = ali * 2.0F;
coh = coh * 1.0F;
// Add the force vectors to acceleration
Vector2D acc = new Vector2D();
acc = acc + sep;
acc = acc + ali;
acc = acc + coh;
acc = Vector2D.Truncate(acc, max_force);
return acc;
}
public static Vector2D Normalize(Vector2D v1)
{
double length = Vector2D.Length(v1);
if (length > 0)
{
return new Vector2D((float)(v1.X / Vector2D.Length(v1)), (float)(v1.Y / Vector2D.Length(v1)));
}
else
{
return new Vector2D((float)(v1.X / 1), (float)(v1.Y / 1));
}
}
public static Vector2D Subtract(Vector2D v1, Vector2D v2)
{
return new Vector2D(v1.X - v2.X, v1.Y - v2.Y);
}
public void Update()
{
heading = Vector2D.Normalize(velocity);
// compute steerforce
steerForce = Vector2D.None();
switch (sb)
{
case SB.None:
break;
case SB.Seek:
if (target != null)
steerForce = SteeringBehaviours.Seek(target.CurrentPosition, ref CurrentPosition, ref velocity, max_speed);
break;
case SB.Arrive:
if (target != null)
steerForce = SteeringBehaviours.Arrive(target.CurrentPosition, ref CurrentPosition, ref velocity, max_speed);
break;
case SB.Flee:
if (target != null)
steerForce = SteeringBehaviours.Flee(target.CurrentPosition, ref CurrentPosition, ref velocity, max_speed);
break;
case SB.Wander:
steerForce = SteeringBehaviours.Wander(ref wanderTarget, ref CurrentPosition, ref velocity, ref heading, ref wandertheta, max_speed);
break;
case SB.Explore:
steerForce = SteeringBehaviours.Explore(ref wanderTarget, ref CurrentPosition, ref velocity, 20);
break;
case SB.LeaderFollowing:
steerForce = SteeringBehaviours.LeaderFollowing(ref heading, target.CurrentPosition, ref CurrentPosition, ref velocity, max_speed, max_force);
break;
case SB.Flocking:
steerForce = SteeringBehaviours.Flocking(ref velocity, max_speed, ref CurrentPosition, max_force);
break;
}
// compute new current position
steerForce = Vector2D.Truncate(steerForce, max_force);
acceleration = steerForce / mass;
velocity = Vector2D.Truncate(velocity + acceleration, max_speed);
heading = Vector2D.Normalize(velocity);
// prevent kind of jitter
//float treshold = 0.099f;
//Vector2D newPosition = Vector2D.Add(velocity, CurrentPosition);
//if (Math.Abs(newPosition.X - CurrentPosition.X) < treshold
// && Math.Abs(newPosition.Y - CurrentPosition.Y) < treshold)
// return; // keep current position
CurrentPosition = Vector2D.Add(velocity, CurrentPosition);
//
if (!World.Instance.mirrored)
{
if (CurrentPosition.X > World.Instance.ClientSize.Width)
CurrentPosition.X = 0;
if (CurrentPosition.Y > World.Instance.ClientSize.Height)
CurrentPosition.Y = 0;
if (CurrentPosition.X < 0)
CurrentPosition.X = World.Instance.ClientSize.Width;
if (CurrentPosition.Y < 0)
CurrentPosition.Y = World.Instance.ClientSize.Height;
}
else
{
if (CurrentPosition.X > World.Instance.ClientSize.Width)
velocity.X *= -1;
if (CurrentPosition.Y > World.Instance.ClientSize.Height)
velocity.Y *= -1;
if (CurrentPosition.X < 0)
velocity.X *= -1;
if (CurrentPosition.Y < 0)
velocity.Y *= -1;
}
}
// 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;
}
private static void RemoveAllTargetsInRange(float range, ref Vector2D currentPosition)
{
float beenRange = range;
List<Vector2D> toKeep = new List<Vector2D>();
if (HasBeen == null)
HasBeen = new List<Vector2D>();
if (ExploreGrid == null)
ExploreGrid = new List<Vector2D>();
foreach (Vector2D vec in ExploreGrid)
{
float diffX = vec.X - currentPosition.X;
float diffY = vec.Y - currentPosition.Y;
if ((diffX <= range && diffX > -1 * range)
&& (diffY <= range && diffY > -1 * range))
{
// do nothing
}
else
toKeep.Add(vec);
}
ExploreGrid.Clear();
foreach (Vector2D vec in toKeep)
{
ExploreGrid.Add(vec);
}
}
// 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 Multiply(Vector2D v1, float v2)
{
return new Vector2D(v1.X * v2, v1.Y * v2);
}
/***
* The seek steering behavior returns a force that directs an agent toward a target position
* */
public static Vector2D Seek(Vector2D targetPosition, ref Vector2D currentPosition, ref Vector2D Velocity, int max_speed)
{
Vector2D desired_V = Vector2D.Normalize(Vector2D.Subtract(targetPosition, currentPosition)) * max_speed;
return Vector2D.Subtract(desired_V, Velocity);
}
/***
* You'll often find wander a useful ingredient when creating an agent's behavior.
* It's designed to produce a steering force that will give the impression of a random walk through the agent's environment.
* */
public static Vector2D Explore(ref Vector2D wanderTarget, ref Vector2D currentPosition, ref Vector2D Velocity, int max_speed)
{
// see: http://www.shiffman.net/teaching/nature/steering/
if (ExploreGrid == null)
{
ExploreGrid = new List<Vector2D>();
HasBeen = new List<Vector2D>();
int height = World.Instance.ClientSize.Height;
int width = World.Instance.ClientSize.Width;
for (int x = 20; x < (width - 20); x++)
{
for (int y = 20; y < (height - 20); y++)
{
ExploreGrid.Add(new Vector2D(x, y));
}
}
}
int arriveRadius = 100;
Vector2D theTarget = new Vector2D(World.Instance.ClientSize.Width / 2, World.Instance.ClientSize.Height / 2);
float diffX = 0;
float diffY = 0;
float distanceToTarget = 0;
if (ExploreGrid.Count <= 0)
{
}
else
{
diffX = ExploreGrid.First().X - currentPosition.X;
diffY = ExploreGrid.First().Y - currentPosition.Y;
distanceToTarget = 30f;
RemoveAllTargetsInRange(distanceToTarget, ref currentPosition);
}
HasBeen.Add(currentPosition);
if (ExploreGrid.Count <= 0)
{
theTarget = new Vector2D(World.Instance.ClientSize.Width / 2, World.Instance.ClientSize.Height / 2);
}
else
{
if ((diffX <= distanceToTarget && diffX > -1 * distanceToTarget)
&& (diffY <= distanceToTarget && diffY > -1 * distanceToTarget))
{
theTarget = new Vector2D(ExploreGrid.First().X, ExploreGrid.First().Y);
}
else
{
theTarget = new Vector2D(ExploreGrid.First().X, ExploreGrid.First().Y);
}
}
Vector2D toTarget = Vector2D.Subtract(theTarget, currentPosition);
wanderTarget = theTarget;
double distance = toTarget.Length();
if (distance > 0)
{
//Arrive
double speed = max_speed * (distance / arriveRadius);
speed = Math.Min(speed, max_speed);
Vector2D desired_V = toTarget * (float)(speed / distance);
return Vector2D.Subtract(desired_V, Velocity);
////Seek
//Vector2D desired_V = Vector2D.Normalize(Vector2D.Subtract(toTarget, currentPosition)) * max_speed;
//return Vector2D.Subtract(desired_V, Velocity);
}
return Vector2D.None();
}
public static float Dot(Vector2D v1, Vector2D v2)
{
return (v1.X * v2.X + v1.Y * v2.Y);
}
public static Vector2D Add(Vector2D v1, Vector2D v2)
{
return new Vector2D(v1.X + v2.X, v1.Y + v2.Y);
}
internal static Vector2D Truncate(Vector2D vec, float max_value)
{
if (vec.Length() > max_value)
{
return Vector2D.Multiply(Vector2D.Normalize(vec), max_value);
}
return vec;
}
public static Vector2D LeaderFollowing(ref Vector2D heading, Vector2D targetPosition, ref Vector2D currentPosition, ref Vector2D Velocity, int max_speed, int max_force)
{
// Separate
Vector2D s = Separate(World.Instance.agents, ref currentPosition, ref Velocity, max_speed, max_force);
//Arrival
Vector2D behindLeader = Vector2D.Subtract(targetPosition, heading);
for (int i = 0; i < 50; i++)
{
behindLeader = Vector2D.Subtract(behindLeader, heading);
}
Vector2D a = Arrive(behindLeader, ref currentPosition, ref Velocity, max_speed);
// combineren
Vector2D total = new Vector2D();
total.X = (a.X * 3) + (s.X * 3);
total.Y = (a.Y * 3) + (s.Y * 3);
total = Vector2D.Truncate(total, max_force);
return total;
}
public static double Length(Vector2D v1)
{
return Math.Sqrt(v1.X * v1.X + v1.Y * v1.Y);
}