#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;
}