bool PlanningProblem::replan(const ObstacleSet &ob_set, Trajectory &trajec)
{
if(trajec.length() < 2)
return false;
Vector2D goals_diff = (goal.goal_point.getPosition() - trajec.getLastStation().getPosition()).to2D();
if(goals_diff.lenght() > agent->radius())
return false;
Vector2D inits_dist = (initialState.getPosition() - trajec.getFirstStation().getPosition()).to2D();
if(inits_dist.lenght() > agent->radius())
return false;
trajec.EditStation(0, initialState);
trajec.EditStation(trajec.length() -1, goal.goal_point);
return true;
}
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));
// }
}