diff options
Diffstat (limited to 'matleap.cpp')
| -rwxr-xr-x | matleap.cpp | 234 |
1 files changed, 234 insertions, 0 deletions
diff --git a/matleap.cpp b/matleap.cpp new file mode 100755 index 0000000..9a3cb25 --- /dev/null +++ b/matleap.cpp @@ -0,0 +1,234 @@ +/// @file matleap.cpp +/// @brief leap motion controller interface +/// @author Jeff Perry <jeffsp@gmail.com> +/// @version 1.0 +/// @date 2013-09-12 + +#include "matleap.h" +#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 = 0; +} + +/// @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 + switch (nrhs) { + case 1: + command = *mxGetPr(prhs[0]); + break; + case 0: + mexErrMsgTxt("Not enough input arguments"); + default: + mexErrMsgTxt("Too many input arguments"); + } + // check that inputs 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: { + // get version command requires 1 outputs + if (nlhs != 0 && nlhs != 1) + mexErrMsgTxt("Wrong number of outputs specified"); + } + break; + case 1: { + // frame grab command only requires one input + if (nrhs > 1) + mexErrMsgTxt("Too many inputs specified"); + // frame grab command requires exactly 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)); +} + +void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { + if (!fg) { + fg = new matleap::frame_grabber; + if (fg == 0) + mexErrMsgTxt("Cannot allocate a frame grabber"); + fg->open_connection(); + mexAtExit(matleap_exit); + } + switch (get_command(nlhs, plhs, nrhs, prhs)) { + // turn on debug + case -1: + fg->set_debug(true); + return; + // get version + case 0: + plhs[0] = mxCreateNumericMatrix(1, 2, mxDOUBLE_CLASS, mxREAL); + *(mxGetPr(plhs[0]) + 0) = MAJOR_REVISION; + *(mxGetPr(plhs[0]) + 1) = MINOR_REVISION; + return; + // get frame + case 1: + get_frame(nlhs, plhs); + return; + default: + // this is a logic error + mexErrMsgTxt("unknown error: please contact developer"); + } +} |