void PlanningProblem::computeCost(Trajectory &plan_)
{
if(!this->checkPlanValidity(plan_)) {
plan_.cost.safety = INFINITY;
plan_.cost.smoothness = INFINITY;
plan_.cost.length = INFINITY;
}
else {
plan_.computeCost();
plan_.cost.safety = 0;
ObstacleSet desired_ob_set = stat_obstacles;
for(int i=0; i< plan_.length(); i++) {
Station st = plan_.getStation(i);
float min_dist = INFINITY;
b2Vec2 tmpv1, tmpv2;
for(int j=0; j<desired_ob_set.size(); j++) {
Obstacle* ob = desired_ob_set[j];
float dist_ = distToObstacle(st, *ob, tmpv1, tmpv2);
min_dist = min(min_dist, dist_);
}
st.cost.min_dist_to_obs = min_dist;
plan_.EditStation(i, st);
plan_.cost.safety += plan_.getStation(i).cost.safety_penalty() / plan_.length();
}
}
}
Station PlanningProblem::getNextStation(const Station &st, Trajectory &path)
{
if(path.isEmpty())
return this->goal.goal_point;
if(path.length() <= 2)
return path.getLastStation();
float max_memberance = 0;
int nearest_segment_index = -1;
for(uint i=1; i<path.length(); i++) { // it doesnt use the last segment (go towards goal on last segment)
Vector2D pnt_1 = path.getStation(i-1).getPosition().to2D();
Vector2D pnt_2 = path.getStation(i).getPosition().to2D();
float dist_st_segment = (st.getPosition().to2D() - pnt_1).lenght() +
(st.getPosition().to2D() - pnt_2).lenght();
float segment_len = (pnt_1 - pnt_2).lenght();
float segment_mem = segment_len /dist_st_segment;
if(segment_mem > max_memberance) {
max_memberance = segment_mem;
nearest_segment_index = i;
}
}
return path.getStation(nearest_segment_index);
}
void Trajectory::copyFrom(Trajectory &other)
{
m_states_vec.clear();
for(int i=0; i<other.length(); i++) {
// other.getStation(i).getPosition().print(std::cout);
Station st = other.getStation(i);
this->appendState(st);
}
this->cost = other.cost;
}
Trajectory PlanningProblem::PruneTrajectory(Trajectory &input_plan, const ObstacleSet& ob_set)
{
if(input_plan.length() < 3)
return input_plan;
Trajectory prunned_plan;
int st_index = 1;
prunned_plan.appendState(input_plan.getStation(0));
while((input_plan.length() - st_index) > 1) {
Station st_A = prunned_plan.getLastStation();
Station st_B = input_plan.getStation(st_index +1);
if(pathHasCollision(st_A, st_B, ob_set)) {
Station new_inserted_st = input_plan.getStation(st_index);
float new_teta = (new_inserted_st.getPosition().to2D() -
prunned_plan.getLastStation().getPosition().to2D()).arctan();
new_inserted_st.setPosition(Vector3D(new_inserted_st.getPosition().to2D(), new_teta));
prunned_plan.appendState(new_inserted_st);
}
st_index ++;
}
prunned_plan.appendState(input_plan.getLastStation());
return prunned_plan;
// for(int i=0; i< p.length(); i++) {
// if(i == 0) {
// opt_plan.appendState(p.getStation(0));
// continue;
// }
// Station _st = p.getStation(i);
// float min_dist_to_ob;
// b2Vec2 st_colid_point;
// b2Vec2 ob_colid_point;
// Obstacle* ob_ = nearestObstacle(_st, stat_obstacles, min_dist_to_ob, st_colid_point, ob_colid_point);
// if(ob_ != NULL && min_dist_to_ob < agent->radius() * 1.5) {
// Vector2D bad_direc = (Vector2D(ob_colid_point) - Vector2D(st_colid_point)).normalized();
// _st.setPosition(_st.getPosition() - bad_direc.to3D() * 0.5);
// }
// if(CheckValidity(_st))
// opt_plan.appendState(_st);
// else
// opt_plan.appendState(p.getStation(i));
// }
}
bool PlanningProblem::checkPlanValidity(Trajectory &plan, float tolerance_coeff)
{
if(plan.length() == 0)
return false;
if(Station::euclideanDistance(plan.getStation(0), initialState) > tolerance_coeff* agent->radius())
return false;
if(goal.minDistTo(plan.getLastStation()) > tolerance_coeff * agent->radius())
return false;
if(Station::euclideanDistance(plan.getLastStation(), goal.goal_point) > tolerance_coeff * agent->radius() )
return false;
// for(int i=0; i<plan.length(); i++) {
// if(!CheckValidity(plan.getStation(i)))
// return false;
// }
return true;
}