void display_points(){
//画面のキャプチャ
capture();
//移動平均
FIFOFilter();
//除算->データ格納
for(int i = 0; i < dPixels; i++)
{
aveDepthData[i] = (unsigned short)(Sum[i]*Kd); //移動加算値Sum[i]から移動平均値kを求める
}
//CameraSpacePointへ変換
ERROR_CHECK2(coordinateMapper->MapDepthFrameToCameraSpace(
width * height, (UINT16*)aveDepthData,
width * height, cameraSpacePoints_ave),"MapDepthFrameToCameraSpace_ave");
//**描画処理**//
glEnable(GL_DEPTH_TEST);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity(); //変換行列の初期化
float cx = 0.0f;
float cy = 0.0f;
float cz = 1.0f;
float x = r * cos(fai) * sin(theta) + cx;
float y = r * sin(fai) + cy;
float z = r * cos(fai) * cos(theta) + cz;
gluLookAt(x, y, z, cx, cy, cz, 0.0f, 1.0f, 0.0f);
glPointSize(1.0f);
glBegin(GL_POINTS);
for(int y = 0; y < height; y++){
for(int x = 0; x < width; x++){
int index = y * width + x;
float px = cameraSpacePoints[index].X;
float py = cameraSpacePoints[index].Y;
float pz = cameraSpacePoints[index].Z;
//float px = cameraSpacePoints_ave[index].X;
//float py = cameraSpacePoints_ave[index].Y;
//float pz = cameraSpacePoints_ave[index].Z;
glColor3f(colorMap[index][0], colorMap[index][1], colorMap[index][2]);
glVertex3f(px, py, pz);
}
}
glEnd();
glutSwapBuffers();
}
/*-------------------------------------------------------------------------
* xmunmap - xmunmap
*-------------------------------------------------------------------------
*/
SYSCALL xmunmap(int virtpage )
{
STATWORD ps;
int bsid, page;
/* sanity check ! */
if ( (virtpage < 4096) ){
kprintf("xmummap call error: virtpage (%d) invalid! \n", virtpage);
return SYSERR;
}
if(OK == bsm_lookup(currpid, VPN2VAD(virtpage), &bsid, &page)){
write_back_frames(currpid, bsid);
}
ERROR_CHECK2( bsm_unmap(currpid, virtpage), ps);
return OK;
}
/*-------------------------------------------------------------------------
* xmmap - xmmap
*-------------------------------------------------------------------------
*/
SYSCALL xmmap(int virtpage, bsd_t source, int npages)
{
/* sanity check ! */
STATWORD ps;
if ( (virtpage < 4096) || INVALID_BSID(source) ||(npages < 1) || ( npages > 128)){
kprintf("xmmap call error: parameter error! \n");
return SYSERR;
}
if(bsm_tab[source].bs_status == BSM_MAPPED_PR){
kprintf("Error: Trying to map private heap\n");
return SYSERR;
}
disable(ps);
ERROR_CHECK2( bsm_map(currpid, virtpage, source, npages), ps );
restore(ps);
return OK;
}
void GLWrapper::drawGeometry(GLuint vao, GLuint elementCount)
{
GLLOG(glBindVertexArray(vao));ERROR_CHECK1(vao);
GLLOG(glDrawElements(GL_TRIANGLES, elementCount, GL_UNSIGNED_INT, 0));ERROR_CHECK2(vao,elementCount);
}
void capture()
{
IMultiSourceFrame *multiFrame = NULL;
IColorFrame *colorFrame = NULL;
IColorFrameReference *colorFrameReference = NULL;
UINT colorBufferSize = 0;
RGBQUAD *colorBuffer = NULL;
IDepthFrame *depthFrame = NULL;
IDepthFrameReference *depthFrameReference = NULL;
UINT bufferSize = 0;
// UINT16 *depthBuffer = NULL;
IBodyIndexFrame *bodyIndexFrame = NULL;
IBodyIndexFrameReference *bodyIndexFrameReference = NULL;
UINT bodyIndexBufferSize = 0;
static int lastTime = 0;
static int currentTime = 0;
HRESULT hr = -1;
//フレームリーダーが読み込み可能になるのを待つループ(各Frameしか取らない)
while(1) {
if((currentTime = GetTickCount()) > 33)
{
hr = multiFrameReader->AcquireLatestFrame(&multiFrame);
lastTime = currentTime;
}else continue;
if(FAILED(hr)) {
//fprintf(stderr, "AcquireLatestFrame(&multiFrame)\n");
Sleep(1);
continue;
}
hr = multiFrame->get_ColorFrameReference(&colorFrameReference);
if(FAILED(hr)) {
Sleep(1);
fprintf(stderr, "ColorFrameReference(&colorFrameReference)\n");
SafeRelease(multiFrame);
continue;
}
hr = colorFrameReference->AcquireFrame(&colorFrame);
if(FAILED(hr)) {
Sleep(1);
fprintf(stderr, "AcquireFrame(&colorFrame)\n");
SafeRelease(colorFrameReference);
SafeRelease(multiFrame);
continue;
}
hr = multiFrame->get_DepthFrameReference(&depthFrameReference);
if (FAILED(hr)) {
Sleep(1);
fprintf(stderr, "DepthFrameReference(&depthFrameReference)\n");
SafeRelease(colorFrame);
SafeRelease(colorFrameReference);
SafeRelease(multiFrame);
continue;
}
hr = depthFrameReference->AcquireFrame(&depthFrame);
if (FAILED(hr)) {
Sleep(1);
fprintf(stderr, "AcquireFrame(&depthFrame)\n");
SafeRelease(depthFrameReference);
SafeRelease(colorFrame);
SafeRelease(colorFrameReference);
SafeRelease(multiFrame);
continue;
}
// hr = depthFrame->AccessUnderlyingBuffer(&bufferSize, &depthBuffer);
hr = depthFrame->CopyFrameDataToArray( dPixels, &depthBuffer[0] );
if (FAILED(hr)) {
Sleep(1);
fprintf(stderr, "AccessUnderlyingBuffer(&bufferSize, &depthBuffer\n");
SafeRelease(depthFrame);
SafeRelease(depthFrameReference);
SafeRelease(colorFrame);
SafeRelease(colorFrameReference);
SafeRelease(multiFrame);
continue;
}
hr = multiFrame->get_BodyIndexFrameReference(&bodyIndexFrameReference);
if (FAILED(hr)) {
Sleep(1);
fprintf(stderr, "BodyIndexReference(&colorFrameReference)\n");
free(depthBuffer);
SafeRelease(depthFrame);
SafeRelease(depthFrameReference);
SafeRelease(colorFrame);
SafeRelease(colorFrameReference);
SafeRelease(multiFrame);
continue;
}
hr = bodyIndexFrameReference->AcquireFrame(&bodyIndexFrame);
if (FAILED(hr)) {
Sleep(1);
fprintf(stderr, "AcquireFrame(&bodyIndexFrame)\n");
SafeRelease(bodyIndexFrameReference);
free(depthBuffer);
SafeRelease(depthFrame);
SafeRelease(depthFrameReference);
SafeRelease(colorFrame);
SafeRelease(colorFrameReference);
SafeRelease(multiFrame);
continue;
}
hr = bodyIndexFrame->AccessUnderlyingBuffer(&bodyIndexBufferSize, &bodyIndexBuffer);
if(FAILED(hr))
{
Sleep(1);
fprintf(stderr, "bodyIndexFrame->AccessUnderlyingBuffer(&bodyIndexBufferSize, &bodyIndexBuffer)");
SafeRelease(bodyIndexFrame);
SafeRelease(bodyIndexFrameReference);
free(depthBuffer);
SafeRelease(depthFrame);
SafeRelease(depthFrameReference);
SafeRelease(colorFrame);
SafeRelease(colorFrameReference);
SafeRelease(multiFrame);
continue;
}
SafeRelease(colorFrameReference);
SafeRelease(bodyIndexFrameReference);
SafeRelease(depthFrameReference);
break;
}
//深度値をBufferへ格納
// ERROR_CHECK(depthFrame->AccessUnderlyingBuffer(&bufferSize, &depthBuffer));
//カラーマップの設定データの読み込みと、colorBufferメモリの確保
if(colorRGBX == NULL)
{
IFrameDescription *colorFrameDescription = NULL;
ERROR_CHECK2(colorFrame->get_FrameDescription(&colorFrameDescription), "FrameDescription");
ERROR_CHECK2(colorFrameDescription->get_Width(&colorWidth), "get_Width");
ERROR_CHECK2(colorFrameDescription->get_Height(&colorHeight), "get_Height");
colorRGBX = new RGBQUAD[colorWidth * colorHeight];
glutReshapeWindow(width, height);
ERROR_CHECK2(colorFrame->get_RawColorImageFormat(&imageFormat), "get_RawColorImageFormat");
SafeRelease(colorFrameDescription);
}
//カラーイメージをcolorBufferへコピー
if(imageFormat == ColorImageFormat_Bgra)
{
ERROR_CHECK2(colorFrame->AccessRawUnderlyingBuffer(&colorBufferSize,
reinterpret_cast<BYTE**>(&colorBuffer)), "AccessRawUnderlyingBuffer");
}else if(colorRGBX)
{
colorBuffer = colorRGBX;
colorBufferSize = colorWidth * colorHeight * sizeof(RGBQUAD);
ERROR_CHECK2(colorFrame->CopyConvertedFrameDataToArray(colorBufferSize,
reinterpret_cast<BYTE*>(colorBuffer),
ColorImageFormat_Bgra), "CopyConvertedFrameDataToArray");
}else
{
//Error
}
//colorMapの初期化 一度だけ実行される
if(colorMap == NULL)
{
colorMap = new float[colorWidth * colorHeight][3];
}
if (colorCoordinates == NULL)
{
colorCoordinates = new ColorSpacePoint[width * height];
}
if (cameraSpacePoints == NULL)
{
cameraSpacePoints = new CameraSpacePoint[width * height];
}
if (cameraSpacePoints_ave == NULL)
{
cameraSpacePoints_ave = new CameraSpacePoint[width * height];
}
ERROR_CHECK2(coordinateMapper->MapDepthFrameToColorSpace(
width * height, (UINT16*)depthBuffer,
width * height, colorCoordinates), "MapDepthFrameToColorSpace");
ERROR_CHECK2(coordinateMapper->MapDepthFrameToCameraSpace(
width * height, (UINT16*)depthBuffer,
width * height, cameraSpacePoints),"MapDepthFrameToCameraSpace");
//colorBufferのデータを、colorMapにコピー
for(int i = 0; i < height; i++){
for(int j = 0; j < width; j++){
int index = i * width + j;
ColorSpacePoint colorPoint = colorCoordinates[index];
int colorX = (int)(floor(colorPoint.X + 0.5));
int colorY = (int)(floor(colorPoint.Y + 0.5));
if(colorX >= 0 && colorX < colorWidth && colorY >= 0 && colorY < colorHeight)
{
int colorIndex = colorX + colorY * colorWidth;
//格納先はミラーモードを解除
float* colorp = colorMap[index];
UCHAR* data = (UCHAR*)(colorBuffer + colorIndex);
colorp[0] = (float)data[2] / 255.0f;
colorp[1] = (float)data[1] / 255.0f;
colorp[2] = (float)data[0] / 255.0f;
}else
{
float* colorp = colorMap[index];
colorp[0] = 0;
colorp[1] = 0;
colorp[2] = 0;
}
}
}
//フレームリソースを解放
SafeRelease(colorFrame);
SafeRelease(multiFrame);
SafeRelease(bodyIndexFrame);
SafeRelease(depthFrame);
}
