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

List of Backups
View the diff.
View the diff current.
View the source.
Go to キネクトによるエージェントの操作(Man-nii-v2.xml).
- 1 (2012-03-01 (Thu) 14:06:53)
- 2 (2012-03-01 (Thu) 14:10:41)

キネクトによるエージェントの操作?

#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," "));
             //エージェント座標からグローバル座標への変換
             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;

}

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