Mentions légales du service

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

#19 wip

parent 651a00eb
No related branches found
No related tags found
1 merge request!44Resolve "implement TtcaDca cost function"
This diff is collapsed.
......@@ -24,6 +24,7 @@
*/
#include <CostFunctions/TtcaDca.h>
#include <core/world.h>
#include <core/agent.h>
#include <math.h>
......@@ -43,7 +44,7 @@ CostFunctionValues TtcaDcaAvoidance::GetCostFunctionGradient(Agent* agent, World
Result.TotalCost = 0;
std::vector<Agent* > agts = world->getNeighboursOfAgent(agent->id_, 50);
std::vector<Agent* > agts = world->getNeighboursOfAgent(agent->getID(), 50);
for (int i = 0; agts.size(); i++)
{
......@@ -60,16 +61,17 @@ CostFunctionValues TtcaDcaAvoidance::GetCostFunctionGradient(Agent* agent, World
dca = (relPos + ttca * relVelocity).magnitude();
Cost = exp(-0.5*( (ttca/sigTtca_)*(ttca/sigTtca_) + (dca/sigDca_)*(dca/sigDca_) ) );
float 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));
Vector2D * ptr = new Vector2D(agent->getVelocity().y(),-agent->getVelocity().x());
float gradTtcaAngle = -(relPos + 2*ttca*relVelocity).dot(ptr) / (relVelocity.dot(relVelocity));
gradTtcaSpeed = (relPos + 2*ttca*relVelocity).dot(agent->getVelocity().getnormalized()) / (relVelocity.dot(relVelocity));
float gradTtcaSpeed = (relPos + 2*ttca*relVelocity).dot((Vector2D *) &(agent->getVelocity().getnormalized())) / (relVelocity.dot(relVelocity));
gradDcaAngle = (relPos + ttca * relVelocity).dot(gradTtcaAngle*relVelocity + ttca*(new Vector2D(agent->getVelocity().y(),-agent->getVelocity().x()))) / dca;
float gradDcaAngle = (relPos + ttca * relVelocity).dot((Vector2D *) &(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 gradDcaSpeed = (relPos + ttca * relVelocity).dot((Vector2D *) &(gradTtcaSpeed*relVelocity - ttca * (agent->getVelocity().getnormalized())))/ dca;
float newSpeed = -Cost * ( gradTtcaSpeed * (ttca/(sigTtca_*sigTtca_)))
-Cost * ( gradDcaSpeed * (dca/(sigDca_*sigDca_)));
......@@ -77,12 +79,9 @@ CostFunctionValues TtcaDcaAvoidance::GetCostFunctionGradient(Agent* agent, World
-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;
Result.Gradient = Grad / agts.size();
}
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