<commit_before>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2006 Artem Pavlenko
 * Copyright (C) 2006 10East Corp.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

//$Id$

//stl
#include <string>
#include <vector>

// boost
#include <boost/shared_ptr.hpp>
#include <boost/utility.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <boost/thread/mutex.hpp>

//mapnik
#include <mapnik/geometry.hpp>
#include <mapnik/placement_finder.hpp>
#include <mapnik/text_path.hpp>

namespace mapnik
{
  placement::placement(string_info *info_, CoordTransform *ctrans_, const proj_transform *proj_trans_, geometry_ptr geom_, std::pair<double, double> dimensions_)
    : info(info_), ctrans(ctrans_), proj_trans(proj_trans_), geom(geom_), label_placement(point_placement), dimensions(dimensions_), has_dimensions(true), shape_path(*ctrans_, *geom_, *proj_trans_), total_distance_(-1.0)
  {
  }

  //For text
  placement::placement(string_info *info_, CoordTransform *ctrans_, const proj_transform *proj_trans_, geometry_ptr geom_, label_placement_e placement_)
    : info(info_), ctrans(ctrans_), proj_trans(proj_trans_), geom(geom_), label_placement(placement_), has_dimensions(false), shape_path(*ctrans_, *geom_, *proj_trans_), total_distance_(-1.0)
  {
  }

  placement::~placement()
  {
  }

  std::pair<double, double> placement::get_position_at_distance(double target_distance)
  {
    double old_x, old_y, new_x, new_y;
    double x, y;
    x = y = 0.0;

    double distance = 0.0;

    shape_path.rewind(0);
    shape_path.vertex(&new_x,&new_y);
    for (unsigned i = 0; i < geom->num_points() - 1; i++)
    {
      double dx, dy;

      old_x = new_x;
      old_y = new_y;

      shape_path.vertex(&new_x,&new_y);

      dx = new_x - old_x;
      dy = new_y - old_y;

      double segment_length = sqrt(dx*dx + dy*dy);

      distance += segment_length;
      if (distance > target_distance)
      {
          x = new_x - dx*(distance - target_distance)/segment_length;
          y = new_y - dy*(distance - target_distance)/segment_length;

          break;
      }
    }

    return std::pair<double, double>(x, y);
  }

  double placement::get_total_distance()
  {
    if (total_distance_ < 0.0)
    {
      double old_x, old_y, new_x, new_y;

      shape_path.rewind(0);

      shape_path.vertex(&old_x,&old_y);

      total_distance_ = 0.0;

      for (unsigned i = 0; i < geom->num_points() - 1; i++)
      {
          double dx, dy;

          shape_path.vertex(&new_x,&new_y);

          dx = new_x - old_x;
          dy = new_y - old_y;

          total_distance_ += sqrt(dx*dx + dy*dy);

          old_x = new_x;
          old_y = new_y;
      }
    }

    return total_distance_;
  }

  void placement::clear_envelopes()
  {
    while (!envelopes.empty())
      envelopes.pop();
  }

  placement_finder::placement_finder(Envelope<double> e)
    : detector_(e)
  {
  }

  bool placement_finder::find_placement(placement *p)
  {
    if (p->label_placement == point_placement)
    {
      return find_placement_horizontal(p);
    }
    else if (p->label_placement == line_placement)
    {
      return find_placement_follow(p);
    }

    return false;
  }

  bool placement_finder::find_placement_follow(placement *p)
  {
    std::pair<double, double> string_dimensions = p->info->get_dimensions();
    double string_width = string_dimensions.first;
//    double string_height = string_dimensions.second;

    double distance = p->get_total_distance();

    //~ double delta = string_width/distance;
    double delta = distance/100.0;

    for (double i = 0; i < (distance - string_width)/2.0; i += delta)
    {
      p->clear_envelopes();

      if ( build_path_follow(p, (distance - string_width)/2.0 + i) ) {
        update_detector(p);
        return true;
      }

      p->clear_envelopes();

      if ( build_path_follow(p, (distance - string_width)/2.0 - i) ) {
        update_detector(p);
        return true;
      }
    }

    p->starting_x = 0;
    p->starting_y = 0;

    return false;
  }

  bool placement_finder::find_placement_horizontal(placement *p)
  {
    double distance = p->get_total_distance();
    //~ double delta = string_width/distance;
    double delta = distance/100.0;

    for (double i = 0; i < distance/2.0; i += delta)
    {
      p->clear_envelopes();

      if ( build_path_horizontal(p, distance/2.0 + i) ) {
        update_detector(p);
        return true;
      }

      p->clear_envelopes();

      if ( build_path_horizontal(p, distance/2.0 - i) ) {
        update_detector(p);
        return true;
      }
    }

    p->starting_x = 0;
    p->starting_y = 0;

    return false;
  }

  void placement_finder::update_detector(placement *p)
  {
    while (!p->envelopes.empty())
    {
      Envelope<double> e = p->envelopes.front();

      detector_.insert(e);

      p->envelopes.pop();
    }
  }

  bool placement_finder::build_path_follow(placement *p, double target_distance)
  {
    double new_x, new_y, old_x, old_y;
    unsigned cur_node = 0;

    double angle = 0.0;
    int orientation = 0;

    p->path.clear();

    double x, y;
    x = y = 0.0;

    double distance = 0.0;

    std::pair<double, double> string_dimensions = p->info->get_dimensions();
//    double string_width = string_dimensions.first;
    double string_height = string_dimensions.second;

    p->shape_path.rewind(0);
    p->shape_path.vertex(&new_x,&new_y);
    for (unsigned i = 0; i < p->geom->num_points() - 1; i++)
    {
        double dx, dy;

        cur_node++;

        old_x = new_x;
        old_y = new_y;

        p->shape_path.vertex(&new_x,&new_y);

        dx = new_x - old_x;
        dy = new_y - old_y;

        double segment_length = sqrt(dx*dx + dy*dy);

        distance += segment_length;
        if (distance > target_distance)
        {
            p->starting_x = new_x - dx*(distance - target_distance)/segment_length;
            p->starting_y = new_y - dy*(distance - target_distance)/segment_length;

            angle = atan2(-dy, dx);

            if (angle > M_PI/2 || angle <= -M_PI/2) {
              orientation = -1;
            }
            else {
              orientation = 1;
            }

            distance -= target_distance;

            break;
        }
    }

    for (unsigned i = 0; i < p->info->num_characters(); i++)
    {
        character_info ci;
        unsigned c;

        while (distance <= 0) {
            double dx, dy;

            cur_node++;

            if (cur_node >= p->geom->num_points()) {
              break;
            }

            old_x = new_x;
            old_y = new_y;

            p->shape_path.vertex(&new_x,&new_y);

            dx = new_x - old_x;
            dy = new_y - old_y;

            angle = atan2(-dy, dx );

            distance += sqrt(dx*dx+dy*dy);
        }

        if (orientation == -1) {
            ci = p->info->at(p->info->num_characters() - i - 1);
        }
        else
        {
            ci = p->info->at(i);
        }
        c = ci.character;

        Envelope<double> e;
        if (p->has_dimensions)
        {
            e.init(x, y, x + p->dimensions.first, y + p->dimensions.second);
        }

        if (orientation == -1) {
            x = new_x - (distance - ci.width)*cos(angle);
            y = new_y + (distance - ci.width)*sin(angle);

            //Center the text on the line.
            x += (((double)string_height/2.0) - 1.0)*cos(angle+M_PI/2);
            y -= (((double)string_height/2.0) - 1.0)*sin(angle+M_PI/2);

            if (!p->has_dimensions)
            {
              e.init(x, y, x + ci.width*cos(angle+M_PI), y - ci.width*sin(angle+M_PI));
              e.expand_to_include(x - ci.height*sin(angle+M_PI), y - ci.height*cos(angle+M_PI));
              e.expand_to_include(x + (ci.width*cos(angle+M_PI) - ci.height*sin(angle+M_PI)), y - (ci.width*sin(angle+M_PI) + ci.height*cos(angle+M_PI)));
            }
        }
        else
        {
            x = new_x - distance*cos(angle);
            y = new_y + distance*sin(angle);

            //Center the text on the line.
            x += (((double)string_height/2.0) - 1.0)*cos(angle-M_PI/2);
            y -= (((double)string_height/2.0) - 1.0)*sin(angle-M_PI/2);

            if (!p->has_dimensions)
            {
              e.init(x, y, x + ci.width*cos(angle), y - ci.width*sin(angle));
              e.expand_to_include(x - ci.height*sin(angle), y - ci.height*cos(angle));
              e.expand_to_include(x + (ci.width*cos(angle) - ci.height*sin(angle)), y - (ci.width*sin(angle) + ci.height*cos(angle)));
            }
        }

        if (!detector_.has_placement(e))
        {
          return false;
        }

        p->envelopes.push(e);

        p->path.add_node(c, x - p->starting_x, -y + p->starting_y, (orientation == -1 ? angle + M_PI : angle));

        distance -= ci.width;
    }

    return true;
  }

  bool placement_finder::build_path_horizontal(placement *p, double target_distance)
  {
    double x, y;

    p->path.clear();

    std::pair<double, double> string_dimensions = p->info->get_dimensions();
    double string_width = string_dimensions.first;
    double string_height = string_dimensions.second;

    x = -string_width/2.0;
    y = -string_height/2.0 + 1.0;

    if (p->geom->type() == LineString)
    {
      std::pair<double, double> starting_pos = p->get_position_at_distance(target_distance);

      p->starting_x = starting_pos.first;
      p->starting_y = starting_pos.second;
    }
    else
    {
      p->geom->label_position(&p->starting_x, &p->starting_y);
      p->ctrans->forward(&p->starting_x, &p->starting_y);
    }

    for (unsigned i = 0; i < p->info->num_characters(); i++)
    {
        character_info ci;;
        ci = p->info->at(i);

        unsigned c = ci.character;

        p->path.add_node(c, x, y, 0.0);

        Envelope<double> e;
        if (p->has_dimensions)
        {
            e.init(p->starting_x - (p->dimensions.first/2.0), p->starting_y - (p->dimensions.second/2.0), p->starting_x + (p->dimensions.first/2.0), p->starting_y + (p->dimensions.second/2.0));
        }
        else
        {
          e.init(p->starting_x + x, p->starting_y - y, p->starting_x + x + ci.width, p->starting_y - y - ci.height);
        }

        if (!detector_.has_placement(e))
        {
          return false;
        }

        p->envelopes.push(e);

        x += ci.width;
    }
    return true;
  }

}

<commit_msg>added missing projection transformation for label placement.  TODO: move to geometry class to always return transformed label position<commit_after>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2006 Artem Pavlenko
 * Copyright (C) 2006 10East Corp.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

//$Id$

//stl
#include <string>
#include <vector>

// boost
#include <boost/shared_ptr.hpp>
#include <boost/utility.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <boost/thread/mutex.hpp>

//mapnik
#include <mapnik/geometry.hpp>
#include <mapnik/placement_finder.hpp>
#include <mapnik/text_path.hpp>

namespace mapnik
{
  placement::placement(string_info *info_, CoordTransform *ctrans_, const proj_transform *proj_trans_, geometry_ptr geom_, std::pair<double, double> dimensions_)
    : info(info_), ctrans(ctrans_), proj_trans(proj_trans_), geom(geom_), label_placement(point_placement), dimensions(dimensions_), has_dimensions(true), shape_path(*ctrans_, *geom_, *proj_trans_), total_distance_(-1.0)
  {
  }

