C++ Trajectory::getStation方法代码示例

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;
}