Une nouvelle version du portail de gestion des comptes externes sera mise en production lundi 09 août. Elle permettra d'allonger la validité d'un compte externe jusqu'à 3 ans. Pour plus de détails sur cette version consulter : https://doc-si.inria.fr/x/FCeS

Commit 227ec97d authored by Fabien Grzeskowiak's avatar Fabien Grzeskowiak
Browse files

Resolve "Split DCA & TTCA"

parent 7e2f77e0
......@@ -61,4 +61,4 @@ foreach(_source IN ITEMS ${source_files})
string(REPLACE "/" "\\" _group_path "${_source_path_rel}")
source_group("${_group_path}" FILES "${_source}")
endforeach()
source_group("3rd-party" FILES ${tinyxml_src})
\ No newline at end of file
source_group("3rd-party" FILES ${tinyxml_src})
......@@ -27,31 +27,53 @@
#define LIB_TTCADCA_H
#include <core/costFunction.h>
#include <string>
/*
* Performs collision avoidance.
*
* Performs collision avoidance by maximising two thing:
* - Time to closest approach => ttca
* - Distance to closest approach => dca
*
* At a given time, if two agents A and B are moving in a 2D plane
* with a speed Va and Vb respectively, then if they keep this speed
* fixed, there will be a time in the future called ttca when they are at the closest
* distance from each other, called dca.
*
* This cost function maximise those 2 metrics together.
*/
class TtcaDcaAvoidance : public CostFunction
class TtcaDca : public CostFunction
{
private:
public:
TtcaDcaAvoidance();
Vector2D GradDca();
Vector2D GradTtca();
float Cost(float ttca, float dca);
/* Parameters
/*
* Parameters of the cost function
*/
float sigDca_;
float sigTtca_;
public:
/*
* Name of the cost function, to be used in configuration file
*/
const static std::string Name;
TtcaDca();
virtual ~TtcaDca(){};
bool setSigTtca(float sigTtca);
bool setSigDca(float sigDca);
inline float getSigDca(){ return sigDca_; }
inline float getSigTtca(){ return sigTtca_; }
/**
* 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
......@@ -67,7 +67,8 @@ class Vector2D {
return Result;
}
float dot(Vector2D * v) { return this->x_ * v->x_ + this->y_ * v->y_;}
float dot(Vector2D& v) { return this->x_ * v.x_ + this->y_ * v.y_; }
float dot(const Vector2D& v) { return this->x_ * v.x_ + this->y_ * v.y_; }
//float dot(Vector2D v) { return this->x_ * v.x_ + this->y_ * v.y_; }
void set(float x, float y) {
this->x_ = x;
......
......@@ -37,8 +37,8 @@ int main( int argc, char * argv[] )
{
CrowdSimulator cs;
cs.loadInitFile("test2.xml");
cs.setOutputDir("./output3/");
cs.loadInitFile("config.xml");
cs.setOutputDir("./output/");
cs.runWorld(1000, 0.0333);
std::cout << "---------------" << std::endl;
......
......@@ -27,90 +27,106 @@
#include <core/world.h>
#include <core/agent.h>
#include <math.h>
#include <limits>
#include <iostream>
#include "tools/Matrix.h"
using namespace std;
TtcaDcaAvoidance::TtcaDcaAvoidance() {
sigTtca_ = 10;
const std::string TtcaDca::Name = "TtcaDca";
const float eps = 0.0000001;
/*
* Initialise parameters with default values
*/
TtcaDca::TtcaDca() {
sigTtca_ = 1.0;
sigDca_ = 1.0;
}
TtcaDcaAvoidance::~TtcaDcaAvoidance() {
bool TtcaDca::setSigTtca(float sigTtca){
bool success = true;
if(sigTtca < 0 || sigTtca > 10){ success = false; }
else{ sigTtca_ = sigTtca; }
return success;
}
bool TtcaDca::setSigDca(float sigDca){
bool success = true;
if(sigDca < 0 || sigDca > 10){ success = false; }
else{ sigDca_ = sigDca; }
return success;
}
/*
* Compute the cost function
*/
float TtcaDca::Cost(float ttca, float dca){
return exp(-0.5*( (ttca/sigTtca_)*(ttca/sigTtca_) + (dca/sigDca_)*(dca/sigDca_) ) );
}
CostFunctionValues TtcaDcaAvoidance::GetCostFunctionGradient(Agent* agent, World * world) {
CostFunctionValues TtcaDca::GetCostFunctionGradient(Agent* agent, World * world) {
CostFunctionValues Result;
Vector2D Velocity = agent->getVelocity();
Vector2D Vnorm = Velocity.getnormalized();
Matrix R(Vector2D(Vnorm.y(), -Vnorm.x()), Vnorm);
Result.TotalCost = 0;
//get the neighbourhood
std::vector<Agent* > agts = world->getNeighboursOfAgent(agent->getID(), 50);
// for each agent of the neighbourhood
for (int i = 0; i < agts.size(); i++)
{
//Vector2D relPos = agts[i]->getPosition() - agent->getPosition();
// Compute relative velocity and relative position
Vector2D relPos = agent->getPosition() - agts[i]->getPosition() ;
//Vector2D relVelocity = agts[i]->getVelocity() - agent->getVelocity();
Vector2D relVelocity = agent->getVelocity() - agts[i]->getVelocity() ;
float ttca = 0;
float dca = 0;
float Cost = 0;
Vector2D Grad = new Vector2D(0,0);
if (relVelocity.sqrMagnitude() > 0)
// there is adaptation only if relative velocity is not zero
if (relVelocity.sqrMagnitude() > eps)
{
ttca = - relPos.dot(relVelocity) / relVelocity.sqrMagnitude();
// computing ttc and dca
ttca = -relPos.dot(relVelocity) / relVelocity.sqrMagnitude();
dca = (relPos + ttca * relVelocity).magnitude();
if (ttca < 0) continue;
float Cost = exp(-0.5*( (ttca/sigTtca_)*(ttca/sigTtca_) + (dca/sigDca_)*(dca/sigDca_) ) );
Result.TotalCost += Cost / agts.size();
if(ttca < 0) continue;
Vector2D tempo = new Vector2D(agent->getVelocity().y(),-agent->getVelocity().x());
Vector2D * ptr;
// saving the total cost
float cost = Cost(ttca,dca);
Result.TotalCost += cost / agts.size();
ptr = &tempo;
float gradTtcaAngle = -(relPos + 2*ttca*relVelocity).dot(ptr) / (relVelocity.dot(relVelocity));
*ptr = agent->getVelocity().getnormalized();
float gradTtcaSpeed = (relPos + 2*ttca*relVelocity).dot(ptr) / (relVelocity.dot(relVelocity));
// computing gradients of the cost regarding the speed and velocity of the main agent
Vector2D velRot = new Vector2D(agent->getVelocity().y(),-agent->getVelocity().x());
*ptr = (gradTtcaAngle*relVelocity + ttca*velRot);
float gradDcaAngle = (relPos + ttca * relVelocity).dot(ptr) / dca;
float gradTtcaAngle = -(relPos + 2*ttca*relVelocity).dot(velRot) / (relVelocity.dot(relVelocity));
float gradTtcaSpeed = (relPos + 2*ttca*relVelocity).dot(agent->getVelocity().getnormalized()) / (relVelocity.dot(relVelocity));
*ptr = gradTtcaSpeed*relVelocity - ttca * (agent->getVelocity().getnormalized());
float gradDcaSpeed = (relPos + ttca * relVelocity).dot(ptr)/ dca;
if( dca > 0 )
{
gradDcaSpeed = (relPos + ttca * relVelocity).dot(ptr)/ dca;
float gradDcaAngle, gradDcaSpeed;
if( dca*dca > eps ) {
gradDcaAngle = (relPos + ttca * relVelocity).dot(gradTtcaAngle*relVelocity + ttca*velRot) / dca;
gradDcaSpeed = (relPos + ttca * relVelocity).dot(gradTtcaSpeed*relVelocity - ttca * (agent->getVelocity().getnormalized()))/ dca;
}
else
{
gradDcaSpeed = 2147483647;
else {
//TODO non fixed gradient
gradDcaAngle = 0;//std::numeric_limits<float>::max();
gradDcaSpeed = 0;//std::numeric_limits<float>::max();
}
float newSpeed = -Cost * ( gradTtcaSpeed * (ttca/(sigTtca_*sigTtca_)))
-Cost * ( gradDcaSpeed * (dca/(sigDca_*sigDca_)));
float newAngle = -Cost * ( gradTtcaAngle * (ttca/(sigTtca_*sigTtca_)))
-Cost * ( gradTtcaAngle * (dca/(sigDca_*sigDca_)));
float newSpeed = cost * ( gradTtcaSpeed * (ttca/(sigTtca_*sigTtca_)))
+cost * ( gradDcaSpeed * (dca/(sigDca_*sigDca_)));
float newAngle = cost * ( gradTtcaAngle * (ttca/(sigTtca_*sigTtca_)))
+cost * ( gradTtcaAngle * (dca/(sigDca_*sigDca_)));
Grad = new Vector2D(cos(newAngle),sin(newAngle));
Grad = Grad * newSpeed;
Grad = -1*R*Vector2D(sin(newAngle), 1 - cos(newAngle))*agent->getVelocity().magnitude();
Grad = Grad + newSpeed * agent->getVelocity().getnormalized();
}
Result.Gradient = Grad / agts.size();
Result.Gradient -= Grad / agts.size();
}
return Result;
}
......@@ -38,12 +38,12 @@ std::shared_ptr<CostFunction>
if (name == SocialForcesGoalReaching::Name) {
return std::make_shared<SocialForcesGoalReaching>();
}
if (name == TtcaDcaAvoidance().getName()) {
return std::make_shared<TtcaDcaAvoidance>();
if (name == TtcaDca::Name) {
return std::make_shared<TtcaDca>();
}
if (name == RandomFunction::Name) {
return std::make_shared<RandomFunction>();
}
std::cerr << "The given cost function name does not exist" << std::endl;
return nullptr;
}
\ No newline at end of file
}
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