CS148 asgn3: Path Planning and Soccer Playing

Author: Patrick Doran
Login: pdoran
Group: Enamored
Due: 03/08/2009

function RapidRandomSearch.createPath(start,goal)
    for (int k = 0 to MAX_SAMPLES)
        state = connect(tree,goal)
        if (state == REACHED)
            return cleanPath(tree,goal)
            
        q = randomSampleFromWorld()
        connect(tree,q);
       

function RapidRandomSearch.connect(tree,goal)
    do
        state = extend(tree,q)
    while(state == ADVANCED)
    return state
        

function RapidRandomSearch.extend(tree,goal)
    qnear = tree.getNearestNode(q)
    Point qnew
    if (newConfig(q,qnear,qnew))
        tree.addLeaf(qnew)
        if (qnew == q)
            return REACHED
        else
            return ADVANCED
    return TRAPPED
        

function RapidRandomSearch.newConfig(q,qnear,qnew)
    vect = q-qnear
    if (vect.magnitude() > INCREMENTAL_DISTANCE)
        vect.normalize()
        vect = vect * INCREMENTAL_DISTANCE

    if (intersectsWorld(qnear,qnear+vect,m_world,m_moverId))
        return false
        
    //RETURN BY REFERENCE with SUCCESS
    qnew = qnear + vect
    return true
        

function RapidRandomSearch.cleanPath(tree,goal)
    waypoints = pathToRoot(tree,goal)
    if (waypoints.size() < 3)
        return waypoints
           
    list newWaypoints
    newWaypoints.addFirst(waypoints.first())
    for i = waypoints.begin to waypoints.end
        for j = waypoints.end to waypoints.begin
            if (line(waypoint[i],waypoint[j]) does not intersect the world
                newWaypoints.addLast(waypoints[j])
                i = j
                break