Mentions légales du service

Skip to content
Snippets Groups Projects
Commit 181a1a0a authored by Fabien Grzeskowiak's avatar Fabien Grzeskowiak
Browse files

#25 add cost function

parent a2ac9bbb
No related branches found
No related tags found
1 merge request!44Resolve "implement TtcaDca cost function"
/* 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_TTCADCA_H
#define LIB_TTCADCA_H
#include <core/costFunction.h>
/*
* Performs collision avoidance.
*
*/
class TtcaDcaAvoidance : public CostFunction
{
private:
public:
TtcaDcaAvoidance();
/* Parameters
*/
float sigDca_ = 1;
float sigTtca_ = 1;
/**
* 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 ~TtcaDcaAvoidance();
};
#endif //LIB_TTCADCA_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/TtcaDca.h>
#include <core/agent.h>
#include <math.h>
#include <iostream>
using namespace std;
TtcaDcaAvoidance::TtcaDcaAvoidance() {
}
TtcaDcaAvoidance::~TtcaDcaAvoidance() {
}
CostFunctionValues TtcaDcaAvoidance::GetCostFunctionGradient(Agent* agent, World * world) {
CostFunctionValues Result;
Result.TotalCost = 0;
std::vector<Agent* > agts = world->getNeighboursOfAgent(agent->id_, 50);
for (int i = 0; agts.size(); i++)
{
Vector2D relPos = agts[i]->getPosition() - agent->getPosition();
Vector2D relVelocity = agts[i]->getVelocity() - agent->getVelocity();
float ttca = 0;
float dca = 0;
if (relVelocity.sqrMagnitude() > 0.01)
{
ttca = - relPos.dot(relVelocity) / relVelocity.sqrMagnitude();
}
dca = (relPos + ttca * relVelocity).magnitude();
Cost = exp(-0.5*( (ttca/sigTtca_)*(ttca/sigTtca_) + (dca/sigDca_)*(dca/sigDca_) ) );
Result.TotalCost += Cost / agts.size();
gradTtcaAngle = -(relPos + 2*ttca*relVelocity).dot(new Vector2D(agent->getVelocity().y(),-agent->getVelocity().x())) / (relVelocity.dot(relVelocity));
gradTtcaSpeed = (relPos + 2*ttca*relVelocity).dot(agent->getVelocity().getnormalized()) / (relVelocity.dot(relVelocity));
gradDcaAngle = (relPos + ttca * relVelocity).dot(gradTtcaAngle*relVelocity + ttca*(new Vector2D(agent->getVelocity().y(),-agent->getVelocity().x()))) / dca;
gradDcaSpeed = (relPos + ttca * relVelocity).dot(gradTtcaSpeed*relVelocity - ttca * (agent->getVelocity().getnormalized())/ dca;
float newSpeed = -Cost * ( gradTtcaSpeed * (ttca/(sigTtca_*sigTtca_)))
-Cost * ( gradDcaSpeed * (dca/(sigDca_*sigDca_)));
float newAngle = -Cost * ( gradTtcaAngle * (ttca/(sigTtca_*sigTtca_)))
-Cost * ( gradTtcaAngle * (dca/(sigDca_*sigDca_)));
Vector2D Grad = newSpeed * new Vector2D(cos(newAngle),sin(newAngle));
}
//Result.Gradient = (AgentVelocity - DesiredVelocity);
//Result.TotalCost = 0.5*(Delta.sqrMagnitude());
//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