Commit 212228b8 authored by LOPEZ GANDIA Axel's avatar LOPEZ GANDIA Axel
Browse files

Merge branch '28-comment-lq2d-temporary-solution' into 'master'

Resolve "Comment lq2D stuff and add temporary solution"

Closes #28

See merge request OCSR/OCSR!42
parents 86407550 03c86b18
......@@ -31,7 +31,7 @@
#include "core/obstacle.h"
#include "tools/vector2D.h"
#include "tools/lq2D.h"
//#include "tools/lq2D.h"
/**
* Contains the state of the world (agents and obstacles).
......@@ -45,10 +45,20 @@ class World
std::vector<Obstacle*> obstacles_;
float time_;
lqInternalDB2D *lqDB;
std::vector<lqClientProxy2D*> lqProxies;
/*lqInternalDB2D *lqDB;
std::vector<lqClientProxy2D*> lqProxies;*/
static void perNeighborCallBackFunction (void* clientObject, float /*distanceSquared*/, void* clientQueryState);
/**
* Finds the nearest neighbor of an agent
* @param agent_id the id of the agent
* @param agentPosition the center of the neighborhood search
* @param search_radius
* @returns a pointer to the nearest neighbor of the agent, or a null pointer if none exists
*/
Agent * findNearestNeighborWithinRadius(size_t agent_id, const Vector2D & agentPosition, float search_radius);
public :
World();
......
......@@ -29,7 +29,7 @@ World::World()//:
//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);
/*lqDB = lqCreateDatabase2D(0, 0, 10000, 10000, 10, 10);
lqProxies.reserve(agents_.size());
for (size_t i=0; i<agents_.size(); i++)
{
......@@ -37,20 +37,31 @@ World::World()//:
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,
/*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);
(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]);
}
}
return neighbors;
}
......@@ -60,6 +71,31 @@ void World::perNeighborCallBackFunction(void *clientObject, float, void *clientQ
results.push_back ((Agent*)clientObject);
}
//Find the nearest neighbor of an agent within a radius
//The behavior is probably incorrect and needs to be fixed
Agent * World::findNearestNeighborWithinRadius(size_t agent_id, const Vector2D& agentPosition, float search_radius)
{
Agent* nearestNeighbour = NULL;
double nearestNeighbourDistance = search_radius;
for (size_t i = 0; i < agents_.size(); i++)
{
Vector2D neighborPosition(agents_[i]->getPosition());
double distance = (neighborPosition - agentPosition).magnitude();
if (agent_id != i &&
distance < search_radius &&
distance < nearestNeighbourDistance)
{
nearestNeighbour = agents_[i];
nearestNeighbourDistance = distance;
}
}
return nearestNeighbour;
}
void World::doStep(double dt)
{
time_ += dt;
......@@ -67,23 +103,29 @@ void World::doStep(double dt)
{
agents_[i]->runPolicy(this);
lqUpdateForNewLocation(lqDB, lqProxies[i], agents_[i]->getPosition().x() + agents_[i]->getNextVelocity().x() * dt,
//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;
Vector2D agentVelocity;
/*Vector2D agentPosition;
Vector2D agentVelocity;*/
for (size_t i = 0; i < agents_.size(); ++i)
{
// Check for collision with nearest neighbor
agentPosition = agents_[i]->getPosition();
/*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)
(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
......@@ -135,8 +177,8 @@ void World::AddAgent(Agent * newagent) {
agents_.push_back(newagent);
newagent->setID(agents_.size() - 1);
lqClientProxy2D *proxy = new lqClientProxy2D;
/*lqClientProxy2D *proxy = new lqClientProxy2D;
lqInitClientProxy2D(proxy, newagent);
lqProxies.push_back(proxy);
lqUpdateForNewLocation(lqDB, proxy, newagent->getPosition().x(), newagent->getPosition().y());
lqUpdateForNewLocation(lqDB, proxy, newagent->getPosition().x(), newagent->getPosition().y());*/
}
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