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 Main(string[] args) { /* Create a new simulator instance. */ SFSimulator sim = new SFSimulator(); /* Set up the scenario. */ setupScenario(sim); /* Perform (and manipulate) the simulation. */ do { updateVisualization(sim); setPreferredVelocities(sim); sim.doStep(); } while (!reachedGoal(sim)); }
static void Main(string[] args) { // Create a new simulator instance SFSimulator sim = new SFSimulator(); // Set up the scenario setupScenario(sim); // Perform (and manipulate) the simulation do { updateVisualization(sim); setPreferredVelocities(sim); sim.doStep(); }while (!reachedGoal(sim)); }
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 setupScenario(SFSimulator sim) { /* Specify the global time step of the simulation. */ sim.setTimeStep(0.25f); /* Specify the default parameters for agents that are subsequently added. */ AgentProperty ap = new AgentProperty(15.0f, 10, 5.0f, 1.2f, 2.0f, 2.0f, 0.5f, 8, 0.6f, 100, 13.3f, 10, 0.000005f, 0.25f, 1.0f, new Vector2()); sim.setAgentDefaults(ap); /* * Add agents, specifying their start position, and store their goals on the * opposite side of the environment. */ for (int i = 0; i < 5; ++i) { for (int j = 0; j < 5; ++j) { sim.addAgent(new Vector2(55.0f + i * 10.0f, 55.0f + j * 10.0f)); goals.Add(new Vector2(-75.0f, -75.0f)); sim.addAgent(new Vector2(-55.0f - i * 10.0f, 55.0f + j * 10.0f)); goals.Add(new Vector2(75.0f, -75.0f)); sim.addAgent(new Vector2(55.0f + i * 10.0f, -55.0f - j * 10.0f)); goals.Add(new Vector2(-75.0f, 75.0f)); sim.addAgent(new Vector2(-55.0f - i * 10.0f, -55.0f - j * 10.0f)); goals.Add(new Vector2(75.0f, 75.0f)); } } /* * Add (polygonal) obstacles, specifying their vertices in counterclockwise * order. */ List<Vector2> obstacle1 = new List<Vector2>(); List<Vector2> obstacle2= new List<Vector2>(); List<Vector2> obstacle3= new List<Vector2>(); List<Vector2> obstacle4= new List<Vector2>(); obstacle1.Add(new Vector2(-10.0f, 40.0f)); obstacle1.Add(new Vector2(-40.0f, 40.0f)); obstacle1.Add(new Vector2(-40.0f, 10.0f)); obstacle1.Add(new Vector2(-10.0f, 10.0f)); obstacle2.Add(new Vector2(10.0f, 40.0f)); obstacle2.Add(new Vector2(10.0f, 10.0f)); obstacle2.Add(new Vector2(40.0f, 10.0f)); obstacle2.Add(new Vector2(40.0f, 40.0f)); obstacle3.Add(new Vector2(10.0f, -40.0f)); obstacle3.Add(new Vector2(40.0f, -40.0f)); obstacle3.Add(new Vector2(40.0f, -10.0f)); obstacle3.Add(new Vector2(10.0f, -10.0f)); obstacle4.Add(new Vector2(-10.0f, -40.0f)); obstacle4.Add(new Vector2(-10.0f, -10.0f)); obstacle4.Add(new Vector2(-40.0f, -10.0f)); obstacle4.Add(new Vector2(-40.0f, -40.0f)); sim.addObstacle(obstacle1); sim.addObstacle(obstacle2); sim.addObstacle(obstacle3); sim.addObstacle(obstacle4); /* Process the obstacles so that they are accounted for in the simulation. */ sim.processObstacles(); }
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 void setupScenario(SFSimulator sim) { // Specify the global time step of the simulation sim.setTimeStep(0.25f); // Specify the default parameters for agents that are subsequently added AgentProperty ap = new AgentProperty(15.0f, 10, 5.0f, 1.2f, 2.0f, 2.0f, 0.5f, 8, 0.6f, 100, 13.3f, 10, 0.000005f, 0.25f, 1.0f, new SFVector2()); sim.setAgentDefaults(ap); // Add agents, specifying their start position, and store their goals on the opposite side of the environment for (int i = 0; i < 5; ++i) { for (int j = 0; j < 5; ++j) { sim.addAgent(new SFVector2(55.0f + i * 10.0f, 55.0f + j * 10.0f)); goals.Add(new SFVector2(-75.0f, -75.0f)); sim.addAgent(new SFVector2(-55.0f - i * 10.0f, 55.0f + j * 10.0f)); goals.Add(new SFVector2(75.0f, -75.0f)); sim.addAgent(new SFVector2(55.0f + i * 10.0f, -55.0f - j * 10.0f)); goals.Add(new SFVector2(-75.0f, 75.0f)); sim.addAgent(new SFVector2(-55.0f - i * 10.0f, -55.0f - j * 10.0f)); goals.Add(new SFVector2(75.0f, 75.0f)); } } // Add (polygonal) obstacles, specifying their vertices in counterclockwise order List <SFVector2> obstacle1 = new List <SFVector2>(); List <SFVector2> obstacle2 = new List <SFVector2>(); List <SFVector2> obstacle3 = new List <SFVector2>(); List <SFVector2> obstacle4 = new List <SFVector2>(); obstacle1.Add(new SFVector2(-10.0f, 40.0f)); obstacle1.Add(new SFVector2(-40.0f, 40.0f)); obstacle1.Add(new SFVector2(-40.0f, 10.0f)); obstacle1.Add(new SFVector2(-10.0f, 10.0f)); obstacle2.Add(new SFVector2(10.0f, 40.0f)); obstacle2.Add(new SFVector2(10.0f, 10.0f)); obstacle2.Add(new SFVector2(40.0f, 10.0f)); obstacle2.Add(new SFVector2(40.0f, 40.0f)); obstacle3.Add(new SFVector2(10.0f, -40.0f)); obstacle3.Add(new SFVector2(40.0f, -40.0f)); obstacle3.Add(new SFVector2(40.0f, -10.0f)); obstacle3.Add(new SFVector2(10.0f, -10.0f)); obstacle4.Add(new SFVector2(-10.0f, -40.0f)); obstacle4.Add(new SFVector2(-10.0f, -10.0f)); obstacle4.Add(new SFVector2(-40.0f, -10.0f)); obstacle4.Add(new SFVector2(-40.0f, -40.0f)); sim.addObstacle(obstacle1); sim.addObstacle(obstacle2); sim.addObstacle(obstacle3); sim.addObstacle(obstacle4); // Process the obstacles so that they are accounted for in the simulation sim.processObstacles(); }