searchalgo.esl

list boundingBoxes;

vector half_dim;
list painted;
vector root;
vector goal;


integer testGridIntersectsBoxes(vector grid_c, vector h_sizes, list BBoxes)
{
    // get bounding box list length
    integer BBoxesSize = llGetListLength(BBoxes);
    // define an index
    integer i;
    // define a vector varaible for the difference between grid and bounding box centers
    vector box_c;
    // define a vector varaible for the sum of half dimensions
    vector box_d;
    // for each bounding box pair
    for (i = 0; i < BBoxesSize; i = i + 2){
        // get the bounding box center, find the difference between the grid element and box centers
        box_c = llList2Vector(BBoxes, i) - grid_c;
        // get the bounding box half dimensions plus half dimensions for th grid element
        box_d = llList2Vector(BBoxes, i + 1) + h_sizes;
        // test intersection
        if (llFabs(box_c.x) <= box_d.x && llFabs(box_c.y) <= box_d.y && llFabs(box_c.z) <= box_d.z){
         //   llOwnerSay("INTERSECTS");
            return TRUE;
        }
    }
    
    return FALSE;
}

list GreedySearch(vector root, vector goal, float grid_d)
{

   integer i;
   vector tempVector;
   list path = [];
   //get the children as a 2 stride list (cost evaluation, child)
   list children = [0.0, <root.x - grid_d, root.y, root.z>, 0.0, <root.x + grid_d, root.y, root.z>, 0.0, <root.x,root.y - grid_d, root.z>,0.0, <root.x, root.y + grid_d, root.z>, 0.0, <root.x,root.y,root.z-grid_d>, 0.0, <root.x,root.y,root.z+grid_d>];
   integer j;
   
   for(i = 0; i < 12; i=i+2)
   {
      //set the evaluation cost
      children = llListReplaceList(children, [llVecDist(llList2Vector(children, i+1), goal)], i,i);
   }
  // llOwnerSay("Length of Children " + (string)llGetListLength(children));
   children = llListSort(children, 2, TRUE);
   for (j = 1; j < 12; j=j+2)
   {
      tempVector = llList2Vector(children, j);
      llSetRot(llRotBetween(tempVector, goal-tempVector));
      //llOwnerSay("GS> tempVector: " + (string) tempVector); 
      //llSetPos(tempVector);
      if (llListFindList(painted, [tempVector]) == -1 && !testGridIntersectsBoxes(tempVector, half_dim, boundingBoxes))
      {
         painted += [tempVector];
      //   llOwnerSay("Painted"); 
         if(tempVector == goal)
         {
            path = [tempVector];
            llOwnerSay("Found Goal"); 
            llOwnerSay("Returning size 1: " + (string)llGetListLength(path) + " Painted size: "+ (string)llGetListLength(painted)); 
            return path;
         }
         
         path = GreedySearch(tempVector, goal, grid_d);
         
         if (llGetListLength(path) > 0)
         {
            path += [tempVector];
            llOwnerSay("Returning size 2: " + (string)llGetListLength(path) + " Painted size: "+ (string)llGetListLength(painted)); 
            return path;
         }
      }
   }
   llOwnerSay("Returning size 3: " + (string)llGetListLength(path) + " Painted size: "+ (string)llGetListLength(painted)); 
   return path;
}

list AStarSearch(vector root, vector goal, float grid_d)
{
   list open = [llVecDist(root, goal), root, -1];
   list closed = [];
   list path = [];
   vector X;
   list children;
   integer i; 
   integer j;
   
   float currentGVal;
   while(llGetListLength(open) > 0)
   {
      open = llListSort(open, 3, TRUE);
      X = llList2Vector(open, 1);
      currentGVal = llList2Float(open, 0) - llVecDist(X, goal);
      closed += [X, llList2Integer(open, 2)];
      j = llGetListLength(closed) - 2;
      open = llDeleteSubList(open, 0,2);
      llOwnerSay((string)llGetListLength(open));
      children = [<X.x - grid_d, X.y, X.z>, <X.x + grid_d, X.y, X.z>, <X.x, X.y - grid_d, X.z>, <X.x, X.y + grid_d, X.z>, <X.x, X.y, X.z-grid_d>, <X.x, X.y, X.z + grid_d>];
      currentGVal += grid_d;
      for (i=0; i < 6; i++)
      {
         X = llList2Vector(children, i);
         //llSetPos(X);
         llSetRot(llRotBetween(X, goal-X));
         if (X.x > 0 && X.y > 0 && X.z == root.z && !testGridIntersectsBoxes(X, half_dim, boundingBoxes))
         {
            if (X == goal)
            {
               path = [X];
               while ( j > -1)
               {
                  path += [llList2Vector(closed, j)];
                  j = llList2Integer(closed, j+1);
               }
               llOwnerSay("Found Path");
               return path;
            }
            if (llListFindList(open, [X]) == -1 && llListFindList(closed, [X]) == -1) 
            {
               open += [currentGVal + llVecDist(X, goal), X, j];
            }
         }
      }
   }
   return path;
}

//
default
{
    state_entry()
    {
        // set the grid half-dimension
        half_dim = <0.25, 0.25, 0.25>;
        // get a map of bounding boxes now
        // This function will sense all objects that are either
        // passive or have active scripts within 96 meter radius
        // in a positive pi hemisphere
        llSensor("", NULL_KEY, (PASSIVE|ACTIVE), 96.0, PI);
        llOwnerSay("Compiled"); 
        root = llGetPos();
        goal = <root.x, root.y+5, root.z>;
    }
       
    touch_start(integer x)
    {
        //painted = [];
        list aPath;
       // llSetRot(llRotBetween(root, goal-root));
        llOwnerSay("ROOT: " + (string)root + " GOAL: " + (string)goal);
        aPath = GreedySearch(root, goal, 2*half_dim.x);
        integer i;
        llOwnerSay("Length of Paths: " + (string)llGetListLength(aPath));
        for(i = llGetListLength(aPath); i > 0; i--)
        {
          llSetPos(llList2Vector(aPath, i));
        }
        llSetPos(llList2Vector(aPath, 0));
    }
    
    sensor(integer num_detected)
    {
        // define an index variable
        integer i;
        // a bounding box variable
        list box;
        // clear the global bounding box list
        boundingBoxes = [];         
        llOwnerSay("Detected: " + (string)num_detected);
        // for each detected object
        for (i = 0; i < num_detected; i++){
            // add the detected position to the bounding box list
            boundingBoxes += [llDetectedPos(i)];
            // get the bounding box
            box = llGetBoundingBox(llDetectedKey(i));
            // add the box half dimension to the list
            boundingBoxes += [0.5*(llList2Vector(box,1) - llList2Vector(box,0))];
        }
    }
}