Mentions légales du service

Skip to content
Snippets Groups Projects
Commit b52be832 authored by LOPEZ GANDIA Axel's avatar LOPEZ GANDIA Axel
Browse files

Merge branch '29-Generic-Cost-Function' into 'master'

Resolve "Generic Cost functions"

Closes #29

See merge request OCSR/OCSR!39
parents c8c6952c 228f1521
No related branches found
No related tags found
1 merge request!39Resolve "Generic Cost functions"
/* 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
/* 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() {
name_ = "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;
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment