void GeneratePointCloud(DepthGenerator& rDepthGen, const XnDepthPixel* pDepth, VISION_DATA &pData)
{
DepthMetaData mDepthMD;
rDepthGen.GetMetaData(mDepthMD);
pData.timeStamp = mDepthMD.Timestamp();
unsigned int uPointNum = mDepthMD.FullXRes() * mDepthMD.FullYRes();
XnPoint3D* pDepthPointSet = new XnPoint3D[uPointNum];
unsigned int i, j, idxshift, idx;
for( j = 0; j < mDepthMD.FullYRes(); ++j)
{
idxshift = j * mDepthMD.FullXRes();
for(i = 0; i < mDepthMD.FullXRes(); ++i)
{
idx = idxshift + i;
pDepthPointSet[idx].X = i;
pDepthPointSet[idx].Y = j;
pDepthPointSet[idx].Z = pDepth[idx];
}
}
XnPoint3D* p3DPointSet = new XnPoint3D[uPointNum];
rDepthGen.ConvertProjectiveToRealWorld(uPointNum, pDepthPointSet, p3DPointSet);
memcpy(pData.pointCloud, p3DPointSet, uPointNum*3*sizeof(float));
delete[] pDepthPointSet;
delete[] p3DPointSet;
}
//----------------------------------------------------
// マウスのクリック処理
//----------------------------------------------------
void glutMouse(int button, int state, int _x, int _y){
int x = _x, y = _y;
XnPoint3D pt[2] = {{0,0,0},{0,0,0}};
// サイズが違う場合，680*480に標準化する
if(!(g_currentWindowSizeX == KINECT_IMAGE_WIDTH && g_currentWindowSizeY == KINECT_IMAGE_HEIGHT)){
x = 640 * _x / g_currentWindowSizeX;
y = 480 * _y / g_currentWindowSizeY;
}
if(state == GLUT_DOWN){
if(button == GLUT_LEFT_BUTTON){ // 左クリック
cout << "click! (" << _x << ", " << _y << ")->(" << x << ", " << y << "), depth = " << *(g_depthMD.Data() + y * KINECT_IMAGE_WIDTH + x) << endl;
pt[0].X = _x;
pt[0].Y = _y;
pt[0].Z = *(g_depthMD.Data() + y * KINECT_IMAGE_WIDTH + x);
//cout << "(_x, _y) -> (x, y) = (" << _x << ", " << _y << ") -> (" << x << ", " << y << ")" << endl;
g_depth.ConvertProjectiveToRealWorld(2, pt, pt);
cout << "change pt[0] => (" << pt[0].X << ", " << pt[0].Y << ", " << pt[0].Z << ")" << endl;
}else if(button == GLUT_RIGHT_BUTTON){ // 右クリック
cout << "click! back = (" << x << ", " << y << ") depth = " << *(g_pBackDepth + y * KINECT_IMAGE_WIDTH + x) << endl;
pt[1].X = _x;
pt[1].Y = _y;
pt[1].Z = *(g_pBackDepth + y * KINECT_IMAGE_WIDTH + x);
g_depth.ConvertProjectiveToRealWorld(2, pt, pt);
cout << "change pt[1] => (" << pt[1].X << ", " << pt[1].Y << ", " << pt[1].Z << ")" << endl;
}
//g_depth.ConvertProjectiveToRealWorld(2, pt, pt);
//cout << "change pt[0] => (" << pt[0].X << ", " << pt[0].Y << ", " << pt[0].Z << ")" << endl;
//cout << "change pt[1] => (" << pt[1].X << ", " << pt[1].Y << ", " << pt[1].Z << ")" << endl;
}
}