  //For text
  placement::placement(string_info *info_, CoordTransform *ctrans_, const proj_transform *proj_trans_, geometry_ptr geom_, label_placement_e placement_)
    : info(info_), ctrans(ctrans_), proj_trans(proj_trans_), geom(geom_), label_placement(placement_), has_dimensions(false), shape_path(*ctrans_, *geom_, *proj_trans_), total_distance_(-1.0)
  {
  }

  placement::~placement()
  {
  }

  std::pair<double, double> placement::get_position_at_distance(double target_distance)
  {
    double old_x, old_y, new_x, new_y;
    double x, y;
    x = y = 0.0;

    double distance = 0.0;

    shape_path.rewind(0);
    shape_path.vertex(&new_x,&new_y);
    for (unsigned i = 0; i < geom->num_points() - 1; i++)
    {
      double dx, dy;

      old_x = new_x;
      old_y = new_y;

      shape_path.vertex(&new_x,&new_y);

      dx = new_x - old_x;
      dy = new_y - old_y;

      double segment_length = sqrt(dx*dx + dy*dy);

      distance += segment_length;
      if (distance > target_distance)
      {
          x = new_x - dx*(distance - target_distance)/segment_length;
          y = new_y - dy*(distance - target_distance)/segment_length;

          break;
      }
    }

    return std::pair<double, double>(x, y);
  }

  double placement::get_total_distance()
  {
    if (total_distance_ < 0.0)
    {
      double old_x, old_y, new_x, new_y;

      shape_path.rewind(0);

      shape_path.vertex(&old_x,&old_y);

      total_distance_ = 0.0;

      for (unsigned i = 0; i < geom->num_points() - 1; i++)
      {
          double dx, dy;

          shape_path.vertex(&new_x,&new_y);

          dx = new_x - old_x;
          dy = new_y - old_y;

          total_distance_ += sqrt(dx*dx + dy*dy);

          old_x = new_x;
          old_y = new_y;
      }
    }

    return total_distance_;
  }

  void placement::clear_envelopes()
  {
    while (!envelopes.empty())
      envelopes.pop();
  }

  placement_finder::placement_finder(Envelope<double> e)
    : detector_(e)
  {
  }

  bool placement_finder::find_placement(placement *p)
  {
    if (p->label_placement == point_placement)
    {
      return find_placement_horizontal(p);
    }
    else if (p->label_placement == line_placement)
    {
      return find_placement_follow(p);
    }

    return false;
  }

  bool placement_finder::find_placement_follow(placement *p)
  {
    std::pair<double, double> string_dimensions = p->info->get_dimensions();
    double string_width = string_dimensions.first;
//    double string_height = string_dimensions.second;

    double distance = p->get_total_distance();

    //~ double delta = string_width/distance;
    double delta = distance/100.0;

    for (double i = 0; i < (distance - string_width)/2.0; i += delta)
    {
      p->clear_envelopes();

      if ( build_path_follow(p, (distance - string_width)/2.0 + i) ) {
        update_detector(p);
        return true;
      }

      p->clear_envelopes();

      if ( build_path_follow(p, (distance - string_width)/2.0 - i) ) {
        update_detector(p);
        return true;
      }
    }

    p->starting_x = 0;
    p->starting_y = 0;

    return false;
  }

  bool placement_finder::find_placement_horizontal(placement *p)
  {
    double distance = p->get_total_distance();
    //~ double delta = string_width/distance;
    double delta = distance/100.0;

    for (double i = 0; i < distance/2.0; i += delta)
    {
      p->clear_envelopes();

      if ( build_path_horizontal(p, distance/2.0 + i) ) {
        update_detector(p);
        return true;
      }

      p->clear_envelopes();

      if ( build_path_horizontal(p, distance/2.0 - i) ) {
        update_detector(p);
        return true;
      }
    }

    p->starting_x = 0;
    p->starting_y = 0;

    return false;
  }

  void placement_finder::update_detector(placement *p)
  {
    while (!p->envelopes.empty())
    {
      Envelope<double> e = p->envelopes.front();

      detector_.insert(e);

      p->envelopes.pop();
    }
  }

  bool placement_finder::build_path_follow(placement *p, double target_distance)
  {
    double new_x, new_y, old_x, old_y;
    unsigned cur_node = 0;

    double angle = 0.0;
    int orientation = 0;

    p->path.clear();

    double x, y;
    x = y = 0.0;

    double distance = 0.0;

    std::pair<double, double> string_dimensions = p->info->get_dimensions();
//    double string_width = string_dimensions.first;
    double string_height = string_dimensions.second;

    p->shape_path.rewind(0);
    p->shape_path.vertex(&new_x,&new_y);
    for (unsigned i = 0; i < p->geom->num_points() - 1; i++)
    {
        double dx, dy;

        cur_node++;

        old_x = new_x;
        old_y = new_y;

        p->shape_path.vertex(&new_x,&new_y);

        dx = new_x - old_x;
        dy = new_y - old_y;

        double segment_length = sqrt(dx*dx + dy*dy);

        distance += segment_length;
        if (distance > target_distance)
        {
            p->starting_x = new_x - dx*(distance - target_distance)/segment_length;
            p->starting_y = new_y - dy*(distance - target_distance)/segment_length;

            angle = atan2(-dy, dx);

            if (angle > M_PI/2 || angle <= -M_PI/2) {
              orientation = -1;
            }
            else {
              orientation = 1;
            }

            distance -= target_distance;

            break;
        }
    }

    for (unsigned i = 0; i < p->info->num_characters(); i++)
    {
        character_info ci;
        unsigned c;

        while (distance <= 0) {
            double dx, dy;

            cur_node++;

            if (cur_node >= p->geom->num_points()) {
              break;
            }

            old_x = new_x;
            old_y = new_y;

            p->shape_path.vertex(&new_x,&new_y);

            dx = new_x - old_x;
            dy = new_y - old_y;

            angle = atan2(-dy, dx );

            distance += sqrt(dx*dx+dy*dy);
        }

        if (orientation == -1) {
            ci = p->info->at(p->info->num_characters() - i - 1);
        }
        else
        {
            ci = p->info->at(i);
        }
        c = ci.character;

        Envelope<double> e;
        if (p->has_dimensions)
        {
            e.init(x, y, x + p->dimensions.first, y + p->dimensions.second);
        }

        if (orientation == -1) {
            x = new_x - (distance - ci.width)*cos(angle);
            y = new_y + (distance - ci.width)*sin(angle);

            //Center the text on the line.
            x += (((double)string_height/2.0) - 1.0)*cos(angle+M_PI/2);
            y -= (((double)string_height/2.0) - 1.0)*sin(angle+M_PI/2);

            if (!p->has_dimensions)
            {
              e.init(x, y, x + ci.width*cos(angle+M_PI), y - ci.width*sin(angle+M_PI));
              e.expand_to_include(x - ci.height*sin(angle+M_PI), y - ci.height*cos(angle+M_PI));
              e.expand_to_include(x + (ci.width*cos(angle+M_PI) - ci.height*sin(angle+M_PI)), y - (ci.width*sin(angle+M_PI) + ci.height*cos(angle+M_PI)));
            }
        }
        else
        {
            x = new_x - distance*cos(angle);
            y = new_y + distance*sin(angle);

            //Center the text on the line.
            x += (((double)string_height/2.0) - 1.0)*cos(angle-M_PI/2);
            y -= (((double)string_height/2.0) - 1.0)*sin(angle-M_PI/2);

            if (!p->has_dimensions)
            {
              e.init(x, y, x + ci.width*cos(angle), y - ci.width*sin(angle));
              e.expand_to_include(x - ci.height*sin(angle), y - ci.height*cos(angle));
              e.expand_to_include(x + (ci.width*cos(angle) - ci.height*sin(angle)), y - (ci.width*sin(angle) + ci.height*cos(angle)));
            }
        }

        if (!detector_.has_placement(e))
        {
          return false;
        }

        p->envelopes.push(e);

        p->path.add_node(c, x - p->starting_x, -y + p->starting_y, (orientation == -1 ? angle + M_PI : angle));

        distance -= ci.width;
    }

    return true;
  }

  bool placement_finder::build_path_horizontal(placement *p, double target_distance)
  {
    double x, y;

    p->path.clear();

    std::pair<double, double> string_dimensions = p->info->get_dimensions();
    double string_width = string_dimensions.first;
    double string_height = string_dimensions.second;

    x = -string_width/2.0;
    y = -string_height/2.0 + 1.0;

    if (p->geom->type() == LineString)
    {
      std::pair<double, double> starting_pos = p->get_position_at_distance(target_distance);

      p->starting_x = starting_pos.first;
      p->starting_y = starting_pos.second;
    }
    else
    {
      p->geom->label_position(&p->starting_x, &p->starting_y);
      //  TODO:
      //  We would only want label position in final 'paper' coords.
      //  Move view and proj transforms to e.g. label_position(x,y,proj_trans,ctrans)?
      double z=0;
      p->proj_trans->backward(p->starting_x, p->starting_y, z);
      p->ctrans->forward(&p->starting_x, &p->starting_y);
    }

    for (unsigned i = 0; i < p->info->num_characters(); i++)
    {
        character_info ci;;
        ci = p->info->at(i);

        unsigned c = ci.character;

        p->path.add_node(c, x, y, 0.0);

        Envelope<double> e;
        if (p->has_dimensions)
        {
            e.init(p->starting_x - (p->dimensions.first/2.0), p->starting_y - (p->dimensions.second/2.0), p->starting_x + (p->dimensions.first/2.0), p->starting_y + (p->dimensions.second/2.0));
        }
        else
        {
          e.init(p->starting_x + x, p->starting_y - y, p->starting_x + x + ci.width, p->starting_y - y - ci.height);
        }

        if (!detector_.has_placement(e))
        {
          return false;
        }

        p->envelopes.push(e);

        x += ci.width;
    }
    return true;
  }

}

<|endoftext|>


<commit_before>#include "behavior.h"
#include <vector>
#include <string>
#include <math.h>
#include "util/input/input-map.h"
#include "util/input/input-source.h"
#include "command.h"
#include "util/debug.h"
#include "character.h"
#include "stage.h"
#include "util/funcs.h"

using namespace std;

