C++ (Cpp) Trajectory::isEmptyの例

コード例 #1

ファイル: planningproblem.cpp プロジェクト: amiryanj/cyrus_ssl

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