FFLocPath.C

Go to the documentation of this file.

#include "Global.H"
#include "FFLocPath.H"
#include "Options.H"
#include "GameObj.H"
#include <utility>
#include <iterator>
#include <math.h>
#include "TR_PFEngine.H"


using namespace std;

#define TEST(x)
//#define TEST(x) x



  void FFLocPath::add_options()
  {
    ::Options o("SimpleTerrain - ForceField");
    o.put("-costonly", "represent boundaries only by higher edge cost");
    //o.put("-makesquares" ,"compute the motion squares of objects");
    o.put("-highres", "use a high resolution world map (slower but more accurate)");
    o.put("-nopush", "turns off pushing");
  }

  //----------------------------------------------------------------------------- 
  static Loc get_center_loc(const Object *obj)
  {
    Loc l;
    obj->get_center(l.x, l.y);
    return l;
  }

//   static ScalarPoint get_center_sp(const Object *obj)
//   {
//     ScalarPoint l;
//     obj->get_center(l.x, l.y);
//     return l;
//   }

    static real8 path_length(Path &path)
  {
    if( path.locs.size() < 2 )
      return 0;
    real8 total = 0;
    for( Vector<Loc>::iterator i = ++path.locs.begin() ; i != path.locs.end() ; i++ )
      total += i->distance(*(i-1));
    return total;
  }
    
  // Assuming path is in the usual reversed order, with goal at locs[0]
  static real8 path_length_from(Path &path, const Loc& start_loc)
  {
    if( path.locs.size() < 1 ) 
      return 0;
    return start_loc.distance(path.locs[path.locs.size()-1]) + path_length(path);
  }

