Commit b44d8bc9 authored by LOPEZ GANDIA Axel's avatar LOPEZ GANDIA Axel

Resolve "Implement social forces cost function"

parent 026245de
/* Crowd Simulator Engine
** Copyright (C) 2018 - Inria Rennes - Rainbow - Julien Pettre
**
** This program is free software; you can redistribute it and/or
** modify it under the terms of the GNU General Public License
** as published by the Free Software Foundation; either version 2
** of the License, or (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program; if not, write to the Free Software
** Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
**
**
**
** Authors: Axel Lopez Gandia, Javad Amirian, Florian Berton,
** Julien Legros, Lucas Pelerin, Beatriz Cabrero Daniel, Fabien Grzeskowiak
**
** Contact: crowd_group@inria.fr
*/
#ifndef LIB_SOCIAL_FORCES_H
#define LIB_SOCIAL_FORCES_H
#include <core/costFunction.h>
/*
* Implements social forces algorithm.
*/
class SocialForces : public CostFunction
{
private:
public:
float AtractorForce = 1;
float RepulsionForce = 3;
SocialForces();
/**
* Computes the value and gradient of the cost function.
* Return the values to the policy to update the agent.
*/
virtual CostFunctionValues GetCostFunctionGradient(Agent* agent, World * world);
virtual ~SocialForces();
};
#endif //LIB_GOAL_REACHING_H
......@@ -26,7 +26,12 @@
#include <vector>
#include <iostream>
#include "core/crowdSimulator.h"
#include <CostFunctions/goalReaching.h>
#include <CostFunctions/socialforces.h>
#include <iostream>
#include <fstream>
using namespace std;
int main( int argc, char * argv[] )
{
......@@ -46,5 +51,4 @@ int main( int argc, char * argv[] )
// }
//agents[20]->printPosition();
}
/* Crowd Simulator Engine
** Copyright (C) 2018 - Inria Rennes - Rainbow - Julien Pettre
**
** This program is free software; you can redistribute it and/or
** modify it under the terms of the GNU General Public License
** as published by the Free Software Foundation; either version 2
** of the License, or (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program; if not, write to the Free Software
** Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
**
**
**
** Authors: Axel Lopez Gandia, Javad Amirian, Florian Berton,
** Julien Legros, Lucas Pelerin, Beatriz Cabrero Daniel, Fabien Grzeskowiak
**
** Contact: crowd_group@inria.fr
*/
#include <CostFunctions/socialforces.h>
#include <core/agent.h>
#include <core/world.h>
#include <algorithm>
#include <iostream>
using namespace std;
SocialForces::SocialForces() {
}
SocialForces::~SocialForces() {
}
CostFunctionValues SocialForces::GetCostFunctionGradient(Agent* agent, World * world) {
CostFunctionValues Result;
Vector2D Goal = agent->getGoal();
Vector2D AgentPos = agent->getPosition();
float PreferedSpeed = agent->getPreferdSpeed();
Vector2D CurrentVel = agent->getVelocity();
Vector2D GoalAgent = Goal - AgentPos;
Vector2D GoalDir = GoalAgent.getnormalized();
Vector2D Atraction = -AtractorForce*(GoalDir*PreferedSpeed - CurrentVel);
vector<Agent*> Neighbors = world->getNeighboursOfAgent(agent->getID(), 100);
Vector2D Repulsion(0, 0);
float dt = 1;
for (Agent* other : Neighbors) {
if (other == agent) {
continue;
}
Vector2D R = other->getPosition() - AgentPos;
float Vb = other->getPreferdSpeed();
Vector2D eb = (other->getGoal() - other->getPosition()).getnormalized();
float B = 0.5*sqrtf(pow(R.magnitude() + (R - Vb * dt*eb).magnitude(), 2) - pow(Vb*dt, 2));
Vector2D DB = (1/(2*B))*(pow(R.magnitude() + (R - Vb * dt*eb).magnitude(), 2) / (R.magnitude() * (R - Vb * dt*eb).magnitude()))*R;
Repulsion += RepulsionForce*exp(-B) * DB;
}
Result.Gradient = Atraction + Repulsion;
// cout << Result.Gradient.x() << " " << Result.Gradient.y() << endl;
return Result;
}
\ No newline at end of file
......@@ -43,8 +43,9 @@ void CrowdSimulator::AppendPosInOutput(std::ofstream &outfile, double time, Agen
{
if(outfile.is_open())
{
outfile << time << " " << agent->getPosition().x() << " " << agent->getPosition().y() << std::endl;
}
outfile << time << "," << agent->getPosition().x() << "," << agent->getPosition().y() << std::endl;
}
}
void CrowdSimulator::addAgentToSimulation(Agent * agent) {
......
......@@ -24,9 +24,10 @@
*/
#include <core/policy.h>
#include <core/agent.h>
Vector2D Policy::getNewVelocity(Agent* agent, World * world) {
Vector2D NewVel = new Vector2D();
Vector2D NewVel = agent->getVelocity();
for(unsigned int i = 0; i < cost_functions_.size(); i++)
{
......
......@@ -85,9 +85,9 @@ void World::doStep(double dt)
(void*)agents_[i]) != NULL)
{
// TODO : decide of a strategy in case of collision
// For now, just stop the agent
agents_[i]->setNextVelocity(new Vector2D(0.0, 0.0));
lqUpdateForNewLocation(lqDB, lqProxies[i], agentPosition.x(), agentPosition.y());
// For now, we do nothing
//agents_[i]->setNextVelocity(new Vector2D(0.0, 0.0));
//lqUpdateForNewLocation(lqDB, lqProxies[i], agentPosition.x(), agentPosition.y());
}
}
updateAllAgents(dt);
......
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