OpenVR framework code

#include "VRSupport.h"
#include "GraphicsContext.h"
#include <KataCore/PlatformHelp.h>
#include <KataPlatform/KataPlatform.h>
#include "OpenGL/Texture_GL.h"
#include "Direct3D11/Texture_D3D11.h"

bool VRSupport::Initialize(GraphicsContext* context)
{
    K_ASSERT(context);
    _context = context;

    //Initialize all of the VR/HMD relevant bits
    auto err = vr::VRInitError_None;
    _hmd = vr::VR_Init(&err, vr::VRApplication_Scene);
    if(err != vr::VRInitError_None)
    {
        _hmd = nullptr; //should already be, but let's make sure
        return false;
    }

    //_renderModels = (vr::IVRRenderModels*) vr::VR_GetGenericInterface(vr::IVRRenderModels_Version, &err);
    //K_ASSERT(err == vr::VRInitError_None);

    _hmd->GetRecommendedRenderTargetSize(&RenderSize.x, &RenderSize.y);
    context->Viewport = V2i(RenderSize.x, RenderSize.y);
    LeftEyeColor = context->CreateTexture("VR_LeftEyeColor", RenderSize.x, RenderSize.y, DataFormat::Rgba8, TextureFlags::BindRenderTarget | TextureFlags::BindShaderResource, nullptr);
    LeftEyeDepth = context->CreateTexture("VR_LeftEyeDepth", RenderSize.x, RenderSize.y, DataFormat::Depth32F, TextureFlags::BindDepthStencil, nullptr);
    LeftEyeFB = context->CreateFrameBuffer("VR_LeftEyeFB", LeftEyeColor, LeftEyeDepth);
    RightEyeColor = context->CreateTexture("VR_RightEyeColor", RenderSize.x, RenderSize.y, DataFormat::Rgba8, TextureFlags::BindRenderTarget | TextureFlags::BindShaderResource, nullptr);
    RightEyeDepth = context->CreateTexture("VR_RightEyeDepth", RenderSize.x, RenderSize.y, DataFormat::Depth32F, TextureFlags::BindDepthStencil, nullptr);
    RightEyeFB = context->CreateFrameBuffer("VR_RightEyeFB", RightEyeColor, RightEyeDepth);

    auto vrcomp = vr::VRCompositor();
    K_ASSERT(vrcomp);

    vr::HmdMatrix34_t matEyeLeft = _hmd->GetEyeToHeadTransform(vr::Eye_Left);
    Matrix4x4 matLeft = ConvertSteamVRMatrix(matEyeLeft);
    MatrixInverse(_eyeMatLeft, matLeft);

    vr::HmdMatrix34_t matEyeRight = _hmd->GetEyeToHeadTransform(vr::Eye_Right);
    Matrix4x4 matRight = ConvertSteamVRMatrix(matEyeRight);
    MatrixInverse(_eyeMatRight, matRight);

    for(uint32_t unTrackedDevice = vr::k_unTrackedDeviceIndex_Hmd + 1; unTrackedDevice < vr::k_unMaxTrackedDeviceCount; unTrackedDevice++)
    {
        if(!_hmd->IsTrackedDeviceConnected(unTrackedDevice))
            continue;

        SetupRenderModelForTrackedDevice(unTrackedDevice);
    }

    return true;
}

void VRSupport::UpdateEvents()
{
    vr::VREvent_t event;
    while(_hmd->PollNextEvent(&event, sizeof(event)))
    {
        switch(event.eventType)
        {
        case vr::VREvent_TrackedDeviceActivated:
        {
            SetupRenderModelForTrackedDevice(event.trackedDeviceIndex);
            printf("Device %u attached. Setting up render model.\n", event.trackedDeviceIndex);
        }
        break;
        case vr::VREvent_TrackedDeviceDeactivated:
        {
            printf("Device %u detached.\n", event.trackedDeviceIndex);
        }
        break;
        case vr::VREvent_TrackedDeviceUpdated:
        {
            printf("Device %u updated.\n", event.trackedDeviceIndex);
        }
        break;
        }
    }

    // Process SteamVR controller state
    for(int i = 0; i < vr::k_unMaxTrackedDeviceCount; ++i)
    {
        _hmd->GetControllerState(i, &_controllerStates[i]);
    }
}