FFLocPath::FFLocPath() {
    sint4 seed = 0;
    ::Options::get("-seed", seed);
    rand.srand(seed);
    bool nopush;
    ::Options::get("-nopush", nopush);
    if (nopush){
      pushing_on = false;
    }
    else{
      pushing_on = true;
    }
    //Options::get("-makesquares", makesquares);
  }

  //-----------------------------------------------------------------------------
  void FFLocPath::init(sint4 tiles_x_, sint4 tiles_y_, sint4 tile_points_, sint4 client_pID_, sint4 neutral_pID_)
  {
    TerrainBasicImp<FF_Task>::init(tiles_x_, tiles_y_, tile_points_,
                   client_pID_, neutral_pID_);
    std::cout << "tiles_x_ = " << tiles_x_ << endl; 
    
    // initialize the internal PFEngine
    Vector<sint4> levels_of_detail;
    bool high_resolution_only;
    Options::get("-highres", high_resolution_only);

    if( !high_resolution_only ) {
    levels_of_detail.push_back(4);  // default - use a resolution reduced by a factor of 4.
      }
      levels_of_detail.push_back(1); // also (or exclusively) maintain a full resolution view.
      pfEngine = boost::shared_ptr<GlobalPathfinder>(new TR_PFEngine(tiles_x_, tiles_y_, tile_points_, client_pID_, neutral_pID_));
    
    // initialize motion squares
/*    if(makesquares){ // temporarily commented to prevent unused var warning -- dd
      sint4 area = tiles_x_ * tiles_y_;
      // sectors = new vector<Sector>(area); 
    }*/

      
  }

  //-----------------------------------------------------------------------------
  void FFLocPath::add_obj(const Object *obj)
  {
    TerrainBasicImp<FF_Task>::add_obj(obj);
    assert(pfEngine!=NULL);
    pfEngine->insert_object(obj);
    
    objects[obj] = Loc(-1,-1);
  }

  //-----------------------------------------------------------------------------
  void FFLocPath::update_obj(const Object *obj)
  {
    TerrainBasicImp<FF_Task>::update_obj(obj);
    assert(pfEngine!=NULL);
    pfEngine->update_object(obj);
  }

  //-----------------------------------------------------------------------------
  void FFLocPath::remove_obj(const Object *obj)
  {
    // shorthand
    typedef ObjIsGoalInTaskId::iterator OGIT;

    // remove from world representation
    assert(pfEngine!=NULL);
    pfEngine->remove_object(obj);

    // If we intended to stop it, we don't need to anymore
    objsToStop.erase(obj);

    // If this is the target of any existing task...
    pair<OGIT,OGIT> range = objIsGoalInTaskId.equal_range(obj);
    for( OGIT i = range.first ; i != range.second ; i++ ) {
      // Change the target of the task to the last known location of the object
      FIND(taskId2task, ti2tIt, i->second);
      assert(ti2tIt != taskId2task.end());
      
      ti2tIt->second->task.goal.target = Goal::LOCATION;
      Loc obj_center;
      ti2tIt->second->task.goal.obj->get_center(obj_center.x, obj_center.y);
      ti2tIt->second->task.goal.loc = obj_center;

    }
    
    TerrainBasicImp<FF_Task>::remove_obj(obj);

    objects.erase(obj);
  }

  //-----------------------------------------------------------------------------
  void FFLocPath::add_segments(const Vector<Segment> &segs)
  {
    TerrainBasicImp<FF_Task>::add_segments(segs);
    assert(pfEngine!=NULL);
    pfEngine->insert_boundaries(segs);
    FORALL(segs,i){
      segments.push_back(*i);
      segment_ends[i->l1] = true;
      segment_ends[i->l2] = true;
    }
    invalidate_paths(segs);
    
  }

  //----------------------------------------------------------------------------------
  void FFLocPath::cancel_task(const Object *obj)
  {
    objsToStop.insert(obj);
    TerrainBasicImp<FF_Task>::cancel_task(obj);
  }

  //----------------------------------------------------------------------------------
  void FFLocPath::remove_task(TaskId tid)
  {
    FIND (taskId2task, it, tid);
    if (it != taskId2task.end() ) {
      Task& task = it->second->task;
      objsToStop.insert(task.objs.begin(), task.objs.end());
    }
    TerrainBasicImp<FF_Task>::remove_task(tid);
  }


  //----------------------------------------------------------------------------------
 real8 FFLocPath::find_path(const Loc &/*l1*/, const Loc &/*l2*/, sint4 /*radius*/, Path * /*path*/, ConsiderObjects /*consider*/)
  {
    assert(0); // unimplemented yet    
    return -1;
  }

  //----------------------------------------------------------------------------------
  real8 FFLocPath::find_path(const Object *obj, const Goal &goal, Path *path, ConsiderObjects consider)
  {
    assert( goal.mode == Goal::TOUCH && "ATTACK, VICINITY modes are currently unimplemented");
    assert( (goal.target == Goal::LOCATION || goal.target == Goal::OBJ) && 
            "Unknown Goal::target type");

    Path result;
    bool found;
    if (goal.target == Goal::LOCATION) {
      found = pfEngine->find_path(obj, goal.loc, result,  consider);
    } else {
      found = pfEngine->find_path(obj, goal.obj, result,  consider);
    }

    if( !found )
      return -1;

    // need to return waypoints in a 'forward' order
    if( path != NULL ) {
      path->locs.clear();
      reverse_copy(result.locs.begin(), result.locs.end(), back_inserter(path->locs));
    }

    // compute and return the length of the path
    return path_length_from(result, get_center_loc(obj));
  }

  //----------------------------------------------------------------------------------

  // fixme: we only check the path of the *center* of the unit vs the new boundaries,
  // and we should account for its size (radius).
  void FFLocPath::invalidate_paths(const Vector<Segment> &segments)
  {
    FORALL(tasks, ti) {
      if (!ti->isValid() ) continue;
      // fixme: better handling of zcat != ON_LAND ?
      if (ti->path->obj->get_zcat() != Object::ON_LAND ) continue;

      FORALL(segments, si) {
        if (ti->path->is_intersect(*si) ) {
          // this path is invalid, mark as pending for (re)planning
          pendingTasks.push_back(ti);
          ti->isPending = true;
          cout << "Marked an invalid path" << endl;
          break;
        }
      }
    }
  }

  void FFLocPath::execute_tasks(Vector<MoveCmd> &cmds, Vector<StatusMsg> &msgs){
  
    //clear the reserved locations in the object map
    FORALL(objects, it){
      it->second = Loc(-1, -1);
    }
    // Execute all active tasks
    /*for (Tasks::iterator j = tasks.begin(); j != tasks.end();) {
      Tasks::iterator i = j++;*/
    FORALL(tasks, i) {

      // Report failure of planning (no path at all)
      if (i->noPathFound ) {
    TerrainBase::StatusMsg s;
    // fixme: this is not robust to future changes in the meaning of TaskId.
    s.task_id = (TerrainBase::TaskId)&(*i);
    s.obj = *(i->task.objs.begin());
    s.type = TerrainBase::StatusMsg::NO_PATH_FAILURE;
    msgs.push_back(s);
    continue;
      }

      // Skip invalid tasks.
      if (!i->isValid() ) { 
    TEST(cout << *i->task.objs.begin() << " task not valid" << endl;);
    continue;
      }

      UnitPath_FF &cur_path = *(i->path);
      const Object *obj = cur_path.obj;

      // Skip tasks that are waiting for a retry
      if (cur_path.sleeping()) {
    TEST(cout << obj << " path sleeping" << endl;);
    continue;
      }

      // Skip objects for which we gave a (move) action that is still pending
      /*
    if (obj->is_pending_action()) {
    TEST(cout << obj << " pending action" << endl;)
    continue;
    }
      */

      bool atGoal = is_at_goal(obj, i->task);

      // Look ahead: if the object will get to a waypoint in the next tick,
      // give the next action now the prevent it from wasting a tick on stopping.
      // This will cut a single tick at each waypoint, though it will really prevent 
      // the object from stopping only if there's no (network or processing) lag
      // between this client and the server.
      // atWaypoint can be true only if the current waypoint is an intermediate and
      // not the *final* waypoint/goal.
      //   bool atWaypoint = cur_path.current_target > 0 &&  // reverse order, goal==0
      //    is_at_location(obj, cur_path.path.at(cur_path.current_target));

      // Skip a moving object which is currently not at a waypoint/goal
      if (/*(obj->get_speed() > 0 || obj->get_moving() != 0) && !atWaypoint && */ !atGoal ) {
    TEST(cout << obj << " atWaypoint=" << atWaypoint << " speed=" << obj->get_speed() << " is_moving=" << obj->get_moving() << endl;);
    Loc target;
    //  bool deflected = get_forcefield_target(cur_path, target);
    //  bool nearWaypoint = is_near_waypoint(obj, cur_path.path.at(cur_path.current_target));
    //  if (nearWaypoint && deflected && cur_path.current_target > 0){
    //    bool valid = slide_wp(cur_path);
    //    if (!valid){
    //      i->isFailed = true;
    //      continue;
    //    }
    get_forcefield_target(cur_path, target);
    //  }
    add_move_command(obj, target, cmds);

    ///////////////////////////////
    
    Loc obj_center;
    obj->get_center(obj_center.x, obj_center.y);

//  FORALL(i->followers, fol_it){
//    get_forcefield_target(*fol_it, obj_center, target);
//    add_move_command(*fol_it, target, cmds);
//  }
    
    real8 distance = obj_center.distance(cur_path.path.at(cur_path.current_target));
    if (cur_path.spot != Loc(-1,-1)){
      real8 spot_distance = cur_path.spot.distance(cur_path.path.at(cur_path.current_target));
      if (distance >= spot_distance - ((real8)obj->get_max_speed())/2){
        if (cur_path.collision_step > 5){
          if (i->replanned < 1){ //fixed cap on number of replannings
           i->isFailed = true;
            failedTasks.push_back(i);
          }
        }
        if (cur_path.collision_step > 10){
          TerrainBase::StatusMsg s;
          //    fixme: this is not robust to future changes in the meaning of TaskId.
          s.task_id = (TerrainBase::TaskId)&(*i);
          s.obj = obj;
          s.type = TerrainBase::StatusMsg::MOVEMENT_FAILURE;
          msgs.push_back(s);
          cout << "Movement failure!" << endl;
        }
       
        cur_path.collision_step++;
      }
      else{
        cur_path.collision_step = 0;
        cur_path.spot = obj_center;
      }
    }
    else{
      cur_path.spot = obj_center;
      cur_path.collision_step = 0;
    }
    
    
    ///////////////////////////////

    continue;
      }



      // ok, so it needs attention:
      // 1. maybe it had reached the goal
      // 2. maybe it's at a waypoint (with the lookahead)
      // 3. maybe it needs the first orders
      // 4. maybe it unexpectedly collided with something

      if (atGoal ) {

    TEST(cout << obj << " arrived" << endl;)

      TerrainBase::StatusMsg s;
    // fixme: this is not robust to future changes in the meaning of TaskId.
    s.task_id = (TerrainBase::TaskId)&(*i);
    s.obj = obj; 
    s.type = TerrainBase::StatusMsg::ARRIVED;
    msgs.push_back(s);  

      }
  //     else if( atWaypoint || 
//       cur_path.no_orders_yet || 
//       cur_path.stopped_to_avoid_collision ) {

//  // in all these cases we should just issue a move command

//  TEST(cout << obj << " atWaypoint=" << atWaypoint
//       << " no_orders_yet=" << cur_path.no_orders_yet
//       << " stopped_to_avoid_collision=" << cur_path.stopped_to_avoid_collision
//       << endl; );

//  // if we are at a waypoint, move to next waypoint
//      if( atWaypoint )
//        cur_path.current_target--;
//  cur_path.no_orders_yet = false;
//  cur_path.stopped_to_avoid_collision = false;
//  Loc target;
//  get_forcefield_target(cur_path, target);
//  add_move_command(obj, target, cmds);

//       } 
      else {

    // Unexpected stop. The current policy is:
    // 1-2. First time => wait a little and send the move command again.
    // 3-4. Second stop at the same place => mark as failed and replan.
    // 5. Third stop at the same place => report failure.

//  TEST(cout << obj << " unexpectedly stopped, collision_step=" << cur_path.collision_step << endl);

//  Loc cur_position; obj->get_center(cur_position.x, cur_position.y);

//  // First collision in this position:
//  if (cur_path.spot != cur_position ) {
//    cur_path.collision_step = 1;
//    cur_path.mark_spot(cur_position);
//    // sleep for a short random time, 500-999 millisecs
//    cur_path.sleep(500 + (rand.rand_uint4() % 500));
//  } else {
//    cur_path.collision_step++;

//    // Just finished the sleeping:
//    if (cur_path.collision_step == 2 ) {
//      Loc target;
//      get_forcefield_target(cur_path, target);
//      add_move_command(obj, target, cmds);
//    }
//    // Second collision
//    else if (cur_path.collision_step == 3 ) {
//      i->isFailed = true;
//    }
//    // Back from replanning
//    else if (cur_path.collision_step == 4 ) {
//      Loc target;
//      get_forcefield_target(cur_path, target);
//      add_move_command(obj, target, cmds);
//    }
//    // Third collision
//    else if (cur_path.collision_step == 5 ) {
//      TerrainBase::StatusMsg s;
//      // fixme: this is not robust to future changes in the meaning of TaskId.
//      s.task_id = (TerrainBase::TaskId)&(*i);
//      s.obj = obj;
//      s.type = TerrainBase::StatusMsg::MOVEMENT_FAILURE;
//      msgs.push_back(s);
//    }
//  }
      }
    }

    //Pushing!
    if (pushing_on){
      FORALL(objects, objIt){
    const GameObj *gob = dynamic_cast<const GameObj*>(objIt->first);
    if (gob){
      if (objIt->second == Loc(-1,-1) && *(gob->sod.owner) == client_pID && objIt->first->get_max_speed() > 0){
        Loc target;
        get_push_target(objIt->first, target);
        add_move_command(objIt->first, target, cmds);
      }
    }
      }
    }
   
 // Priority of commands:
    // 1. stops to avoid a collision.
    // 2. moves.
    // 3. other stops (resulting from a cancelled task, etc.).
    
    // 2 above 3:
    FORALL(cmds, c) {
      objsToStop.erase(c->obj);
    }

    // With all movement commands were given while assuming an ideal-no-collisions
    // world, now check if any of our controlled objects are going to collide
    // (because of an existing movement vector, and also consider commands
    // we just intended to give).
    //resolve_friendly_collisions();

    // 1 above 2:
    for(typeof(cmds.begin()) c = cmds.begin() ; c != cmds.end() ; ) {
      FIND(objsToStop, s, c->obj);
      if( s != objsToStop.end() )
    c = cmds.erase(c);
      else
    c++;
    }

    // Return also the stop commands
    FORALL(objsToStop, oi) {
      const Object *obj = *oi;

      TerrainBase::MoveCmd m;
      m.obj = obj;
      obj->get_center(m.next_loc.x, m.next_loc.y); // doesn't really matter
      m.speed = 0; // 0 -> stop
      cmds.push_back(m);
    }
    objsToStop.clear();

    // remove finished tasks
    FORALL (msgs, t) {
      remove_task(t->task_id);
    }
    //motionSquares.clear();
  }

  void FFLocPath::add_move_command(const Object *obj, Loc target, Vector<TerrainBase::MoveCmd> &cmds){
    //hack: 
    if (abs(target.x) >= 500000){
       target.x = 499999;
     }
     if( abs(target.y) >= 500000){
         target.y = 499999;
     }

    TerrainBase::MoveCmd m;
    m.obj = obj;
    m.next_loc = target;
    m.speed = obj->get_max_speed();
    cmds.push_back(m);
  
    FIND (objects, it, obj);
    // assert(it != objects.end() && "hrmm... all objects should be in objects map");
    if (it != objects.end()){
      Loc curr_loc;
      obj->get_center(curr_loc.x, curr_loc.y);
      Vec_2D vec(target.x - curr_loc.x, target.y - curr_loc.y);
      
      real8 k = ((real8)obj->get_max_speed()) / vec.length();
      
      vec = vec.multiply(k);
 
      it->second.x = curr_loc.x + vec.x;
      it->second.y = curr_loc.y + vec.y;
    }
  
  }

  sint4 FFLocPath::what_team(const Object *obj){
    const GameObj *p = dynamic_cast<const GameObj*>(obj);
    if (p){
      return *(p->sod.owner);
    }
    else
      return -1;
  }

  void FFLocPath::get_push_target(const Object *obj, Loc &output){
        //const Object *obj = path.obj;
    if (obj->get_shape() != Object::CIRCLE){
      return; //only circle movement implemented -- this also functions as a crude check for buildings
    }
  

    Vec_2D force_vec(0,0);

    FORALL(objects, i){
      const GameObj *gob = dynamic_cast<const GameObj*>(i->first);
      if (!gob){continue;}
      
      //don't consider any object that's not your own as a force that can push you
      if (*(gob->sod.owner) != client_pID){continue;}
      
      if (obj == i->first){
    continue;
      }
      Vec_2D vec;
      if (i->first->get_shape() == Object::CIRCLE){
    Loc obstacle_center;
    if (i->second == Loc(-1, -1)){
      i->first->get_center(obstacle_center.x, obstacle_center.y);
    }
    else{
      obstacle_center = i->second;
    }
    vec = get_point_repulsion(obj, obstacle_center, i->first->get_radius());
    force_vec = force_vec + vec;
      }
      else if (i->first->get_shape() == Object::RECTANGLE){
    Loc u_l, u_r, l_r, l_l; //upper-left, upper-right, lower-right, lower-left
    i->first->get_p1(u_l.x, u_l.y);
    i->first->get_p2(l_r.x, l_r.y);
    u_r.x = l_r.x;
    u_r.y = u_l.y;
      
    l_l.x = u_l.x;
    l_l.y = l_r.y;
      
    Segment seg;
    seg.l1 = u_l;
    seg.l2 = u_r;      
    vec = get_line_repulsion(obj, seg);
    //force_vec = force_vec + vec;

    seg.l1 = u_r;
    seg.l2 = l_r;
    vec = vec + get_line_repulsion(obj, seg);
    //force_vec = force_vec + vec;

    seg.l1 = l_r;
    seg.l2 = l_l;
    vec = vec + get_line_repulsion(obj, seg);
    //force_vec = force_vec + vec;

    seg.l1 = l_l;
    seg.l2 = u_l;
    vec = vec + get_line_repulsion(obj, seg);
    //force_vec = force_vec + vec;
    if (vec == Vec_2D(0,0)){
      vec = vec + get_point_repulsion(obj, u_l);
      vec = vec + get_point_repulsion(obj, u_r);
      vec = vec + get_point_repulsion(obj, l_l);
      vec = vec + get_point_repulsion(obj, l_r);
    }
    force_vec = force_vec + vec;
    
      }
    }
    Vec_2D wall_repulsion;
    FORALL(segment_ends, ends_it){
      ends_it->second = true;
    }
    FORALL(segments, seg_it){
      Vec_2D vec;
      vec = get_line_repulsion(obj, *seg_it);
      if (vec != Vec_2D(0,0)){
    FIND(segment_ends, ends_it, seg_it->l1);
    if (ends_it != segment_ends.end())
      ends_it->second = false;
    FIND(segment_ends, ends_it2, seg_it->l2);
    if (ends_it != segment_ends.end())
      ends_it->second = false;
      }
      wall_repulsion = wall_repulsion + get_line_repulsion(obj, *seg_it);
      // force_vec = force_vec + vec;
    }
    FORALL(segment_ends, endit){
      if (endit->second){
    wall_repulsion = wall_repulsion + get_point_repulsion(obj, endit->first);
      }
    }
    force_vec = force_vec + wall_repulsion;

    if (force_vec != Vec_2D(0,0)){
      Vec_2D rand_vec;
      Vec_2D half_force = Vec_2D(force_vec.x/2, force_vec.y/2);
      
      uint4 rand_angle_int = rand.rand_uint4() % 62;
      real8 rand_angle = ((real8)rand_angle_int)/ 10;
      rand_vec.x = Vec_2D::round(half_force.length() * cos(rand_angle));
      rand_vec.y = Vec_2D::round(half_force.length() * sin(rand_angle));
      
      force_vec = force_vec + rand_vec;
    }

   //  if (force_vec == Vec_2D(0,0)){
//       output = path.path.at(path.current_target);
//       return false;
//     }
    
    Loc object_center;
    obj->get_center(object_center.x, object_center.y);
    
    output.x = object_center.x + force_vec.x;
    output.y = object_center.y + force_vec.y;
  }

  void FFLocPath::get_forcefield_target(const Object *obj, const Loc &attractor, Loc &output){
    //const Object *obj = path.obj;
    assert(obj->get_shape() == Object::CIRCLE && "Only Circle Movement implemented");
  

    Vec_2D force_vec(0,0);

    FORALL(objects, i){
      if (obj == i->first){
    continue;
      }
      Vec_2D vec;
      if (i->first->get_shape() == Object::CIRCLE){
    Loc obstacle_center;
    if (i->second == Loc(-1, -1)){
      i->first->get_center(obstacle_center.x, obstacle_center.y);
    }
    else{
      obstacle_center = i->second;
    }
    vec = get_point_repulsion(obj, obstacle_center, i->first->get_radius());
    force_vec = force_vec + vec;
      }
      else if (i->first->get_shape() == Object::RECTANGLE){
    Loc u_l, u_r, l_r, l_l; //upper-left, upper-right, lower-right, lower-left
    i->first->get_p1(u_l.x, u_l.y);
    i->first->get_p2(l_r.x, l_r.y);
    u_r.x = l_r.x;
    u_r.y = u_l.y;
      
    l_l.x = u_l.x;
    l_l.y = l_r.y;
      
    Segment seg;
    seg.l1 = u_l;
    seg.l2 = u_r;      
    vec = get_line_repulsion(obj, seg);
    //force_vec = force_vec + vec;

    seg.l1 = u_r;
    seg.l2 = l_r;
    vec = vec + get_line_repulsion(obj, seg);
    //force_vec = force_vec + vec;

    seg.l1 = l_r;
    seg.l2 = l_l;
    vec = vec + get_line_repulsion(obj, seg);
    //force_vec = force_vec + vec;

    seg.l1 = l_l;
    seg.l2 = u_l;
    vec = vec + get_line_repulsion(obj, seg);
    //force_vec = force_vec + vec;
    if (vec == Vec_2D(0,0)){
      vec = vec + get_point_repulsion(obj, u_l);
      vec = vec + get_point_repulsion(obj, u_r);
      vec = vec + get_point_repulsion(obj, l_l);
      vec = vec + get_point_repulsion(obj, l_r);
    }
    force_vec = force_vec + vec;
    
      }
    }
    Vec_2D wall_repulsion;
    FORALL(segment_ends, ends_it){
      ends_it->second = true;
    }
    FORALL(segments, seg_it){
      Vec_2D vec;
      vec = get_line_repulsion(obj, *seg_it);
      if (vec != Vec_2D(0,0)){
    FIND(segment_ends, ends_it, seg_it->l1);
    if (ends_it != segment_ends.end())
      ends_it->second = false;
    FIND(segment_ends, ends_it2, seg_it->l2);
    if (ends_it != segment_ends.end())
      ends_it->second = false;
      }
      wall_repulsion = wall_repulsion + get_line_repulsion(obj, *seg_it);
      // force_vec = force_vec + vec;
    }
    FORALL(segment_ends, endit){
      if (endit->second){
    wall_repulsion = wall_repulsion + get_point_repulsion(obj, endit->first);
      }
    }
    force_vec = force_vec + wall_repulsion;
  
   //  if (force_vec == Vec_2D(0,0)){
//       output = path.path.at(path.current_target);
//       return false;
//     }
    
    Vec_2D attract_vec, scaled_attract_vec;
    Loc obj_center;
    obj->get_center(obj_center.x, obj_center.y);
    attract_vec = Vec_2D(obj_center, attractor);
         
    real8 k;
    if (attract_vec.length() < 1){
      k = ATTRACTOR_FORCE;
    }
    else{
      k = ATTRACTOR_FORCE / attract_vec.length();
    }
    scaled_attract_vec = attract_vec.multiply(k); 
  
    force_vec = force_vec + scaled_attract_vec;
  
    restrict_vector(attract_vec, force_vec);

    if (force_vec == Vec_2D(0,0)){
      force_vec = Vec_2D(attract_vec.y, -attract_vec.x);
    }
    
    Loc object_center;
    obj->get_center(object_center.x, object_center.y);
    
    output.x = object_center.x + force_vec.x;
    output.y = object_center.y + force_vec.y;
 
  }

  void FFLocPath::get_forcefield_target(UnitPath_FF &path, Loc &output){
    Loc attractor = get_attractor(path);
    get_forcefield_target(path.obj, attractor, output);
  }
  

  Loc FFLocPath::get_attractor(UnitPath_FF &path){
    Loc object_center, attractor;
    Vec_2D to_wp, scaled_to_wp;
    path.obj->get_center(object_center.x, object_center.y);
    if (path.current_target >= ((sint4)path.path.size()) - 1){
      attractor = object_center;
    }
    else{
      attractor = find_point_on_line(object_center, path.path.at(path.current_target), path.path.at(path.current_target+1));
    }
    to_wp = Vec_2D(path.path.at(path.current_target).x - attractor.x, path.path.at(path.current_target).y - attractor.y);
    real8 offset = path.obj->get_max_speed() * PATH_LOOK_AHEAD;
    //std::cout << "k = " << offset/to_wp.length() << "\n";
    if (to_wp.length() == 0)
      scaled_to_wp = Vec_2D(1,1);
    else
      scaled_to_wp = to_wp.multiply(offset / to_wp.length());
    while (scaled_to_wp.length() > to_wp.length()){
      offset = offset - to_wp.length();
      attractor = path.path.at(path.current_target);
      if (path.current_target == 0){
    scaled_to_wp = Vec_2D(0,0);
    break;
      }
      path.current_target--;
      //std::cout << "new wp: " << path.path.at(path.current_target).x 
      //        << ", " << path.path.at(path.current_target).y << "\n"; 
      to_wp = Vec_2D(path.path.at(path.current_target).x - attractor.x, path.path.at(path.current_target).y - attractor.y);
      scaled_to_wp = to_wp.multiply(offset / to_wp.length());
    }
    
    attractor.x = attractor.x + scaled_to_wp.x;
    attractor.y = attractor.y + scaled_to_wp.y;
    
    return attractor;
  }
  
  Loc FFLocPath::find_point_on_line(Loc loc, Loc line_start, Loc line_end){
    
    Vec_2D line_to_obj, n;
   
    line_to_obj = Vec_2D(loc.x - line_start.x, loc.y - line_start.y);
    n = Vec_2D(line_end.y - line_start.y, -(line_end.x - line_start.x)); //perpendicular to segment
  
    line_to_obj = line_to_obj.project_onto(n);
  
    Loc point_on_line(loc.x - line_to_obj.x, loc.y - line_to_obj.y);
    
    return point_on_line;
 
  }