namespace Mugen{

namespace PaintownUtil = ::Util;

Behavior::Behavior(){
}

Behavior::~Behavior(){
}

void Behavior::hit(Object * enemy){
}

DummyBehavior::DummyBehavior(){
}

void DummyBehavior::flip(){
}

vector<string> DummyBehavior::currentCommands(const Mugen::Stage & stage, Character * owner, const std::vector<Command*> & commands, bool reversed){
    vector<string> out;
    return out;
}

DummyBehavior::~DummyBehavior(){
}

static void debugit(const string & what, const InputMap<Mugen::Keys> & right){
    Global::debug(0) << "Debug input map " << what << std::endl;
    const std::map<Keyboard::KeyType, PaintownUtil::ReferenceCount<KeyState<Mugen::Keys> > > & keys = right.getKeyStates();
    for (std::map<Keyboard::KeyType, PaintownUtil::ReferenceCount<KeyState<Mugen::Keys> > >::const_iterator it = keys.begin(); it != keys.end(); it++){
        Keyboard::KeyType key = it->first;
        const PaintownUtil::ReferenceCount<KeyState<Mugen::Keys> > & state = it->second;
        if (state != NULL){
            Global::debug(0) << "Mapped " << key << " to " << state->out << std::endl;
        }
    }
}

HumanBehavior::HumanBehavior(const InputMap<Mugen::Keys> & right, const InputMap<Mugen::Keys> & left):
right(right),
left(left){
}

InputMap<Keys> & HumanBehavior::getInput(bool facingRight){
    if (facingRight){
        return right;
    }
    return left;
}

/* turned around so we have to swap forward/backward */
void HumanBehavior::flip(){
    bool forward = input.forward;
    input.forward = input.back;
    input.back = forward;
}

Mugen::Input HumanBehavior::updateInput(InputMap<Keys> & keys, Mugen::Input old){
    vector<InputMap<Keys>::InputEvent> eventsHold = InputManager::getEvents(keys, InputSource());
    for (vector<InputMap<Keys>::InputEvent>::iterator it = eventsHold.begin(); it != eventsHold.end(); it++){
        InputMap<Keys>::InputEvent event = *it;

        switch (event.out){
            /* the keys map already sets up left/right to be forward/backward */
            case Mugen::Right: old.forward = event.enabled; break;
            case Mugen::Left: old.back = event.enabled; break;
            case Mugen::Up: old.up = event.enabled; break;
            case Mugen::Down: old.down = event.enabled; break;
            case Mugen::A: old.a = event.enabled; break;
            case Mugen::B: old.b = event.enabled; break;
            case Mugen::C: old.c = event.enabled; break;
            case Mugen::X: old.x = event.enabled; break;
            case Mugen::Y: old.y = event.enabled; break;
            case Mugen::Z: old.z = event.enabled; break;
            case Mugen::Start: old.start = event.enabled; break;
            default: break;
        }
    }

    return old;
}

vector<string> HumanBehavior::currentCommands(const Mugen::Stage & stage, Character * owner, const vector<Command*> & commands, bool reversed){
    vector<string> out;

    // InputMap<Mugen::Keys>::Output output = InputManager::getMap(getInput(reversed));
    input = updateInput(getInput(reversed), input);

    for (vector<Command*>::const_iterator it = commands.begin(); it != commands.end(); it++){
        Command * command = *it;
        if (command->handle(input)){
            Global::debug(1) << "command: " << command->getName() << endl;
            out.push_back(command->getName());
        }
    }

    return out;

}

HumanBehavior::~HumanBehavior(){
}

RandomAIBehavior::RandomAIBehavior(){
}

void RandomAIBehavior::flip(){
}

static string randomCommand(const vector<Command*> & commands){
    if (commands.size() == 0){
        return "";
    }

    int choice = PaintownUtil::rnd(commands.size());
    return commands[choice]->getName();
}

vector<string> RandomAIBehavior::currentCommands(const Mugen::Stage & stage, Character * owner, const vector<Command*> & commands, bool reversed){
    vector<string> out;
    if (PaintownUtil::rnd(100) > 90){
        out.push_back(randomCommand(commands));
    }
    return out;
}

RandomAIBehavior::~RandomAIBehavior(){
}

LearningAIBehavior::LearningAIBehavior(int difficulty):
direction(Forward),
difficulty(difficulty),
dontMove(0){
    /* make walking more likely to begin with */
    moves["holdfwd"].points -= 10;
    moves["holdfwd"].minimumDistance = 999999;
    moves["holdfwd"].maximumDistance = 0;
    moves["holdback"].points -= 10;
    moves["holdback"].minimumDistance = 999999;
    moves["holdback"].maximumDistance = 0;

    /* this is just to give the AI a starting move that does nothing, this is
     * basically a noop. eventualy the AI will learn other moves to do.
     *
     * the name is something that the user can't write for a command (hopefully)
     */
    moves["not-possible+#$*(@#$"].points = 5;
}

/* the command with the most points will be chosen. points are tallied by
 * adding all the quantities associated with that move thus far. right now they are:
 *  - starting points is the number of times the move hit successfully
 *  - add points if the move is within range, where range is determined by the last
 *     successful hit for that command
 *  - add a random number of points to shake things up
 *  - subtract points if the move is out of range
 *  - subtract points based on the number of times the move has been tried
 *  - subtract points if the move has been done recently
 */
string LearningAIBehavior::selectBestCommand(int distance, const vector<Command*> & commands){
    Move * currentMove = NULL;
    string what = "";
    double points = 0;

    for (vector<Command*>::const_iterator it = commands.begin(); it != commands.end(); it++){
        string name = (*it)->getName();
        Move & move = moves[name];
        double morePoints = move.points + PaintownUtil::rnd(10);
        if (move.minimumDistance != -1){
            if (distance < move.maximumDistance + 10 && distance > move.minimumDistance - 10){
                morePoints += 2;
            } else if (distance > move.maximumDistance + 10){
                morePoints -= 3;
            }
        }

        morePoints -= log((double) move.attempts * 3);
        morePoints -= move.consecutive * 2;

        if (currentMove == NULL){
            currentMove = &move;
            what = name;
            points = morePoints;
        } else if (morePoints > points){
            currentMove = &move;
            what = name;
            points = morePoints;
        }
    }

    for (map<string, Move>::iterator it = moves.begin(); it != moves.end(); it++){
        Move & move = (*it).second;
        if (currentMove != &move){
            move.consecutive /= 2;
        }
    }

    currentMove->attempts += 1;
    currentMove->consecutive += 1;

    return what;
}

void LearningAIBehavior::flip(){
}

static LearningAIBehavior::Direction randomDirection(){
    int what = PaintownUtil::rnd(100);
    if (what > 70){
        return LearningAIBehavior::Forward;
    } else if (what > 40){
        return LearningAIBehavior::Backward;
    } else if (what > 20){
        return LearningAIBehavior::Stopped;
    } else {
        return LearningAIBehavior::Crouch;
    }
}

vector<string> LearningAIBehavior::currentCommands(const Mugen::Stage & stage, Character * owner, const vector<Command*> & commands, bool reversed){

    vector<string> out;

    /* maybe attack */
    if (PaintownUtil::rnd(200) < difficulty * 2){
        const Character * enemy = stage.getEnemy(owner);
        int xDistance = (int) fabs(owner->getX() - enemy->getX());
        string command = selectBestCommand(xDistance, commands);
        out.push_back(command);
        lastCommand = command;
        lastDistance = xDistance;
    } else {
        /* otherwise move around */
        dontMove += 1;
        if (direction == Forward){
            out.push_back("holdfwd");
        } else if (direction == Backward){
            out.push_back("holdback");
        } else if (direction == Crouch){
            out.push_back("holddown");
        } else if (direction == Stopped){
        }

        /* after keeping a direction for 40 ticks, maybe change directions */
        if (dontMove > 40 && PaintownUtil::rnd(10) > 8){
            direction = randomDirection();
            dontMove = 0;
        }

        /* make the AI jump sometimes */
        if (PaintownUtil::rnd(100) == 0){
            out.push_back("holdup");
        }
    }

    return out;
}

/* hit succeeded, reinforce learning behavior for that move */
void LearningAIBehavior::hit(Object * enemy){
    Move & move = moves[lastCommand];
    move.points += 1;
    if (move.points > 5){
        move.points = 5;
    }

    if (move.maximumDistance == -1 || lastDistance > move.maximumDistance){
        move.maximumDistance = lastDistance;
    }

    if (move.minimumDistance == -1 || lastDistance < move.minimumDistance){
        move.minimumDistance = lastDistance;
    }
}

LearningAIBehavior::~LearningAIBehavior(){
}

/* crummy regex parsing */
static vector<string> parseCommands(string stuff){
    vector<string> out;

    while (PaintownUtil::matchRegex(stuff, "\\s*(\\w+)")){
        string get;
        get = PaintownUtil::captureRegex(stuff, "\\s*(\\w+)",0);
        out.push_back(get);
        stuff = PaintownUtil::captureRegex(stuff, "\\s*\\w+,?\\s*(.*)", 0);
    }

    return out;
}

ScriptedBehavior::ScriptedBehavior(const Filesystem::AbsolutePath & path){
    if (!Storage::instance().exists(path)){
        ostringstream out;
        out << "Could not load script file '" << path.path() << "'";
        throw MugenException(out.str(), __FILE__, __LINE__);
    }

    ifstream file(path.path().c_str());
    while (file.good() && !file.eof()){

        char line[1024];
        file.getline(line, sizeof(line) - 1);
        line[sizeof(line) - 1] = '\0';
        string whole(line);

        if (PaintownUtil::matchRegex(whole, "\\d+\\s*$")){
            int ticks = atoi(PaintownUtil::captureRegex(whole, "(\\d+)", 0).c_str());
            actions.push_back(Action(ticks, vector<string>()));
        } else if (PaintownUtil::matchRegex(whole, "\\d+\\s+")){
            int ticks = atoi(PaintownUtil::captureRegex(whole, "(\\d+)", 0).c_str());
            string command = PaintownUtil::captureRegex(whole, "\\d+\\s+(.*)", 0);
            vector<string> commands = parseCommands(command);
            actions.push_back(Action(ticks, commands));
            // Global::debug(0) << "Added action " << PaintownUtil::join(commands, ", ") << " for " << ticks << std::endl;
            // Global::debug(1) << "Added no action for " << ticks << std::endl;
        } else {
            string command = PaintownUtil::captureRegex(whole, "(\\w+)\\s*", 0);
            vector<string> commands = parseCommands(command);
            if (commands.size() > 0){
                actions.push_back(Action(1, commands));
            }
            // Global::debug(1) << "Added action " << command << " for 1" << std::endl;
        }
    }

    currentAction = actions.begin();
}

std::vector<std::string> ScriptedBehavior::currentCommands(const Stage & stage, Character * owner, const std::vector<Command*> & commands, bool reversed){
    vector<string> out;

    if (currentAction != actions.end()){
        Action & action = *currentAction;

        action.ticks -= 1;
        out = action.commands;

        if (action.ticks == 0){
            currentAction++;

            // Global::debug(0) << "Next action " << PaintownUtil::join(action.commands, ", ") << std::endl;
        }
    }

    return out;
}

void ScriptedBehavior::flip(){
}

ScriptedBehavior::~ScriptedBehavior(){
}

}
<commit_msg>[mugen] ignore movement commands when the learning AI decides on a new move<commit_after>#include "behavior.h"
#include <vector>
#include <string>
#include <math.h>
#include "util/input/input-map.h"
#include "util/input/input-source.h"
#include "command.h"
#include "util/debug.h"
#include "character.h"
#include "stage.h"
#include "util/funcs.h"

using namespace std;

