Adds a clearAgents() method

setup.py

@@ -1,3 +1,4 @@
+import numpy
 from setuptools import setup, Extension
 from setuptools.command.build_ext import build_ext as _build_ext
 from Cython.Build import cythonize
@@ -48,4 +49,5 @@ def run(self):
         'Topic :: Games/Entertainment :: Simulation',
         'Topic :: Software Development :: Libraries :: Python Modules',
     ],
+    include_dirs=[numpy.get_include()],
 )

src/RVOSimulator.cpp

@@ -366,4 +366,13 @@ namespace RVO {
 	{
 		timeStep_ = timeStep;
 	}
+
+	void RVOSimulator::clearAgents()
+	{
+		for (size_t i = 0; i < agents_.size(); ++i) {
+			delete agents_[i];
+		}
+		agents_.clear();
+		kdTree_->agents_.clear();
+	}
 }

src/RVOSimulator.h

@@ -575,6 +575,11 @@ namespace RVO {
 		 */
 		void setTimeStep(float timeStep);
 
+		/**
+		 * \brief      Clears all agents from the simulator, but retains obstacles.
+		 */
+		void clearAgents();
+
 	private:
 		std::vector<Agent *> agents_;
 		Agent *defaultAgent_;

src/rvo2.pyx

@@ -73,6 +73,7 @@ cdef extern from "RVOSimulator.h" namespace "RVO":
         void setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst)
         void setAgentVelocity(size_t agentNo, const Vector2 & velocity)
         void setTimeStep(float timeStep)
+        void clearAgents()
 
 
 cdef class PyRVOSimulator:
@@ -87,6 +88,9 @@ cdef class PyRVOSimulator:
                                         timeHorizon, timeHorizonObst, radius,
                                         maxSpeed, c_velocity)
 
+    def __dealloc__(self):
+        del self.thisptr
+
     def addAgent(self, tuple pos, neighborDist=None,
                  maxNeighbors=None, timeHorizon=None,
                  timeHorizonObst=None, radius=None, maxSpeed=None,
@@ -222,3 +226,5 @@ cdef class PyRVOSimulator:
         self.thisptr.setAgentVelocity(agent_no, c_velocity)
     def setTimeStep(self, float time_step):
         self.thisptr.setTimeStep(time_step)
+    def clearAgents(self):
+        self.thisptr.clearAgents()

test/reproduce_a_scenario.py

@@ -0,0 +1,124 @@
+import rvo2
+import numpy as np
+
+def reproduce_test_scenario(plotting=False):
+    sim = rvo2.PyRVOSimulator(1/60., 1.5, 5, 1.5, 2, 0.4, 2)
+
+    obstacles = [
+        [(0.3, 0.1), (0.1, 0.1), (0.1, -0.1)]
+    ]
+    for obstacle in obstacles:
+        sim.addObstacle(obstacle)
+    sim.processObstacles()
+
+    a0 = sim.addAgent((-1, -1))
+    a1 = sim.addAgent((1, -1))
+    a2 = sim.addAgent((1, 1))
+    a3 = sim.addAgent((-1, 1), 1.5, 5, 1.5, 2, 0.4, 2, (0, 0))
+    sim.setAgentPrefVelocity(a0, (1, 1))
+    sim.setAgentPrefVelocity(a1, (-1, 1))
+    sim.setAgentPrefVelocity(a2, (-1, -1))
+    sim.setAgentPrefVelocity(a3, (1, -1))
+
+    positions_log = []
+    for step in range(200):
+        sim.doStep()
+        positions = [sim.getAgentPosition(agent_no) for agent_no in (a0, a1, a2, a3)]
+        positions_log.append(positions)
+
+    positions_log = np.array(positions_log)
+#     np.savez("positions_log_for_test_scenario.npz",
+#              positions_log=positions_log,
+#              obstacles=np.array(obstacles))
+
+    archive = np.load("positions_log_for_test_scenario.npz")
+
+    test_passed = np.allclose(archive['positions_log'], positions_log)
+
+    if plotting:
+        plot_obstacles(np.array(obstacles))
+        plot_positions_log(positions_log)
+        plot_positions_log(archive['positions_log'])
+
+    if test_passed:
+        print("Test passed!")
+    else:
+        print("Test failed!")
+    return test_passed
+
+def test_clear_agents(plotting=False):
+    sim = rvo2.PyRVOSimulator(1/60., 1.5, 5, 1.5, 2, 0.4, 2)
+
+    obstacles = [
+        [(0.3, 0.1), (0.1, 0.1), (0.1, -0.1)]
+    ]
+    for obstacle in obstacles:
+        sim.addObstacle(obstacle)
+    sim.processObstacles()
+
+    a0 = sim.addAgent((-1, -1))
+    a1 = sim.addAgent((1, -1))
+    a2 = sim.addAgent((1, 1))
+    a3 = sim.addAgent((-1, 1), 1.5, 5, 1.5, 2, 0.4, 2, (0, 0))
+    sim.setAgentPrefVelocity(a0, (1, 1))
+    sim.setAgentPrefVelocity(a1, (-1, 1))
+    sim.setAgentPrefVelocity(a2, (-1, -1))
+    sim.setAgentPrefVelocity(a3, (1, -1))
+
+    positions_log = []
+    for step in range(200):
+        sim.doStep()
+        positions = [sim.getAgentPosition(agent_no) for agent_no in (a0, a1, a2, a3)]
+        positions_log.append(positions)
+    positions_log = np.array(positions_log)
+
+    # clear agents, reset agents, run same scenario
+    sim.clearAgents()
+
+    a0 = sim.addAgent((-1, -1))
+    a1 = sim.addAgent((1, -1))
+    a2 = sim.addAgent((1, 1))
+    a3 = sim.addAgent((-1, 1), 1.5, 5, 1.5, 2, 0.4, 2, (0, 0))
+    sim.setAgentPrefVelocity(a0, (1, 1))
+    sim.setAgentPrefVelocity(a1, (-1, 1))
+    sim.setAgentPrefVelocity(a2, (-1, -1))
+    sim.setAgentPrefVelocity(a3, (1, -1))
+
+    print('Running simulation')
+    new_positions_log = []
+    for step in range(200):
+        sim.doStep()
+        positions = [sim.getAgentPosition(agent_no) for agent_no in (a0, a1, a2, a3)]
+        new_positions_log.append(positions)
+    new_positions_log = np.array(new_positions_log)
+
+    test_passed = np.allclose(new_positions_log, positions_log)
+
+    if plotting:
+        plot_obstacles(np.array(obstacles))
+        plot_positions_log(positions_log)
+        plot_positions_log(new_positions_log)
+
+    if test_passed:
+        print("Test passed!")
+    else:
+        print("Test failed!")
+    return test_passed
+
+
+def plot_positions_log(positions_log):
+    n_traj = positions_log.shape[1]
+    for i in range(n_traj):
+        plt.plot(positions_log[:,i,0], positions_log[:,i,1])
+
+def plot_obstacles(obstacles):
+    for obstacle in obstacles:
+        c = np.concatenate([obstacle, obstacle[:1]], axis=0)
+        plt.plot(c[:,0], c[:,1], 'k')
+
+
+if __name__ == "__main__":
+    from matplotlib import pyplot as plt
+    reproduce_test_scenario(plotting=True)
+    test_clear_agents()
+    plt.show()