キネクトによるエージェントの操作(Man-nii-v2.xml)
[
Front page
] [
New
|
List of pages
|
Search
|
Recent changes
|
Help
]
Start:
[[キネクトによるエージェントの操作]]
#highlight(cpp){{
#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+g...
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,...
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) < l...
my->setJointQuaternion(type,w,x,y,z);
continue;
}
}
}
}
extern "C" Controller * createController ()
{
return new AgentController;
}
}}
#highlight(end)
End:
[[キネクトによるエージェントの操作]]
#highlight(cpp){{
#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+g...
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,...
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) < l...
my->setJointQuaternion(type,w,x,y,z);
continue;
}
}
}
}
extern "C" Controller * createController ()
{
return new AgentController;
}
}}
#highlight(end)
Page: