/// @file matleap.cpp
/// @brief leap motion controller interface
/// @author Jeff Perry <jeffsp@gmail.com>
/// @version 1.0
/// @date 2013-09-12
// original by Jeff Perry, updated and adjusted by Niklas Halle <niklas@niklashalle.net>
#include "matleap.hpp"
#include <array>
// Under Windows, a Leap::Controller must be allocated after the MEX
// startup code has completed. Also, a Leap::Controller must be
// deleted in the function specified by mexAtExit after all global
// destructors are called. If the Leap::Controller is not allocated
// and freed in this way, the MEX function will crash and cause MATLAB
// to hang or close abruptly. Linux and OS/X don't have these
// constraints, and you can just create a global Leap::Controller
// instance.
// Global instance pointer
matleap::frame_grabber *fg = 0;
int version = 4; // 1: orig, 2: with arm info, 3: with more hand info
// Exit function
void matleap_exit() {
fg->close_connection();
delete fg;
fg = nullptr;
}
/// @brief process interface arguments
///
/// @param nlhs matlab mex output interface
/// @param plhs[] matlab mex output interface
/// @param nrhs matlab mex input interface
/// @param prhs[] matlab mex input interface
///
/// @return command number
int get_command(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
int command;
// check inputs - we take exactly one parameter
switch (nrhs) {
case 1:
command = *mxGetPr(prhs[0]);
break;
case 0:
mexErrMsgTxt("Not enough input arguments");
default:
mexErrMsgTxt("Too many input arguments");
}
// check that outputs agree with command
switch (command) {
case -1:
// set debug command requires 0 outputs
if (nlhs != 0) mexErrMsgTxt("Wrong number of outputs specified");
break;
case 0:
case 1:
// get version and frame grab command can populate zero or one output
if (nlhs != 0 && nlhs != 1) mexErrMsgTxt("Wrong number of outputs specified");
break;
default:
mexErrMsgTxt("An unknown command was specified");
}
return command;
}
/// @brief helper function
///
/// @param v values to fill array with
///
/// @return created and filled array
mxArray *create_and_fill(const Leap::Vector &v) {
mxArray *p = mxCreateNumericMatrix(1, 3, mxDOUBLE_CLASS, mxREAL);
*(mxGetPr(p) + 0) = v.x;
*(mxGetPr(p) + 1) = v.y;
*(mxGetPr(p) + 2) = v.z;
return p;
}
/// @brief get a frame from the leap controller
///
/// @param nlhs matlab mex output interface
/// @param plhs[] matlab mex output interface
void get_frame(int nlhs, mxArray *plhs[]) {
// get the frame
const matleap::frame &f = fg->get_frame();
// create matlab frame struct
const char *frame_field_names[] =
{
"id",
"timestamp",
"gesture",
"hands",
"version"
};
int frame_fields = sizeof(frame_field_names) / sizeof(*frame_field_names);
plhs[0] = mxCreateStructMatrix(1, 1, frame_fields, frame_field_names);
// fill the frame struct
mxSetFieldByNumber(plhs[0], 0, 0, mxCreateDoubleScalar(f.id));
mxSetFieldByNumber(plhs[0], 0, 1, mxCreateDoubleScalar(f.timestamp));
mxSetFieldByNumber(plhs[0], 0, 2, mxCreateDoubleScalar(f.has_gesture ? 1 : 0));
if (f.detectedHands > 0) {
const char *hand_field_names[] =
{
"id", // 0
"confidence", // 2
"visible_time",
"pinch_strength",
"grab_strength",
"palm",
"digits"
};
int hand_fields = sizeof(hand_field_names) / sizeof(*hand_field_names);
mxArray *p = mxCreateStructMatrix(1, f.detectedHands, hand_fields, hand_field_names);
mxSetFieldByNumber(plhs[0], 0, 3, p);
// 3 because hands is the third (fourth) field name in
// the overall struct we are creating.
for (int i = 0; i < f.detectedHands; ++i) {
// one by one, get the fields for the hand
mxSetFieldByNumber(p, i, 0, mxCreateDoubleScalar(f.hands[i].id()));
mxSetFieldByNumber(p, i, 1, mxCreateDoubleScalar(f.hands[i].confidence()));
mxSetFieldByNumber(p, i, 2, mxCreateDoubleScalar(f.hands[i].timeVisible()));
mxSetFieldByNumber(p, i, 3, mxCreateDoubleScalar(f.hands[i].pinchStrength()));
mxSetFieldByNumber(p, i, 4, mxCreateDoubleScalar(f.hands[i].grabStrength()));
// palm
const char *palm_field_names[] =
{
"position",
"stabilized_position",
"velocity",
"normal",
"width",
"direction",
};
int palm_fields = sizeof(palm_field_names) / sizeof(*palm_field_names);
mxArray *palm = mxCreateStructMatrix(1, 1, palm_fields, palm_field_names);
mxSetFieldByNumber(p, i, 5, palm);
mxSetFieldByNumber(palm, 0, 0, create_and_fill(f.hands[i].palmPosition()));
mxSetFieldByNumber(palm, 0, 1, create_and_fill(f.hands[i].stabilizedPalmPosition()));
mxSetFieldByNumber(palm, 0, 2, create_and_fill(f.hands[i].palmVelocity()));
mxSetFieldByNumber(palm, 0, 3, create_and_fill(f.hands[i].palmNormal()));
mxSetFieldByNumber(palm, 0, 4, mxCreateDoubleScalar(f.hands[i].palmWidth()));
mxSetFieldByNumber(palm, 0, 5, create_and_fill(f.hands[i].direction()));
// get bones for all fingers
const char *digit_field_names[] =
{
"finger_id", // 0
"is_extended",
"bones",
};
int digit_fields = sizeof(digit_field_names) / sizeof(*digit_field_names);
Leap::Finger digits[5];
std::copy(f.hands[i].fingers().begin(), f.hands[i].fingers().end(), std::begin(digits));
mxArray *d = mxCreateStructMatrix(1, 5, digit_fields, digit_field_names);
mxSetFieldByNumber(p, i, 6, d);
for (int d_it = 0; d_it < 5; d_it++) {
mxSetFieldByNumber(d, d_it, 0, mxCreateDoubleScalar(digits[d_it].id()));
mxSetFieldByNumber(d, d_it, 1, mxCreateDoubleScalar(digits[d_it].isExtended()));
const char *bone_field_names[] =
{
"prev_joint", // 0
"next_joint", // 1
"width",// 2
"rotation"
};
int bone_fields = sizeof(bone_field_names) / sizeof(*bone_field_names);
mxArray *bones = mxCreateStructMatrix(1, 4, bone_fields, bone_field_names);
mxSetFieldByNumber(d, d_it, 2, bones);
auto const boneParts = {Leap::Bone::TYPE_METACARPAL, Leap::Bone::TYPE_PROXIMAL,
Leap::Bone::TYPE_INTERMEDIATE, Leap::Bone::TYPE_DISTAL};
Leap::Bone bone;
for (auto bi : boneParts) {
bone = digits[d_it].bone(bi);
mxSetFieldByNumber(bones, bi, 0, create_and_fill(bone.prevJoint())); // 0
mxSetFieldByNumber(bones, bi, 1, create_and_fill(bone.nextJoint())); // 1
mxSetFieldByNumber(bones, bi, 2, mxCreateDoubleScalar(bone.width())); // 2
mxSetFieldByNumber(bones, bi, 3, create_and_fill(bone.direction()));
}
}
} // re: for f.hands.count()
} // re: if f.hands.count() > 0
mxSetFieldByNumber(plhs[0], 0, 5, mxCreateDoubleScalar(version));
}
/// @brief entry point for matlab
///
/// @param nlhs count of left hand side
/// @param plhs pointer to left hand side array
/// @param nrhs count of right hand side
/// @param prhs pointer to right hand side array
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, mxArray const *prhs[]) {
// if there is no frame grabber yet, create one
if (!fg) {
fg = new matleap::frame_grabber;
if (fg == nullptr) {
mexErrMsgTxt("Cannot allocate a frame grabber");
}
fg->open_connection();
// register exit handler
mexAtExit(matleap_exit);
}
// parse rhs and lhs to determine what is requested from us
switch (get_command(nlhs, plhs, nrhs, prhs)) {
case -1: // turn on debug
fg->set_debug(true);
break;
case 0: // get version
plhs[0] = mxCreateNumericMatrix(1, 2, mxDOUBLE_CLASS, mxREAL);
*(mxGetPr(plhs[0]) + 0) = MAJOR_REVISION;
*(mxGetPr(plhs[0]) + 1) = MINOR_REVISION;
break;
case 1: // get frame
get_frame(nlhs, plhs);
break;
default:
// this is a logic error
mexErrMsgTxt("unknown error: please contact developer");
break;
}
}
/* -------- frame grabber class -------- */
void matleap::frame_grabber::open_connection() {
mexPrintf("Waiting for connection");
while (!controllerConnection.isConnected()) {
mexPrintf(".");
usleep(500000);
}
//controllerConnection.enableGesture(Leap::Gesture::TYPE_SWIPE);
controllerConnection.enableGesture(Leap::Gesture::TYPE_CIRCLE);
//controllerConnection.enableGesture(Leap::Gesture::TYPE_KEY_TAP);
//controllerConnection.enableGesture(Leap::Gesture::TYPE_SCREEN_TAP);
mexPrintf(" Connected!\n");
// TODO: check if needed
//LeapSetPolicyFlags(*controllerConnection, eLeapPolicyFlag_BackgroundFrames, 0);
}
void matleap::frame_grabber::close_connection() {
mexPrintf("Good bye.");
}
matleap::frame_grabber::~frame_grabber() {
if (debug) mexPrintf("Closing matleap frame grabber\n");
}
void matleap::frame_grabber::set_debug(bool flag) {
if (flag == debug) return;
if (flag) mexPrintf("Toggle debug to %i\n", flag);
debug = flag;
}
matleap::frame const &matleap::frame_grabber::get_frame() {
auto const &frame = controllerConnection.frame();
current_frame.id = frame.id();
if (debug) mexPrintf("Got frame with id %d\n", current_frame.id);
current_frame.timestamp = frame.timestamp();
current_frame.detectedHands = frame.hands().count();
current_frame.hands = frame.hands();
current_frame.has_gesture = !frame.gestures().isEmpty();
return current_frame;
}
matleap::frame_grabber::frame_grabber()
: debug(false) {
}