//   bool FFLocPath::slide_wp(UnitPath_FF &path){
  
//     Loc next_wp, curr_wp, temp_wp;
//     next_wp = path.path.at(path.current_target -1);
//     curr_wp = path.path.at(path.current_target);
//     real8 d = next_wp.distance(curr_wp);
  
//     Vec_2D wp_vector(next_wp.x - curr_wp.x, next_wp.y - curr_wp.y);
  
//     for(uint4 i = 1; ; i++){
//       real8 k = ((real8)(i * path.obj->get_max_speed())) / d;
//       if (k > 1){
//  return false;
//       }
//       Vec_2D scaled_wp_vec = wp_vector.multiply(k);
   
//       temp_wp.x = curr_wp.x + scaled_wp_vec.x;
//       temp_wp.y = curr_wp.y + scaled_wp_vec.y;
//       if (!is_near_waypoint(path.obj, temp_wp))
//  break;
//     }
//     path.path.at(path.current_target) = temp_wp;
//     return true;
//   }

  void FFLocPath::restrict_vector(const Vec_2D &target_vec, Vec_2D &vec){

    real8 dot_prod = vec.dot_prod(target_vec);
    if (dot_prod < 0){
      Vec_2D perpendicular(target_vec.y, -target_vec.x);
      vec = vec.project_onto(perpendicular);
    }
  
  
  }

  Vec_2D FFLocPath::get_point_repulsion(const Object *moving_obj, const Loc &point, sint4 radius){
    Vec_2D vec;
    Loc center;
    moving_obj->get_center(center.x, center.y);
    real8 d = center.distance(point) - moving_obj->get_radius() - radius +1;
  
    if (d < moving_obj->get_max_speed() * FIELD_RANGE_MULTIPLIER){
      vec.x = center.x - point.x;
      vec.y = center.y - point.y;
      real8 k = REPULSOR_BASELINE / (d * d * vec.length());
      vec = vec.multiply(k);
    }
    return vec;
  }
