interpret_motion.c -運動認識プログラム
#include
#include
#include
#define MAXSIZE 8
/* 抽出した点に関する情報を格納するための構造体 */
struct point
{
int rsize; /* 赤色の球の面積 */
int gsize; /* 緑色の球の面積 */
int row_r; /* 赤色の球の重心のy座標 */
int col_r; /* 赤色の球の重心のx座標 */
int row_g; /* 緑色の球の重心のy座標 */
int col_g; /* 緑色の球の重心のx座標 */
int left_r; /* 赤色の球の直径の左端の点のx座標 */
int right_r; /* 赤色の球の直径の右端の点のx座標 */
int up_r; /* 赤色の球の直径の上端の点のy座標 */
int down_r; /* 赤色の球の直径の下端の点のy座標 */
int left_g; /* 緑色の球の直径の左端の点のx座標 */
int right_g; /* 緑色の球の直径の右端の点のx座標 */
int up_g; /* 緑色の球の直径の上端の点のy座標 */
int down_g; /* 緑色の球の直径の下端の点のy座標 */
};
/* 運動を表す文字定数 */
#define KEEP 1
#define UP 2
#define DOWN 3
#define RIGHT 4
#define LEFT 5
#define FORWARD 6
#define BACK 7
#define ROTX 8
#define ROT_X 9
#define ROTY 10
#define ROT_Y 11
#define ROTZ 12
#define ROT_Z 13
/* 運動認識 */
int interpret_motion(struct point oldpoint, struct point newpoint)
{
int dif_row_r,dif_col_r,dif_row_g,dif_col_g; /* 変化量を格納 */
double dif_rsize,dif_gsize;
int thres_col=20,thres_row=20; /* 運動認識に */
double thres_sizemin=0.6,thres_sizemax=1.6; /* おけるしきい値 */
double slant,oldslant;
double thres_slant=1.0;
int motion = KEEP; /* 認識結果を格納 */
/* 識別子が視界から外れた場合 */
if ((newpoint.rsize == 0) || (newpoint.gsize == 0)){
printf("ball got out of frame!\n");
return(motion);
}
/* 変化量の計算 */
dif_row_r = newpoint.row_r - oldpoint.row_r;
dif_col_r = newpoint.col_r - oldpoint.col_r;
dif_row_g = newpoint.row_g - oldpoint.row_g;
dif_col_g = newpoint.col_g - oldpoint.col_g;
dif_rsize = (double)newpoint.rsize / (double)oldpoint.rsize;
dif_gsize = (double)newpoint.gsize / (double)oldpoint.gsize;
/* 2つの球の重心を通る直線の傾きの計算 */
slant = (double)fabs(newpoint.row_r - newpoint.row_g) /
(double)fabs(newpoint.col_r - newpoint.col_g);
oldslant = (double)fabs(oldpoint.row_r - oldpoint.row_g) /
(double)fabs(oldpoint.col_r - oldpoint.col_g);
/* printf("dif_col = %d\n", newpoint.col_r - newpoint.col_g);
printf("dif_row_r = %d dif_row_g = %d\n",dif_row_r,dif_row_g);
printf("dif_col_r = %d dif_col_g = %d\n",dif_col_r,dif_col_g);
printf("dif_rsize = %f dif_gsize = %f\n",dif_rsize,dif_gsize);
printf("slant = %f\n",slant); */
/* 2つの球の重心を通る直線の傾きがしきい値よりも大きい場合 */
if (slant > thres_slant)
/* 2つの球の面積の変化方向が同じ場合 */
if (dif_rsize > thres_sizemin && dif_rsize < thres_sizemax &&
dif_gsize > thres_sizemin && dif_gsize < thres_sizemax) {
/* 初期位置で2つの球が横に並んでいた場合 */
if (oldslant < 1.0)
/* 位置の変化方向が左の場合 */
if (dif_col_r > 0 || dif_col_g > 0)
motion = ROTZ; /* +z軸回りの回転 */
/* 位置の変化方向が右の場合 */
else if(dif_col_r < 0 || dif_col_g < 0)
motion = ROT_Z; /* -z軸回りの回転運動 */
}
else {
/* 初期位置で2つの球が縦に並んでいた場合 */
if (oldslant > 1.0) {
/* 位置の変化方向が球1が下、球2が上、*/
/* かつ */
/* 面積の変化方向が球1が小、球2が大 */
if (((dif_rsize < thres_sizemin && dif_row_r > thres_row
&& dif_gsize > 1.0 ) ||
(dif_gsize > thres_sizemax && dif_row_g < -thres_row
&& dif_gsize < 1.0 )) ||
((dif_rsize > thres_sizemax && dif_row_r < -thres_row
&& dif_gsize < 1.0 ) ||
(dif_gsize < thres_sizemin && dif_row_g > thres_row
&& dif_rsize > 1.0 )))
motion = ROTX; /* +x軸回りの回転運動 */
/* 位置の変化方向が球1が下、球2が上 */
/* かつ */
/* 面積の変化方向が球1が大、球2が小 */
else if (((dif_rsize > thres_sizemax && dif_row_r > thres_row) ||
(dif_gsize < thres_sizemin && dif_row_g < -thres_row)) ||
((dif_rsize < thres_sizemin && dif_row_r < -thres_row) ||
(dif_gsize > thres_sizemax && dif_row_g > thres_row)))
motion = ROT_X; /* -x軸回りの回転運動 */
}
}
/* 2つの球の面積の変化方向が同じ場合 */
else if (dif_rsize > thres_sizemin && dif_rsize < thres_sizemax &&
dif_gsize > thres_sizemin && dif_gsize < thres_sizemax) {
/* 2つの球の位置の変化方向が上 */
if ((dif_row_r < -thres_row) && (dif_row_g < -thres_row))
motion = UP; /* +y方向への並進運動 */
/* 2つの球の位置の変化方向が下 */
else if ((dif_row_r > thres_row) && (dif_row_g > thres_row))
motion = DOWN; /* -y方向への並進運動 */
/* 2つの球の位置の変化方向が右 */
else if ((dif_col_r < -thres_col) && (dif_col_g < -thres_col))
motion = RIGHT; /* +x方向への並進運動 */
/* 2つの球の位置の変化方向が左 */
else if ((dif_col_r > thres_col) && (dif_col_g > thres_col))
motion = LEFT; /* -x方向への並進運動 */
}
else {
/* 2つの球の面積の変化方向が大 */
if ((dif_rsize > thres_sizemax) && (dif_gsize > thres_sizemax))
motion = FORWARD; /* +z方向への並進運動 */
/* 2つの球の面積の変化方向が小 */
else if ((dif_rsize < thres_sizemin) && (dif_gsize < thres_sizemin))
motion = BACK; /* -z方向への並進運動 */
/* 位置の変化方向が球1が左、球2が右 */
/* かつ */
/* 面積の変化方向が球1が大、球2が小 */
else if (((dif_rsize > thres_sizemax && dif_gsize < 1.0
&& dif_col_r > thres_col) ||
(dif_rsize > 1.0 && dif_gsize < thres_sizemin
&& dif_col_g < -thres_col)) ||
((dif_rsize < thres_sizemin && dif_gsize > 1.0
&& dif_col_r < -thres_col) ||
(dif_rsize < 1.0 && dif_gsize > thres_sizemax
&& dif_col_g > thres_col)))
motion = ROTY; /* +y軸回りの回転運動 */
/* 位置の変化方向が球1が左、球2が右 */
/* かつ */
/* 面積の変化方向が球1が小、球2が大 */
else if (((dif_rsize < thres_sizemin && dif_gsize > 1.0
&& dif_col_r > thres_col) ||
(dif_rsize < 1.0 && dif_gsize > thres_sizemax
&& dif_col_g < -thres_col)) ||
((dif_rsize > thres_sizemax && dif_gsize < 1.0
&& dif_col_r < -thres_col) ||
(dif_rsize > 1.0 && dif_gsize < thres_sizemin
&& dif_col_g > thres_col)))
motion = ROT_Y; /* -y軸回りの回転運動 */
}
return(motion);
}
programのページへ戻る