キネクトによるエージェントの操作(Man-nii-v2.xml)

#include <string>
#include "Controller.h"
#include "Logger.h"
#include "ControllerEvent.h"

#define PI 3.141592
#define DEG2RAD(DEG) ( (PI) * (DEG) / 180.0 )

using namespace std;

class AgentController : public Controller
{
public:
  double onAction(ActionEvent &evt);
  void onRecvMessage(RecvMessageEvent &evt);
  void onInit(InitEvent &evt);

private:
  //初期位置
  double m_posx, m_posy, m_posz;
  double m_yrot;
  double m_range;
  //データ数（関節数）最大値
  int m_maxsize;
};

void AgentController::onInit(InitEvent &evt)
{
  SimObj *my = getObj(myname());
  m_posx = my->x();
  m_posy = my->y();
  m_posz = my->z();
  m_range = 0.1;
  m_maxsize = 15;
  double qw = my->qw();
  double qy = my->qy();
  m_yrot = acos(fabs(qw))*2;
  if(qw*qy > 0)
    m_yrot = -1*m_yrot;

}

double AgentController::onAction(ActionEvent &evt)
{
  return 10.0;
}

void AgentController::onRecvMessage(RecvMessageEvent &evt)
{
  //自分自身の取得
  SimObj *my = getObj(myname());

//メッセージ取得
  char *all_msg = (char*)evt.getString(0);

char *msg = strtok(all_msg," ");
  if(strcmp(msg,"KINECT_DATA") == 0)
    {
      int i = 0;
      while(true)
        {
          i++;
          if(i == m_maxsize+1) break;
          char *type = strtok(NULL,":");

//体の位置
          if(strcmp(type,"POSITION") == 0)
            {
              double x = atof(strtok(NULL,","));
              double y = atof(strtok(NULL,","));
              double z = atof(strtok(NULL," "));
              //キネクト座標からSIGVerse座標への変換
              double gx = cos(m_yrot)*x - sin(m_yrot)*z;
              double gz = sin(m_yrot)*x + cos(m_yrot)*z;
              my->setPosition(m_posx+gx,m_posy+y,m_posz+gz);
              continue;
            }

//体全体の回転
          else if(strcmp(type,"WAIST") == 0)
            {
              double w = atof(strtok(NULL,","));
              double x = atof(strtok(NULL,","));
              double y = atof(strtok(NULL,","));
              double z = atof(strtok(NULL," "));
              my->setJointQuaternion("ROOT_JOINT0",w,x,y,z);
              continue;
            }

else if(strcmp(type,"END") == 0)  break;

//関節の回転
          else
            {
              double w = atof(strtok(NULL,","));
              double x = atof(strtok(NULL,","));
              double y = atof(strtok(NULL,","));
              double z = atof(strtok(NULL," "));
              double angle = acos(w)*2;
              double tmp = sin(angle/2);
              double vx = x/tmp;
              double vy = y/tmp;
              double vz = z/tmp;
              double len = sqrt(vx*vx+vy*vy+vz*vz);
              if(len < (1 - m_range) || (1 + m_range) < len) continue;
              my->setJointQuaternion(type,w,x,y,z);
              continue;
            }
        }
    }
}

extern "C"  Controller * createController ()
{
  return new AgentController;
}

キネクトによるエージェントの操作(Man-nii-v2.xml)

recent(20)