static void setPreferredVelocities(SFSimulator sim)
{
/*
* Set the preferred velocity to be a vector of unit magnitude (speed) in the
* direction of the goal.
*/
#pragma omp parallel for
for (int i = 0; i < sim.getNumAgents(); ++i) {
Vector2 goalVector = goals[i] - sim.getAgentPosition(i);
if ((goalVector.LengthSquared()) > 1.0f) {
goalVector = Vector2.Normalize(goalVector);
}
sim.setAgentPrefVelocity(i, goalVector);
/*
* Perturb a little to avoid deadlocks due to perfect symmetry.
*/
float angle = (float)r.NextDouble() * 2.0f * (float)Math.PI ;
float dist = (float)r.NextDouble() * 0.0001f ;
sim.setAgentPrefVelocity(i, sim.getAgentPrefVelocity(i) +
dist * new Vector2((float) Math.Cos(angle), (float)Math.Sin(angle)));
}
}
static void updateVisualization(SFSimulator sim)
{
// Output the current position of all the agents
for (int i = 0; i < sim.getNumAgents(); ++i)
{
Console.WriteLine(sim.getAgentPosition(i));
}
Console.WriteLine("-------");
}
static bool reachedGoal(SFSimulator sim)
{
/* Check if all agents have reached their goals. */
for (int i = 0; i < sim.getNumAgents(); ++i) {
if ((sim.getAgentPosition(i) - goals[i]).LengthSquared() > 20.0f * 20.0f) {
return false;
}
}
return true;
}
static bool reachedGoal(SFSimulator sim)
{
// Check if all agents have reached their goals
for (int i = 0; i < sim.getNumAgents(); ++i)
{
if ((new Vector2(sim.getAgentPosition(i).X - goals[i].X, sim.getAgentPosition(i).Y - goals[i].Y).LengthSquared() > 20.0f * 20.0f))
{
return(false);
}
}
return(true);
}
static void setPreferredVelocities(SFSimulator sim)
{
// Set the preferred velocity to be a vector of unit magnitude (speed) in the direction of the goal
#pragma omp parallel for
for (int i = 0; i < sim.getNumAgents(); ++i)
{
Vector2 goalVector = new Vector2(goals[i].X - sim.getAgentPosition(i).X, goals[i].Y - sim.getAgentPosition(i).Y); //goals[i] - sim.getAgentPosition(i);
if ((goalVector.LengthSquared()) > 1.0f)
{
goalVector = Vector2.Normalize(goalVector);
}
sim.setAgentPrefVelocity(i, new SFVector2(goalVector.X, goalVector.Y));
// Perturb a little to avoid deadlocks due to perfect symmetry
float angle = (float)r.NextDouble() * 2.0f * (float)Math.PI;
float dist = (float)r.NextDouble() * 0.0001f;
var distVector = new SFVector2((float)Math.Cos(angle) * dist, (float)Math.Sin(angle) * dist);
sim.setAgentPrefVelocity(i, new SFVector2(sim.getAgentPrefVelocity(i).X + distVector.X, sim.getAgentPrefVelocity(i).Y + distVector.Y));
}
}
static void updateVisualization(SFSimulator sim)
{
/* Output the current position of all the agents. */
for (int i = 0; i < sim.getNumAgents(); ++i) {
Console.WriteLine(sim.getAgentPosition(i) );
}
Console.WriteLine("-------");
}
