コード例 #1
0
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;
}
コード例 #2
0
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));
//    }
}