Pathfinder beta v0.9

//Pathfinder script
//Created by Psyfire Tigereye
//Beta version 0.9

//For more info on pathfinding, check out
// http://www.policyalmanac.org/games/aStarTutorial.htm

////// A note about nodes //////
//A node is a rotation with the following format: <x, y, g, p>
//x is the x coordinate of the node, y is the y coordinate.
//x and y both use llGetRegionCorner() / 256 values.
//g is the G cost of the node, i.e. how many steps it took to get there from startNode.
//p is the parent node. It is the index in closedNodes that it's parent is located at.
//A p value of -1 is used for the first node, to indicate that it's where you started.

//Customization
//The name of the note card w/ the sim data.
string simDataCard = "simData";

//The com channel of the link message 
integer searchStartMsg = 142;
integer readyMsg = 143;
integer badPath = 144;
integer searchDone= 145;

//Global variables
list openNodes;
list closedNodes;
key mapSizeReq;
key simDataReq;
string simData;
integer nextLine;
vector dest;
integer reqSource;
integer currentNodeIndex;
integer xStart;
integer yStart;
integer xMax;
integer yMax;
integer yStep;

//Functions

//Returns the H cost (estimated distance to the target).
integer hCost(rotation testNode) {
    integer retv = llAbs((integer)testNode.x - (integer)dest.x)
        + llAbs((integer)testNode.y - (integer)dest.y);
    return retv;
}

//Finds the node on the open list with the lowest F cost (distance traveled + H)
integer lowestF() {
    integer listLength = llGetListLength(openNodes);
    rotation testNode = llList2Rot(openNodes, 0);
    integer i; integer lowest = (integer)testNode.z + hCost(testNode);
    integer retv = 0; integer testF;
    for (i = 1; i < listLength; ++i) {
        testNode = llList2Rot(openNodes, i);
        testF = (integer)testNode.z + hCost(testNode);
        if (testF <= lowest) {
            lowest = testF;
            retv = i;
        }
    }
    return retv;
}

//Checks if there's a node at the specific x and y coordinates.
integer isSim(float x, float y) {
    if ((x < xStart || x > xMax) || (y < yStart || y > yStart + yStep)) {
        return FALSE;
    }
    x -= xStart; y -= yStart;
    integer index = (integer)(y * yStep + x);
    //Entry will be 1 if it's a sim, or 0 if not, so this give TRUE or FALSE.
    return (integer)llGetSubString(simData, index, index);
}

//What to do when there's no way to the target.
noPath(integer why) {
    list reasons = ["Bad target", "Bad start", "No path"];
    llMessageLinked(reqSource, badPath, llList2String(reasons, why), NULL_KEY);
}

//Returns a list of the sims N, S, E, and W of the sim at x, y.
list borderLands(float x, float y) {
    list borders = [x, y - 1.0];
    borders += [x, y + 1.0];
    borders += [x - 1.0, y];
    borders += [x + 1.0, y];
    integer i;
    list retList;
    for (i = 0; i < 8; i += 2) {
        if (isSim(llList2Integer(borders, i), llList2Integer(borders, i + 1))) {
            retList += llList2List(borders, i, i + 1);
        }
    }
    return retList;
}

//Returns a value based on if the sim is on a list already or not.
integer checkNode(integer x, integer y) {
    if (x == dest.x && y == dest.y) {
        return -3;
    }
    integer i;
    rotation tempNode;
    integer len = llGetListLength(closedNodes);
    for (i = 0; i < len; ++i) {
        tempNode = llList2Rot(closedNodes, i);
        if (x == tempNode.x && y == tempNode.y) {
            return -1;
        }
    }
    len = llGetListLength(openNodes);
    for (i = 0; i < len; ++i) {
        tempNode = llList2Rot(closedNodes, i);
        if (x == tempNode.x && y == tempNode.y) {
            return i;
        }
    }
    return -2;
}

