void TriangleMesh::UpdateDepth( StereoKinectHeadTracking* tracking, vector<NUI_DEPTH_IMAGE_POINT>& points )
{
INuiCoordinateMapper* coordMapper;
tracking->GetSensor()->NuiGetCoordinateMapper(&coordMapper);
for ( unsigned int i = 0; i < _x; i++ )
{
for ( unsigned int j = 0; j < _y; j++ )
{
NUI_DEPTH_IMAGE_POINT o = points[(_y-j-1)*_x + i];
Vector4 p;
coordMapper->MapDepthPointToSkeletonPoint(NUI_IMAGE_RESOLUTION_640x480, &points[(_y-j-1)*_x + i], &p);
//p = NuiTransformDepthImageToSkeleton(o.x, o.y, o.depth, NUI_IMAGE_RESOLUTION_640x480);
glm::vec3 rwPoint = glm::vec3(p.x, p.y, p.z);
glm::vec3 vwPoint = tracking->SensorToVirtualWorldCoordinates(rwPoint);
vwPoint += glm::vec3(-0.027f, 0.03f, 0.013f); // Seen error
if (false/*i == _x/2 && j == 0*/) {
std::cout << "Depth conversion " << i <<","<<j<<":" << std::endl;
std::cout << o.x << "," << o.y << "," << o.depth << std::endl;
std::cout << "->" << p.x << "," << p.y << "," << p.z << std::endl;
std::cout << "VW:" << vwPoint.x << "," << vwPoint.y << "," << vwPoint.z << endl;
std::cout << "-------------------------" << std::endl;
}
_mesh[ i*_y + j ].coord[0] = vwPoint.x;
_mesh[ i*_y + j ].coord[1] = vwPoint.y;
_mesh[ i*_y + j ].coord[2] = vwPoint.z;
}
}
UpdateVBO();
}
void Line::Render()
{
Mat4 Identity;
UpdateVBO();
glDisable(GL_DEPTH_TEST);
glBindTexture(GL_TEXTURE_2D, 0);
// Set the color.
SetShaderParameters(false, false, false, false, false, true);
WindowFrame.SetUniform(U_COLOR, R, G, B, A);
WindowFrame.SetUniform(U_MVP, &(Identity[0][0]));
// Assign position attrib. pointer
lnvbo->Bind();
glVertexAttribPointer(WindowFrame.EnableAttribArray(A_POSITION), 2, GL_FLOAT, GL_FALSE, 0, 0);
ColorBuffer->Bind();
glVertexAttribPointer(WindowFrame.EnableAttribArray(A_COLOR), 4, GL_FLOAT, GL_FALSE, sizeof(float) * 4, 0);
glDrawArrays(GL_LINES, 0, 2);
WindowFrame.DisableAttribArray(A_POSITION);
WindowFrame.DisableAttribArray(A_COLOR);
Image::ForceRebind();
glEnable(GL_DEPTH_TEST);
}
void TriangleMesh::UpdateDepth( float* _depth_map )
{
for ( unsigned int i = 0; i < _x; i++ )
{
for ( unsigned int j = 0; j < _y; j++ )
{
_mesh[ i*_y + j ].coord[2] = _start_z + _depth_map[ (_y-j-1)*_x + i ];
}
}
UpdateVBO();
}
void Terrain::Reset()
{
for (int i = 0; i < m_heightField.xDim; ++i) {
for (int j = 0; j < m_heightField.zDim; ++j) {
m_heightField.values[i][j] = 0.0f;
}
}
isGeomDirty = true;
UpdateVBO();
}
static int
Recompute(void)
{
if(!ComputeKernel)
return CL_SUCCESS;
if (NDRangeCount > MaxNDRange)
{
printf("Reach Max NDRange, Quitting\n");
Cleanup();
exit(0);
}
if ( EnableStideExec && (ExecutionCount / ExecuteStride) % 2 == 1)
{
printf("GL only for frame %d\n", ExecutionCount);
return CL_SUCCESS;
}
int err = 0;
int currentBuffer = CurrentBuffer;
int nextBuffer = (CurrentBuffer+1)%2;
if(Animated || Update)
{
glFinish();
// If use shared context, then data should be already in GL VBOs even for the 1st frame
#if (USE_GL_ATTACHMENTS)
err = clEnqueueAcquireGLObjects(ComputeCommands, 1, &ComputePosBuffer[currentBuffer], 0, 0, 0);
if (err != CL_SUCCESS)
{
printf("Failed to acquire GL object! %d\n", err);
return EXIT_FAILURE;
}
err = clEnqueueAcquireGLObjects(ComputeCommands, 1, &ComputePosBuffer[nextBuffer], 0, 0, 0);
if (err != CL_SUCCESS)
{
printf("Failed to acquire GL object! %d\n", err);
return EXIT_FAILURE;
}
#if (DEBUG_INFO)
float *DataCurPos = (float *)calloc(1, 4 * sizeof(float) * DataBodyCount);
float *DataNexPos = (float *)calloc(1, 4 * sizeof(float) * DataBodyCount);
err = clEnqueueReadBuffer( ComputeCommands, ComputePosBuffer[currentBuffer], CL_TRUE, 0, 4 * sizeof(float) * DataBodyCount, DataCurPos, 0, NULL, NULL );
if (err != CL_SUCCESS)
{
printf("Failed to read buffer! %d\n", err);
return EXIT_FAILURE;
}
err = clEnqueueReadBuffer( ComputeCommands, ComputePosBuffer[nextBuffer], CL_TRUE, 0, 4 * sizeof(float) * DataBodyCount, DataNexPos, 0, NULL, NULL );
if (err != CL_SUCCESS)
{
printf("Failed to read buffer! %d\n", err);
return EXIT_FAILURE;
}
for (int i = 0; i < DataBodyCount; ++i)
printf("Before NDRange %d - %d: org [%f %f %f %f] - org curr [%f %f %f %f] - org next [%f %f %f %f]\n", NDRangeCount, i,
DataInput[4 * i], DataInput[4 * i + 1], DataInput[4 * i + 2], DataInput[4 * i + 3],
DataCurPos[4 * i], DataCurPos[4 * i + 1], DataCurPos[4 * i + 2], DataCurPos[4 * i + 3],
DataNexPos[4 * i], DataNexPos[4 * i + 1], DataNexPos[4 * i + 2], DataNexPos[4 * i + 3]);
free(DataCurPos);
free(DataNexPos);
#endif
#else
// Not sharing context with OpenGL, needs to explicitly send data to GPU for the 1st frame
if (!NDRangeCount)
{
printf("1st Frame! Let's send data to GPU!\n");
err = clEnqueueWriteBuffer(ComputeCommands, ComputePosBuffer[currentBuffer], 1, 0,
4 * sizeof(float) * DataBodyCount, DataInput, 0, 0, NULL);
if (err != CL_SUCCESS)
{
printf("Failed to write buffer! %d\n", err);
return EXIT_FAILURE;
}
err = clEnqueueWriteBuffer(ComputeCommands, ComputePosBuffer[nextBuffer], 1, 0,
4 * sizeof(float) * DataBodyCount, DataInput, 0, 0, NULL);
if (err != CL_SUCCESS)
{
printf("Failed to write buffer! %d\n", err);
return EXIT_FAILURE;
}
}
#endif
Update = 0;
err = CL_SUCCESS;
err |= clSetKernelArg(ComputeKernel, 0, sizeof(cl_mem), &ComputePosBuffer[currentBuffer]);
err |= clSetKernelArg(ComputeKernel, 1, sizeof(cl_mem), &ComputeVelBuffer[currentBuffer]);
err |= clSetKernelArg(ComputeKernel, 5, sizeof(cl_mem), &ComputePosBuffer[nextBuffer]);
err |= clSetKernelArg(ComputeKernel, 6, sizeof(cl_mem), &ComputeVelBuffer[nextBuffer]);
if (err)
return -10;
size_t global[1];
size_t local[1];
global[0] = DataBodyCount;
local[0] = GroupSize;
#if (DEBUG_INFO)
if(FrameCount <= 1)
printf("Global[%4d] Local[%4d]\n",
(int)global[0], (int)local[0]);
#endif
err = clEnqueueNDRangeKernel(ComputeCommands, ComputeKernel, 1, NULL, global, local, 0, NULL, NULL);
if (err)
{
printf("Failed to enqueue kernel! %d\n", err);
return err;
}
NDRangeCount++;
#if (DEBUG_INFO)
float *DataCurPos = (float *)calloc(1, 4 * sizeof(float) * DataBodyCount);
float *DataNexPos = (float *)calloc(1, 4 * sizeof(float) * DataBodyCount);
err = clEnqueueReadBuffer( ComputeCommands, ComputePosBuffer[currentBuffer], CL_TRUE, 0, 4 * sizeof(float) * DataBodyCount, DataCurPos, 0, NULL, NULL );
if (err != CL_SUCCESS)
{
printf("Failed to read buffer! %d\n", err);
return EXIT_FAILURE;
}
err = clEnqueueReadBuffer( ComputeCommands, ComputePosBuffer[nextBuffer], CL_TRUE, 0, 4 * sizeof(float) * DataBodyCount, DataNexPos, 0, NULL, NULL );
if (err != CL_SUCCESS)
{
printf("Failed to read buffer! %d\n", err);
return EXIT_FAILURE;
}
for (int i = 0; i < DataBodyCount; ++i)
printf("After NDRange %d - %d: org [%f %f %f %f] - org curr [%f %f %f %f] - org next [%f %f %f %f]\n", NDRangeCount, i,
DataInput[4 * i], DataInput[4 * i + 1], DataInput[4 * i + 2], DataInput[4 * i + 3],
DataCurPos[4 * i], DataCurPos[4 * i + 1], DataCurPos[4 * i + 2], DataCurPos[4 * i + 3],
DataNexPos[4 * i], DataNexPos[4 * i + 1], DataNexPos[4 * i + 2], DataNexPos[4 * i + 3]);
free(DataCurPos);
free(DataNexPos);
#endif
#if (USE_GL_ATTACHMENTS)
// Release control and the data is already in VBOs
err = clEnqueueReleaseGLObjects(ComputeCommands, 1, &ComputePosBuffer[currentBuffer], 0, 0, 0);
if (err != CL_SUCCESS)
{
printf("Failed to release GL object! %d\n", err);
return EXIT_FAILURE;
}
err = clEnqueueReleaseGLObjects(ComputeCommands, 1, &ComputePosBuffer[nextBuffer], 0, 0, 0);
if (err != CL_SUCCESS)
{
printf("Failed to release GL object! %d\n", err);
return EXIT_FAILURE;
}
#else
// Explicitly copy data back to host
err = clEnqueueReadBuffer( ComputeCommands, ComputePosBuffer[nextBuffer], CL_TRUE, 0, DataParticleCount * sizeof(float), DataInput, 0, NULL, NULL );
if (err != CL_SUCCESS)
{
printf("Failed to read buffer! %d\n", err);
return EXIT_FAILURE;
}
// Data in host side, copy to VBOs
UpdateVBO(nextBuffer, DataInput, nextBuffer);
#endif
clFinish(ComputeCommands);
}
// Notify GL side which attribute index is using
glUniform1i(UniformCurBufIdxLocation, nextBuffer);
// Switch buffers
CurrentBuffer = nextBuffer;
return CL_SUCCESS;
}
void Terrain::Update() {
if (isGeomDirty || isColorDirty) {
UpdateVBO();
}
}