#if 0
  Vec_2D FFLocPath::get_line_repulsion(const Object *moving_obj, const Segment &seg){
    Loc q, p;
    Vec_2D line_to_obj, n;
    moving_obj->get_center(q.x, q.y);
    p = seg.l1;
    line_to_obj = Vec_2D(q.x - p.x, q.y - p.y);
    n = Vec_2D(seg.l1.y - seg.l2.y, -(seg.l1.x - seg.l2.x)); //perpendicular to segment
  
    line_to_obj = line_to_obj.project_onto(n);
  
    Loc point_on_line(q.x - line_to_obj.x, q.y - line_to_obj.y);
    Vec_2D to_l1, to_l2;
  
    to_l1 = Vec_2D(seg.l1.x - point_on_line.x, seg.l1.y - point_on_line.y);
    to_l2 = Vec_2D(seg.l2.x - point_on_line.x, seg.l2.y - point_on_line.y);
  
    if (to_l1.dot_prod(to_l2) > 0){ //if both vectors are pointing in the same direction
    //   if(to_l1.length() > to_l2.length()){
//  return get_point_repulsion(moving_obj, seg.l2);
//       }
//       else{
//  return get_point_repulsion(moving_obj, seg.l1);
//       }
    }
    else{
      real8 d = q.distance(point_on_line) - moving_obj->get_radius() +1;
      if (d < moving_obj->get_max_speed() * FIELD_RANGE_MULTIPLIER){
    real8 k = REPULSOR_BASELINE / (d * d * line_to_obj.length());
    return line_to_obj.multiply(k);
      }
    }
    return Vec_2D(0,0);
  }