//The actual search process.
search() {
    integer found = FALSE;
    integer lowestOpen;
    rotation currentNode;
    rotation tempNode;
    list neighbors;
    integer status;
    integer len; integer i;
    integer newX; integer newY;
    while (!found) {
        lowestOpen = lowestF();
        currentNode = llList2Rot(openNodes, lowestOpen);
        closedNodes += currentNode;
        openNodes = llDeleteSubList(openNodes, lowestOpen, lowestOpen);
        currentNodeIndex = llGetListLength(closedNodes) - 1;
        neighbors = borderLands(currentNode.x, currentNode.y);
        len = llGetListLength(neighbors);
        if (len == 0 && llGetListLength(openNodes) == 0) {
            noPath(2);
            return;
        }
        for (i = 0; i < len; i += 2) {
            newX = llList2Integer(neighbors, i);
            newY = llList2Integer(neighbors, i + 1);
            status = checkNode(newX, newY);
            if (status == -2 && isSim(newX, newY)) {
                openNodes += [<newX, newY, currentNode.z + 1, currentNodeIndex>];
            }
            else if (status == -3) {
                closedNodes += [<newX, newY, 0, currentNodeIndex>];
                found = TRUE;
            }
            else if (status >= 0) {
                tempNode = llList2Rot(openNodes, status);
                if (tempNode.z <= currentNode.z) {
                    tempNode.z = currentNode.z + 1;
                    tempNode.s = currentNodeIndex;
                    openNodes = llListInsertList(llDeleteSubList(openNodes,
                        status, status), [tempNode], status);
                }
            }
        }
    }
    //Finish code
    list reversePath;
    rotation next = llList2Rot(closedNodes, - 1);
    while (next.s >= 0) {
        reversePath += next;
        next = llList2Rot(closedNodes, (integer)next.s);
    }
    reversePath += next;
    len = llGetListLength(reversePath);
    rotation fromNode = llList2Rot(reversePath, len);
    rotation toNode;
    list finalPath;
    for (i = len - 1; i >= 0; --i) {
        toNode = llList2Rot(reversePath, i);
        finalPath += [(integer)(toNode.x - fromNode.x),
            (integer)(toNode.y - fromNode.y)];
        fromNode = toNode;
    }
    llMessageLinked(reqSource, searchDone, llList2CSV(finalPath), NULL_KEY);
}

//States
default {
    state_entry() {
        mapSizeReq = llGetNotecardLine(simDataCard, 1);
    }

    dataserver(key queryid, string data) {
        if (queryid == mapSizeReq) {
            list splitData = llCSV2List(data);
            xStart = (integer)llList2String(splitData, 0);
            xMax = (integer)llList2String(splitData, 1);
            yStart = (integer)llList2String(splitData, 2);
            yMax = (integer)llList2String(splitData, 3);
            yStep = xMax - xStart + 1;
            nextLine = 2;
            simDataReq = llGetNotecardLine(simDataCard, nextLine);
        }
        if (queryid == simDataReq) {
            if (data != EOF) {
                simData += data;
                ++nextLine;
                simDataReq = llGetNotecardLine(simDataCard, nextLine);
            }
            else {
                llMessageLinked(LINK_SET, readyMsg, "Ready", NULL_KEY);
            }
        }
    }

    link_message(integer sender_num, integer num, string str, key id) {
        if (num == searchStartMsg) {
            //Clear old search values
            closedNodes = [];
            //Get ready for this search
            reqSource = sender_num;
            list destInfo = llCSV2List(str);
            dest = <(float)llList2String(destInfo, 0), 
                (float)llList2String(destInfo, 1), 0>;
            vector here = llGetRegionCorner();
            here.x /= 256; here.y /= 256;
            //Check if the start or destination are islands, or non-existant
            if (!isSim(dest.x, dest.y) ||
                    llGetListLength(borderLands(dest.x, dest.y)) == 0) {
                noPath(0);
                return;
            }
            else if (!isSim(here.x, here.y) ||
                    llGetListLength(borderLands(here.x, here.y)) == 0) {
                noPath(1);
                return;
            }
            openNodes = [<here.x, here.y, 0, -1>];
            list avoid = llCSV2List((string)id);
            integer i;
            integer len = llGetListLength(avoid);
            for (i = 0; i < len; i += 2) {
                closedNodes += [<(float)llList2String(avoid, i),
                    (float)llList2String(avoid, i + 1), 0, 0>];
            }
            search();
        }
    }
}