void VRSupport::UpdateHMD()
{
    vr::VRCompositor()->WaitGetPoses(_trackedDevicePose, vr::k_unMaxTrackedDeviceCount, NULL, 0);

    _validPoseCount = 0;
    _poseClasses = "";
    for(int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice)
    {
        if(_trackedDevicePose[nDevice].bPoseIsValid)
        {
            _validPoseCount++;
            _deviceMat[nDevice] = ConvertSteamVRMatrix(_trackedDevicePose[nDevice].mDeviceToAbsoluteTracking);
            Matrix4x4 scaling;
            MatrixScaling(scaling, 1, 1, -1);
            _deviceMat[nDevice] = _deviceMat[nDevice] * scaling;
            if(_deviceClassChar[nDevice] == 0)
            {
                switch(_hmd->GetTrackedDeviceClass(nDevice))
                {
                case vr::TrackedDeviceClass_Controller:        _deviceClassChar[nDevice] = 'C'; break;
                case vr::TrackedDeviceClass_HMD:               _deviceClassChar[nDevice] = 'H'; break;
                case vr::TrackedDeviceClass_Invalid:           _deviceClassChar[nDevice] = 'I'; break;
                case vr::TrackedDeviceClass_Other:             _deviceClassChar[nDevice] = 'O'; break;
                case vr::TrackedDeviceClass_TrackingReference: _deviceClassChar[nDevice] = 'T'; break;
                default:                                       _deviceClassChar[nDevice] = '?'; break;
                }
            }
            _poseClasses += _deviceClassChar[nDevice];
        }
    }

    if(_trackedDevicePose[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid)
    {
        _hmdMat = _deviceMat[vr::k_unTrackedDeviceIndex_Hmd];
        MatrixInverse(_hmdViewMat, _hmdMat);
    }
}

void VRSupport::Submit()
{
    auto api = PlatformGetInt(PV_Windows_RenderingApi);
    if(api == WINRA_OpenGL)
    {
        TextureGL* left = (TextureGL*) _context->GetRawTexture(LeftEyeColor);
        vr::Texture_t leftEyeTex = { (void*)left->_texture, vr::API_OpenGL, vr::ColorSpace_Gamma };
        vr::VRCompositor()->Submit(vr::Eye_Left, &leftEyeTex);
        TextureGL* right = (TextureGL*) _context->GetRawTexture(RightEyeColor);
        vr::Texture_t rightEyeTex = { (void*)right->_texture, vr::API_OpenGL, vr::ColorSpace_Gamma };
        vr::VRCompositor()->Submit(vr::Eye_Right, &rightEyeTex);
    }
    else if(api == WINRA_Direct3D11)
    {
        Texture_D3D11* left = (Texture_D3D11*)_context->GetRawTexture(LeftEyeColor);
        vr::Texture_t leftEyeTex = { (void*)left->_texture, vr::API_DirectX, vr::ColorSpace_Gamma };
        vr::VRCompositor()->Submit(vr::Eye_Left, &leftEyeTex);
        Texture_D3D11* right = (Texture_D3D11*)_context->GetRawTexture(RightEyeColor);
        vr::Texture_t rightEyeTex = { (void*)right->_texture, vr::API_DirectX, vr::ColorSpace_Gamma };
        vr::VRCompositor()->Submit(vr::Eye_Right, &rightEyeTex);
    }
}

void VRSupport::SetSceneProperties(V3 cameraPosition, float sceneScale)
{
    _cameraPosition = cameraPosition;
    _sceneScale = sceneScale;
}

std::string VRSupport::GetTrackedDeviceString(vr::TrackedDeviceIndex_t unDevice, vr::TrackedDeviceProperty prop, vr::TrackedPropertyError *peError)
{
    uint32_t unRequiredBufferLen = _hmd->GetStringTrackedDeviceProperty(unDevice, prop, NULL, 0, peError);
    if(unRequiredBufferLen == 0)
        return "";

    char *pchBuffer = new char[unRequiredBufferLen];
    unRequiredBufferLen = _hmd->GetStringTrackedDeviceProperty(unDevice, prop, pchBuffer, unRequiredBufferLen, peError);
    std::string sResult = pchBuffer;
    delete[] pchBuffer;
    return sResult;
}

VRRenderModel* VRSupport::FindOrLoadRenderModel(const char *pchRenderModelName)
{
    VRRenderModel* pRenderModel = NULL;
    for(auto i = _renderModels.begin(); i != _renderModels.end(); i++)
    {
        if(!stricmp((*i)->GetName().c_str(), pchRenderModelName))
        {
            pRenderModel = *i;
            break;
        }
    }

    // load the model if we didn't find one
    if(!pRenderModel)
    {
        vr::RenderModel_t *pModel;
        vr::EVRRenderModelError error;
        while(1)
        {
            error = vr::VRRenderModels()->LoadRenderModel_Async(pchRenderModelName, &pModel);
            if(error != vr::VRRenderModelError_Loading)
                break;

            PlatformHelp::Sleep(1);
        }

        if(error != vr::VRRenderModelError_None)
        {
            kprintf(LP_Warning, "Unable to load render model %s - %s\n", pchRenderModelName, vr::VRRenderModels()->GetRenderModelErrorNameFromEnum(error));
            return NULL; // move on to the next tracked device
        }

        vr::RenderModel_TextureMap_t *pTexture;
        while(1)
        {
            error = vr::VRRenderModels()->LoadTexture_Async(pModel->diffuseTextureId, &pTexture);
            if(error != vr::VRRenderModelError_Loading)
                break;

            PlatformHelp::Sleep(1);
        }

        if(error != vr::VRRenderModelError_None)
        {
            kprintf(LP_Warning, "Unable to load render texture id:%d for render model %s\n", pModel->diffuseTextureId, pchRenderModelName);
            vr::VRRenderModels()->FreeRenderModel(pModel);
            return NULL; // move on to the next tracked device
        }

        /*pRenderModel = new VRRenderModel_GL(pchRenderModelName);
        if(!pRenderModel->Init(*pModel, *pTexture))
        {
            kprintf(LP_Warning, "Unable to create GL model from render model %s\n", pchRenderModelName);
            delete pRenderModel;
            pRenderModel = NULL;
        }
        else
        {
            _renderModels.push_back(pRenderModel);
        }
        vr::VRRenderModels()->FreeRenderModel(pModel);
        vr::VRRenderModels()->FreeTexture(pTexture);*/
    }
    return pRenderModel;
}

void VRSupport::SetupRenderModelForTrackedDevice(vr::TrackedDeviceIndex_t unTrackedDeviceIndex)
{
    if(unTrackedDeviceIndex >= vr::k_unMaxTrackedDeviceCount)
        return;

    // try to find a model we've already set up
    std::string sRenderModelName = GetTrackedDeviceString(unTrackedDeviceIndex, vr::Prop_RenderModelName_String);
    VRRenderModel* pRenderModel = FindOrLoadRenderModel(sRenderModelName.c_str());
    if(!pRenderModel)
    {
        std::string sTrackingSystemName = GetTrackedDeviceString(unTrackedDeviceIndex, vr::Prop_TrackingSystemName_String);
        kprintf(LP_Warning, "Unable to load render model for tracked device %d (%s.%s)", unTrackedDeviceIndex, sTrackingSystemName.c_str(), sRenderModelName.c_str());
    }
    else
    {
        _trackedDeviceToRenderModel[unTrackedDeviceIndex] = pRenderModel;
        _showTrackedDevice[unTrackedDeviceIndex] = true;
    }
}

void VRSupport::DrawRenderModels()
{
    bool bIsInputCapturedByAnotherProcess = _hmd->IsInputFocusCapturedByAnotherProcess();
    //Draw VR render models
    for(uint32_t unTrackedDevice = 0; unTrackedDevice < vr::k_unMaxTrackedDeviceCount; unTrackedDevice++)
    {
        if(!_trackedDeviceToRenderModel[unTrackedDevice] || !_showTrackedDevice[unTrackedDevice])
            continue;

        const vr::TrackedDevicePose_t & pose = _trackedDevicePose[unTrackedDevice];
        if(!pose.bPoseIsValid)
            continue;

        if(bIsInputCapturedByAnotherProcess && _hmd->GetTrackedDeviceClass(unTrackedDevice) == vr::TrackedDeviceClass_Controller)
            continue;

        Matrix4x4 vrScaleMat, vrTransMat;
        MatrixScaling(vrScaleMat, 1.0f / _sceneScale, 1.0f / _sceneScale, 1.0f / _sceneScale);
        MatrixTranslation(vrTransMat, _cameraPosition.x, _cameraPosition.y, _cameraPosition.z);
        Matrix4x4 matDeviceToTracking = _deviceMat[unTrackedDevice] * vrScaleMat * vrTransMat;
        _context->Pipeline.SetWorld(matDeviceToTracking);
        _context->Pipeline.ModulateColor = V4(1.3f);
        _context->Commit();
        gl::VertexAttrib4f(VE_Diffuse, 1.3f, 1.3f, 1.3f, 1.3f);
        _trackedDeviceToRenderModel[unTrackedDevice]->Draw();
        //_context->DrawModelGeometry(_context->Models.Sphere);
    }
}

Matrix4x4 VRSupport::GetProjMatrixForEye(vr::Hmd_Eye eye) const
{
    vr::HmdMatrix44_t mat = _hmd->GetProjectionMatrix(eye, 0.1f, 250.0f, vr::API_DirectX);

    Matrix4x4 proj = {
        mat.m[0][0], mat.m[1][0], mat.m[2][0], mat.m[3][0],
        mat.m[0][1], mat.m[1][1], mat.m[2][1], mat.m[3][1],
        mat.m[0][2], mat.m[1][2], mat.m[2][2], mat.m[3][2],
        mat.m[0][3], mat.m[1][3], mat.m[2][3], mat.m[3][3]
    };
    Matrix4x4 scaling;
    MatrixScaling(scaling, 1, 1, 1);
    return scaling * proj;
}

Matrix4x4 VRSupport::GetViewMatrixForEye(vr::Hmd_Eye eye) const
{
    Matrix4x4 vrScaleMat, vrTransMat;
    MatrixScaling(vrScaleMat, _sceneScale, _sceneScale, _sceneScale);
    MatrixTranslation(vrTransMat, -_cameraPosition.x, -_cameraPosition.y, -_cameraPosition.z);
    if(eye == vr::Eye_Left)
    {
        return vrTransMat * vrScaleMat * _hmdViewMat * _eyeMatLeft;
    }
    else if(eye == vr::Eye_Right)
    {
        return vrTransMat * vrScaleMat * _hmdViewMat * _eyeMatRight;
    }

    return Matrix4x4::Identity;
}

Vector3D VRSupport::GetHmdPosition() const
{
    return _hmdMat.GetPosition() + _cameraPosition;
}

Vector3D VRSupport::GetDevicePosition(int device) const
{
    return GetDevicePose(device).GetPosition();
}

Matrix4x4 VRSupport::GetDevicePose(int device) const
{
    Matrix4x4 vrScaleMat, vrTransMat;
    MatrixScaling(vrScaleMat, 1.0f / _sceneScale, 1.0f / _sceneScale, 1.0f / _sceneScale);
    MatrixTranslation(vrTransMat, _cameraPosition.x, _cameraPosition.y, _cameraPosition.z);
    Matrix4x4 matDeviceToTracking = _deviceMat[device] * vrScaleMat * vrTransMat;
    return matDeviceToTracking;
}

bool VRSupport::IsController(int device) const
{
    auto deviceClass = _hmd->GetTrackedDeviceClass(device);
    return deviceClass == vr::TrackedDeviceClass_Controller;
}

bool VRSupport::IsHmd(int device) const
{
    auto deviceClass = _hmd->GetTrackedDeviceClass(device);
    return deviceClass == vr::TrackedDeviceClass_HMD;
}

bool VRSupport::GetButtonState(int controller, vr::EVRButtonId button) const
{
    return (_controllerStates[controller].ulButtonPressed & ButtonMaskFromId(button)) != 0;
}

V2 VRSupport::GetAxisState(int device, int axis) const
{
    const auto& ctrlAxis = _controllerStates[device].rAxis[axis];
    return V2(ctrlAxis.x, ctrlAxis.y);
}

void VRSupport::HapticPulse(int device, int axis, unsigned short strength)
{
    strength = clamp(strength, 0, 3999);
    _hmd->TriggerHapticPulse(device, axis, strength);
}