namespace Mugen{

namespace PaintownUtil = ::Util;

Behavior::Behavior(){
}

Behavior::~Behavior(){
}

void Behavior::hit(Object * enemy){
}

DummyBehavior::DummyBehavior(){
}

void DummyBehavior::flip(){
}

vector<string> DummyBehavior::currentCommands(const Mugen::Stage & stage, Character * owner, const std::vector<Command*> & commands, bool reversed){
    vector<string> out;
    return out;
}

DummyBehavior::~DummyBehavior(){
}

static void debugit(const string & what, const InputMap<Mugen::Keys> & right){
    Global::debug(0) << "Debug input map " << what << std::endl;
    const std::map<Keyboard::KeyType, PaintownUtil::ReferenceCount<KeyState<Mugen::Keys> > > & keys = right.getKeyStates();
    for (std::map<Keyboard::KeyType, PaintownUtil::ReferenceCount<KeyState<Mugen::Keys> > >::const_iterator it = keys.begin(); it != keys.end(); it++){
        Keyboard::KeyType key = it->first;
        const PaintownUtil::ReferenceCount<KeyState<Mugen::Keys> > & state = it->second;
        if (state != NULL){
            Global::debug(0) << "Mapped " << key << " to " << state->out << std::endl;
        }
    }
}

HumanBehavior::HumanBehavior(const InputMap<Mugen::Keys> & right, const InputMap<Mugen::Keys> & left):
right(right),
left(left){
}

InputMap<Keys> & HumanBehavior::getInput(bool facingRight){
    if (facingRight){
        return right;
    }
    return left;
}

/* turned around so we have to swap forward/backward */
void HumanBehavior::flip(){
    bool forward = input.forward;
    input.forward = input.back;
    input.back = forward;
}

Mugen::Input HumanBehavior::updateInput(InputMap<Keys> & keys, Mugen::Input old){
    vector<InputMap<Keys>::InputEvent> eventsHold = InputManager::getEvents(keys, InputSource());
    for (vector<InputMap<Keys>::InputEvent>::iterator it = eventsHold.begin(); it != eventsHold.end(); it++){
        InputMap<Keys>::InputEvent event = *it;

        switch (event.out){
            /* the keys map already sets up left/right to be forward/backward */
            case Mugen::Right: old.forward = event.enabled; break;
            case Mugen::Left: old.back = event.enabled; break;
            case Mugen::Up: old.up = event.enabled; break;
            case Mugen::Down: old.down = event.enabled; break;
            case Mugen::A: old.a = event.enabled; break;
            case Mugen::B: old.b = event.enabled; break;
            case Mugen::C: old.c = event.enabled; break;
            case Mugen::X: old.x = event.enabled; break;
            case Mugen::Y: old.y = event.enabled; break;
            case Mugen::Z: old.z = event.enabled; break;
            case Mugen::Start: old.start = event.enabled; break;
            default: break;
        }
    }

    return old;
}

vector<string> HumanBehavior::currentCommands(const Mugen::Stage & stage, Character * owner, const vector<Command*> & commands, bool reversed){
    vector<string> out;

    // InputMap<Mugen::Keys>::Output output = InputManager::getMap(getInput(reversed));
    input = updateInput(getInput(reversed), input);

    for (vector<Command*>::const_iterator it = commands.begin(); it != commands.end(); it++){
        Command * command = *it;
        if (command->handle(input)){
            Global::debug(1) << "command: " << command->getName() << endl;
            out.push_back(command->getName());
        }
    }

    return out;

}

HumanBehavior::~HumanBehavior(){
}

RandomAIBehavior::RandomAIBehavior(){
}

void RandomAIBehavior::flip(){
}

static string randomCommand(const vector<Command*> & commands){
    if (commands.size() == 0){
        return "";
    }

    int choice = PaintownUtil::rnd(commands.size());
    return commands[choice]->getName();
}

vector<string> RandomAIBehavior::currentCommands(const Mugen::Stage & stage, Character * owner, const vector<Command*> & commands, bool reversed){
    vector<string> out;
    if (PaintownUtil::rnd(100) > 90){
        out.push_back(randomCommand(commands));
    }
    return out;
}

RandomAIBehavior::~RandomAIBehavior(){
}

LearningAIBehavior::LearningAIBehavior(int difficulty):
direction(Forward),
difficulty(difficulty),
dontMove(0){
    /* make walking more likely to begin with */
    /*
    moves["holdfwd"].points -= 10;
    moves["holdfwd"].minimumDistance = 999999;
    moves["holdfwd"].maximumDistance = 0;
    moves["holdback"].points -= 10;
    moves["holdback"].minimumDistance = 999999;
    moves["holdback"].maximumDistance = 0;
    */

    /* this is just to give the AI a starting move that does nothing, this is
     * basically a noop. eventualy the AI will learn other moves to do.
     *
     * the name is something that the user can't write for a command (hopefully)
     */
    moves["not-possible+#$*(@#$"].points = 5;
}

/* the command with the most points will be chosen. points are tallied by
 * adding all the quantities associated with that move thus far. right now they are:
 *  - starting points is the number of times the move hit successfully
 *  - add points if the move is within range, where range is determined by the last
 *     successful hit for that command
 *  - add a random number of points to shake things up
 *  - subtract points if the move is out of range
 *  - subtract points based on the number of times the move has been tried
 *  - subtract points if the move has been done recently
 */
string LearningAIBehavior::selectBestCommand(int distance, const vector<Command*> & commands){
    Move * currentMove = NULL;
    string what = "";
    double points = 0;

    for (vector<Command*>::const_iterator it = commands.begin(); it != commands.end(); it++){
        string name = (*it)->getName();

        /* Skip movement keys */
        if (name == "holdfwd" ||
            name == "holdback" ||
            name == "holddown" ||
            name == "holdup"){
            continue;
        }

        Move & move = moves[name];
        double morePoints = move.points + PaintownUtil::rnd(10);
        if (move.minimumDistance != -1){
            if (distance < move.maximumDistance + 10 && distance > move.minimumDistance - 10){
                morePoints += 2;
            } else if (distance > move.maximumDistance + 10){
                morePoints -= 3;
            }
        }

        morePoints -= log((double) move.attempts * 3);
        morePoints -= move.consecutive * 2;

        if (currentMove == NULL){
            currentMove = &move;
            what = name;
            points = morePoints;
        } else if (morePoints > points){
            currentMove = &move;
            what = name;
            points = morePoints;
        }
    }

    for (map<string, Move>::iterator it = moves.begin(); it != moves.end(); it++){
        Move & move = (*it).second;
        if (currentMove != &move){
            move.consecutive /= 2;
        }
    }

    currentMove->attempts += 1;
    currentMove->consecutive += 1;

    return what;
}

void LearningAIBehavior::flip(){
}

static LearningAIBehavior::Direction randomDirection(){
    int what = PaintownUtil::rnd(100);
    if (what > 70){
        return LearningAIBehavior::Forward;
    } else if (what > 40){
        return LearningAIBehavior::Backward;
    } else if (what > 20){
        return LearningAIBehavior::Stopped;
    } else {
        return LearningAIBehavior::Crouch;
    }
}

vector<string> LearningAIBehavior::currentCommands(const Mugen::Stage & stage, Character * owner, const vector<Command*> & commands, bool reversed){

    vector<string> out;

    /* maybe attack */
    if (PaintownUtil::rnd(200) < difficulty * 2){
        const Character * enemy = stage.getEnemy(owner);
        int xDistance = (int) fabs(owner->getX() - enemy->getX());
        string command = selectBestCommand(xDistance, commands);
        out.push_back(command);
        lastCommand = command;
        lastDistance = xDistance;
    } else {
        /* otherwise move around */
        dontMove += 1;
        if (direction == Forward){
            out.push_back("holdfwd");
        } else if (direction == Backward){
            out.push_back("holdback");
        } else if (direction == Crouch){
            out.push_back("holddown");
        } else if (direction == Stopped){
        }

        /* after keeping a direction for 40 ticks, maybe change directions */
        if (dontMove > 40 && PaintownUtil::rnd(10) > 8){
            direction = randomDirection();
            dontMove = 0;
        }

        /* make the AI jump sometimes */
        if (PaintownUtil::rnd(100) == 0){
            out.push_back("holdup");
        }
    }

    return out;
}

/* hit succeeded, reinforce learning behavior for that move */
void LearningAIBehavior::hit(Object * enemy){
    Move & move = moves[lastCommand];
    move.points += 1;
    if (move.points > 5){
        move.points = 5;
    }

    if (move.maximumDistance == -1 || lastDistance > move.maximumDistance){
        move.maximumDistance = lastDistance;
    }

    if (move.minimumDistance == -1 || lastDistance < move.minimumDistance){
        move.minimumDistance = lastDistance;
    }
}

LearningAIBehavior::~LearningAIBehavior(){
}

/* crummy regex parsing */
static vector<string> parseCommands(string stuff){
    vector<string> out;

    while (PaintownUtil::matchRegex(stuff, "\\s*(\\w+)")){
        string get;
        get = PaintownUtil::captureRegex(stuff, "\\s*(\\w+)",0);
        out.push_back(get);
        stuff = PaintownUtil::captureRegex(stuff, "\\s*\\w+,?\\s*(.*)", 0);
    }

    return out;
}

ScriptedBehavior::ScriptedBehavior(const Filesystem::AbsolutePath & path){
    if (!Storage::instance().exists(path)){
        ostringstream out;
        out << "Could not load script file '" << path.path() << "'";
        throw MugenException(out.str(), __FILE__, __LINE__);
    }

    ifstream file(path.path().c_str());
    while (file.good() && !file.eof()){

        char line[1024];
        file.getline(line, sizeof(line) - 1);
        line[sizeof(line) - 1] = '\0';
        string whole(line);

        if (PaintownUtil::matchRegex(whole, "\\d+\\s*$")){
            int ticks = atoi(PaintownUtil::captureRegex(whole, "(\\d+)", 0).c_str());
            actions.push_back(Action(ticks, vector<string>()));
        } else if (PaintownUtil::matchRegex(whole, "\\d+\\s+")){
            int ticks = atoi(PaintownUtil::captureRegex(whole, "(\\d+)", 0).c_str());
            string command = PaintownUtil::captureRegex(whole, "\\d+\\s+(.*)", 0);
            vector<string> commands = parseCommands(command);
            actions.push_back(Action(ticks, commands));
            // Global::debug(0) << "Added action " << PaintownUtil::join(commands, ", ") << " for " << ticks << std::endl;
            // Global::debug(1) << "Added no action for " << ticks << std::endl;
        } else {
            string command = PaintownUtil::captureRegex(whole, "(\\w+)\\s*", 0);
            vector<string> commands = parseCommands(command);
            if (commands.size() > 0){
                actions.push_back(Action(1, commands));
            }
            // Global::debug(1) << "Added action " << command << " for 1" << std::endl;
        }
    }

    currentAction = actions.begin();
}

std::vector<std::string> ScriptedBehavior::currentCommands(const Stage & stage, Character * owner, const std::vector<Command*> & commands, bool reversed){
    vector<string> out;

    if (currentAction != actions.end()){
        Action & action = *currentAction;

        action.ticks -= 1;
        out = action.commands;

        if (action.ticks == 0){
            currentAction++;

            // Global::debug(0) << "Next action " << PaintownUtil::join(action.commands, ", ") << std::endl;
        }
    }

    return out;
}

void ScriptedBehavior::flip(){
}

ScriptedBehavior::~ScriptedBehavior(){
}

}
<|endoftext|>


<commit_before>/*
- Designer will publish:
http://docs.ros.org/indigo/api/trajectory_msgs/html/msg/JointTrajectory.html
- Designer will receive: (with root link as 0,0,0)
http://docs.ros.org/indigo/api/sensor_msgs/html/msg/JointState.html
*/
#include <cstdlib>
#include <string>
#include <ros/ros.h>
#include <lcm/lcm-cpp.hpp>

