// Do the projection from u,v,depth to X,Y,Z directly in an opengl matrix  
// These numbers come from a combination of the ros kinect_node wiki, and  
// nicolas burrus' posts.  
void LoadVertexMatrix()  
{  
 float fx = 594.21f;  
 float fy = 591.04f;  
 float a = -0.0030711f;  
 float b = 3.3309495f;  
 float cx = 339.5f;  
 float cy = 242.7f;  
 GLfloat mat[16] = {  
 1/fx,     0,  0, 0,  
 0,    -1/fy,  0, 0,  
 0,       0,  0, a,  
 -cx/fx, cy/fy, -1, b  
 };  
 glMultMatrixf(mat);  
}

/**  
 * Преобразует глубину в реальное значение (в миллиметрах)  
 */  
double raw_depth_to_millimeters(int depth_value){  
 double depth_value_f = (float) depth_value;  
 if (depth_value < 2047){  
 float depth = 1000.0 / (depth_value_f  * -0.0030711016 + 3.3309495161);  
 return depth;  
 }  
 return 0.0f;  
}

/**  
 * Преобразует вируальную точку point в точку с реальными координатами (в миллиметрах)  
 */  
Ogre::Vector3 depth_to_realword(Ogre::Vector3 point){  
 double fx_d = 1.0 / 5.9421434211923247e+02;  
 double fy_d = 1.0 / 5.9104053696870778e+02;  
 double cx_d = 3.3930780975300314e+02;  
 double cy_d = 2.4273913761751615e+02;

double depth = raw_depth_to_millimeters(point.z);

return Ogre::Vector3(  
 (point.x - cx_d) * depth * fx_d,  
 (point.y - cy_d) * depth * fy_d,  
 depth);  
}

