Flexible A* Pathfinding System

//if(path.size())
   //   mMoveState = ModeMove; //needed for some reason
   //else
   //   stopMove();

// Check if we should mMove, or if we are 'close enough'
      // AStar clearence is used to give us enough room to get around corners
      if (mFabs(xDiff) < tol && mFabs(yDiff) < tol) {
         if (path.size())
         {
			/*if(mMoveDestination != path[1]) {*/
				mMoveDestination = path[0];
				path.pop_front();
			//}
			//else {
			//	stopMove();
			//	throwCallback("onReachDestination");
			//}
         }
         else
         {
            stopMove();
            throwCallback("onReachDestination");
         }
      }
      else {
          if(path.size())
              mMoveState = ModeMove;
...

for (tmp = list->next; tmp; tmp = tmp->next) //iterate thru list
    {
        if (disSqr(tmp->loc, Pend) < disSqr(end->loc, Pend) &&
            !gServerContainer.castRay(tmp->loc, Pend, STATIC_COLLISION_MASK, &rInfo)) //find closest one
            end = tmp;

        if (disSqr(tmp->loc, Pstart) < disSqr(start->loc, Pstart) &&
            !gServerContainer.castRay(tmp->loc, Pstart, STATIC_COLLISION_MASK, &rInfo)) //find closest one
            start = tmp;
    }

struct NMPoint
{
   U32 id;
   F32 interval;
   Point3F loc; //in world space to prevent confusion
   NMPoint *adjs[8];
   NMPoint *prev, *next; //for linear linked-list behavior
   AStarPoint *asp;
   NavMesh* mesh;
   F32 DisSqr; // New - cache the distances.
};

#define ASTAR_COLLISION_MASK   ( InteriorObjectType | StaticTSObjectType | StaticShapeObjectType  ) 
#define GRIDSIZE   4
#define GATHERSIZE  ((GRIDSIZE * 3) * (GRIDSIZE * 3))

NMPoint *AStar::FindClosestPoint(Point3F pPoint, bool CheckCollision)
    {
        RayInfo rInfo;
        NMPoint *end, *bkup, *tmp;

        end = bkup = list;
        end->DisSqr = disSqr(end->loc,pPoint);

        // Calc them one time
        for (tmp = list->next; tmp; tmp = tmp->next) //iterate thru list
        {
            // Calculate the distance only one time
            tmp->DisSqr = disSqr(tmp->loc,pPoint);
            // get the closest
            if (tmp->DisSqr < end->DisSqr)
            {
                bkup = end;
                end = tmp;
            }
        }

        if (!CheckCollision)
            return end;

        // try the most obvious choice - It works the vast majority of the time.
        if (!gServerContainer.castRay(end->loc, pPoint, ASTAR_COLLISION_MASK, &rInfo))
            return end;

        // try the second closest. Ok, it's a longshot, but finding it was free.
        if (!gServerContainer.castRay(bkup->loc, pPoint, ASTAR_COLLISION_MASK, &rInfo))
            return bkup;

        // Hardly ever gets this far.
        // try the child points of the selected node.
        // This should be the nodes all around the player.
        for (int i=0;i<8;i++)
        {
            NMPoint *tmp = end->adjs[i];
            if (tmp)
            {
                // this might give us something a little out of order, but isn't that better than casting through all points?
                if (!gServerContainer.castRay(tmp->loc, pPoint, ASTAR_COLLISION_MASK, &rInfo))
                    return tmp;
            }
        }

        // Check the points around the location.
        // Very Very small chance we get here. 
        // Lets gather the nodes all around us (at least the ones we haven't already looked at). 
        // Yeah, I unrolled the loop. Due to laziness, there is no speed improvement.
        for (tmp = list->next; tmp; tmp = tmp->next) //iterate thru list again. That OctTree would be handy here.
        {
            if (tmp->DisSqr < GATHERSIZE)
            {
                // Don't add if we've already checked it.
                if (end->adjs[0] == tmp) continue;
                if (end->adjs[1] == tmp) continue;
                if (end->adjs[2] == tmp) continue;
                if (end->adjs[3] == tmp) continue;
                if (end->adjs[4] == tmp) continue;
                if (end->adjs[5] == tmp) continue;
                if (end->adjs[6] == tmp) continue;
                if (end->adjs[7] == tmp) continue;
                if (!gServerContainer.castRay(tmp->loc, pPoint, ASTAR_COLLISION_MASK, &rInfo))
                    return tmp;
            }
        }

        // Didn't find anything around us at all. We were probably blown completely off the navmesh really far
        // by an explosion or something. Probably should look for the closest Navmeshes, and check those for proximity.
        // We could Brute Force it, but no guarantee even then. Lets just return the location and see if he can make it
        // there on his own.

	return end;
// delete the line above, and define the ASTAR_BRUTE_FORCE if you want to go the extra mile to get a node.
#ifdef ASTAR_BRUTE_FORCE
        bool found = false;
        end = list;
        // do it the hard way.
        for (tmp = list->next; tmp; tmp = tmp->next) //iterate thru list
        {
            if (tmp->DisSqr < end->DisSqr)
            {
                if (!gServerContainer.castRay(end->loc, pPoint, ASTAR_COLLISION_MASK, &rInfo))
                {
                    found = true;
                    end = tmp;
                }
            }
        }
        if (found)
            return end;
        return NULL;
#endif
    }

S32 AStar::findBasicPath (Point3F Pstart, Point3F Pend, Vector<Point3F> &Vpoints)
{
#ifdef AI_RENDER
    //LastPoints.clear();
#endif
    if (!list)
        return -1;

    if (Pstart == Pend) //may seem stupid, but probably good to do
        return 1;

    NMPoint *start, *end, *tmp;
    start = end = list;
    RayInfo rInfo;
    

    start = FindClosestPoint(Pstart,true);  
    if (!start) return -1;

    end   = FindClosestPoint(Pend, false);  // don't care if the End Node--->end is blocked
    if (!end) return -1;

    // Remainder of code remains the same as it was
    if (start == end)
    {
        return 1;
    }

    // ... code deleted for brevity
}

if (mAimObject)
         mAimLocation = mAimObject->getPosition() + mAimOffset;
      else
         if (!mAimLocationSet)
            mAimLocation = mMoveDestination;

//Change above to:

      if (mAimObject && mTargetInLOS)
         mAimLocation = mAimObject->getPosition() + mAimOffset;
      else
         if (!mAimLocationSet && !mTargetInLOS)
            mAimLocation = mMoveDestination;

NMPoint* pt;
for (pt = list; pt->next;)
{
	pt = pt->next;
	delete pt->prev;
}
delete pt;