#endif

Vec_2D FFLocPath::get_line_repulsion(const Object *moving_obj, const Segment &seg)
  {
    Loc loc;
    moving_obj->get_center(loc.x, loc.y);
    const Loc &line_start = seg.l1;
    const Loc &line_end = seg.l2;

    sint4 line_x = line_end.x - line_start.x;
    sint4 line_y = line_end.y - line_start.y;

    sint4 v_x = loc.x - line_start.x;
    sint4 v_y = loc.y - line_start.y;

    sint4 a = line_x * v_x + line_y * v_y;
    sint4 b = line_x * line_x + line_y * line_y;

    Loc proj(line_start.x + line_x * a / b, line_start.y + line_y * a / b);

    Vec_2D line_to_obj(loc.x - proj.x, loc.y - proj.y);
    Vec_2D to_l1, to_l2;

    to_l1 = Vec_2D(seg.l1.x - proj.x, seg.l1.y - proj.y);
    to_l2 = Vec_2D(seg.l2.x - proj.x, seg.l2.y - proj.y);

    if (to_l1.dot_prod(to_l2) > 0){ //if both vectors are pointing in the same direction
    //   if(to_l1.length() > to_l2.length()){
//  return get_point_repulsion(moving_obj, seg.l2);
//       }
//       else{
//  return get_point_repulsion(moving_obj, seg.l1);
//       }
    }
    else{
      real8 d = loc.distance(proj) - moving_obj->get_radius() +1;
      if (d < moving_obj->get_max_speed() * FIELD_RANGE_MULTIPLIER /2){
        real8 k = REPULSOR_BASELINE /  (d * d * line_to_obj.length());
    return line_to_obj.multiply(k);
      }
    }
    return Vec_2D(0,0);

    /*
    Loc c;
    moving_obj->get_center(c.x, c.y);

    sint4 ux = seg.l2.x - seg.l1.x;
    sint4 uy = seg.l2.y - seg.l1.y;
    sint4 vx = c.x - seg.l1.x;
    sint4 vy = c.y - seg.l1.y;

    sint4 u_v = ux * vx + uy * vy; // u dot v
    if (u_v <= 0) return Vec_2D(0,0);

    sint4 u_u = ux * ux + uy * uy; // u dot u
    if (u_u <= u_v) return Vec_2D(0,0);

    real8 b = real8(u_v) / real8(u_u);

    Loc proj((sint4)(seg.l1.x+b*ux), (sint4)(seg.l1.y+b*uy)); // projection to the segment

    real8 d1 = c.distance(proj);
    real8 d2 = d1 - moving_obj->get_radius() +1;

    if (d2 < moving_obj->get_max_speed() * FIELD_RANGE_MULTIPLIER) {
      real8 k = REPULSOR_BASELINE / (d2 * d2 * d1);
      Vec_2D vec(c.x - proj.x, c.y - proj.y);
      return vec.multiply(k);
    }
    return Vec_2D(0,0);
    */
  }

  //-------------------------------------------------------------------------

  bool FFLocPath::plan_tasks(uint4 max_time)
  {
    assert(pfEngine!=NULL);
    sint4 stop_time = 0;
    if(max_time > 0){
      stop_time = (sint4)SDL_GetTicks() + max_time;
    }

    // cout << "Chance To Plan Task\n" ;

    // try to plan one failed and one pending tasks
    bool did_something = plan_failed_task();
    bool did_some_more = plan_pending_task();
    sint4 counter = 0;
    //Continue planning until we have reached the max_time
    while( (sint4)SDL_GetTicks() < stop_time && (!pendingTasks.empty() || !failedTasks.empty() )) {
      if(counter++ % 2 == 0){
    //did_something = plan_failed_task(stop_time);
    did_something = plan_failed_task();
      }
      else{
    //did_some_more = plan_pending_task(stop_time);
    did_some_more = plan_pending_task();
      }
      did_some_more = did_something || did_some_more; // careful not to short-circuit!
    }



    did_something = did_some_more || did_something;
    return did_something;


  }

  //----------------------------------------------------------------------------------
