ST_ForceField.C

Go to the documentation of this file.

#include "Global.H"
#include "ST_ForceField.H"
#include "ST_PFEngine.H"
#include "Options.H"
#include "GameObj.H"
#include "LinAlg.H"

#include <utility>
#include <iterator>
#include <math.h>



using namespace std;

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

namespace SimpleTerrain {

  void ST_ForceField::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");
    o.put("-wpattract", "units are attracted to wps, and not forced to stay on the path");
    o.put("-nostoptoplan", "units do not stop to plan tasks");

  }

  static void __assert_object_set_valid(const std::set<const Object *> & objs)
  {
#ifndef NDEBUG    
    FORALL (objs, it) {
      const GameObj * obj = dynamic_cast<const GameObj *>( (*it) );
      assert( obj != NULL );
    }
#else
    objs.empty(); // use objs to avoid warning
#endif

#if 0  // old    
    std::set<const Object *>::const_iterator it;
    for(it = objs.begin(); it != objs.end(); it++) {
      const GameObj * obj = dynamic_cast<const GameObj *>( (*it) );
      assert( obj != NULL );
    }
#endif    
  }
    
  //-----------------------------------------------------------------------------
  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);
  }

  ST_ForceField::ST_ForceField() {
    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("-wpattract", wp_attract);
    ::Options::get("-nostoptoplan", stop_to_plan);
    stop_to_plan = !stop_to_plan;
    
    cout << "STOP TO PLAN: " << stop_to_plan << endl;
  }

  //-----------------------------------------------------------------------------
  void ST_ForceField::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<ST_Terrain::PFEngine>(new ST_Terrain::PFEngine(tiles_x_, tiles_y_, tile_points_, levels_of_detail));

    //Add the boundaries of the map to the list of segments for the forcefields
    
    Loc topLeft(0,0);
    Loc bottomRight(tiles_x_ * tile_points_, tiles_y_ * tile_points_);
    Loc bottomLeft(topLeft.x, bottomRight.y);
    Loc topRight(bottomRight.x, topLeft.y);

    segments.push_back(Segment(topLeft, topRight));
    segments.push_back(Segment(topLeft, bottomLeft));
    segments.push_back(Segment(bottomLeft, bottomRight));
    segments.push_back(Segment(topRight, bottomRight));
    
    segment_ends[topLeft] = true;
    segment_ends[bottomRight] = true;
    segment_ends[topRight] = true;
    segment_ends[bottomLeft] = true;

    // initialize motion squares
    /*    if(makesquares){ // temporarily commented to prevent unused var warning -- dd
      sint4 area = tiles_x_ * tiles_y_;
      // sectors = new vector<Sector>(area);
      }*/
  }

    bool is_obj_static(const Object * obj)
    {
        return (obj->get_max_speed() == 0);
    }
    
  //-----------------------------------------------------------------------------
  void ST_ForceField::add_obj(const Object *obj)
  {
    TerrainBasicImp<FF_Task>::add_obj(obj);
    assert(pfEngine!=NULL);
    if(is_obj_static(obj))
    {
        pfEngine->insert_object(obj);
    }

    objects[obj] = Loc(-1,-1);
  }

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

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

    // remove from world representation
    assert(pfEngine!=NULL);
    if(is_obj_static(obj))
    {
        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 ST_ForceField::add_segments(const Vector<Segment> &segs)
  {
    TerrainBasicImp<FF_Task>::add_segments(segs);
    assert(pfEngine!=NULL);
    FORALL(segs,i){
      pfEngine->insert_boundary(*i);
      segments.push_back(*i);
      segment_ends[i->l1] = true;
      segment_ends[i->l2] = true;
    }
    invalidate_paths(segs);

  }

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

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

  //---------------------------------------------------------------------------
  real8 ST_ForceField::find_path(const Loc &l1, const Loc &l2, sint4 radius, Path * path, ConsiderObjects consider)
  {
    Path result;
    bool found;
    found = pfEngine->find_path(l1, l2, radius, result, 0, 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, l2);
    //    assert(0); // unimplemented yet    
    //return -1;
  }


  //----------------------------------------------------------------------------------
  real8 ST_ForceField::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, 0, consider);
    } else {
      found = pfEngine->find_path(obj, goal.obj, result, 0, 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 ST_ForceField::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 ST_ForceField::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.
        __assert_object_set_valid(i->task.objs);

    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() ) {
        __assert_object_set_valid(i->task.objs);
    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 > REPLAN_TRIGGER){
          if (i->replanned < REPLAN_HARDCAP){ //fixed cap on number of replannings
        i->isFailed = true;
        failedTasks.push_back(i);
          }
        }
        if (cur_path.collision_step > OCCILATION_TRIGGER){
          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);

        // BEGIN CHANGES: ADD THIS CODE
        if(i->task.goal.target == Goal::LOCATION)
      {
            add_move_command(obj, i->task.goal.loc, cmds);
      }
        // END CHANGES
      }
      //     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();
  }

  // BEGIN CHANGES (Replace old function with this one)
  void ST_ForceField::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;
    }

    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 length = vec.length();
    sint4 speed = std::min(obj->get_max_speed(), static_cast<sint4>(length));

    TerrainBase::MoveCmd m;
    m.obj = obj;
    m.next_loc = target;
    m.speed = 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()){
      real8 k = static_cast<real8>(speed) / length;

      vec = vec.multiply(k);

      it->second.x = curr_loc.x + vec.x;
      it->second.y = curr_loc.y + vec.y;
    }

  }
  // END CHANGES

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

  void ST_ForceField::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 ST_ForceField::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 ST_ForceField::get_forcefield_target(UnitPath_FF &path, Loc &output){
    Loc attractor = get_attractor(path);
    get_forcefield_target(path.obj, attractor, output);
  }


  Loc ST_ForceField::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());

    //If wp_attract is set, set the attractor to the wp unless you're near it.
    if (wp_attract && scaled_to_wp.length() <= to_wp.length()){
      return path.path.at(path.current_target);
    }
    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;
  }

