Mentions légales du service

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

#28 Added generic empty function.

parent 228f1521
No related branches found
No related tags found
1 merge request!41Resolve "Generic Cost functions"
......@@ -23,23 +23,21 @@
** Contact: crowd_group@inria.fr
*/
#ifndef LIB_SOCIAL_FORCES_H
#define LIB_SOCIAL_FORCES_H
#ifndef LIB_GENERIC_COST_H
#define LIB_GENERIC_COST_H
#include <core/costFunction.h>
/*
* Implements social forces algorithm.
* Empty Cost Function, use as template.
*/
class SocialForces : public CostFunction
class GenericCost : public CostFunction
{
private:
public:
float AtractorForce = 1;
float RepulsionForce = 3;
SocialForces();
GenericCost();
/**
* Computes the value and gradient of the cost function.
......@@ -47,7 +45,7 @@ class SocialForces : public CostFunction
*/
virtual CostFunctionValues GetCostFunctionGradient(Agent* agent, World * world);
virtual ~SocialForces();
virtual ~GenericCost();
};
#endif //LIB_GOAL_REACHING_H
#endif //LIB_GENERIC_COST_H
......@@ -23,7 +23,7 @@
** Contact: crowd_group@inria.fr
*/
#include <CostFunctions/socialforces.h>
#include <CostFunctions/GenericCost.h>
#include <core/agent.h>
#include <core/world.h>
......@@ -31,47 +31,20 @@
#include <iostream>
using namespace std;
SocialForces::SocialForces() {
name_ = "SocialForces";
GenericCost::GenericCost() {
name_ = "Generic";
}
SocialForces::~SocialForces() {
GenericCost::~GenericCost() {
}
CostFunctionValues SocialForces::GetCostFunctionGradient(Agent* agent, World * world) {
CostFunctionValues GenericCost::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;
//Here fields of Result should be filled.
//Result.TotalCost contains the total cost of the function.
//Result.Gradient contains the gradient that will drive the agent.
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