#include "lcmtypes/robotiqhand/command_t.hpp"
#include <trajectory_msgs/JointTrajectory.h>
#include <ipab_msgs/PlannerRequest.h>
#include <ipab_msgs/RobotiqCommand.h>
#include <std_srvs/Trigger.h>

using namespace std;

class LCM2ROS {
public:
  LCM2ROS(boost::shared_ptr<lcm::LCM> &lcm_, ros::NodeHandle &nh_);
  ~LCM2ROS() {}

private:
  boost::shared_ptr<lcm::LCM> lcm_;
  ros::NodeHandle nh_;
  ros::NodeHandle* rosnode;

  ros::ServiceClient rosserviceclient_Emergency_Release_;
  ros::ServiceClient rosserviceclient_Recover_;
  ros::ServiceClient rosserviceclient_Tactile_Open_;
  ros::ServiceClient rosserviceclient_Tactile_Close_;

  void handCommandHandler(const lcm::ReceiveBuffer* rbuf, const std::string &channel, const robotiqhand::command_t* msg);
  ros::Publisher hand_command_pub_;
};

LCM2ROS::LCM2ROS(boost::shared_ptr<lcm::LCM> &lcm_, ros::NodeHandle &nh_): lcm_(lcm_), nh_(nh_) {
  lcm_->subscribe("ROBOTIQ_LEFT_COMMAND", &LCM2ROS::handCommandHandler, this);
  hand_command_pub_ = nh_.advertise<ipab_msgs::RobotiqCommand>("/gripper/ddapp_command", 10);

  rosserviceclient_Emergency_Release_ = nh_.serviceClient<std_srvs::Trigger>("/gripper/sdh_controller/emergency_stop");
  rosserviceclient_Recover_ = nh_.serviceClient<std_srvs::Trigger>("/gripper/sdh_controller/recover");
  rosserviceclient_Tactile_Open_ = nh_.serviceClient<std_srvs::Trigger>("/gripper/sdh_controller/tactile_open");
  rosserviceclient_Tactile_Close_ = nh_.serviceClient<std_srvs::Trigger>("/gripper/sdh_controller/tactile_close");

  rosnode = new ros::NodeHandle();
}

void LCM2ROS::handCommandHandler(const lcm::ReceiveBuffer* rbuf, const std::string &channel, const robotiqhand::command_t* msg) {
  ROS_ERROR("LCM2ROS_SDH got hand command");

  // TODO: implement engaging on receiving this flag as true and have an auto-timeout to disengage

  // Check whether to perform an emergency release
  if (msg->emergency_release == 1) {
    std_srvs::Trigger er_trigger;
    if (rosserviceclient_Emergency_Release_.call(er_trigger)) {
      ROS_ERROR("Emergency release activated"); // TODO: use returned information whether successful
    } else {
      ROS_ERROR("Emergency release NOT activated");
    }
    return;
  }

  ipab_msgs::RobotiqCommand m;
  m.header.stamp = ros::Time().fromSec(msg->utime * 1E-6);

  m.activate = (bool) msg->activate;
  m.emergency_release = (bool) msg->emergency_release;
  m.do_move = (bool) msg->do_move;

  m.mode = msg->mode;
  m.position = msg->position;
  m.force = msg->force;
  m.velocity = msg->velocity;

  m.ifc = (bool) msg->ifc;
  m.positionA = msg->positionA;
  m.positionB = msg->positionB;
  m.positionC = msg->positionC;

  m.isc = (bool) msg->isc;
  m.positionS = msg->positionS;

  hand_command_pub_.publish(m);

  // TODO: Implement direct call of driver services
  if (msg->do_move) {
    // Command position, force, and velocity (TODO: only position implemented right now)

    // TODO: implement different modes
    switch (msg->mode) {
    // -1 - ignore (use previous mode)
    //  0 - basic (normal)
    //  1 - pinch
    //  2 - wide
    //  3 - scissor
    case 0: // Basic Position Mode
      if (msg->position == 0) { // Open
        std_srvs::Trigger tactile_open_trigger;
        if (rosserviceclient_Tactile_Open_.call(tactile_open_trigger)) {
          ROS_INFO("Tactile opening commanded"); // TODO: use returned information whether successful
        } else {
          ROS_ERROR("Tactile opening FAILED");
        }
        return;
      } else if (msg->position == 254) { // Close
        std_srvs::Trigger tactile_close_trigger;
        if (rosserviceclient_Tactile_Close_.call(tactile_close_trigger)) {
          ROS_INFO("Tactile closing commanded"); // TODO: use returned information whether successful
        } else {
          ROS_ERROR("Tactile closing FAILED");
        }
        return;
      }
      break;
    }

    // TODO: individual finger control and individual scissor control not yet implemented
    // Individual finger control
    // 1 - active
    // 0 - inactive
    // int8_t ifc;
    // int16_t positionA;
    // int16_t positionB;
    // int16_t positionC;

    // Indiviual scissor control
    // 1 - active
    // 0 - inactive
    // int8_t isc;
    // int16_t positionS;
  } else if (!msg->do_move && msg->activate) {
    // Reset / calibrate
    std_srvs::Trigger recover_trigger;
    if (rosserviceclient_Recover_.call(recover_trigger)) {
      ROS_INFO("Reset hand from emergency stop");
    } else {
      ROS_ERROR("Could not recover");
    }
    return;
  } else {
    ROS_WARN("Setting modes not supported - only basic mode supported!");
  }
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "lcm2ros_sdh", ros::init_options::NoSigintHandler);
  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if (!lcm->good()) {
    std::cerr << "ERROR: lcm is not good()" << std::endl;
  }
  ros::NodeHandle nh;

  LCM2ROS handlerObject(lcm, nh);
  cout << "\nlcm2ros_sdh translator ready\n";
  ROS_ERROR("LCM2ROS_SDH Translator Ready");

  while (0 == lcm->handle());
  return 0;
}
<commit_msg>lcm2ros_sdh activate and deactivate hand<commit_after>/*
- Designer will publish:
http://docs.ros.org/indigo/api/trajectory_msgs/html/msg/JointTrajectory.html
- Designer will receive: (with root link as 0,0,0)
http://docs.ros.org/indigo/api/sensor_msgs/html/msg/JointState.html
*/
#include <cstdlib>
#include <string>
#include <ros/ros.h>
#include <lcm/lcm-cpp.hpp>

#include "lcmtypes/robotiqhand/command_t.hpp"
#include <trajectory_msgs/JointTrajectory.h>
#include <ipab_msgs/PlannerRequest.h>
#include <ipab_msgs/RobotiqCommand.h>
#include <std_srvs/Trigger.h>

using namespace std;

class LCM2ROS {
public:
  LCM2ROS(boost::shared_ptr<lcm::LCM> &lcm_, ros::NodeHandle &nh_);
  ~LCM2ROS() {}

private:
  boost::shared_ptr<lcm::LCM> lcm_;
  ros::NodeHandle nh_;
  ros::NodeHandle* rosnode;

  ros::ServiceClient rosserviceclient_Emergency_Release_;
  ros::ServiceClient rosserviceclient_Recover_;
  ros::ServiceClient rosserviceclient_Tactile_Open_;
  ros::ServiceClient rosserviceclient_Tactile_Close_;

  ros::ServiceClient rosserviceclient_Engage_;
  ros::ServiceClient rosserviceclient_Disengage_;

  void handCommandHandler(const lcm::ReceiveBuffer* rbuf, const std::string &channel, const robotiqhand::command_t* msg);
  ros::Publisher hand_command_pub_;
};

LCM2ROS::LCM2ROS(boost::shared_ptr<lcm::LCM> &lcm_, ros::NodeHandle &nh_): lcm_(lcm_), nh_(nh_) {
  lcm_->subscribe("ROBOTIQ_LEFT_COMMAND", &LCM2ROS::handCommandHandler, this);
  hand_command_pub_ = nh_.advertise<ipab_msgs::RobotiqCommand>("/gripper/ddapp_command", 10);

  rosserviceclient_Emergency_Release_ = nh_.serviceClient<std_srvs::Trigger>("/gripper/sdh_controller/emergency_stop");
  rosserviceclient_Recover_ = nh_.serviceClient<std_srvs::Trigger>("/gripper/sdh_controller/recover");
  rosserviceclient_Tactile_Open_ = nh_.serviceClient<std_srvs::Trigger>("/gripper/sdh_controller/tactile_open");
  rosserviceclient_Tactile_Close_ = nh_.serviceClient<std_srvs::Trigger>("/gripper/sdh_controller/tactile_close");

  rosserviceclient_Engage_ = nh_.serviceClient<std_srvs::Trigger>("/gripper/sdh_controller/engage");
  rosserviceclient_Disengage_ = nh_.serviceClient<std_srvs::Trigger>("/gripper/sdh_controller/disengage");

  rosnode = new ros::NodeHandle();
}

void LCM2ROS::handCommandHandler(const lcm::ReceiveBuffer* rbuf, const std::string &channel, const robotiqhand::command_t* msg) {
  ROS_ERROR("LCM2ROS_SDH got hand command");

  // TODO: implement engaging on receiving this flag as true and have an auto-timeout to disengage

  // Check whether to perform an emergency release
  if (msg->emergency_release == 1) {
    std_srvs::Trigger er_trigger;
    if (rosserviceclient_Emergency_Release_.call(er_trigger)) {
      ROS_ERROR("Emergency release activated"); // TODO: use returned information whether successful
    } else {
      ROS_ERROR("Emergency release NOT activated");
    }
    return;
  }

  // Check whether to disengage hand (activate = 0)
  if (msg->activate == 0) {
    std_srvs::Trigger disengage_trigger;
    if (rosserviceclient_Disengage_.call(disengage_trigger))
      ROS_DEBUG("Disengaged hand");
    else
      ROS_ERROR("Error disengaging hand");
    return;
  } else if (msg->activate == 0 && msg->do_move == 0) {
    std_srvs::Trigger engage_trigger;
    if (rosserviceclient_Engage_.call(engage_trigger))
      ROS_DEBUG("Engaged hand");
    else
      ROS_ERROR("Error engaging hand");
  }

  ipab_msgs::RobotiqCommand m;
  m.header.stamp = ros::Time().fromSec(msg->utime * 1E-6);

  m.activate = (bool) msg->activate;
  m.emergency_release = (bool) msg->emergency_release;
  m.do_move = (bool) msg->do_move;

  m.mode = msg->mode;
  m.position = msg->position;
  m.force = msg->force;
  m.velocity = msg->velocity;

  m.ifc = (bool) msg->ifc;
  m.positionA = msg->positionA;
  m.positionB = msg->positionB;
  m.positionC = msg->positionC;

  m.isc = (bool) msg->isc;
  m.positionS = msg->positionS;

  hand_command_pub_.publish(m);

  // TODO: Implement direct call of driver services
  if (msg->do_move) {
    // Command position, force, and velocity (TODO: only position implemented right now)

    // TODO: implement different modes
    switch (msg->mode) {
    // -1 - ignore (use previous mode)
    //  0 - basic (normal)
    //  1 - pinch
    //  2 - wide
    //  3 - scissor
    case 0: // Basic Position Mode
      if (msg->position == 0) { // Open
        std_srvs::Trigger tactile_open_trigger;
        if (rosserviceclient_Tactile_Open_.call(tactile_open_trigger)) {
          ROS_INFO("Tactile opening commanded"); // TODO: use returned information whether successful
        } else {
          ROS_ERROR("Tactile opening FAILED");
        }
        return;
      } else if (msg->position == 254) { // Close
        std_srvs::Trigger tactile_close_trigger;
        if (rosserviceclient_Tactile_Close_.call(tactile_close_trigger)) {
          ROS_INFO("Tactile closing commanded"); // TODO: use returned information whether successful
        } else {
          ROS_ERROR("Tactile closing FAILED");
        }
        return;
      }
      break;
    }

    // TODO: individual finger control and individual scissor control not yet implemented
    // Individual finger control
    // 1 - active
    // 0 - inactive
    // int8_t ifc;
    // int16_t positionA;
    // int16_t positionB;
    // int16_t positionC;

    // Indiviual scissor control
    // 1 - active
    // 0 - inactive
    // int8_t isc;
    // int16_t positionS;
  } /*else if (!msg->do_move && msg->activate) {
    // Reset / calibrate
    std_srvs::Trigger recover_trigger;
    if (rosserviceclient_Recover_.call(recover_trigger)) {
      ROS_INFO("Reset hand from emergency stop");
    } else {
      ROS_ERROR("Could not recover");
    }
    return;
  } else {
    ROS_WARN("Setting modes not supported - only basic mode supported!");
  }*/
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "lcm2ros_sdh", ros::init_options::NoSigintHandler);
  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if (!lcm->good()) {
    std::cerr << "ERROR: lcm is not good()" << std::endl;
  }
  ros::NodeHandle nh;

  LCM2ROS handlerObject(lcm, nh);
  cout << "\nlcm2ros_sdh translator ready\n";
  ROS_ERROR("LCM2ROS_SDH Translator Ready");

  while (0 == lcm->handle());
  return 0;
}
<|endoftext|>