bool FFLocPath::plan_pending_task()
  {
    if (pendingTasks.empty()) return false;

    TIter tit = pendingTasks.front(); pendingTasks.pop_front();
    // mark the task as handled
    tit->isPending = false;

    // if the object is moving, stop it first and come back to it later
    assert(tit->task.objs.size() > 0);

//     bool moving = false;
//     FORALL(tit->task.objs, it){
//       if ((*it)->get_speed()>0 || (*it)->get_moving()){
//  moving = true;
//  break;
//       }
//     }

//     if (moving) {
//       FORALL(tit->task.objs, it){
//  objsToStop.insert(*it);
//       }
//       pendingTasks.push_back(tit);
//       return false;
//       // fixme: might be better to try another pending task already in this tick...
//     }

    // plan
    TEST(cout << obj << " planning pending path" << endl);
    plan_task(tit);
    return true;
  }

  //-------------------------------------------------------------------
  struct FindFailed {
    bool operator()(FF_Task &task) { return task.isFailed && !task.noPathFound; };
  };

  //-------------------------------------------------------------------
  FFLocPath::TIter FFLocPath::get_random_failed_task(void)
  {
    // find a failed path for which we don't yet know that 'noPathFound'
    // (ie we still didn't try to replan and found that there is no path)
    if( tasks.empty() )
      return tasks.end();

    // start the search with in a random position to avoid planning the same
    // path repeatedly (in case it is immediately marked as failed again).
    int skip = rand(tasks.size());
    TIter start_at = tasks.begin();
    advance(start_at, skip);

    // search from the middle to the end
    TIter tit = find_if(start_at, tasks.end(), FindFailed());
    // if found
    if( tit != tasks.end() )
      return tit;
    // else, search from start to middle
    tit = find_if(tasks.begin(), start_at, FindFailed());
    return (tit != start_at)? tit : tasks.end();
  }
  //-------------------------------------------------------------------------------

  //----------------------------------------------------------------------------------
  bool FFLocPath::plan_failed_task()
  {
    if (failedTasks.empty()) return false;

    TIter tit = failedTasks.front(); failedTasks.pop_front();

    // if the object is moving, stop it first and come back to it later
    assert(tit->task.objs.size() > 0);

   //  bool moving = false;
//     FORALL(tit->task.objs, it){

//  if ((*it)->get_speed()>0 || (*it)->get_moving()){
//  moving = true;
//  break;
//       }


//     }

//     if (moving) {
//       FORALL(tit->task.objs, it){
//  objsToStop.insert(*it);
//       }
//       failedTasks.push_back(tit);
//       return true;
//       // fixme: might be better to try another pending task already in this tick...
//     }

    // plan
    TEST(cout << obj << " planning pending path" << endl);
    plan_task(tit);
    tit->replanned++;

    return true;
  }

  //-------------------------------------------------------------------