#if 0
  Loc ST_ForceField::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;
  }
#else
  Loc ST_ForceField::find_point_on_line(Loc loc, Loc line_start, Loc line_end)
  {
    sint4 line_x = line_end.x - line_start.x;
    sint4 line_y = line_end.y - line_start.y;

    if (line_x == 0 && line_y == 0) return loc;

    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);

    return proj;
  }
#endif

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

  //   bool ST_ForceField::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 ST_ForceField::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 ST_ForceField::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 ST_ForceField::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);
  }
#else
  Vec_2D ST_ForceField::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);
    */
  }
#endif

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

  bool ST_ForceField::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(0);//stop_time);
    bool did_some_more = plan_pending_task(0);//stop_time);
    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);
      else
    did_some_more = plan_pending_task(stop_time);


      did_some_more = did_something || did_some_more; // careful not to short-circuit!
    }



    did_something = did_some_more || did_something;
    return did_something;


  }

  //----------------------------------------------------------------------------------
  bool ST_ForceField::plan_pending_task(uint4 stop_time)
  {
    if (pendingTasks.empty()) return false;
      
    TIter tit = pendingTasks.front(); pendingTasks.pop_front();

    // EXPLANATION: There is currently an unsolved bug which seems to
    // push invalid TIters onto the pendingTasks queue. In order to guard
    // against this phenomena, we are going to do some basic sanity
    // checking on the Task. WARNING: This is a TEMPORARY measure. We
    // should hunt for the source of the bug ASAP, and not be content
    // with being able to detect it.
    TIter it;
    for(it = tasks.begin(); it != tasks.end(); it++)
      {           
    if( &(*it) == &(*tit) )
      {
        break;
      }
      }
    if( it == tasks.end() )
      {
    std::cerr << "[CRITICAL WARNING] Segfault avoided in ";
    std::cerr << "ST_ForceField::plan_pending_task(void)" << std::endl;
    return false;
      }

    // mark the task as handled
    tit->isPending = false;

    __assert_object_set_valid(tit->task.objs);

    if (tit->task.objs.size() <= 0)
      {
    //ERR("No Objects");
    cerr << "Invalid Task: no Objects in task\n";
    clean_task((TerrainBase::TaskId) &tit);
      }

    // if the object is moving, stop it first and come back to it later
    if (stop_to_plan)
      {
    bool moving = false;
    std::set<const Object*>::const_iterator it;
    for(it = tit->task.objs.begin(); it != tit->task.objs.end(); it++)
      {
        // There is a known segfault somewhere around here
        const Object * obj = (*it);
        const GameObj * p = dynamic_cast<const GameObj*>(obj);
        if (p == NULL)
          {
        ERR("Object not a GameObj"); // TODO: Check
          }
        GameObj * p2 = const_cast<GameObj*>(p);
        GameObj::check_ptr(p2);
        if (!GameObj::get_obj_id(p2))
          {
        ERR("OBJECT IS NOT THERE...\n");
          }
        if (obj->get_speed() > 0 || obj->get_moving())
          {
        moving = true;
        break;
          }
      }
            
    if (moving) 
      {
        __assert_object_set_valid(tit->task.objs);
        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, stop_time);
    return true;
  }

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

  //-------------------------------------------------------------------
  ST_ForceField::TIter ST_ForceField::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 ST_ForceField::plan_failed_task(uint4 stop_time)
  {
    if (failedTasks.empty()) return false;

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

    // EXPLANATION: There is currently an unsolved bug which seems to
    // push invalid TIters onto the failedTasks queue. In order to guard
    // against this phenomena, we are going to do some basic sanity
    // checking on the Task. WARNING: This is a TEMPORARY measure. We
    // should hunt for the source of the bug ASAP, and not be content
    // with being able to detect it.
    TIter it;
    for(it = tasks.begin(); it != tasks.end(); it++)
      {           
    if( &(*it) == &(*tit) )
      {
        break;
      }
      }
    if( it == tasks.end() )
      {
    std::cerr << "[CRITICAL WARNING] Segfault avoided in ";
    std::cerr << "ST_ForceField::plan_failed_task(void)" << std::endl;
    return false;
      }

    // if the object is moving, stop it first and come back to it later

    __assert_object_set_valid(tit->task.objs);
    assert(tit->task.objs.size() > 0);

    if (stop_to_plan){
      
      bool moving = false;
      __assert_object_set_valid(tit->task.objs);
      FORALL(tit->task.objs, it){
    
    if ((*it)->get_speed()>0 || (*it)->get_moving()){
      moving = true;
      break;
    }
    
    
      }
      
      if (moving) {
        __assert_object_set_valid(tit->task.objs);
    FORALL(tit->task.objs, it){
      objsToStop.insert(*it);
    }
    failedTasks.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, stop_time);
    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 ST_ForceField::plan_task(TIter tit, uint4 stop_time)
  {
    Path newpath;
    bool found;

    __assert_object_set_valid(tit->task.objs);
    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, 0, IGNORE_ALL_MOBILE_OBJS, stop_time);
      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
      found = pfEngine->find_path(obj, tit->task.goal.obj, newpath, 0, IGNORE_ALL_MOBILE_OBJS, stop_time);
    }

    //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 {
      // fixme: maybe add to "failed"/sleep etc. ?
      tit->noPathFound = true;
    }
  }

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

  // BEGIN CHANGES
  bool ST_ForceField::is_at_location(const Object* obj, const Loc& target)
  {
    Loc position; obj->get_center(position.x, position.y);

    return position.distance(target) <= obj->get_max_speed();
  }
  // END CHANGES

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

  bool ST_ForceField::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 ST_ForceField::is_at_goal(const Object* obj, const Task& task)
  {

    typeof(obj2taskId.begin()) o2t;
    // 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 :

        // using 1-tick look-ahead; also suffers from low-resolution accuracy problems
    // hack: low-resolution problems overcome by setting path[0] to the goal loc during plan_task method
    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 ST_ForceField::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_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 ST_ForceField::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_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);
  }

  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_));
    }
  }



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

} //end of namespace