drawColor

void drawColor(IntRect* pLocation, IntPair* pPointer, int pointerRed, int pointerGreen, int pointerBlue)
{
    if (g_DrawConfig.Streams.Color.Coloring == COLOR_OFF)
        return;

    if (!isColorOn() && !isIROn())
    {
        drawClosedStream(pLocation, "Color");
        return;
    }

    openni::VideoFrameRef colorMD;
    int depthWidth = 0, depthHeight = 0, depthFullWidth = 0, depthFullHeight = 0;
    int depthOriginX = 0, depthOriginY = 0;

    if (isColorOn())
    {
        colorMD = getColorFrame();
        if (!colorMD.isValid())
            return;
    }
     else if (isIROn())
     {
         colorMD = getIRFrame();
     }
    else
        return;

    if (!colorMD.isValid())
        return;

    if (colorMD.getFrameIndex() == 0)
    {
        return;
    }

    openni::VideoFrameRef depthMetaData = getDepthFrame();

    int width = colorMD.getWidth();
    int height = colorMD.getHeight();
    int fullWidth = colorMD.getVideoMode().getResolutionX();
    int fullHeight = colorMD.getVideoMode().getResolutionY();
    int originX = colorMD.getCropOriginX();
    int originY = colorMD.getCropOriginY();

    XnUInt8* pColor = (XnUInt8*)colorMD.getData();
    bool useDepth = false;
    openni::PixelFormat format = colorMD.getVideoMode().getPixelFormat();

    openni::DepthPixel* pDepth = NULL;

    if (depthMetaData.isValid())
    {
        useDepth = true;
        depthWidth = depthMetaData.getWidth();
        depthHeight = depthMetaData.getHeight();
        depthFullWidth = depthMetaData.getVideoMode().getResolutionX();
        depthFullHeight = depthMetaData.getVideoMode().getResolutionY();
        depthOriginX = depthMetaData.getCropOriginX();
        depthOriginY = depthMetaData.getCropOriginY();

        pDepth = (openni::DepthPixel*)depthMetaData.getData();
    }

    for (XnUInt16 nY = 0; nY < height; nY++)
    {
        XnUInt8* pTexture = TextureMapGetLine(&g_texColor, nY + originY) + originX*4;

        if (format == openni::PIXEL_FORMAT_YUV422)
         {
            YUV422ToRGB888(pColor, pTexture, width*2, g_texColor.Size.X*g_texColor.nBytesPerPixel);
             pColor += width*2;
         }
        else if (format == openni::PIXEL_FORMAT_YUYV)
        {
            YUYVToRGB888(pColor, pTexture, width*2, g_texColor.Size.X*g_texColor.nBytesPerPixel);
            pColor += width*2;
        }
         else
        {
            XnDouble dRealY = (nY + originY) / (XnDouble)fullHeight;
            XnInt32 nDepthY = dRealY * depthFullHeight - depthOriginY;

            for (XnUInt16 nX = 0; nX < width; nX++, pTexture+=4)
            {
                XnInt32 nDepthIndex = 0;

                if (useDepth)
                {
                    XnDouble dRealX = (nX + originX) / (XnDouble)fullWidth;

                    XnInt32 nDepthX = dRealX * depthFullWidth - depthOriginX;

                    if (nDepthX >= depthWidth || nDepthY >= depthHeight || nDepthX < 0 || nDepthY < 0)
                    {
                        nDepthIndex = -1;
                    }
                    else
                    {
                        nDepthIndex = nDepthY*depthWidth + nDepthX;
                    }
                }

                switch (format)
                 {
                case openni::PIXEL_FORMAT_RGB888:
                    pTexture[0] = pColor[0];
                    pTexture[1] = pColor[1];
                    pTexture[2] = pColor[2];
                    pColor+=3;
                     break;
                case openni::PIXEL_FORMAT_GRAY8:
                     pTexture[0] = pTexture[1] = pTexture[2] = *pColor;
                     pColor+=1;
                     break;
                case openni::PIXEL_FORMAT_GRAY16:
                     pTexture[0] = pTexture[1] = pTexture[2] = *((XnUInt16*)pColor) >> 2;
                     pColor+=2;
                     break;
                default:
                    assert(0);
                    return;
                 }

                // decide if pixel should be lit or not
                if (g_DrawConfig.Streams.Color.Coloring == DEPTH_MASKED_COLOR &&
                    (!depthMetaData.isValid() || nDepthIndex == -1 || pDepth[nDepthIndex] == 0))
                {
                    pTexture[3] = 0;
                }
                else
                {
                    pTexture[3] = 255;
                }
            }
        }
    }

    if (pPointer != NULL)
    {
        TextureMapDrawCursor(&g_texColor, *pPointer, pointerRed, pointerGreen, pointerBlue);
    }

    TextureMapUpdate(&g_texColor);
    TextureMapDraw(&g_texColor, pLocation);
}