void TutorialApplication::createScene(void)  
{

    /* Зачем дополнительно создавать submesh пока не понял */  
    Ogre::MeshPtr mesh = Ogre::MeshManager::getSingleton().createManual("CustomMesh", "General");  
    Ogre::SubMesh *subMesh = mesh->createSubMesh();

    /* Подготавливаем структуру для трех вершин (треугольник у нас) */  
    mesh->sharedVertexData = new Ogre::VertexData;  
    mesh->sharedVertexData->vertexCount = 3;

    /* Получаем ссылку на дескриптор буфера (описывает структуру) */  
    Ogre::VertexDeclaration *decl = mesh->sharedVertexData->vertexDeclaration;  
    size_t offset = 0;

    /* первый элемент буфера - это сама вершина (ее координаты) */  
    decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_POSITION);  
    offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);

    /* вторая часть буфера - нормаль вершины */  
    decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_NORMAL);  
    offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);

    /* Третья часть - это цвет вершины */  
    decl->addElement(0, offset, Ogre::VET_COLOUR, Ogre::VES_DIFFUSE);  
    offset += Ogre::VertexElement::getTypeSize(Ogre::VET_COLOUR);

    /* Генерируем вертексный буфер по описанию, которое выше */  
    Ogre::HardwareVertexBufferSharedPtr vertexBuffer = Ogre::HardwareBufferManager::getSingleton().  
        createVertexBuffer(offset, mesh->sharedVertexData->vertexCount, Ogre::HardwareBuffer::HBU_STATIC);

    // подготавливаем цвета  
    // можно писать цвета руками и использовать не Ogre::VET_COLOUR, а VET_FLOAT3|4 (4 - это если альфаканал нужен)  
    Ogre::RenderSystem* rs = Ogre::Root::getSingleton().getRenderSystem();  
    Ogre::uint32 red, green, blue;  
    rs->convertColourValue(Ogre::ColourValue(1,0,0,1), &red);  
    rs->convertColourValue(Ogre::ColourValue(0,1,0,1), &green);  
    rs->convertColourValue(Ogre::ColourValue(0,0,1,1), &blue);

    const float sqrt13 = 0.577350269f; /* sqrt(1/3) - это для нормалей */

    /* блокируем буфер на запись и берем указатель на него */  
    float *pVertex = static_cast<float *>(vertexBuffer->lock(Ogre::HardwareBuffer::HBL_DISCARD));

    // Заполняем буфер  
    *pVertex++ = 0.0f; *pVertex++ = 1.0f; *pVertex++ = 0.0f; // вершина  
    *pVertex++ = -sqrt13; *pVertex++ = sqrt13; *pVertex++ = -sqrt13; // нормаль  
    *(*(Ogre::uint32**)&pVertex)++ = red;   //цвета  
    *pVertex++ = -1.0f; *pVertex++ = -1.0f; *pVertex++ = 0.0f; // вершина  
    *pVertex++ = sqrt13; *pVertex++ = sqrt13; *pVertex++ = -sqrt13; // нормаль  
    *(*(Ogre::uint32**)&pVertex)++ = green; // цвета  
    *pVertex++ = 1.0f; *pVertex++ = -1.0f; *pVertex++ = 0.0f; // вершина  
    *pVertex++ = -sqrt13; *pVertex++ = -sqrt13; *pVertex++ = -sqrt13; // нормаль  
    *(*(Ogre::uint32**)&pVertex)++ = blue;  // цвета

    /* разблокируем */  
    vertexBuffer->unlock();

    /* Создаем буфер для индексов */  
    Ogre::HardwareIndexBufferSharedPtr indexBuffer = Ogre::HardwareBufferManager::getSingleton().  
        createIndexBuffer(Ogre::HardwareIndexBuffer::IT_16BIT, mesh->sharedVertexData->vertexCount, Ogre::HardwareBuffer::HBU_STATIC);

    /* Получаем блокировку на запись и пишем индексы в буфер */  
    uint16_t *indices = static_cast<uint16_t *>(indexBuffer->lock(Ogre::HardwareBuffer::HBL_NORMAL));

    /* Задаем нужный индексы вершин, которые будет треугольник представлять */  
    indices[0] = 0;  
    indices[1] = 1;  
    indices[2] = 2;

    /* записали - разблокировали */  
    indexBuffer->unlock();

    /* Теперь надо прицепить к нашей геометрии созданный буфер */  
    mesh->sharedVertexData->vertexBufferBinding->setBinding(0, vertexBuffer);  
    subMesh->useSharedVertices = true;  
    subMesh->indexData->indexBuffer = indexBuffer;  
    subMesh->indexData->indexCount = mesh->sharedVertexData->vertexCount;  
    subMesh->indexData->indexStart = 0;

    /* Если не объявить рамку, то огр не сможет правильно обсчитать сетку  
     * и она будет видна лишь в корневой ноде (если ее туда прицепить),  
     * а в дочерних - не будет.  
     * Для этого можно зачитать http://www.ogre3d.org/forums/viewtopic.php?f=2&t=60200  
     */  
    mesh->_setBounds(Ogre::AxisAlignedBox(-1, -1, -1, 1, 1, 1));

    /* нарисовали - грузим */  
    mesh->load();

    /*  
     * А теперь нужно задефайнить материал.  
     * Если этого не сделать, то новоиспеченный триангл будет выглядеть белым,  
     * а не многоцветным как задумано выше  
     */  
    Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().create("Test/ColourTest", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);  
    material->getTechnique(0)->getPass(0)->setVertexColourTracking(Ogre::TVC_AMBIENT);

    /* Создаем ноду на базе того, что накодили выше. */  
    Ogre::Entity *entity = mSceneMgr->createEntity("CustomEntity", "CustomMesh", "General");  
    entity->setMaterialName("Test/ColourTest", "General");  
    Ogre::SceneNode *node = mSceneMgr->getRootSceneNode()->createChildSceneNode();  
    node->attachObject(entity);

    mCamera->lookAt(Ogre::Vector3(0, 0, 0));  
    mCamera->setPosition(Ogre::Vector3(0, 10, 50));  
}