sLeapMotion.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 "sLeapMotion.h"

extern int LEAPMOTION_CONNECTED_CB;
extern int LEAPMOTION_DISCONNECTED_CB;

extern int LEAPMOTION_CIRCLE_CB;
extern int LEAPMOTION_SWIPE_CB;
extern int LEAPMOTION_KEYTAP_CB;
extern int LEAPMOTION_SCREENTAP_CB;

sLeapMotion::sLeapMotion()
{
  mConnected = false;
  mTerminate = false;
  mTopTracking = false;
  mInit = false;
  std::vector<int> fids;
  mLastPointableIds.resize(16, fids);
  mLastHandIds.resize(2, -1);
  mLastToolsIds.resize(2, -1);
  //mThread = boost::thread(boost::bind(&sLeapMotion::UpdateThread, this));
  mLastGestureTick = 0;
}

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

  if (mInit)
    mController.removeListener(*this);

  mGestureList.clear();
  mConnected = false;
}

void sLeapMotion::Init()
{
  mController.addListener(*this);
  mInit = true;
}

void sLeapMotion::onInit(const Controller& controller)
{
  //std::cout << "Initialized" << std::endl;
}

void sLeapMotion::onConnect(const Controller& controller)
{
  controller.enableGesture(Gesture::TYPE_CIRCLE);
  controller.enableGesture(Gesture::TYPE_KEY_TAP);
  controller.enableGesture(Gesture::TYPE_SCREEN_TAP);
  controller.enableGesture(Gesture::TYPE_SWIPE);
  Leap::Controller::PolicyFlag flags = (mTopTracking) ? Controller::POLICY_OPTIMIZE_HMD : Controller::POLICY_DEFAULT;
  controller.setPolicy(flags);

  if (!mConnected)
  {
    OBJpostEvent(LEAPMOTION_CONNECTED_CB, SCOL_PTR this, 0);
    mConnected = true;
  }
}

void sLeapMotion::onDisconnect(const Controller& controller)
{
  mConnected = false;

  //Note: not dispatched when running in a debugger.
  OBJpostEvent(LEAPMOTION_DISCONNECTED_CB, SCOL_PTR this, 0);
}

void sLeapMotion::onExit(const Controller& controller)
{
  //std::cout << "Exited" << std::endl;
}

void sLeapMotion::DetectHands(const HandList hands)
{
  //TODO check found / lost hands
  for (int i = 0; i < hands.count(); ++i)
  {
    // Get the first hand
    const Hand hand = hands[i];

    DetectFingers(hand);

    // Get the hand's sphere radius and palm position
    /*std::cout << "Hand sphere radius: " << hand.sphereRadius()
              << " mm, palm position: " << hand.palmPosition() << std::endl;
    
    // Get the hand's normal vector and direction
    const Vector normal = hand.palmNormal();
    const Vector direction = hand.direction();

    // Calculate the hand's pitch, roll, and yaw angles
    std::cout << "Hand pitch: " << direction.pitch() * RAD_TO_DEG << " degrees, "
              << "roll: " << normal.roll() * RAD_TO_DEG << " degrees, "
              << "yaw: " << direction.yaw() * RAD_TO_DEG << " degrees" << std::endl;
              */
  }
}

void sLeapMotion::DetectFingers(const Hand hand)
{
  // Check if the hand has any fingers
  const FingerList fingers = hand.fingers();
  if (!fingers.isEmpty())
  {
    // Calculate the hand's average finger tip position
    Vector avgPos;
    for (int i = 0; i < fingers.count(); ++i)
    {
      avgPos += fingers[i].tipPosition();
    }
    avgPos /= (float)fingers.count();
  }
}

void sLeapMotion::DetectTools(const ToolList tools)
{
}

void sLeapMotion::DetectGestures(const GestureList gestures)
{
  int ltick = GetTickCount() - mLastGestureTick;
  for (int g = 0; g < gestures.count(); ++g)
  {
    Gesture gesture = gestures[g];
    HandList hands = gesture.hands();
    SGesture sgesture;

    if (!gesture.isValid())
      continue;

    //get hand and pointable index
    if (gesture.state() == Gesture::STATE_START || (gesture.type() == Gesture::TYPE_KEY_TAP) || (gesture.type() == Gesture::TYPE_SCREEN_TAP))
    {
      int handIndex = -1;
      int pointableId = -1;

      if (hands.isEmpty() || !hands[0].isValid())
        continue;

      if (std::find(mLastHandIds.begin(), mLastHandIds.end(), hands[0].id()) == mLastHandIds.end())
        continue;
    
      for (unsigned int i = 0; i < mLastHandIds.size(); ++i)
      {
        if (mLastHandIds[i] == hands[0].id())
          handIndex = i;
      }

      if (hands[0].id() >= (int)mLastPointableIds.size())
        continue;

      PointableList pointables = gesture.pointables();
    
      for (unsigned int j = 0; j < mLastPointableIds[hands[0].id()].size(); j++)
      {
        if (mLastPointableIds[hands[0].id()][j] == pointables[0].id())
          pointableId = j;
      }
      sgesture = SGesture(gesture.id(), handIndex, pointableId);

      if ((gesture.type() == Gesture::TYPE_CIRCLE) || (gesture.type() == Gesture::TYPE_SWIPE))
        mGestureList.push_back(sgesture);
    }

    int sgesturepos = 0;
    if (sgesture.id == -1)
    {
      for (unsigned int i = 0; (i < mGestureList.size()) && (sgesture.id == -1); i++)
      {
        if (mGestureList[i].id == gesture.id())
        {
          sgesture = mGestureList[i];
          sgesturepos = i;
        }
      }
    }

    // no previous or start gesture found
    if (sgesture.id == -1)
      continue;

    // get gesture from list
    if (gesture.state() == Gesture::STATE_STOP && ((gesture.type() == Gesture::TYPE_CIRCLE) || (gesture.type() == Gesture::TYPE_SWIPE)))
    {
      mGestureList.erase(mGestureList.begin() + sgesturepos);
    }

    //avoid to fast calls
    if ((ltick < 16) && gesture.state() == Gesture::STATE_UPDATE)
      continue;

    switch (gesture.type())
    {
      case Gesture::TYPE_CIRCLE:
      {
        SCbData* data = new SCbData();
        CircleGesture circle = gesture;

        if (circle.pointable().direction().angleTo(circle.normal()) <= PI/4)
        {
          data->dir = 1.0f;
        }
        else
        {
          data->dir = -1.0f;
        }

        // Calculate angle swept since last frame
        float sweptAngle = 0;
        if (circle.state() != Gesture::STATE_START)
        {
          CircleGesture previousUpdate = CircleGesture(mController.frame(1).gesture(circle.id()));
          sweptAngle = (circle.progress() - previousUpdate.progress()) * 2 * PI;
        }

        data->progress = circle.progress();

        data->handId = sgesture.handId;
        data->id = sgesture.pointableId;

        data->fparam = circle.radius();

        data->state = (int)gesture.state();

        if (gesture.state() != Gesture::STATE_INVALID)
          OBJpostEvent(LEAPMOTION_CIRCLE_CB, SCOL_PTR this, (int)data);
        else
          delete data;
        break;
      }
      case Gesture::TYPE_SWIPE:
      {
        SCbData* data = new SCbData();
        SwipeGesture swipe = gesture;

        data->progress = ((float)swipe.duration() / 1000);

        data->handId = sgesture.handId;
        data->id = sgesture.pointableId;

        data->vDir = swipe.direction();
        data->fparam = swipe.speed();
        data->svec = swipe.startPosition();

        data->state = (int)gesture.state();

        if (gesture.state() != Gesture::STATE_INVALID)
          OBJpostEvent(LEAPMOTION_SWIPE_CB, SCOL_PTR this, (int)data);
        else
          delete data;
        break;
      }
      case Gesture::TYPE_KEY_TAP:
      {
        SCbData* data = new SCbData();
        KeyTapGesture tap = gesture;

        data->progress = tap.progress();
        data->vDir = tap.direction();
        data->svec = tap.position();

        data->handId = sgesture.handId;
        data->id = sgesture.pointableId;

        data->state = (int)gesture.state();

        if (gesture.state() == Gesture::STATE_STOP)
          OBJpostEvent(LEAPMOTION_KEYTAP_CB, SCOL_PTR this, (int)data);
        else
          delete data;
        break;
      }
      case Gesture::TYPE_SCREEN_TAP:
      {
        ScreenTapGesture screentap = gesture;

        SCbData* data = new SCbData();
        data->progress = ((float)screentap.duration() / 1000);
        data->vDir = screentap.direction();
        data->svec = screentap.position();

        data->handId = sgesture.handId;
        data->id = sgesture.pointableId;

        data->state = (int)gesture.state();

        if (gesture.state() == Gesture::STATE_STOP)
          OBJpostEvent(LEAPMOTION_SCREENTAP_CB, SCOL_PTR this, (int)data);
        else
          delete data;
        break;
      }
      default:
        break;
    }
  }
  mLastGestureTick = GetTickCount();
}

void sLeapMotion::onFrame(const Controller& controller)
{
  Frame lframe(controller.frame());
  DetectGestures(lframe.gestures());

  {
    //all leap motion listener callbacks are in a foreign thread
    boost::mutex::scoped_lock l(mMutex);
    // Get the most recent frame
    mLastFrame = lframe;
  }
  //DetectHands(mLastFrame.hands());
  //DetectTools(mLastFrame.tools());  
}

void sLeapMotion::onFocusGained(const Controller& controller)
{
  //std::cout << "Focus Gained" << std::endl;
}

void sLeapMotion::onFocusLost(const Controller& controller)
{
  //std::cout << "Focus Lost" << std::endl;
}

// Hands
bool sLeapMotion::GetHand(int id, Hand &hand)
{
  HandList hands;
  {
    boost::mutex::scoped_lock l(mMutex);
    if (!mConnected || !mLastFrame.isValid() || mLastFrame.hands().count() < id)
      return false;

    hands = mLastFrame.hands();
  }

  if (id >= (int)mLastHandIds.size())
    mLastHandIds.resize(id+1, -1);

  if (mLastHandIds[id] != -1)
  {
    for (int i = 0; i < hands.count(); ++i)
    {
      Hand fhand = hands[i];

      if (mLastHandIds[id] == fhand.id())
        hand = fhand;
    }
  }

  //previous id not found, search for a new hand id
  if (!hand.isValid())
  {
    bool foundnew = false;
    for (int i = 0; i < hands.count() && !foundnew; ++i)
    {
      hand = hands[i];
      if (hand.isValid() && (((id == 0) && hand.isLeft()) || ((id == 1) && (hand.isRight()))))
      {
        std::vector<int>::iterator vit = std::find(mLastHandIds.begin(), mLastHandIds.end(), hand.id());
        if (vit != mLastHandIds.end())
          mLastHandIds[vit-mLastHandIds.begin()] = -1;
        
        foundnew = true;
      }
      else if (((id > 1)/* || hands.count() < 2*/) && hand.isValid() && std::find(mLastHandIds.begin(), mLastHandIds.end(), hand.id()) == mLastHandIds.end())
        foundnew = true;
    }

    if (!foundnew)
      hand = Hand();

    mLastHandIds[id] = hand.id();
  }

  //resize pointables list
  if (hand.id() >= (int)mLastPointableIds.size())
  {
    std::vector<int> fids;
    mLastPointableIds.resize(hand.id()+1, fids);
  }
  
  if (hand.isValid())
    return true;
  else
    return false;
}

bool sLeapMotion::GetLeftHand(Hand &hand)
{
  boost::mutex::scoped_lock l(mMutex);
  if (!mConnected || !mLastFrame.isValid())
    return false;

  hand = mLastFrame.hands().leftmost();
  if (hand.isValid())
    return true;
  else
    return false;
}

bool sLeapMotion::GetRightHand(Hand &hand)
{
  boost::mutex::scoped_lock l(mMutex);
  if (!mConnected || !mLastFrame.isValid())
    return false;

  hand = mLastFrame.hands().rightmost();
  if (hand.isValid())
    return true;
  else
    return false;
}

bool sLeapMotion::GetHandPosition(const Hand hand, bool optim, Vector &vec)
{
  if (optim)
    vec = hand.stabilizedPalmPosition();
  else
    vec = hand.palmPosition();

  if (vec.isValid())
  {
    vec -= hand.direction();
    return true;
  }
  else
    return false;
}

bool sLeapMotion::GetHandOrientation(const Hand hand, Vector &vec)
{
  const Vector normal = hand.palmNormal();
  const Vector direction = hand.direction();

  vec.x = direction.pitch();
  vec.y = direction.yaw();
  vec.z = normal.roll();

  if (vec.isValid())
    return true;
  else
    return false;
}

bool sLeapMotion::GetHandPalmNormal(const Hand hand, Vector &vec)
{
  vec = hand.palmNormal();

  if (vec.isValid())
    return true;
  else
    return false;
}

bool sLeapMotion::GetHandDirection(const Hand hand, Vector &vec)
{
  vec = hand.direction();

  if (vec.isValid())
    return true;
  else
    return false;
}

bool sLeapMotion::GetHandVelocity(const Hand hand, Vector &vec)
{
  vec = hand.palmVelocity();

  if (vec.isValid())
    return true;
  else
    return false;
}

std::vector<SCbData> sLeapMotion::GetHandFingers(const Hand hand, bool optim)
{
  boost::mutex::scoped_lock l(mMutex);
  std::vector<SCbData> lfingers;
  lfingers.resize(5);

  //resize pointables list
  if (hand.id() >= (int)mLastPointableIds.size())
  {
    std::vector<int> fids;
    mLastPointableIds.resize(hand.id()+1, fids);
  }

  if (mLastPointableIds[hand.id()].empty())
  {
    std::vector<int> fids;
    fids.resize(5, -1);
    mLastPointableIds[hand.id()] = fids;
  }

  const FingerList fingers = hand.fingers();

  for (int i = 0; i < 5; ++i)
  {
    Finger finger = fingers[i];
    SCbData data;

    mLastPointableIds[hand.id()][i] = finger.id();

    Vector screenPos(0.0f, 0.0f, 0.0f);

    if (finger.isValid())
    {
      ScreenList screens = mController.locatedScreens();
      Screen screen = screens.closestScreenHit(finger);
      if (screen.isValid() && finger.isExtended())
        screenPos = screen.intersect(finger, true);

      data.svec = optim ? finger.stabilizedTipPosition() : finger.tipPosition();
      data.evec = data.svec - finger.direction() * -0.25f;
      data.screenPos = screenPos;
      Vector dir = finger.direction();
      data.pyr.x = dir.pitch();
      data.pyr.y = dir.yaw();
      data.pyr.z = dir.roll();

      for (unsigned int joint = 0; joint < 4; joint++)
        data.subVecs.push_back(finger.jointPosition((Leap::Finger::Joint)joint));

      data.id = finger.id();
    }
    
    lfingers[i] = data;
  }

  return lfingers;
}

// Tools
bool sLeapMotion::GetTool(int id, Tool &tool)
{
  boost::mutex::scoped_lock l(mMutex);
  if (!mConnected || !mLastFrame.isValid() || mLastFrame.tools().count() < id)
    return false;

  ToolList tools = mLastFrame.tools();

  if (id >= (int)mLastToolsIds.size())
    mLastToolsIds.resize(id+1, -1);

  if (mLastToolsIds[id] != -1)
  {
    for (int i = 0; i < tools.count(); ++i)
    {
      Tool ftool = tools[i];
      
      if (mLastToolsIds[id] == ftool.id())
        tool = ftool;
    }
  }

  //previous id not found, search for a new hand id
  if (!tool.isValid())
  {
    bool foundnew = false;
    for (int i = 0; i < tools.count() && !foundnew; ++i)
    {
      tool = tools[i];
      if (tool.isValid() && std::find(mLastToolsIds.begin(), mLastToolsIds.end(), tool.id()) == mLastToolsIds.end())
        foundnew = true;
    }

    if (!foundnew)
      tool = Tool();

    mLastToolsIds[id] = tool.id();
  }

  if (tool.isValid())
    return true;
  else
    return false;
}

bool sLeapMotion::GetToolPosition(const Tool tool, bool optim, Vector &vec)
{
  if (optim)
    vec = tool.stabilizedTipPosition();
  else
    vec = tool.tipPosition();

  if (vec.isValid())
    return true;
  else
    return false;
}

bool sLeapMotion::GetToolOrientation(const Tool tool, Vector &vec)
{
  const Vector direction = tool.direction();

  vec.x = direction.pitch();
  vec.y = direction.yaw();
  vec.z = direction.roll();

  return true;
}

bool sLeapMotion::GetToolVelocity(const Tool tool, Vector &vec)
{
  vec = tool.tipVelocity();

  if (vec.isValid())
    return true;
  else
    return false;
}

void sLeapMotion::SetTopTrackingEnable(bool state)
{
  mTopTracking = state;

  if (mTopTracking)
    mController.setPolicy(Controller::POLICY_OPTIMIZE_HMD);
  else
    mController.clearPolicy(Controller::POLICY_OPTIMIZE_HMD);
}

bool sLeapMotion::GetTopTrackingEnable()
{
  return mTopTracking;
}