   int dimX = c_gridDims.x - 1;
   int dimY = c_gridDims.y - 1;
   if (type == 0)
-  {
+    {
     dimX = c_depthMapDims.x;
     dimY = c_depthMapDims.y;
-  }
+    }
   int i = coordinates[0];
   int j = coordinates[1];
   int k = coordinates[2];
   return (k*dimY + j)*dimX + i;
 }
 
+__device__ void computeVoxelCenter(int voxelCoordinate[3], double output[3])
+{
+  output[0] = c_gridOrig.x + (voxelCoordinate[0] + 0.5) * c_gridSpacing.x;
+  output[1] = c_gridOrig.y + (voxelCoordinate[1] + 0.5) * c_gridSpacing.y;
+  output[2] = c_gridOrig.z + (voxelCoordinate[2] + 0.5) * c_gridSpacing.z;
+}
 
 
 // ----------------------------------------------------------------------------

    double* h_matrixRT = new double[16];
    vtkMatrixToDoubleTable(currentData->GetMatrixTR(), h_matrixRT);

    printf("id : %d (size : %d)\n", i, currentData->GetDepthMap()->GetNumberOfCells());

    CudaErrorCheck(cudaMemcpy(d_depthMap, h_depthMap, nbPixelOnDepthMap * sizeof(double), cudaMemcpyHostToDevice));
    CudaErrorCheck(cudaMemcpy(d_matrixK, h_matrixK, matrix4Size * sizeof(double), cudaMemcpyHostToDevice));
    CudaErrorCheck(cudaMemcpy(d_matrixRT, h_matrixRT, matrix4Size * sizeof(double), cudaMemcpyHostToDevice));

    // run code into device
    depthMapKernel << <dimGrid, dimBlock >> >(d_depthMap, d_matrixK, d_matrixRT, d_outScalar);

    cudaError_t err = cudaGetLastError();
    if (cudaSuccess != err)
      std::cout << "CUDA ERROR__: " << cudaGetErrorString(err) << std::endl;

    // Wait that all threads have finished
    CudaErrorCheck(cudaDeviceSynchronize());



// ----------------------------------------------------------------------------
__device__ void transformFrom4Matrix(double matrix[16], double point[3], double output[3])
{
  output[0] = matrix[0] * point[0] + matrix[1] * point[1] + matrix[2] * point[2] + matrix[3];
  output[1] = matrix[4] * point[0] + matrix[5] * point[1] + matrix[6] * point[2] + matrix[7];
  output[2] = matrix[8] * point[0] + matrix[9] * point[1] + matrix[10] * point[2] + matrix[11];
}


// ----------------------------------------------------------------------------
__device__ double norm(double vec[3])
