C++ Station::getPosition方法代码示例

bool PlanningProblem::hasCollision(const Station &st, const Obstacle &ob)
{
    b2Transform st_Transform;
    st_Transform.Set(st.getPosition().to2D().toB2vec2(), st.getPosition().Teta());

    return b2TestOverlap(ob.shape, 0, agent->shape, 1, ob.predictedTransform(0), st_Transform);
}

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

Trajectory PlanningProblem::GRRTsolve()
{
    this->planningResult = false;
    randomTree.clear();
    float start_time = currentTimeMSec();
    int tryExtensionCounter = 0;
    randomTree.appendNewStation(NULL, initialState);

    for(int step=0; step < MAX_RRT_STEP_TRY/5; step++)
    {
        Station target;
        float toss = uni_rand(0, 1);
        if(toss < GOAL_PROB)
            target.setPosition(goal.goal_point.getPosition());
        else {
            Station tempSt = SampleStateUniform();
            target.setPosition(tempSt.getPosition());
        }

        if( !target.isValid() )
            continue;
//            throw "can not sampled!";
        SpatialVertex* near_ver = randomTree.getNearestVertex(target);
        if(near_ver == NULL)
            continue;

        int greedyCounter = 0;
        while(greedyCounter < 5){
            tryExtensionCounter ++;
            Station extended = RRTExtend(near_ver->state, target, agent->radius() * 2);
            if(!extended.isValid())
                break;
            randomTree.appendNewStation(near_ver, extended);
            if(Station::dubinDistance(extended, target) < agent->radius() ) {
                if((target.getPosition() - goal.goal_point.getPosition()).lenght2D() < agent->radius() /2)
                    planningResult = true;
                break;
            }
//            if(target != goal.goal_point)  break;
            greedyCounter ++;
        }

        cout << "Step = " << step << endl;

    }

    if(planningResult)
    {
        float finish_time = currentTimeMSec();
        this->planningTime = finish_time - start_time;
//        cout << "Greedy RRT Planning succeed in " << planningTime << "mili seconds" << endl;
        return buildTrajectoryFromRandomTree();
    }
    return Trajectory();
}

bool PlanningProblem::pathHasCollision(Station &from, Station &to, const Obstacle &ob)
{
    b2PolygonShape road_from_to;
    Vector2D center((from.getPosition() + to.getPosition()).to2D() /2.0);
    Vector2D half_diff((to.getPosition() - from.getPosition()).to2D() /2.0);
    float safet_latera_bound = 0.1;
    road_from_to.SetAsBox(agent->radius() * (1 + safet_latera_bound), half_diff.lenght() + agent->radius(),
                          center.toB2vec2(), M_PI_2 + half_diff.arctan());

//    if(ob == NULL)
//        continue;
    return b2TestOverlap(ob.shape, 0, &road_from_to, 1, ob.transform, identity_trans);
}

Station PlanningProblem::RRTExtend(const Station &start, const Station &target, float extension_len)
{
    Vector3D diff_vec = target.getPosition() - start.getPosition();
    diff_vec.normalize2D();
    diff_vec *= extension_len;
    float start_angle = start.getPosition().Teta();
    start_angle = continuousRadian(start_angle, diff_vec.to2D().arctan() - M_PI);
    float rotate_angle = (diff_vec.to2D().arctan() - start_angle) / 1.5;

    diff_vec.setTeta(rotate_angle);
    Station temp_station;
    temp_station.setPosition((diff_vec + start.getPosition()).standardizeTeta());
    bool valid = CheckValidity(temp_station);
    if(valid)
        return temp_station;
    return Station();
}

// this function returns -1 if two objects has collision
// And otherwise returns the distance
float PlanningProblem::distToObstacle(Station A, const Obstacle &ob, b2Vec2& A_point, b2Vec2& ob_point)
{        
    b2DistanceProxy state_proxy, ob_proxy;
    state_proxy.Set(agent->shape, 0);
    ob_proxy.Set(ob.shape, 1);
    b2DistanceInput dist_in;
    dist_in.proxyA = state_proxy;
    dist_in.proxyB = ob_proxy;
    dist_in.transformA = b2Transform(A.getPosition().toB2vec2(),
                                    b2Rot(A.getPosition().Teta()));
    dist_in.transformB = ob.transform;
    b2SimplexCache dist_cache;
    dist_cache.count = 0;
    b2DistanceOutput dis_out;
    b2Distance(&dis_out, &dist_cache, &dist_in);
    A_point = dis_out.pointA;
    ob_point = dis_out.pointB;    
    if(hasCollision(A, ob)) {
        return -1;
    }
    return dis_out.distance;
}

Vector2D PlanningProblem::GoalAttractiveForce(const Station &st)
{
    Vector2D diff_to_goal = (goal.goal_point.getPosition() - st.getPosition()).to2D();
    double stren = 1.5 * tanh(diff_to_goal.lenght()/25.0) + 0.1;
    return diff_to_goal.normalized() * stren;
}

Vector2D PlanningProblem::PathDirectedForce(const Station &st, Trajectory &path_)
{
    Station sub_goal = getNextStation(st, path_);

    return (sub_goal.getPosition().to2D() - st.getPosition().to2D()).normalized() * 1.0;
}