Une MAJ de sécurité est nécessaire sur notre version actuelle. Elle sera effectuée lundi 02/08 entre 12h30 et 13h. L'interruption de service devrait durer quelques minutes (probablement moins de 5 minutes).

Commit cc2218c8 authored by LOPEZ GANDIA Axel's avatar LOPEZ GANDIA Axel
Browse files

#53 Added nanoflann for neighbour search. Fixed small error in FOE function....

#53 Added nanoflann for neighbour search. Fixed small error in FOE function. Delayed writting to disk to the end of the simulation.
parent e0065eab
This diff is collapsed.
......@@ -31,7 +31,7 @@ set (CMAKE_CXX_STANDARD 14)
#set(Boost_USE_STATIC_LIBS ON)
#find_package(Boost COMPONENTS system filesystem regex REQUIRED)
include_directories( ./include )
include_directories(./3rd-party/tinyxml/)
include_directories(./3rd-party/tinyxml/ ./3rd-party/nanoflann/)
link_directories( ./lib/${CMAKE_BUILD_TYPE} )
......@@ -39,13 +39,13 @@ link_directories( ./lib/${CMAKE_BUILD_TYPE} )
#include_directories( ${Boost_INCLUDE_DIRS} )
file( GLOB_RECURSE source_files src/* include/*)
file( GLOB_RECURSE tinyxml_src 3rd-party/tinyxml/*)
file( GLOB_RECURSE 3rd_party 3rd-party/tinyxml/* 3rd-party/nanoflann/*)
add_executable(my_app main.cpp ${source_files} ${tinyxml_src})
#add_executable(test_xmlparser ./test/test_xmlparser.cpp ${source_files} ${tinyxml_src})
#add_executable(test_socialforces ./test/SocialForcesExample.cpp ${source_files} ${tinyxml_src})
#add_executable(test_ttcaDca ./test/DcaTtcaExample.cpp ${source_files} ${tinyxml_src})
add_executable(test_localSearch ./test/TestLocalSearch.cpp ${source_files} ${tinyxml_src})
add_executable(my_app main.cpp ${source_files} ${3rd_party})
#add_executable(test_xmlparser ./test/test_xmlparser.cpp ${source_files} ${3rd_party})
#add_executable(test_socialforces ./test/SocialForcesExample.cpp ${source_files} ${3rd_party})
#add_executable(test_ttcaDca ./test/DcaTtcaExample.cpp ${source_files} ${3rd_party})
add_executable(test_localSearch ./test/TestLocalSearch.cpp ${source_files} ${3rd_party})
target_link_libraries(my_app)
#target_link_libraries(test_xmlparser)
......@@ -63,4 +63,4 @@ foreach(_source IN ITEMS ${source_files})
string(REPLACE "/" "\\" _group_path "${_source_path_rel}")
source_group("${_group_path}" FILES "${_source}")
endforeach()
source_group("3rd-party" FILES ${tinyxml_src})
source_group("3rd-party" FILES ${3rd_party})
......@@ -32,7 +32,7 @@
#include "core/obstacle.h"
#include "tools/vector2D.h"
//#include "tools/lq2D.h"
#include <nanoflann.hpp>
/**
* Contains the state of the world (agents and obstacles).
......@@ -54,11 +54,14 @@ class World
float time_;
/*lqInternalDB2D *lqDB;
std::vector<lqClientProxy2D*> lqProxies;*/
static void perNeighborCallBackFunction (void* clientObject, float /*distanceSquared*/, void* clientQueryState);
//nanoflann kdtree class
typedef nanoflann::KDTreeSingleIndexAdaptor <
nanoflann::L2_Simple_Adaptor<float, World >,
World,
2 /* dim */
> kd_tree_t;
kd_tree_t kdtree;
/**
* Finds the nearest neighbor of an agent
* @param agent_id the id of the agent
......@@ -77,6 +80,9 @@ class World
* @returns a vector containing the agents located in the neighbourhood
*/
std::vector<Agent*> getNeighboursOfAgent(int agent_id, float search_radius);
std::vector<Agent*> getNeighbours(Vector2D position, float search_radius);
/**
* Computes each agent's new velocity
* @param dt
......@@ -92,6 +98,11 @@ class World
//void setAgents(const std::vector<Agent *> &agents);
//void setObstacles(const std::vector<Obstacle *> &obstacles);
/*
* Get agent id.
*/
Agent* getAgent(int id);
int getNumAgents();
std::vector<Agent *> getAgents() const;
std::vector<Obstacle *> getObstacles() const;
float getTime() const;
......@@ -105,6 +116,40 @@ class World
* Creates an Agent. Agents should only be created through this function.
*/
Agent* createAgent();
/*******Nanoflann interface********/
// Must return the number of data agents
inline size_t kdtree_get_point_count() const { return agents_.size(); }
// Returns the dim'th component of the idx'th point in the class:
// Since this is inlined and the "dim" argument is typically an immediate value, the
// "if/else's" are actually solved at compile time.
inline float kdtree_get_pt(const size_t idx, const size_t dim) const
{
if (dim == 0) return agents_[idx]->getPosition().x();
else return agents_[idx]->getPosition().y();
/*switch (dim)
{
case 0:
return getAgent(idx)->getPosition().x();
break;
case 1:
return getAgent(idx)->getPosition().y();
break;
default:
return 0;
break;
}*/
}
// Optional bounding-box computation: return false to default to a standard bbox computation loop.
// Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
// Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
template <class BBOX>
bool kdtree_get_bbox(BBOX& /* bb */) const { return false; }
/*******End Nanoflann interface********/
};
#endif //LIB_WORLD_H
/*!
* @file lq2D.h
* @brief This is a C++ modified version of Opensteer's original spatial database.
*/
/*
// ----------------------------------------------------------------------------
// Copyright (c) 2002-2003, Sony Computer Entertainment America
// Original author: Craig Reynolds <craig_reynolds@playstation.sony.com>
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//
// ----------------------------------------------------------------------------
*/
/* ------------------------------------------------------------------ */
/* */
/* Locality Query (LQ) Facility */
/* */
/* ------------------------------------------------------------------ */
/*
This utility is a spatial database which stores objects each of
which is associated with a 2d point (a location in a 2d space).
The points serve as the "search key" for the associated object.
It is intended to efficiently answer "sphere inclusion" queries,
also known as range queries: basically questions like:
Which objects are within a radius R of the location L?
In this context, "efficiently" means significantly faster than the
naive, brute force O(n) testing of all known points. Additionally
it is assumed that the objects move along unpredictable paths, so
that extensive preprocessing (for example, constructing a Delaunay
triangulation of the point set) may not be practical.
Overview of usage: an application using this facility would first
create a database with lqCreateDatabase. For each client object
the application wants to put in the database it creates a
lqClientProxy and initializes it with lqInitClientProxy. When a
client object moves, the application calls lqUpdateForNewLocation.
To perform a query lqMapOverAllObjectsInLocality is passed an
application-supplied call-back function to be applied to all
client objects in the locality. See lqCallBackFunction below for
more detail. The lqFindNearestNeighborWithinRadius function can
be used to find a single nearest neighbor using the database.
Note that "locality query" is also known as neighborhood query,
neighborhood search, near neighbor search, and range query. For
additional information on this and related topics see:
http://www.red3d.com/cwr/boids/ips.html
*/
#ifndef _lq2D_h
#define _lq2D_h
/* ------------------------------------------------------------------ */
/* */
/* Data types use by LQ */
/* */
/* ------------------------------------------------------------------ */
/* ------------------------------------------------------------------ */
/* This structure is a proxy for (and contains a pointer to) a client
(application) object in the spatial database. One of these exists
for each client object. This might be included within the
structure of a client object, or could be allocated separately. */
typedef struct lqClientProxy2D
{
/* previous object in this bin, or NULL */
struct lqClientProxy2D* prev;
/* next object in this bin, or NULL */
struct lqClientProxy2D* next;
/* bin ID (pointer to pointer to bin contents list) */
struct lqClientProxy2D** bin;
/* pointer to client object */
void* object;
/* the object's location ("key point") used for spatial sorting */
float x;
float y;
} lqClientProxy2D;
/* ------------------------------------------------------------------ */
/* This structure represents the spatial database. Typically one of
these would be created, by a call to lqCreateDatabase, for a given
application. */
typedef struct lqInternalDB2D
{
/* the origin is the super-brick corner minimum coordinates */
float originx, originy;
/* length of the edges of the super-brick */
float sizex, sizey;
/* number of sub-brick divisions in each direction */
int divx, divy;
/* pointer to an array of pointers, one for each bin */
lqClientProxy2D** bins;
/* extra bin for "everything else" (points outside super-brick) */
lqClientProxy2D* other;
} lqInternalDB2D;
/* ------------------------------------------------------------------ */
/* */
/* Basic API */
/* */
/* ------------------------------------------------------------------ */
/* Allocate and initialize an LQ database, returns a pointer to it.
The application needs to call this before using the LQ facility.
The nine parameters define the properties of the "super-brick":
(1) origin: coordinates of one corner of the super-brick, its
minimum x, and y extent.
(2) size: the width, height and depth of the super-brick.
(3) the number of subdivisions (sub-bricks) along each axis.
This routine also allocates the bin array, and initialize its
contents. */
lqInternalDB2D* lqCreateDatabase2D (float originx, float originy,
float sizex, float sizey,
int divx, int divy);
/* ------------------------------------------------------------------ */
/* Deallocates the LQ database */
void lqDeleteDatabase2D (lqInternalDB2D*);
/* ------------------------------------------------------------------ */
/* The application needs to call this once on each lqClientProxy at
setup time to initialize its list pointers and associate the proxy
with its client object. */
void lqInitClientProxy2D (lqClientProxy2D* proxy, void* clientObject);
/* ------------------------------------------------------------------ */
/* Call for each client object every time its location changes. For
example, in an animation application, this would be called each
frame for every moving object. */
void lqUpdateForNewLocation (lqInternalDB2D* lq,
lqClientProxy2D* object,
float x, float y);
/* ------------------------------------------------------------------ */
/* Apply an application-specific function to all objects in a certain
locality. The locality is specified as a sphere with a given
center and radius. All objects whose location (key-point) is
within this sphere are identified and the function is applied to
them. The application-supplied function takes three arguments:
(1) a void* pointer to an lqClientProxy's "object".
(2) the square of the distance from the center of the search
locality disc (x,y) to object's key-point.
(3) a void* pointer to the caller-supplied "client query state"
object -- typically NULL, but can be used to store state
between calls to the lqCallBackFunction.
This routine uses the LQ database to quickly reject any objects in
bins which do not overlap with the sphere of interest. Incremental
calculation of index values is used to efficiently traverse the
bins of interest. */
/* type for a pointer to a function used to map over client objects */
typedef void (* lqCallBackFunction2D) (void* clientObject,
float distanceSquared,
void* clientQueryState);
void lqMapOverAllObjectsInLocality (lqInternalDB2D* lq,
float x, float y,
float dirx, float diry,
float radius,
bool restrictedView,
lqCallBackFunction2D func,
void* clientQueryState);
/* Given a bin's list of client proxies, traverse the list and invoke
the given lqCallBackFunction on each object that falls within the
search radius. */
void lqTraverseBinClientObjectList(lqClientProxy2D* object,
float x, float y,
float dirx, float diry,
float radiusSquared,
bool restrictedView,
lqCallBackFunction2D func,
void* clientQueryState);
/* ------------------------------------------------------------------ */
/* */
/* Other API */
/* */
/* ------------------------------------------------------------------ */
/* Search the database to find the object whose key-point is nearest
to a given location yet within a given radius. That is, it finds
the object (if any) within a given search sphere which is nearest
to the sphere's center. The ignoreObject argument can be used to
exclude an object from consideration (or it can be NULL). This is
useful when looking for the nearest neighbor of an object in the
database, since otherwise it would be its own nearest neighbor.
The function returns a void* pointer to the nearest object, or
NULL if none is found. */
void* lqFindNearestNeighborWithinRadius (lqInternalDB2D* lq,
float x, float y,
float dirx, float diry,
float radius,
void* ignoreObject);
/* ------------------------------------------------------------------ */
/* Adds a given client object to a given bin, linking it into the bin
contents list. */
void lqAddToBin (lqClientProxy2D* object, lqClientProxy2D** bin);
/* ------------------------------------------------------------------ */
/* Removes a given client object from its current bin, unlinking it
from the bin contents list. */
void lqRemoveFromBin (lqClientProxy2D* object);
/* ------------------------------------------------------------------ */
/* Given an LQ database object and the nine basic parameters: fill in
the object's slots, allocate the bin array, and initialize its
contents. Normally the application does NOT call this directly, it
is called by lqCreateDatabase. */
void lqInitDatabase2D (lqInternalDB2D* lq,
float originx, float originy,
float sizex, float sizey,
int divx, int divy);
/* ------------------------------------------------------------------ */
/* Find the bin ID for a location in space. The location is given in
terms of its XYZ coordinates. The bin ID is a pointer to a pointer
to the bin contents list. */
lqClientProxy2D** lqBinForLocation2D (lqInternalDB2D* lq, float x, float y);
/* ------------------------------------------------------------------ */
/* Apply a user-supplied function to all objects in the database,
regardless of locality (cf lqMapOverAllObjectsInLocality) */
void lqMapOverAllObjects (lqInternalDB2D* lq,
lqCallBackFunction2D func,
void* clientQueryState);
/* ------------------------------------------------------------------ */
/* Removes (all proxies for) all objects from all bins */
void lqRemoveAllObjects (lqInternalDB2D* lq);
/* ------------------------------------------------------------------ */
#ifndef NULL
#define NULL 0
#endif
/* ------------------------------------------------------------------ */
#endif /* _lq_h */
......@@ -76,7 +76,7 @@ CostFunctionValues FOEAvoidance::GetCostFunctionGradient(Agent* agent, World * w
float Cost = 0;
float gradtheta = 0;
float gradv = 0;
vector<Agent*> Neighborhs = world->getNeighboursOfAgent(agent->getID(), 100);
vector<Agent*> Neighborhs = world->getNeighboursOfAgent(agent->getID(), 30);
if (!(Velocity.magnitude() > 0)) {
return Result;
}
......@@ -105,7 +105,7 @@ CostFunctionValues FOEAvoidance::GetCostFunctionGradient(Agent* agent, World * w
}
gradtheta = gradtheta * coefficient_;
gradv = gradv * coefficient_;
gradtheta = clamp(gradtheta, -1.5f, 1.5f);
gradtheta = clamp(gradtheta, -0.1f, 0.1f);
//gradv = clamp(gradv, -1.f, 1.f);
gradv = 0;
//gradtheta = -gradtheta;
......
......@@ -89,7 +89,7 @@ void CrowdSimulator::stepWorld(float dt)
for(int i=0; i<agents.size(); i++)
poss[agents[i]->getID()] = agents[i]->getPosition();
writer->appendPedPositions(poss, t);
writer->flush(); //! @todo: should be removed after test
//writer->flush(); //! @todo: should be removed after test
}
/**
......@@ -100,6 +100,7 @@ void CrowdSimulator::runWorld(int steps, float dt) {
for (int i = 0; i < steps; ++i) {
stepWorld(dt);
}
writer->flush();
}
void CrowdSimulator::createNewWorld(/*Type of world*/) {
......
......@@ -25,50 +25,31 @@
#include <core/world.h>
World::World()//:
using namespace nanoflann;
using namespace std;
World::World() : kdtree(2, *this, KDTreeSingleIndexAdaptorParams(10 /*max leaf*/))
//agents_(),obstacles_()
{
//! @warning the parameters used for initializing lqDB are not optimized, it should be determined, yet should work well
/*lqDB = lqCreateDatabase2D(0, 0, 10000, 10000, 10, 10);
lqProxies.reserve(agents_.size());
for (size_t i=0; i<agents_.size(); i++)
{
lqClientProxy2D *proxy = new lqClientProxy2D;
lqInitClientProxy2D(proxy, agents_[i]);
lqProxies.push_back(proxy);
lqUpdateForNewLocation(lqDB, proxy, agents_[i]->getPosition().x(), agents_[i]->getPosition().y());
}*/
time_ = 0;
}
std::vector<Agent *> World::getNeighboursOfAgent(int agent_id, float search_radius)
{
std::vector<Agent*> neighbors;
/*lqMapOverAllObjectsInLocality (lqDB,
agents_[agent_id]->getPosition().x(), agents_[agent_id]->getPosition().y(),
agents_[agent_id]->getVelocity().x(), agents_[agent_id]->getVelocity().y(),
search_radius,
false,
perNeighborCallBackFunction,
(void*)&neighbors);*/
Vector2D agentPosition = agents_[agent_id]->getPosition();
for (size_t i = 0; i < agents_.size(); i++)
{
Vector2D neighborPosition(agents_[i]->getPosition());
if (i != agent_id &&
(neighborPosition - agentPosition).magnitude() < search_radius)
{
neighbors.push_back(agents_[i].get());
}
}
return neighbors;
return getNeighbours(agents_[agent_id]->getPosition(), search_radius);
}
void World::perNeighborCallBackFunction(void *clientObject, float, void *clientQueryState)
{
std::vector<Agent*> & results = *((std::vector<Agent*>*) clientQueryState);
results.push_back ((Agent*)clientObject);
std::vector<Agent*> World::getNeighbours(Vector2D position, float search_radius) {
float querypt[2] = { position.x(), position.y() };
vector<pair<size_t, float> > neighbors;
SearchParams params; params.sorted = false;
kdtree.radiusSearch(querypt, search_radius, neighbors, params);
vector<Agent*> agents;
for (pair<size_t, float> neighbor : neighbors) {
agents.push_back(agents_[neighbor.first].get());
}
return agents;
}
//Find the nearest neighbor of an agent within a radius
......@@ -94,20 +75,14 @@ Agent * World::findNearestNeighborWithinRadius(size_t agent_id, const Vector2D&
}
void World::doStep(double dt)
{
kdtree.buildIndex();
time_ += dt;
for (size_t i = 0; i < agents_.size(); ++i)
{
agents_[i]->runPolicy(this);
//Updates the LQ database, not needed if no LQ
/*lqUpdateForNewLocation(lqDB, lqProxies[i], agents_[i]->getPosition().x() + agents_[i]->getNextVelocity().x() * dt,
agents_[i]->getPosition().y() + agents_[i]->getNextVelocity().y() * dt);
*/
}
/*Vector2D agentPosition;
......@@ -115,22 +90,13 @@ void World::doStep(double dt)
for (size_t i = 0; i < agents_.size(); ++i)
{
// Check for collision with nearest neighbor
/*agentPosition = agents_[i]->getPosition();
agentVelocity = agents_[i]->getNextVelocity();
if (lqFindNearestNeighborWithinRadius(lqDB,
agentPosition.x() + agentVelocity.x() * dt,
agentPosition.y() + agentVelocity.y() * dt,
agentVelocity.x() * dt, agentVelocity.y() * dt,
agents_[i]->getRadius(),
(void*)agents_[i]) != NULL)*/
//The behavior of finding the nearest neighbor is probably incorrect
Vector2D agentNextPosition = agents_[i]->getPosition() + agents_[i]->getNextVelocity() * dt;
if(findNearestNeighborWithinRadius(i, agentNextPosition, agents_[i]->getRadius()) != NULL)
{
// TODO : decide of a strategy in case of collision
// For now, we do nothing
//agents_[i]->setNextVelocity(new Vector2D(0.0, 0.0));
//lqUpdateForNewLocation(lqDB, lqProxies[i], agentPosition.x(), agentPosition.y());
}
}
updateAllAgents(dt);
......@@ -167,8 +133,6 @@ float World::getTime() const
return time_;
}
float World::getDeltaTime() const
{
return delta_time_;
......@@ -184,26 +148,21 @@ void World::setIterations(int iterations)
iterations_ = iterations;
}
int World::getIterations() const
{
return iterations_;
}
Agent* World::createAgent() {
agents_.push_back(std::make_unique<Agent>());
Agent* agent = agents_.back().get();
agent->setID(agents_.size() - 1);
return agent;
}
/*void World::AddAgent(Agent * newagent) {
agents_.push_back(newagent);
newagent->setID(agents_.size() - 1);
//lqClientProxy2D *proxy = new lqClientProxy2D;
//lqInitClientProxy2D(proxy, newagent);
//lqProxies.push_back(proxy);
//lqUpdateForNewLocation(lqDB, proxy, newagent->getPosition().x(), newagent->getPosition().y());
Agent* World::getAgent(int id) {
return agents_[id].get();
}
*/
\ No newline at end of file
int World::getNumAgents() {
return agents_.size();
}
\ No newline at end of file
This diff is collapsed.
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment