sOculus.cpp

/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com

Copyright (c) 2013 I-maginer

This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt

-----------------------------------------------------------------------------
*/

#include "sOculus.h"

extern HWND HScol;
extern int OCULUS_CONNECTED_CB;
extern int OCULUS_DISCONNECTED_CB;

bool sOculus::isInitialized = false;

sOculus::sOculus()
{
  mConnected = false;
  mTerminate = false;
  mDebug = false;
  mUpdated = false;
  mHmd = 0;
  mTimeWarpOffset = 0.0;
  mLastTime = 0.0;

  if(!isInitialized)
        {
                ovr_Initialize();
                isInitialized = true;
        }

  mThread = boost::thread(boost::bind(&sOculus::UpdateThread, this));
}

sOculus::~sOculus()
{
  mTerminate = true;
  mThread.join();

  Disconnect();
  
        if(isInitialized)
        {
                ovr_Shutdown();
                isInitialized = false;
        }
}

bool sOculus::IsConnected()
{
  return mConnected;
}

void sOculus::UpdateThread()
{
  while (!mTerminate)
  {
    {
      boost::mutex::scoped_lock l(mMutex);
      int nbHmd = ovrHmd_Detect();
      if (!mConnected)
      {
        if (nbHmd > 0)
        {
          for(int i = 0; (i < nbHmd) && (mHmd == 0); i++)
            mHmd = ovrHmd_Create(i);
        }
        else if (mDebug)
        {
          mHmd = ovrHmd_CreateDebug(ovrHmd_DK2);
        }

        if (mHmd != 0)
        {
          mConnected = true;
          mUpdated = false;
          ovrHmd_SetEnabledCaps(mHmd, ovrHmdCap_LowPersistence | ovrHmdCap_DynamicPrediction);

                if ((mHmd->Type == ovrHmd_DK1) || (mHmd->Type == ovrHmd_DKHD))
            ovrHmd_ConfigureTracking(mHmd, ovrTrackingCap_Orientation | ovrTrackingCap_MagYawCorrection, 0);
          else
            ovrHmd_ConfigureTracking(mHmd, ovrTrackingCap_Orientation | ovrTrackingCap_MagYawCorrection | ovrTrackingCap_Position, 0);

          //MMechostr(MSKDEBUG,"A new Oculus SENSOR has been connected.\n");
          OBJpostEvent(OCULUS_CONNECTED_CB, (int)this, 0);
        }
      }


      if (mConnected && (nbHmd < 1) && !mDebug)
      {
        /*int caps = ovrHmd_GetEnabledCaps(mHmd); // don't work
        if ((caps != 0) && !(caps & ovrHmdCap_Present) && !mDebug) //unplugged
        {
          //MMechostr(MSKDEBUG,"Oculus SENSOR has been disconnected.\n");
        }*/
        Disconnect();
      }
    }

    // Sleep for 30ms so we don't eat the CPU
    Sleep(30);
  }
}

ovrHmdType sOculus::GetType()
{
  if (mHmd != 0)
  {

    return mHmd->Type;
  }

  return ovrHmd_None;
}

bool sOculus::Connect()
{
  boost::mutex::scoped_lock l(mMutex);
  if (mConnected)
    return false;

  int nbHmd = ovrHmd_Detect();
  if (nbHmd > 0)
  {
    for(int i = 0; (i < nbHmd) && (mHmd == 0); i++)
      mHmd = ovrHmd_Create(i);
  }

  if (mHmd != 0)
  {
    ovrHmd_SetEnabledCaps(mHmd, ovrHmdCap_LowPersistence | ovrHmdCap_DynamicPrediction);

    if ((mHmd->Type == ovrHmd_DK1) || (mHmd->Type == ovrHmd_DKHD))
      ovrHmd_ConfigureTracking(mHmd, ovrTrackingCap_Orientation | ovrTrackingCap_MagYawCorrection, 0);
    else
      ovrHmd_ConfigureTracking(mHmd, ovrTrackingCap_Orientation | ovrTrackingCap_MagYawCorrection | ovrTrackingCap_Position, 0);
    
    mConnected = true;
    mUpdated = false;
    MMechostr(MSKDEBUG,"A new Oculus SENSOR has been connected.\n");
    OBJpostEvent(OCULUS_CONNECTED_CB, (int)this, 0);
    return true;
  }

  return false;
}

void sOculus::Disconnect()
{
  if (mConnected)
  {
    if(mHmd)
      ovrHmd_Destroy(mHmd);

    mHmd = 0;
    mConnected = false;
    OBJpostEvent(OCULUS_DISCONNECTED_CB, (int)this, 0);
  }
}

void sOculus::SetLowPersistance(bool state)
{
  if (mHmd)
          ovrHmd_SetEnabledCaps(mHmd, (state) ? ovrHmd_GetEnabledCaps(mHmd) | ovrHmdCap_LowPersistence : ovrHmd_GetEnabledCaps(mHmd) & (~ovrHmdCap_LowPersistence));
}

bool sOculus::GetLowPersistance()
{
  if (!mHmd)
    return false;

        if (ovrHmd_GetEnabledCaps(mHmd) & ovrHmdCap_LowPersistence)
    return true;
  else
    return false;
}

void sOculus::ResetSensor()
{
  boost::mutex::scoped_lock l(mMutex);
  if (mHmd)
          ovrHmd_RecenterPose(mHmd);
}

Quatf sOculus::GetSensorOrientation()
{
  if (!mConnected || !(mTrackingState.StatusFlags & ovrStatus_OrientationTracked))
    return Quatf(0.0f, 0.0f, 0.0f, 1.0f);

  // Get acceleration
  Posef pose = mTrackingState.HeadPose.ThePose;
  return pose.Rotation;
}

Vector3f sOculus::GetSensorYawPitchRoll()
{
  if (!mConnected)
    return Vector3f(0.0f, 0.0f, 0.0f);

  // Get acceleration
  Posef pose = mTrackingState.HeadPose.ThePose;

  // Get orientation quaternion to control view
  Quatf q = pose.Rotation;

  // Create a matrix from quaternion,
  // where elements [0][0] through [3][3] contain rotation.
  Matrix4f bodyFrameMatrix(q); 

  // Get Euler angles from quaternion, in specified axis rotation order.
  Vector3f ypr;
  q.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&ypr.x, &ypr.y, &ypr.z);
  return ypr;
}

Vector3f sOculus::GetSensorAcceleration()
{
  if (!mConnected)
    return Vector3f(0.0f, 0.0f, 0.0f);

  // Get acceleration
  return mTrackingState.HeadPose.LinearAcceleration;
}

Vector3f sOculus::GetSensorPosition()
{
  if (!mConnected || !(mTrackingState.StatusFlags & ovrStatus_PositionTracked))
    return Vector3f(0.0f, 0.0f, 0.0f);

  Posef pose = mTrackingState.HeadPose.ThePose;

  // Get acceleration
  return pose.Translation;
}

bool sOculus::GetStereoConfigDistortionMesh(ovrEyeType eye, ovrDistortionMesh &mesh)
{
  boost::mutex::scoped_lock l(mMutex);
  if (!mHmd)
    return false;

  ovrEyeRenderDesc eyeRenderDesc = ovrHmd_GetRenderDesc(mHmd, eye, mHmd->DefaultEyeFov[eye]);
  if (ovrHmd_CreateDistortionMesh(mHmd, eyeRenderDesc.Eye, eyeRenderDesc.Fov, ovrDistortionCap_Chromatic|ovrDistortionCap_TimeWarp|ovrDistortionCap_Vignette,   &mesh))
    return true;
  else
    return false;
}

bool sOculus::GetProjectionMatrix(ovrEyeType eye, float nearclip, float farclip, ovrMatrix4f &mat)
{
  boost::mutex::scoped_lock l(mMutex);
  if (!mHmd)
    return false;

  ovrFovPort fov = mHmd->DefaultEyeFov[eye];
  mat = ovrMatrix4f_Projection(fov, nearclip, farclip, true);
  return true;
}

void sOculus::WaitForFrame()
{
  if (mUpdated)
    ovr_WaitTillTime(mFrameTiming.TimewarpPointSeconds);
}

void sOculus::SetTimewarpOffset(double value)
{
  mTimeWarpOffset = value;
}

bool sOculus::GetTimeWarpMatrix(ovrEyeType eye, ovrMatrix4f* mat)
{
  //boost::mutex::scoped_lock l(mMutex);
  mat[0].M[0][0] = 1.0f;
  mat[0].M[0][1] = 0.0f;
  mat[0].M[0][2] = 0.0f;
  mat[0].M[0][3] = 0.0f;
  mat[0].M[1][0] = 0.0f;
  mat[0].M[1][1] = 1.0f;
  mat[0].M[1][2] = 0.0f;
  mat[0].M[1][3] = 0.0f;
  mat[0].M[2][0] = 0.0f;
  mat[0].M[2][1] = 0.0f;
  mat[0].M[2][2] = 1.0f;
  mat[0].M[2][3] = 0.0f;
  mat[0].M[3][0] = 0.0f;
  mat[0].M[3][1] = 0.0f;
  mat[0].M[3][2] = 0.0f;
  mat[0].M[3][3] = 1.0f;

  mat[1].M[0][0] = 1.0f;
  mat[1].M[0][1] = 0.0f;
  mat[1].M[0][2] = 0.0f;
  mat[1].M[0][3] = 0.0f;
  mat[1].M[1][0] = 0.0f;
  mat[1].M[1][1] = 1.0f;
  mat[1].M[1][2] = 0.0f;
  mat[1].M[1][3] = 0.0f;
  mat[1].M[2][0] = 0.0f;
  mat[1].M[2][1] = 0.0f;
  mat[1].M[2][2] = 1.0f;
  mat[1].M[2][3] = 0.0f;
  mat[1].M[3][0] = 0.0f;
  mat[1].M[3][1] = 0.0f;
  mat[1].M[3][2] = 0.0f;
  mat[1].M[3][3] = 1.0f;

  if (!mHmd || !mUpdated)
    return false;

  ovrHmd_GetEyeTimewarpMatricesDebug(mHmd, eye, mEyepos[eye], mat, mTimeWarpOffset);

  return true;
}

Sizei sOculus::GetStereoConfigFovTextureSize(ovrEyeType eye)
{
  boost::mutex::scoped_lock l(mMutex);
  if (!mHmd)
    return Sizei(512, 512);

  return ovrHmd_GetFovTextureSize(mHmd, eye, mHmd->DefaultEyeFov[eye], 1.0f);
}

bool sOculus::GetStereoConfigUvScaleOffset(ovrEyeType eye, Sizei textSize, Vector2f &scale, Vector2f &offset)
{
  boost::mutex::scoped_lock l(mMutex);
  if (!mHmd)
    return false;

  ovrEyeRenderDesc eyeRenderDesc = ovrHmd_GetRenderDesc(mHmd, eye, mHmd->DefaultEyeFov[eye]);

  ovrRecti viewport;
        viewport.Pos.x = 0;
        viewport.Pos.y = 0;
        viewport.Size.w = textSize.w;
        viewport.Size.h = textSize.h;

  ovrVector2f UVScaleOffset[2];
  ovrHmd_GetRenderScaleAndOffset(eyeRenderDesc.Fov, textSize, viewport, UVScaleOffset);
  scale = UVScaleOffset[0];
  offset = UVScaleOffset[1];
  return true;
}

bool sOculus::GetWindowPosAndSize(Vector2i &pos, Sizei &size, int &monitorIndex)
{
  boost::mutex::scoped_lock l(mMutex);
  if (!mHmd)
    return false;

  if (mDebug)
    pos = Vector2i(-1920, 0);
  else
    pos = mHmd->WindowsPos;
  size = mHmd->Resolution;
  monitorIndex = (mHmd->DisplayId < 0) ? 1 : mHmd->DisplayId;
  return true;
}

float sOculus::GetStereoConfigAspect()
{
  boost::mutex::scoped_lock l(mMutex);
  if (mHmd)
  {
    ovrFovPort fovLeft = mHmd->DefaultEyeFov[ovrEye_Left];
          ovrFovPort fovRight = mHmd->DefaultEyeFov[ovrEye_Right];

          float combinedTanHalfFovHorizontal = std::max(fovLeft.LeftTan, fovLeft.RightTan);
          float combinedTanHalfFovVertical = std::max(fovLeft.UpTan, fovLeft.DownTan);
    return combinedTanHalfFovHorizontal / combinedTanHalfFovVertical;
  }
  else
    return 0.8f;
}

float sOculus::GetStereoConfigFovY()
{
  boost::mutex::scoped_lock l(mMutex);
  if (mHmd)
  {
    ovrFovPort fovLeft = mHmd->DefaultEyeFov[ovrEye_Left];
          ovrFovPort fovRight = mHmd->DefaultEyeFov[ovrEye_Right];

    return (atanf(fovLeft.UpTan) + atanf(fovLeft.DownTan) + atanf(fovRight.UpTan) + atanf(fovRight.DownTan)) * 0.5f;
  }
  else
    return 1.919862f;
}

float sOculus::GetStereoIPD()
{
  boost::mutex::scoped_lock l(mMutex);
  if (mHmd)
  {
    return ovrHmd_GetFloat(mHmd, OVR_KEY_IPD, 0.064f);
  }
  else
    return 0.064f;
}

bool sOculus::UpdateStart()
{
  if (mHmd)
  {
    mFrameTiming = ovrHmd_BeginFrameTiming(mHmd, 0);
    mUpdated = true;

    ovrEyeRenderDesc eyeRenderDesc[2] = {ovrHmd_GetRenderDesc(mHmd, ovrEye_Left, mHmd->DefaultEyeFov[ovrEye_Left]), ovrHmd_GetRenderDesc(mHmd, ovrEye_Right, mHmd->DefaultEyeFov[ovrEye_Right])};
    ovrVector3f useHmdToEyeViewOffset[2] = {eyeRenderDesc[0].HmdToEyeViewOffset, eyeRenderDesc[1].HmdToEyeViewOffset};
    
    //mEyepos[0] = Posef(mTrackingState.HeadPose.ThePose.Orientation, ((Posef)mTrackingState.HeadPose.ThePose).Apply(-((Vector3f)useHmdToEyeViewOffset[0])));
    //mEyepos[1] = Posef(mTrackingState.HeadPose.ThePose.Orientation, ((Posef)mTrackingState.HeadPose.ThePose).Apply(-((Vector3f)useHmdToEyeViewOffset[1])));

    ovrHmd_GetEyePoses(mHmd, 0, useHmdToEyeViewOffset, mEyepos, &mTrackingState);

    return true;
  }

  return false;
}

void sOculus::UpdateEnd()
{
  if (mHmd && mUpdated)
  {
    ovrHmd_EndFrameTiming(mHmd);
    mUpdated = false;
  }
}