Commit 1a46f3e2 authored by Fabien Grzeskowiak's avatar Fabien Grzeskowiak
Browse files

Resolve "implement TtcaDca cost function"

parent 212228b8
...@@ -42,9 +42,11 @@ file( GLOB_RECURSE source_files src/* include/*) ...@@ -42,9 +42,11 @@ file( GLOB_RECURSE source_files src/* include/*)
add_executable(my_app main.cpp ${source_files} ) add_executable(my_app main.cpp ${source_files} )
add_executable(test_xmlparser ./test/test_xmlparser.cpp ${source_files}) add_executable(test_xmlparser ./test/test_xmlparser.cpp ${source_files})
add_executable(test_socialforces ./test/SocialForcesExample.cpp ${source_files}) add_executable(test_socialforces ./test/SocialForcesExample.cpp ${source_files})
add_executable(test_ttcaDca ./test/DcaTtcaExample.cpp ${source_files})
target_link_libraries(my_app ${Boost_LIBRARIES}) target_link_libraries(my_app ${Boost_LIBRARIES})
target_link_libraries(test_xmlparser ${Boost_LIBRARIES}) target_link_libraries(test_xmlparser ${Boost_LIBRARIES})
target_link_libraries(test_socialforces ${Boost_LIBRARIES}) target_link_libraries(test_socialforces ${Boost_LIBRARIES})
target_link_libraries(test_ttcaDca ${Boost_LIBRARIES})
#message(${Boost_LIBRARIES}) #message(${Boost_LIBRARIES})
#message(${Boost_LIBRARY_DIRS}) #message(${Boost_LIBRARY_DIRS})
\ No newline at end of file
/* 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_;
float 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
This diff is collapsed.
/* 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/world.h>
#include <core/agent.h>
#include <math.h>
#include <iostream>
using namespace std;
TtcaDcaAvoidance::TtcaDcaAvoidance() {
sigTtca_ = 10;
sigDca_ = 1.0;
}
TtcaDcaAvoidance::~TtcaDcaAvoidance() {
}
CostFunctionValues TtcaDcaAvoidance::GetCostFunctionGradient(Agent* agent, World * world) {
CostFunctionValues Result;
Result.TotalCost = 0;
std::vector<Agent* > agts = world->getNeighboursOfAgent(agent->getID(), 50);
for (int i = 0; i < agts.size(); i++)
{
//Vector2D relPos = agts[i]->getPosition() - agent->getPosition();
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)
{
ttca = - relPos.dot(relVelocity) / relVelocity.sqrMagnitude();
dca = (relPos + ttca * relVelocity).magnitude();
float Cost = exp(-0.5*( (ttca/sigTtca_)*(ttca/sigTtca_) + (dca/sigDca_)*(dca/sigDca_) ) );
Result.TotalCost += Cost / agts.size();
Vector2D tempo = new Vector2D(agent->getVelocity().y(),-agent->getVelocity().x());
Vector2D * ptr;
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));
Vector2D velRot = new Vector2D(agent->getVelocity().y(),-agent->getVelocity().x());
*ptr = (gradTtcaAngle*relVelocity + ttca*velRot);
float gradDcaAngle = (relPos + ttca * relVelocity).dot(ptr) / dca;
*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;
}
else
{
gradDcaSpeed = 2147483647;
}
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;
}
Result.Gradient = Grad / agts.size();
}
return Result;
}
/* Crowd Simulator Engine
** Copyright (C) 2019 - 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 <vector>
#include <iostream>
#include <core/crowdSimulator.h>
#include <core/agent.h>
#include <core/costFunction.h>
#include <core/world.h>
#include <tools/vector2D.h>
#include <CostFunctions/goalReaching.h>
#include <CostFunctions/TtcaDca.h>
#include <iostream>
#include <fstream>
using namespace std;
int main( int argc, char * argv[] )
{
CrowdSimulator crowdSimulator;
Agent * agent1 = new Agent();
Agent * agent2 = new Agent();
Policy * policy = new Policy();
TtcaDcaAvoidance * avoidCostFunc = new TtcaDcaAvoidance();
GoalReaching * goalReaching = new GoalReaching();
Vector2D goalPos(0, 50);
Vector2D goalPos2(-25, 25);
policy->addCostFunction(avoidCostFunc, 1.0);
policy->addCostFunction(goalReaching, 0.2);
agent1->setPolicy(policy);
agent1->setPosition(Vector2D(0, 0));
agent1->setVelocity(Vector2D(0, 0));
agent1->setPreferdSpeed(2);
agent1->setRadius(1);
agent1->setGoal(goalPos);
agent2->setPolicy(policy);
agent2->setPosition(Vector2D(25, 25));
agent2->setVelocity(Vector2D(0, 0));
agent2->setPreferdSpeed(2);
agent1->setRadius(1);
agent2->setGoal(goalPos2);
crowdSimulator.addAgentToSimulation(agent1);
crowdSimulator.addAgentToSimulation(agent2);
crowdSimulator.setOutputDir("/home/fabien/git/CrowdVizu/build/TrajExample/TtcaDca/");
float dt = 0.033;
int iter = 0;
int MAXITERATIONS = 10000;
while ((agent1->getPosition() - goalPos).magnitude() > 5 && iter++ < MAXITERATIONS)
{
crowdSimulator.stepWorld(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