<commit_before>#include <iostream>
using namespace std;

int fib(int n) {
    if(n == 0)
        return 0;
    if(n == 1)
        return 1;
    return fib(n-1) + fib(n-2);
}

int main() {
    int number;

	cout << "Enter a positive integer: ";
	cin >> number;
	cout << number << " fibonacci = " << fib(number) << endl;

    system("PAUSE");
    return 0;
}
<commit_msg>Update task_2.cpp<commit_after>#include <iostream>
using namespace std;

int fib(int n) {
    if(n == 0)
        return 0;
    if(n == 1)
        return 1;
    return fib(n-1) + fib(n-2);
}

int main() {
    int number;

    cout << "Enter a positive integer: ";
    cin >> number;
    cout << number << " fibonacci = " << fib(number) << endl;

    system("PAUSE");
    return 0;
}
<|endoftext|>


<commit_before>/******************************************************************
 *
 * Round for C
 *
 * Copyright (C) Satoshi Konno 2015
 *
 * This is licensed under BSD-style license, see file COPYING.
 *
 ******************************************************************/

#include <boost/test/unit_test.hpp>

#include <round/util/semaphore.h>
#include <round/util/thread.h>
#include <round/util/timer.h>

BOOST_AUTO_TEST_SUITE(semaphore)

BOOST_AUTO_TEST_CASE(RoundSemaphoreBasicTest) {
  RoundSemaphore *sem = round_semaphore_new(1);
  BOOST_CHECK(sem);

  BOOST_CHECK_EQUAL(round_semaphore_post(sem), true);
  BOOST_CHECK_EQUAL(round_semaphore_wait(sem), true);

  BOOST_CHECK_EQUAL(round_semaphore_post(sem), true);
  BOOST_CHECK_EQUAL(round_semaphore_post(sem), true);
  BOOST_CHECK_EQUAL(round_semaphore_wait(sem), true);
  BOOST_CHECK_EQUAL(round_semaphore_wait(sem), true);

  BOOST_CHECK_EQUAL(round_semaphore_post(sem), true);
  BOOST_CHECK_EQUAL(round_semaphore_wait(sem), true);
  BOOST_CHECK_EQUAL(round_semaphore_post(sem), true);
  BOOST_CHECK_EQUAL(round_semaphore_wait(sem), true);

  BOOST_CHECK(round_semaphore_delete(sem));
}

BOOST_AUTO_TEST_CASE(RoundSemaphoreMaxCountTest)
{
  for (size_t semMaxCount = 1; semMaxCount<10; semMaxCount++) {
    RoundSemaphore *sem = round_semaphore_new(semMaxCount);
    BOOST_CHECK(sem);

    for (size_t n=0; n<semMaxCount; n++) {
      BOOST_CHECK_EQUAL(round_semaphore_wait(sem), true);
    }

    for (size_t n=0; n<semMaxCount; n++) {
      BOOST_CHECK_EQUAL(round_semaphore_post(sem), true);
    }

    BOOST_CHECK(round_semaphore_delete(sem));
  }
}

static const int SEMAPHORE_THREAD_TEST_LOOP_NUM = 10;

void RoundSemaphorePostThread(RoundThread *thread)
{
  RoundSemaphore *sem = (RoundSemaphore *)round_thread_getuserdata(thread);
  for (int n = 0; n < SEMAPHORE_THREAD_TEST_LOOP_NUM; n++) {
    round_sleep(100);
    BOOST_CHECK_EQUAL(round_semaphore_post(sem), true);
  }
}

BOOST_AUTO_TEST_CASE(RoundSemaphoreThreadTest)
{
  RoundSemaphore *sem = round_semaphore_new(0);

  RoundThread *thread = round_thread_new();
  round_thread_setaction(thread, RoundSemaphorePostThread);
  round_thread_setuserdata(thread, sem);
  BOOST_CHECK(round_thread_start(thread));

  for (int n = 0; n < SEMAPHORE_THREAD_TEST_LOOP_NUM; n++) {
    BOOST_CHECK_EQUAL(round_semaphore_wait(sem), true);
  }

  BOOST_CHECK(round_thread_stop(thread));
  BOOST_CHECK(round_thread_delete(thread));
  BOOST_CHECK(round_semaphore_delete(sem));
}

BOOST_AUTO_TEST_SUITE_END()
<commit_msg>* Added a unit test for round_semaphore_timedwait().<commit_after>/******************************************************************
 *
 * Round for C
 *
 * Copyright (C) Satoshi Konno 2015
 *
 * This is licensed under BSD-style license, see file COPYING.
 *
 ******************************************************************/

#include <boost/test/unit_test.hpp>

#include <round/util/semaphore.h>
#include <round/util/thread.h>
#include <round/util/timer.h>

BOOST_AUTO_TEST_SUITE(semaphore)

BOOST_AUTO_TEST_CASE(RoundSemaphoreBasicTest) {
  RoundSemaphore *sem = round_semaphore_new(0);
  BOOST_CHECK(sem);

  BOOST_CHECK_EQUAL(round_semaphore_post(sem), true);
  BOOST_CHECK_EQUAL(round_semaphore_wait(sem), true);

  BOOST_CHECK_EQUAL(round_semaphore_post(sem), true);
  BOOST_CHECK_EQUAL(round_semaphore_post(sem), true);
  BOOST_CHECK_EQUAL(round_semaphore_wait(sem), true);
  BOOST_CHECK_EQUAL(round_semaphore_wait(sem), true);

  BOOST_CHECK_EQUAL(round_semaphore_post(sem), true);
  BOOST_CHECK_EQUAL(round_semaphore_wait(sem), true);
  BOOST_CHECK_EQUAL(round_semaphore_post(sem), true);
  BOOST_CHECK_EQUAL(round_semaphore_wait(sem), true);

  BOOST_CHECK(round_semaphore_delete(sem));
}

BOOST_AUTO_TEST_CASE(RoundSemaphoreMaxCountTest)
{
  for (size_t semMaxCount = 1; semMaxCount<10; semMaxCount++) {
    RoundSemaphore *sem = round_semaphore_new(semMaxCount);
    BOOST_CHECK(sem);

    for (size_t n=0; n<semMaxCount; n++) {
      BOOST_CHECK_EQUAL(round_semaphore_wait(sem), true);
    }

    for (size_t n=0; n<semMaxCount; n++) {
      BOOST_CHECK_EQUAL(round_semaphore_post(sem), true);
    }

    BOOST_CHECK(round_semaphore_delete(sem));
  }
}

static const int SEMAPHORE_THREAD_TEST_LOOP_NUM = 10;

void RoundSemaphorePostThread(RoundThread *thread)
{
  RoundSemaphore *sem = (RoundSemaphore *)round_thread_getuserdata(thread);
  for (int n = 0; n < SEMAPHORE_THREAD_TEST_LOOP_NUM; n++) {
    round_sleep(100);
    BOOST_CHECK_EQUAL(round_semaphore_post(sem), true);
  }
}

BOOST_AUTO_TEST_CASE(RoundSemaphoreThreadTest)
{
  RoundSemaphore *sem = round_semaphore_new(0);

  RoundThread *thread = round_thread_new();
  round_thread_setaction(thread, RoundSemaphorePostThread);
  round_thread_setuserdata(thread, sem);
  BOOST_CHECK(round_thread_start(thread));

  for (int n = 0; n < SEMAPHORE_THREAD_TEST_LOOP_NUM; n++) {
    BOOST_CHECK_EQUAL(round_semaphore_wait(sem), true);
  }

  BOOST_CHECK(round_thread_stop(thread));
  BOOST_CHECK(round_thread_delete(thread));
  BOOST_CHECK(round_semaphore_delete(sem));
}

BOOST_AUTO_TEST_CASE(RoundSemaphoreTimeoutTest) {
  RoundSemaphore *sem = round_semaphore_new(0);
  BOOST_CHECK(sem);

  BOOST_CHECK_EQUAL(round_semaphore_post(sem), true);
  BOOST_CHECK_EQUAL(round_semaphore_wait(sem), true);

  BOOST_CHECK_EQUAL(round_semaphore_timedwait(sem, 1), false);

  BOOST_CHECK_EQUAL(round_semaphore_post(sem), true);
  BOOST_CHECK_EQUAL(round_semaphore_wait(sem), true);

  BOOST_CHECK(round_semaphore_delete(sem));
}

BOOST_AUTO_TEST_SUITE_END()
<|endoftext|>


<commit_before>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkContourWidget.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
#include "vtkContourWidget.h"
#include "vtkOrientedGlyphContourRepresentation.h"
#include "vtkCommand.h"
#include "vtkCallbackCommand.h"
#include "vtkRenderWindowInteractor.h"
#include "vtkObjectFactory.h"
#include "vtkRenderer.h"
#include "vtkWidgetCallbackMapper.h"
#include "vtkSphereSource.h"
#include "vtkProperty.h"
#include "vtkEvent.h"
#include "vtkWidgetEvent.h"

#include <vtkstd/vector>
#include <vtkstd/set>
#include <vtkstd/algorithm>
#include <vtkstd/iterator>

vtkCxxRevisionMacro(vtkContourWidget, "1.5");
vtkStandardNewMacro(vtkContourWidget);

//----------------------------------------------------------------------
vtkContourWidget::vtkContourWidget()
{
  this->ManagesCursor = 0;

  this->WidgetState = vtkContourWidget::Start;
  this->CurrentHandle = 0;

  // These are the event callbacks supported by this widget
  this->CallbackMapper->SetCallbackMethod(vtkCommand::LeftButtonPressEvent,
                                          vtkWidgetEvent::Select,
                                          this, vtkContourWidget::SelectAction);
  this->CallbackMapper->SetCallbackMethod(vtkCommand::RightButtonPressEvent,
                                          vtkWidgetEvent::AddFinalPoint,
                                          this, vtkContourWidget::AddFinalPointAction);
  this->CallbackMapper->SetCallbackMethod(vtkCommand::MouseMoveEvent,
                                          vtkWidgetEvent::Move,
                                          this, vtkContourWidget::MoveAction);
  this->CallbackMapper->SetCallbackMethod(vtkCommand::LeftButtonReleaseEvent,
                                          vtkWidgetEvent::EndSelect,
                                          this, vtkContourWidget::EndSelectAction);
  this->CallbackMapper->SetCallbackMethod(vtkCommand::KeyPressEvent,
                                          vtkEvent::NoModifier, 46, 1, NULL,
                                          vtkWidgetEvent::Delete,
                                          this, vtkContourWidget::DeleteAction);

  this->CreateDefaultRepresentation();

}

//----------------------------------------------------------------------
vtkContourWidget::~vtkContourWidget()
{
}

//----------------------------------------------------------------------
void vtkContourWidget::CreateDefaultRepresentation()
{
  if ( ! this->WidgetRep )
    {
    vtkOrientedGlyphContourRepresentation *rep =
      vtkOrientedGlyphContourRepresentation::New();

    this->WidgetRep = rep;

    vtkSphereSource *ss = vtkSphereSource::New();
    ss->SetRadius(0.5);
    rep->SetActiveCursorShape( ss->GetOutput() );
    ss->Delete();

    rep->GetProperty()->SetColor(.25,1.0,.25);

    rep->GetActiveProperty()->SetRepresentationToSurface();
    rep->GetActiveProperty()->SetAmbient(0.1);
    rep->GetActiveProperty()->SetDiffuse(0.9);
    rep->GetActiveProperty()->SetSpecular(0.0);
    }
}

//----------------------------------------------------------------------
void vtkContourWidget::ClearContour()
{
}

//----------------------------------------------------------------------
void vtkContourWidget::SetEnabled(int enabling)
{
  // The handle widgets are not actually enabled until they are placed.
  // The handle widgets take their representation from the vtkContourRepresentation.
  if ( enabling )
    {
    if ( this->WidgetState == vtkContourWidget::Start )
      {
      reinterpret_cast<vtkContourRepresentation*>(this->WidgetRep)->VisibilityOff();
      }
    else
      {
      reinterpret_cast<vtkContourRepresentation*>(this->WidgetRep)->VisibilityOn();
      }
    }

  this->Superclass::SetEnabled(enabling);
}

// The following methods are the callbacks that the measure widget responds to.
//-------------------------------------------------------------------------
void vtkContourWidget::SelectAction(vtkAbstractWidget *w)
{
  vtkContourWidget *self = reinterpret_cast<vtkContourWidget*>(w);
  vtkContourRepresentation *rep =
    reinterpret_cast<vtkContourRepresentation*>(self->WidgetRep);

  int X = self->Interactor->GetEventPosition()[0];
  int Y = self->Interactor->GetEventPosition()[1];
  double pos[2];
  pos[0] = X;
  pos[1] = Y;

  switch ( self->WidgetState )
    {
    case vtkContourWidget::Start:
    case vtkContourWidget::Define:
      self->AddNode();
      break;
    case vtkContourWidget::Manipulate:
      if ( rep->ActivateNode(X,Y) )
        {
        self->StartInteraction();
        rep->SetCurrentOperationToTranslate();
        rep->StartWidgetInteraction(pos);
        self->EventCallbackCommand->SetAbortFlag(1);
        }
      else if ( rep->AddNodeOnContour( X, Y ) )
        {
        if ( rep->ActivateNode(X,Y) )
          {
          rep->SetCurrentOperationToTranslate();
          rep->StartWidgetInteraction(pos);
          }
        self->EventCallbackCommand->SetAbortFlag(1);
        }
      break;
    }

  if ( rep->GetNeedToRender() )
    {
    self->Render();
    rep->NeedToRenderOff();
    }
}

//-------------------------------------------------------------------------
void vtkContourWidget::AddFinalPointAction(vtkAbstractWidget *w)
{
  vtkContourWidget *self = reinterpret_cast<vtkContourWidget*>(w);
  vtkContourRepresentation *rep =
    reinterpret_cast<vtkContourRepresentation*>(self->WidgetRep);

  if ( self->WidgetState ==  vtkContourWidget::Define &&
       rep->GetNumberOfNodes() >= 1 )
    {
    self->AddNode();
    self->WidgetState = vtkContourWidget::Manipulate;
    self->EventCallbackCommand->SetAbortFlag(1);
    }

  if ( rep->GetNeedToRender() )
    {
    self->Render();
    rep->NeedToRenderOff();
    }
}

//------------------------------------------------------------------------
void vtkContourWidget::AddNode()
{
  int X = this->Interactor->GetEventPosition()[0];
  int Y = this->Interactor->GetEventPosition()[1];

  // If the rep already has at least 2 nodes, check how close we are to
  // the first
  if ( reinterpret_cast<vtkContourRepresentation*>(this->WidgetRep)->
       GetNumberOfNodes() > 1 )
    {
    int pixelTolerance2 =
      reinterpret_cast<vtkContourRepresentation*>(this->WidgetRep)->
      GetPixelTolerance();
    pixelTolerance2 *= pixelTolerance2;

    double displayPos[2];
    if ( !reinterpret_cast<vtkContourRepresentation*>(this->WidgetRep)->
         GetNthNodeDisplayPosition( 0, displayPos ) )
      {
      vtkErrorMacro("Can't get first node display position!");
      return;
      }

    if ( (X - displayPos[0]) * (X - displayPos[0]) +
         (Y - displayPos[1]) * (Y - displayPos[1]) <
         pixelTolerance2 )
      {
      // yes - we have made a loop. Stop defining and switch to
      // manipulate mode
      this->WidgetState = vtkContourWidget::Manipulate;
      reinterpret_cast<vtkContourRepresentation*>(this->WidgetRep)->ClosedLoopOn();
      this->EventCallbackCommand->SetAbortFlag(1);
      this->InvokeEvent( vtkCommand::WidgetValueChangedEvent, NULL );
      return;
      }
    }

  if ( reinterpret_cast<vtkContourRepresentation*>(this->WidgetRep)->
       AddNodeAtDisplayPosition( X, Y ) )
    {
    this->WidgetState = vtkContourWidget::Define;
    reinterpret_cast<vtkContourRepresentation*>(this->WidgetRep)->VisibilityOn();
    this->EventCallbackCommand->SetAbortFlag(1);
    }
}

//-------------------------------------------------------------------------
void vtkContourWidget::DeleteAction(vtkAbstractWidget *w)
{
  vtkContourWidget *self = reinterpret_cast<vtkContourWidget*>(w);

  if ( self->WidgetState == vtkContourWidget::Start )
    {
    return;
    }

  vtkContourRepresentation *rep =
    reinterpret_cast<vtkContourRepresentation*>(self->WidgetRep);

  if ( self->WidgetState == vtkContourWidget::Define )
    {
    rep->DeleteLastNode();
    }
  else
    {
    int X = self->Interactor->GetEventPosition()[0];
    int Y = self->Interactor->GetEventPosition()[1];
    rep->ActivateNode( X, Y );
    rep->DeleteActiveNode();
    rep->ActivateNode( X, Y );
    if ( rep->GetNumberOfNodes() < 3 )
      {
      rep->ClosedLoopOff();
      self->WidgetState = vtkContourWidget::Define;
      }
    }

  if ( rep->GetNeedToRender() )
    {
    self->Render();
    rep->NeedToRenderOff();
    }
}

//-------------------------------------------------------------------------
void vtkContourWidget::MoveAction(vtkAbstractWidget *w)
{
  vtkContourWidget *self = reinterpret_cast<vtkContourWidget*>(w);

  if ( self->WidgetState == vtkContourWidget::Start ||
       self->WidgetState == vtkContourWidget::Define )
    {
    return;
    }

  int X = self->Interactor->GetEventPosition()[0];
  int Y = self->Interactor->GetEventPosition()[1];
  vtkContourRepresentation *rep =
    reinterpret_cast<vtkContourRepresentation*>(self->WidgetRep);

  if ( rep->GetCurrentOperation() == vtkContourRepresentation::Inactive )
    {
    reinterpret_cast<vtkContourRepresentation*>(self->WidgetRep)->
      ComputeInteractionState( X, Y );
    reinterpret_cast<vtkContourRepresentation*>(self->WidgetRep)->
      ActivateNode( X, Y );
    }
  else
    {
    double pos[2];
    pos[0] = X;
    pos[1] = Y;
    self->WidgetRep->WidgetInteraction(pos);
    }

  self->Interaction();

  if ( self->WidgetRep->GetNeedToRender() )
    {
    self->Render();
    self->WidgetRep->NeedToRenderOff();
    }
}

//-------------------------------------------------------------------------
void vtkContourWidget::EndSelectAction(vtkAbstractWidget *w)
{
  vtkContourWidget *self = reinterpret_cast<vtkContourWidget*>(w);
  vtkContourRepresentation *rep =
    reinterpret_cast<vtkContourRepresentation*>(self->WidgetRep);

  // Do nothing if inactive
  if ( rep->GetCurrentOperation() == vtkContourRepresentation::Inactive )
    {
    return;
    }

  rep->SetCurrentOperationToInactive();
  self->EventCallbackCommand->SetAbortFlag(1);
  self->EndInteraction();

  if ( self->WidgetRep->GetNeedToRender() )
    {
    self->Render();
    self->WidgetRep->NeedToRenderOff();
    }
}

// These are callbacks that are active when the user is manipulating the
// handles of the measure widget.
//----------------------------------------------------------------------
void vtkContourWidget::StartInteraction()
{
  this->Superclass::StartInteraction();
  this->InvokeEvent(vtkCommand::StartInteractionEvent,NULL);
}

//----------------------------------------------------------------------
void vtkContourWidget::Interaction()
{
  this->InvokeEvent(vtkCommand::InteractionEvent,NULL);
}

//----------------------------------------------------------------------
void vtkContourWidget::EndInteraction()
{
  this->Superclass::EndInteraction();
  this->InvokeEvent(vtkCommand::EndInteractionEvent,NULL);
}

//----------------------------------------------------------------------
void vtkContourWidget::PrintSelf(ostream& os, vtkIndent indent)
{
  //Superclass typedef defined in vtkTypeMacro() found in vtkSetGet.h
  this->Superclass::PrintSelf(os,indent);

}
<commit_msg>BUG: better interaction events<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkContourWidget.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
#include "vtkContourWidget.h"
#include "vtkOrientedGlyphContourRepresentation.h"
#include "vtkCommand.h"
#include "vtkCallbackCommand.h"
#include "vtkRenderWindowInteractor.h"
#include "vtkObjectFactory.h"
#include "vtkRenderer.h"
#include "vtkWidgetCallbackMapper.h"
#include "vtkSphereSource.h"
#include "vtkProperty.h"
#include "vtkEvent.h"
#include "vtkWidgetEvent.h"

#include <vtkstd/vector>
#include <vtkstd/set>
#include <vtkstd/algorithm>
#include <vtkstd/iterator>

vtkCxxRevisionMacro(vtkContourWidget, "1.6");
vtkStandardNewMacro(vtkContourWidget);