//   bool ST_ForceField::plan_failed_task(void)
//   {
//     assert(pfEngine!=NULL);

//     // get a failed task
//     TIter tit = get_random_failed_task();
//     if( tit == tasks.end() )
//       return false;

//     // if the object is moving, stop it first and come back to it later
//     assert(tit->task.objs.size() > 0);
//     const Object *obj = *(tit->task.objs.begin());

//     if( obj->get_speed() > 0 || obj->get_moving() ) {
//       objsToStop.insert(obj);
//       return false;
//       // fixme: might be better to try another failed task already in this tick...
//     }
//     tit->replanned++;
//     // find-path
//     TEST(cout << obj << " replanning path" << endl);
//     plan_task(tit);
//     return true;
//   }

  //-------------------------------------------------------------------
  void FFLocPath::plan_task(TIter tit)
  {
    Path newpath;
    bool found;

    set<const Object*>::iterator obj_it = tit->task.objs.begin();

    // Object to move
    const Object *obj = *obj_it;


    // obj_it++;
    //remove the members of the group from the representation
  //   for (; obj_it != tit->task.objs.end(); obj_it++){
//       tit->followers.push_back(*obj_it);
//       pfEngine->remove_object(*obj_it);
//     }
//    cout << "CALLING GLOBAL PATHFINDER\n";
    if (tit->task.goal.target == Goal::LOCATION) {
      found = pfEngine->find_path(obj, tit->task.goal.loc, newpath, IGNORE_ALL_MOBILE_OBJS);
      if (found){
    newpath.locs[0] = tit->task.goal.loc;
      }
    } else {
      assert (tit->task.goal.target == Goal::OBJ);
      assert (!pushing_on ||
          what_team(tit->task.goal.obj) != client_pID ||
          tit->task.goal.mode != Goal::TOUCH ||
          tit->task.goal.obj->get_max_speed() <= 0); //because you'll push the object out of the way
      cout << "--------asking for path to obj-------\n";
      found = pfEngine->find_path(obj, tit->task.goal.obj, newpath, IGNORE_ALL_MOBILE_OBJS);
    }

    //add the members of the group back into the representation
 //    obj_it = tit->task.objs.begin();
//     obj_it++;
//     for (; obj_it != tit->task.objs.end(); obj_it++){
//       pfEngine->insert_object(*obj_it);
//     }

    // insert results into data structures
    if( found ) {
      assert(newpath.locs.size() > 0);
      tit->set_path(obj, newpath.locs);
      tit->isFailed = false;
    }
    else {
      cout << "WTF!!?! NO Path Found\n";
      // fixme: maybe add to "failed"/sleep etc. ?
      tit->noPathFound = true;
    }
  }

  //-------------------------------------------------------------------

  bool FFLocPath::is_at_location(const Object* obj, const Loc& target)
  {
    Loc position; obj->get_center(position.x, position.y);
    return position.distance(target) <= obj->get_speed();
  }

  //-------------------------------------------------------------------
  
  bool FFLocPath::is_near_location(const Object *obj, const Loc& target, sint4 range){
    Loc position; obj->get_center(position.x, position.y);
    return position.distance(target) <= range;
  }

  //-------------------------------------------------------------------
  bool FFLocPath::is_at_goal(const Object* obj, const Task& task)
  {
    // Getting to the goal depends on the definition of the task:
    if (task.goal.target == Goal::OBJ ) {
      switch(task.goal.mode) {
      case Goal::TOUCH :
    return( obj->distance_to(*task.goal.obj) <= 1 );
    // using 1-tick look-ahead:
    //return( obj->distance_to(*task.goal_obj) <= obj->get_speed() );
    break;

      case Goal::VICINITY :
    
    return (obj->distance_to(*task.goal.obj) <= task.goal.distance);
    //assert(0 && "Not implemented yet");
    break;

      case Goal::ATTACK :

    return(is_in_range(obj, task.goal.obj));
             
    break;

      default:
    ERR("Unknown mode");
      }
    } else if (task.goal.target == Goal::LOCATION ) {
      switch(task.goal.mode) {
      case Goal::TOUCH :
    return is_at_location(obj, task.goal.loc);
    break;

      case Goal::VICINITY :
    return is_near_location(obj, task.goal.loc, task.goal.distance);
    //assert(0 && "Not implemented yet");
    break;

      case Goal::ATTACK :
    return(is_in_range(obj, task.goal.loc));
    break;

      default:
    ERR("Unknown mode");
      }
    } else {
      ERR("Unknown target type");
    }
    return false;
  }
  

  //-------------------------------------------------------------------

  bool FFLocPath::is_in_range(const Object *moving_obj, const Loc goal_loc){
    //fixme: does not check if unit can see the target. 
    const GameObj *gob = dynamic_cast<const GameObj*>(moving_obj);
    sint4 range = 1;
    if (gob){
      ScriptObj *weapon = gob->component("weapon");
      const sintptr *r = weapon->get_const_int_ptr("max_ground_range");
      if (r){
    range = *r;
      }
    }
    sint4 x,y;
    moving_obj->get_center(x,y);
    Loc obj(x,y);
  
    return ( obj.distance(goal_loc) <= range );
    
    // this is the check in ShooterAI. Other way makes more intuitive sense though...
    //     real8 rdist = moving_obj->distance_to(*goal_obj);
    //     sint4 sdist = (sint4)floor(rdist);
    //     return (sdist <= range);
  }

  bool FFLocPath::is_in_range(const Object *moving_obj, const Object *goal_obj){
    //fixme: does not check if unit can see each other. 
    const GameObj *gob = dynamic_cast<const GameObj*>(moving_obj);
    sint4 range = 1;
    if (gob){
      ScriptObj *weapon = gob->component("weapon");
      const sintptr *r = weapon->get_const_int_ptr("max_ground_range");
      if (r){
    range = *r;
      }
    }
  
    //if not a GameObj or with no weapon range, should behave exactly
    //like the check for touch.
    return ( moving_obj->distance_to(*goal_obj) <= range );
    
    // this is the check in ShooterAI. Other way makes more intuitive sense though...
    //     real8 rdist = moving_obj->distance_to(*goal_obj);
    //     sint4 sdist = (sint4)floor(rdist);
    //     return (sdist <= range);
  }


  //-----------------------------------------------------------------------
  //------------------------Vec_2D-----------------------------------------
  //-----------------------------------------------------------------------
  Vec_2D Vec_2D::operator+(const Vec_2D &other) const{
    return Vec_2D(x + other.x, y + other.y);
  }
 
  Vec_2D Vec_2D::operator-(const Vec_2D &other) const{
    return Vec_2D(x - other.x, y - other.y);
  }

  sint4 Vec_2D::dot_prod(const Vec_2D &other) const{
    return x*other.x + y*other.y;
  }

  real8 Vec_2D::length() const{
    real8 length = sqrt(((real8)x)*x + y*y);
    return length;
  }

  Vec_2D Vec_2D::project_onto(const Vec_2D &other) const{
    real8 k = ((real8)this->dot_prod(other)) / other.dot_prod(other);
    return other.multiply(k);
  }

  Vec_2D Vec_2D::multiply(real8 k) const{
    return Vec_2D(round(k * x), round(k * y));
  }

  bool Vec_2D::operator==(const Vec_2D &other) const{
    return (x == other.x && y == other.y); 
  }
  
  bool Vec_2D::operator!=(const Vec_2D &other) const{
    return !(*this == other); 
  }
  sint4 Vec_2D::round(real8 real){
    real8 i;
    i = floor(real + 0.5);
    return (sint4)i;
  }

  bool Loc_less::operator() (Loc a, Loc b) const {
    if (a.x < b.x){
      return true;
    }
    else if (a.x == b.x && a.y < b.y){
      return true;
    }
    return false;
  }




  //----------------------------------------------------------------------------------

#define EPS 1e-9f


//   UnitPath_FF::UnitPath_FF(const Object *obj_, const Vector<Loc> &path_)
//     : obj(obj_), path(path_), no_orders_yet(true), 
//       stopped_to_avoid_collision(false), collision_step(0), wakeup(0)
//   {
//     current_target = path.size()-1;
//     if (path.size() > 0) {
//       goal = path[0];
//     }
//     spot = Loc(-1, -1);
//   }


//   bool UnitPath_FF::is_intersect(const Segment &segment) const
//   {
//     typeof(path.begin()) pj = path.begin();
//     typeof(path.begin()) pi = pj++;
//     for( ; pj != path.end() ; pi++, pj++ ) {
//       if (Segment(*pi, *pj).touches(segment) )
//         return true;
//     }
//     return false;
//   }

  //----------------------------------------------------------------------------------
  //----------------------------------------------------------------------------------

  void FF_Task::set_path(const Object *obj_, const Vector<Loc> &path_)
  {
    // overwrite an existing path, retaining the collision-monitoring data.
    if( path ) {
      Loc   saved_spot = path->spot;
      int   saved_collision_step = path->collision_step;
      bool  saved_stopped_to_avoid_collision = path->stopped_to_avoid_collision;
      uint4 saved_wakeup = path->wakeup;

      path = boost::shared_ptr<UnitPath_FF>(new UnitPath_FF(obj_, path_));

      path->spot = saved_spot;
      path->collision_step = saved_collision_step;
      path->stopped_to_avoid_collision = saved_stopped_to_avoid_collision;
      path->wakeup = saved_wakeup;
    } 
    // No previous path was planned, so just create a new one.
    else {
       path = boost::shared_ptr<UnitPath_FF>(new UnitPath_FF(obj_, path_));
    }
  }



  //----------------------------------------------------------------------------------