//----------------------------------------------------------------------
vtkContourWidget::vtkContourWidget()
{
  this->ManagesCursor = 0;

  this->WidgetState = vtkContourWidget::Start;
  this->CurrentHandle = 0;

  // These are the event callbacks supported by this widget
  this->CallbackMapper->SetCallbackMethod(vtkCommand::LeftButtonPressEvent,
                                          vtkWidgetEvent::Select,
                                          this, vtkContourWidget::SelectAction);
  this->CallbackMapper->SetCallbackMethod(vtkCommand::RightButtonPressEvent,
                                          vtkWidgetEvent::AddFinalPoint,
                                          this, vtkContourWidget::AddFinalPointAction);
  this->CallbackMapper->SetCallbackMethod(vtkCommand::MouseMoveEvent,
                                          vtkWidgetEvent::Move,
                                          this, vtkContourWidget::MoveAction);
  this->CallbackMapper->SetCallbackMethod(vtkCommand::LeftButtonReleaseEvent,
                                          vtkWidgetEvent::EndSelect,
                                          this, vtkContourWidget::EndSelectAction);
  this->CallbackMapper->SetCallbackMethod(vtkCommand::KeyPressEvent,
                                          vtkEvent::NoModifier, 46, 1, NULL,
                                          vtkWidgetEvent::Delete,
                                          this, vtkContourWidget::DeleteAction);

  this->CreateDefaultRepresentation();

}

//----------------------------------------------------------------------
vtkContourWidget::~vtkContourWidget()
{
}

//----------------------------------------------------------------------
void vtkContourWidget::CreateDefaultRepresentation()
{
  if ( ! this->WidgetRep )
    {
    vtkOrientedGlyphContourRepresentation *rep =
      vtkOrientedGlyphContourRepresentation::New();

    this->WidgetRep = rep;

    vtkSphereSource *ss = vtkSphereSource::New();
    ss->SetRadius(0.5);
    rep->SetActiveCursorShape( ss->GetOutput() );
    ss->Delete();

    rep->GetProperty()->SetColor(.25,1.0,.25);

    rep->GetActiveProperty()->SetRepresentationToSurface();
    rep->GetActiveProperty()->SetAmbient(0.1);
    rep->GetActiveProperty()->SetDiffuse(0.9);
    rep->GetActiveProperty()->SetSpecular(0.0);
    }
}

//----------------------------------------------------------------------
void vtkContourWidget::ClearContour()
{
}

//----------------------------------------------------------------------
void vtkContourWidget::SetEnabled(int enabling)
{
  // The handle widgets are not actually enabled until they are placed.
  // The handle widgets take their representation from the vtkContourRepresentation.
  if ( enabling )
    {
    if ( this->WidgetState == vtkContourWidget::Start )
      {
      reinterpret_cast<vtkContourRepresentation*>(this->WidgetRep)->VisibilityOff();
      }
    else
      {
      reinterpret_cast<vtkContourRepresentation*>(this->WidgetRep)->VisibilityOn();
      }
    }

  this->Superclass::SetEnabled(enabling);
}

// The following methods are the callbacks that the measure widget responds to.
//-------------------------------------------------------------------------
void vtkContourWidget::SelectAction(vtkAbstractWidget *w)
{
  vtkContourWidget *self = reinterpret_cast<vtkContourWidget*>(w);
  vtkContourRepresentation *rep =
    reinterpret_cast<vtkContourRepresentation*>(self->WidgetRep);

  int X = self->Interactor->GetEventPosition()[0];
  int Y = self->Interactor->GetEventPosition()[1];
  double pos[2];
  pos[0] = X;
  pos[1] = Y;

  switch ( self->WidgetState )
    {
    case vtkContourWidget::Start:
    case vtkContourWidget::Define:
      self->AddNode();
      break;
    case vtkContourWidget::Manipulate:
      if ( rep->ActivateNode(X,Y) )
        {
        self->StartInteraction();
        rep->SetCurrentOperationToTranslate();
        rep->StartWidgetInteraction(pos);
        self->EventCallbackCommand->SetAbortFlag(1);
        }
      else if ( rep->AddNodeOnContour( X, Y ) )
        {
        if ( rep->ActivateNode(X,Y) )
          {
          rep->SetCurrentOperationToTranslate();
          rep->StartWidgetInteraction(pos);
          }
        self->EventCallbackCommand->SetAbortFlag(1);
        }
      break;
    }

  if ( rep->GetNeedToRender() )
    {
    self->Render();
    rep->NeedToRenderOff();
    }
}

//-------------------------------------------------------------------------
void vtkContourWidget::AddFinalPointAction(vtkAbstractWidget *w)
{
  vtkContourWidget *self = reinterpret_cast<vtkContourWidget*>(w);
  vtkContourRepresentation *rep =
    reinterpret_cast<vtkContourRepresentation*>(self->WidgetRep);

  if ( self->WidgetState ==  vtkContourWidget::Define &&
       rep->GetNumberOfNodes() >= 1 )
    {
    self->AddNode();
    self->WidgetState = vtkContourWidget::Manipulate;
    self->EventCallbackCommand->SetAbortFlag(1);
    }

  if ( rep->GetNeedToRender() )
    {
    self->Render();
    rep->NeedToRenderOff();
    }
}

//------------------------------------------------------------------------
void vtkContourWidget::AddNode()
{
  int X = this->Interactor->GetEventPosition()[0];
  int Y = this->Interactor->GetEventPosition()[1];

  // If the rep already has at least 2 nodes, check how close we are to
  // the first
  if ( reinterpret_cast<vtkContourRepresentation*>(this->WidgetRep)->
       GetNumberOfNodes() > 1 )
    {
    int pixelTolerance2 =
      reinterpret_cast<vtkContourRepresentation*>(this->WidgetRep)->
      GetPixelTolerance();
    pixelTolerance2 *= pixelTolerance2;

    double displayPos[2];
    if ( !reinterpret_cast<vtkContourRepresentation*>(this->WidgetRep)->
         GetNthNodeDisplayPosition( 0, displayPos ) )
      {
      vtkErrorMacro("Can't get first node display position!");
      return;
      }

    if ( (X - displayPos[0]) * (X - displayPos[0]) +
         (Y - displayPos[1]) * (Y - displayPos[1]) <
         pixelTolerance2 )
      {
      // yes - we have made a loop. Stop defining and switch to
      // manipulate mode
      this->WidgetState = vtkContourWidget::Manipulate;
      reinterpret_cast<vtkContourRepresentation*>(this->WidgetRep)->ClosedLoopOn();
      this->EventCallbackCommand->SetAbortFlag(1);
      this->InvokeEvent( vtkCommand::WidgetValueChangedEvent, NULL );
      this->InvokeEvent(vtkCommand::InteractionEvent,NULL);
      return;
      }
    }

  if ( reinterpret_cast<vtkContourRepresentation*>(this->WidgetRep)->
       AddNodeAtDisplayPosition( X, Y ) )
    {
    this->WidgetState = vtkContourWidget::Define;
    reinterpret_cast<vtkContourRepresentation*>(this->WidgetRep)->VisibilityOn();
    this->EventCallbackCommand->SetAbortFlag(1);
    this->InvokeEvent(vtkCommand::InteractionEvent,NULL);
    }
}

//-------------------------------------------------------------------------
void vtkContourWidget::DeleteAction(vtkAbstractWidget *w)
{
  vtkContourWidget *self = reinterpret_cast<vtkContourWidget*>(w);

  if ( self->WidgetState == vtkContourWidget::Start )
    {
    return;
    }

  vtkContourRepresentation *rep =
    reinterpret_cast<vtkContourRepresentation*>(self->WidgetRep);

  if ( self->WidgetState == vtkContourWidget::Define )
    {
    rep->DeleteLastNode();
    }
  else
    {
    int X = self->Interactor->GetEventPosition()[0];
    int Y = self->Interactor->GetEventPosition()[1];
    rep->ActivateNode( X, Y );
    if ( rep->DeleteActiveNode() )
      {
      self->InvokeEvent(vtkCommand::InteractionEvent,NULL);
      }
    rep->ActivateNode( X, Y );
    if ( rep->GetNumberOfNodes() < 3 )
      {
      rep->ClosedLoopOff();
      self->WidgetState = vtkContourWidget::Define;
      }
    }

  if ( rep->GetNeedToRender() )
    {
    self->Render();
    rep->NeedToRenderOff();
    }
}

//-------------------------------------------------------------------------
void vtkContourWidget::MoveAction(vtkAbstractWidget *w)
{
  vtkContourWidget *self = reinterpret_cast<vtkContourWidget*>(w);

  if ( self->WidgetState == vtkContourWidget::Start ||
       self->WidgetState == vtkContourWidget::Define )
    {
    return;
    }

  int X = self->Interactor->GetEventPosition()[0];
  int Y = self->Interactor->GetEventPosition()[1];
  vtkContourRepresentation *rep =
    reinterpret_cast<vtkContourRepresentation*>(self->WidgetRep);

  if ( rep->GetCurrentOperation() == vtkContourRepresentation::Inactive )
    {
    reinterpret_cast<vtkContourRepresentation*>(self->WidgetRep)->
      ComputeInteractionState( X, Y );
    reinterpret_cast<vtkContourRepresentation*>(self->WidgetRep)->
      ActivateNode( X, Y );
    }
  else
    {
    double pos[2];
    pos[0] = X;
    pos[1] = Y;
    self->WidgetRep->WidgetInteraction(pos);
    self->Interaction();
    }

  if ( self->WidgetRep->GetNeedToRender() )
    {
    self->Render();
    self->WidgetRep->NeedToRenderOff();
    }
}

//-------------------------------------------------------------------------
void vtkContourWidget::EndSelectAction(vtkAbstractWidget *w)
{
  vtkContourWidget *self = reinterpret_cast<vtkContourWidget*>(w);
  vtkContourRepresentation *rep =
    reinterpret_cast<vtkContourRepresentation*>(self->WidgetRep);

  // Do nothing if inactive
  if ( rep->GetCurrentOperation() == vtkContourRepresentation::Inactive )
    {
    return;
    }

  rep->SetCurrentOperationToInactive();
  self->EventCallbackCommand->SetAbortFlag(1);
  self->EndInteraction();

  if ( self->WidgetRep->GetNeedToRender() )
    {
    self->Render();
    self->WidgetRep->NeedToRenderOff();
    }
}

// These are callbacks that are active when the user is manipulating the
// handles of the measure widget.
//----------------------------------------------------------------------
void vtkContourWidget::StartInteraction()
{
  this->Superclass::StartInteraction();
  this->InvokeEvent(vtkCommand::StartInteractionEvent,NULL);
}

//----------------------------------------------------------------------
void vtkContourWidget::Interaction()
{
  this->InvokeEvent(vtkCommand::InteractionEvent,NULL);
}

//----------------------------------------------------------------------
void vtkContourWidget::EndInteraction()
{
  this->Superclass::EndInteraction();
  this->InvokeEvent(vtkCommand::EndInteractionEvent,NULL);
}

//----------------------------------------------------------------------
void vtkContourWidget::PrintSelf(ostream& os, vtkIndent indent)
{
  //Superclass typedef defined in vtkTypeMacro() found in vtkSetGet.h
  this->Superclass::PrintSelf(os,indent);

}
<|endoftext|>