void LeapListener::onFrame(const Controller& controller) {
const Frame frame = controller.frame();
static int64_t lastFrameID = 0;
if(frame.id() < lastFrameID+10)
return;
Leap::HandList hands = frame.hands();
Leap::Hand hand = hands[0];
if(hand.isValid()) {
float pitch = hand.direction().pitch();
float yaw = hand.direction().yaw();
float roll = hand.palmNormal().roll();
float height = hand.palmPosition().y;
std::cout << "Pitch: " << RAD_TO_DEG*pitch << " Yaw: " << RAD_TO_DEG*yaw << " Roll: " << RAD_TO_DEG*roll << " Height: " << height << " Frame: " << frame.id() << std::endl;
// switch(gest.type()) {
// case Gesture::TYPE_CIRCLE:
// std::cout << "Takeoff" << std::endl;
// if (jakopter_takeoff() < 0)
// return;
// break;
// case Gesture::TYPE_SWIPE:
// std::cout << "Land" << std::endl;
// if (jakopter_land() < 0)
// return;
// break;
// default:
// break;
// }
char c = 's';
if (height > 300)
c = 'u';
else if(height < 75)
c = 'k';
else if (height < 150)
c = 'd';
if (roll > 0.7)
c = 'l';
else if (roll < -0.7)
c = 'r';
if (pitch > 0.7)
c = 'b';
else if (pitch < -0.7)
c = 'f';
FILE *cmd = fopen(CMDFILENAME,"w");
fprintf(cmd, "%c\n", c);
fclose(cmd);
}
lastFrameID = frame.id();
}
GestureFrame LMRecorder::prepareDataClone(const Leap::Frame frame, double timestamp)
{
GestureFrame outputFrame;
outputFrame.setTimestamp(timestamp);
Leap::HandList handsInFrame = frame.hands();
for(int handIndex=0; handIndex<handsInFrame.count(); handIndex++)
{
Leap::Hand currHand = handsInFrame[handIndex];
//create GestureHand
GestureHand gestureHand(
currHand.id(),
Vertex(currHand.palmPosition().x, currHand.palmPosition().y, currHand.palmPosition().z),
Vertex(0, 0, 0/*currHand.stabilizedPalmPosition().x, currHand.stabilizedPalmPosition().y, currHand.stabilizedPalmPosition().z*/),
Vertex(currHand.palmNormal().x, currHand.palmNormal().y, currHand.palmNormal().z),
Vertex(currHand.direction().x, currHand.direction().y, currHand.direction().z)
);
gestureHand.setOrderValue(currHand.palmPosition().x);
Vertex planeNormalVec = gestureHand.getDirection().crossProduct(gestureHand.getPalmNormal()).getNormalized();
Leap::FingerList fingersInCurrHand = currHand.fingers();
for (int fingerIndex=0; fingerIndex<fingersInCurrHand.count(); fingerIndex++)
{
Leap::Finger currFinger = fingersInCurrHand[fingerIndex];
Leap::Vector leapFingerTipPos = currFinger.tipPosition();
Vertex fingerTipPos(leapFingerTipPos.x, leapFingerTipPos.y, leapFingerTipPos.z);
float distance = getPointDistanceFromPlane(fingerTipPos, gestureHand.getPalmPosition(), planeNormalVec);
//create GestureFinger
GestureFinger gestureFinger(
currFinger.id(),
fingerTipPos,
Vertex(currFinger.stabilizedTipPosition().x, currFinger.stabilizedTipPosition().y, currFinger.stabilizedTipPosition(). z),
Vertex(currFinger.direction().x, currFinger.direction().y, currFinger.direction().z),
currFinger.length(),
currFinger.width()
);
gestureFinger.setOrderValue(distance);
gestureHand.addFinger(gestureFinger);
}
gestureHand.sortFingers();
outputFrame.addHand(gestureHand);
}
outputFrame.sortHands();
}
void LeapListener::onFrame(const Controller& controller) {
const Frame frame = controller.frame();
Leap::HandList hands = frame.hands();
Leap::Hand hand = hands[0];
if(hand.isValid()){
leapData.pitch = hand.direction().pitch();
leapData.yaw = hand.direction().yaw();
leapData.roll = hand.palmNormal().roll();
leapData.height = hand.palmPosition().y;
std::cout << "Frame id: " << frame.id()
<< ", timestamp: " << frame.timestamp()
<< ", height: " << leapData.height
<< ", pitch: " << RAD_TO_DEG * leapData.pitch
<< ", yaw: " << RAD_TO_DEG * leapData.yaw
<< ", roll: " << RAD_TO_DEG * leapData.roll << std::endl;
}
}
void convertHandRotation(const Leap::Hand& hand, MatrixF& outRotation)
{
// We need to convert from Motion coordinates to
// Torque coordinates. The conversion is:
//
// Motion Torque
// a b c a b c a -c b
// d e f --> -g -h -i --> -g i -h
// g h i d e f d -f e
const Leap::Vector& handToFingers = hand.direction();
Leap::Vector handFront = -handToFingers;
const Leap::Vector& handDown = hand.palmNormal();
Leap::Vector handUp = -handDown;
Leap::Vector handRight = handUp.cross(handFront);
outRotation.setColumn(0, Point4F( handRight.x, -handRight.z, handRight.y, 0.0f));
outRotation.setColumn(1, Point4F( -handFront.x, handFront.z, -handFront.y, 0.0f));
outRotation.setColumn(2, Point4F( handUp.x, -handUp.z, handUp.y, 0.0f));
outRotation.setPosition(Point3F::Zero);
}
//-------------------------------------------------------------------------------------
bool DigitalForensicsVisualisation::updateFrame(const Ogre::FrameEvent& evt)
{
pointLight->setPosition(mCamera->getPosition());
//leap
const Frame frame = leapController.frame();
Leap::Hand rightMost = frame.hands().rightmost();
#pragma region hand
//
//
//if (!frame.hands().isEmpty() && !handOrientationFlag)
//{
//
// palmNode->resetOrientation();
// handOrientationFlag = true;
//}
//else if (handOrientationFlag && frame.hands().isEmpty() )
//{
// handOrientationFlag = false;
//}
#pragma endregion hand
if (!frame.hands().isEmpty())
{
Leap::Hand rightMost = frame.hands().rightmost();
float pitchValue = rightMost.direction().pitch() * RAD_TO_DEG;
float rollValue = rightMost.palmNormal().roll() * RAD_TO_DEG;
float yawValue = rightMost.direction().yaw() * RAD_TO_DEG;
// to detect open and closed hand
Ogre::Vector3 indexTip = toVector(rightMost.fingers()[1].tipPosition());
Ogre::Vector3 pinkyTip = toVector(rightMost.fingers()[3].tipPosition());
float angle = std::abs(indexTip.x - pinkyTip.x);
angle /= rightMost.fingers()[1].length(); // to normalise
if (rightMost.grabStrength() == 1)
{
if (mCamera->getOrientation().getPitch() + (Ogre::Radian) (rightMost.palmPosition().y - previousPosition.y) / 80 < (Ogre::Radian) 0.17 && mCamera->getOrientation().getPitch() + (Ogre::Radian) (rightMost.palmPosition().y - previousPosition.y) / 80 > (Ogre::Radian) -1.4)
mCamera->pitch((Ogre::Radian) (rightMost.palmPosition().y - previousPosition.y) / 80);
//mCamera->yaw((Ogre::Radian) (rightMost.palmPosition().x - previousPosition.x) / -80);
//mCamera->roll((Ogre::Radian) (rollValue - previousFrameRoll) / 30);
}
else if (angle > 0.65)
{
//palmNode->setPosition(toVector(frame.hands().rightmost().palmPosition())); // between 100 and 250
if ((mCamera->getPosition().y + (frame.hands().rightmost().palmPosition().y - previousPosition.y)*2 ) > -290)
mCamera->setPosition(mCamera->getPosition() + (toVector(frame.hands().rightmost().palmPosition()) - previousPosition)*2 );
}
//else
//{
// mCamera->yaw((Ogre::Radian) (yawValue - previousFrameYaw) / -10);
//}
previousPosition = toVector(frame.hands().rightmost().palmPosition());
#pragma region hand
//palmNode->pitch((Ogre::Radian) (pitchValue - previousFramePitch) );
//
//palmNode->roll((Ogre::Radian) (rollValue - previousFrameRoll) );
//
//palmNode->yaw((Ogre::Radian) (yawValue - previousFrameYaw) );
//previousFramePitch = rightMost.direction().pitch() * RAD_TO_DEG;
//previousFrameYaw = rightMost.direction().yaw() * RAD_TO_DEG;
//previousFrameRoll = rightMost.palmNormal().roll() * RAD_TO_DEG;
// Get fingers
//FingerList fingers;
//fingers = rightMost.fingers();
//int i = 0; //between 0 and 19 (finger bones)
//
//for (FingerList::const_iterator fl = fingers.begin(); fl != fingers.end(); ++fl)
//{
//
// Finger finger;
// finger = *fl;
//
//
// /*char* dummy = (char*) malloc(128);
// sprintf (dummy, "finger id: %d, length: %f, width: %f\n", finger.id(), finger.length(), finger.width());
// OutputDebugString (dummy);
// free(dummy);*/
// // Get finger bones
// for (int b = 0; b < 4; ++b)
// {
// Bone::Type boneType;
// boneType = static_cast<Bone::Type>(b);
// Bone bone;
// bone = finger.bone(boneType);
// bonesArr[i++]->setPosition(bone.center().x, bone.center().y, bone.center().z);
// }
//}
// to detect fist
/*char* dummy = (char*) malloc(8);
sprintf (dummy, "%f\n", rightMost.grabStrength());
OutputDebugString(dummy);*/
// to detect open and closed hand
//Ogre::Vector3 indexTip = toVector(rightMost.fingers()[1].tipPosition());
//Ogre::Vector3 pinkyTip = toVector(rightMost.fingers()[3].tipPosition());
//float angle = std::abs(indexTip.x - pinkyTip.x);
//angle /= rightMost.fingers()[1].length(); // to normalise
//
//char* dummy = (char*) malloc(8);
//sprintf (dummy, "%f\n", angle);
//OutputDebugString(dummy);
#pragma endregion hand
}
return true;
}
int main(int argc, char **argv)
{
//int i;
//static IplImage *src_img = 0, *src_gray = 0;
CascadeClassifier face_cascade;
//OK 2014.02.14 精度は荒いが速度はよい ※速度重視
face_cascade.load("..\\..\\data\\haarcascade_frontalface_alt2.xml");
//setup image files used in the capture process
Mat captureFrame;
Mat grayscaleFrame;
static pLeapData pLeapData;
pLeapData.init();
static bool mLeapnot = false;
static bool mTakOffFlag = false;
static bool mSendCommandflag = false;
static int mSendCommandcounter = 0;
static int mSoundCommandcounter = 0;
static int mSoundCommandOKcounter = 20;
float pitch = 0; //前p：-0.5 後p： 0.9
float yaw = 0; //左y：-1.0 右y： 0.7
float roll = 0; //左R： 0.8 右R：-1.0
float pitch_pre = 0; //前p：-0.5 後p： 0.9
float yaw_pre = 0; //左y：-1.0 右y： 0.7
float roll_pre = 0; //左R： 0.8 右R：-1.0
float PosX = 0; //左右 左 -150 〜 右 150
float PosY = 0; //上下昇降 下 50 〜 上 300
float PosZ = 0; //前後 手前-100 〜 奥 100
float PosX_pre = 0; //左右 左 -150 〜 右 150
float PosY_pre = 0; //上下昇降 下 50 〜 上 300
float PosZ_pre = 0; //前後 手前-100 〜 奥 100
float Para_pre = 0.80f; //
float Para_cur = 0.2f; //
Leap::Frame frame; // controller is a Leap::Controller object
Leap::HandList hands;
Leap::Hand firstHand;
//double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
int mbatValue = 0;
//マウスイベント用
//http://ameblo.jp/banquet-of-merry-widow/entry-11101618791.html
MouseParam mparam;
mparam.x = 0; mparam.y = 0; mparam.event = 0; mparam.flags = 0;
//ウインドウへコールバック関数とコールバック関数からイベント情報を受け取る変数を渡す。
//setMouseCallback( wname, &mfunc, &mparam );
// AR.Drone class
// ARDrone ardrone;
if(mNonDronDebug == true)
{
}else
{
// Initialize
//if (!ardrone.open()) {
if ( initdrone(&ardrone) == -1) {
printf("Failed to initialize.\n");
return -1;
}
}
#ifdef MCISOUND
PlayWaveSound();
#endif
// Battery
printf("Battery = %d%%\n", ardrone.getBatteryPercentage());
// Instructions
printf("***************************************\n");
printf("* CV Drone sample program *\n");
printf("* - How to Play - *\n");
printf("***************************************\n");
printf("* *\n");
printf("* - Controls - *\n");
printf("* 'Space' -- Takeoff/Landing *\n");
printf("* 'Up' -- Move forward *\n");
printf("* 'Down' -- Move backward *\n");
printf("* 'Left' -- Turn left *\n");
printf("* 'Right' -- Turn right *\n");
printf("* 'Q' -- Move upward *\n");
printf("* 'A' -- Move downward *\n");
printf("* *\n");
printf("* - Others - *\n");
printf("* 'C' -- Change camera *\n");
printf("* 'Esc' -- Exit *\n");
printf("* *\n");
printf("* 'F' --mFaceDetectMode：スイッチ *\n");
printf("* 'L' --LeapMode：スイッチ *\n");
printf("* *\n");
printf("***************************************\n\n");
//
//2014.01.15 add
Leap::Controller leapController;
// Get an image
static IplImage *image;
//ardrone.setCamera(0);
ardrone.setCamera(1);//下面カメラ指定
//顔検出後の枠用
CvPoint pt1;
pt1.x = 100;
pt1.y = 100;
CvScalar rcolor;
rcolor = CV_RGB( 128, 80, 128);
//ウィンドウの表示
cvNamedWindow ("FaceDetectW", CV_WINDOW_AUTOSIZE);
cvNamedWindow ("camera", CV_WINDOW_AUTOSIZE);
//ウインドウへコールバック関数とコールバック関数からイベント情報を受け取る変数を渡す。
cvSetMouseCallback( "camera", &mMouseEventfunc, &mparam );
time_t now = time(NULL);
struct tm *pnow = localtime(&now);
while (1)
{
// Key input
int key = cvWaitKey(33);
//int key = cvWaitKey(15);
if (key == 0x1b){
break;
}
//2014.03.09 add
vx = 0.0;
vy = 0.0;
vz = 0.0;
vr = 0.0;
//音声出力タイミング用ワーク
if (mSendCommandflag == true)
{
if(mSendCommandcounter++ > 50)
{
mSendCommandflag = false;
mSendCommandcounter = 0;
}
}
// Update
if(mNonDronDebug == false)
{
if (!ardrone.update())
break;
// Get an image
image = ardrone.getImage();
if((mbatValue = ardrone.getBatteryPercentage()) < 30){
printf("Battery = %d%%\n",mbatValue );
if(mArDroneCommandFlag == false)
ardrone.move3D(0.0, 0.0, 0.0, 0.0);
msleep(80);
ardrone.landing();
printf("Landing\n");
msleep(180);
}
//}
#ifndef FACEDETECT
try{
//2014.02.15 FaceDetection追加
// (3)メモリを確保し，読み込んだ画像のグレースケール化，ヒストグラムの均一化を行う
CvMemStorage *storage = 0;
storage = cvCreateMemStorage (0);
cvClearMemStorage (storage);
//Mat captureFrame;
//Mat grayscaleFrame;
Mat captureFrameMat = cvarrToMat(image);
cvtColor(captureFrameMat, grayscaleFrame, CV_BGR2GRAY);
equalizeHist(grayscaleFrame, grayscaleFrame);
// mFaceDetectMode：Fキーにてスイッチ
if((mFaceDetectMode == true)
&&((ardrone.getCameraMode() == 0)||(ardrone.getCameraMode() == 2)))//正面カメラの場合に有効
{
// (4)物体（顔）検出
//create a vector array to store the face found
std::vector<Rect> faces;
face_cascade.detectMultiScale(grayscaleFrame, faces, 1.2, 4, CV_HAAR_FIND_BIGGEST_OBJECT|CV_HAAR_SCALE_IMAGE, Size(30,30));
//printf("FaceNum:%02d\n",faces.size());
// (5)検出された全ての顔位置に，四角を描画する
Point pt1;
Point pt2;
Point cPt1;//Center Mark
int mFaceHeight=0;
int mFaceWidth=0;
//複数検出の場合は、最大のものをTrackingする。
for(int i = 0; i < (signed)faces.size(); i++)
{
if(i==0)
{
pt1.x = faces[i].x + faces[i].width;
pt1.y = faces[i].y + faces[i].height;
mFaceHeight = faces[i].height;
mFaceWidth = faces[i].width;
pt2.x = faces[i].x ;
pt2.y = faces[i].y ;
cPt1.x = faces[i].x + faces[i].width/2;
cPt1.y = faces[i].y + faces[i].height/2;
}else
{
//最大の検出対象の値をキープ
if(faces[i-1].height < faces[i].height)
{
pt1.x = faces[i].x + faces[i].width;
pt1.y = faces[i].y + faces[i].height;
mFaceHeight = faces[i].height;
mFaceWidth = faces[i].width;
pt2.x = faces[i].x;
pt2.y = faces[i].y;
cPt1.x = faces[i].x + faces[i].width/2;
cPt1.y = faces[i].y + faces[i].height/2;
}
}
}
//printf("FaceNum:%02d",faces.size());
if(faces.size() > 0)
{
//顔検出した場合の処理
mFaceLostFlag = false;
rectangle(captureFrameMat, pt1, pt2, cvScalar(0, 255, 0, 0), 1, 8, 0);
//Center Mark
circle (captureFrameMat,cPt1,5,rcolor,-2);
//double vx=0.0, vy=0.0, vr=0.0, vz=0.0;
if((cPt1.x > 0)&&(cPt1.x < 200)){
vr = 1.0;
}
if((cPt1.x > 280)&&(cPt1.x <350)){
vr = 1.0;
}
if((cPt1.x > 450)&&(cPt1.x < 520)){
vr = -1.0;
}
if((cPt1.x > 600)&&(cPt1.x < 800)){
vr = -1.0;
}
if((cPt1.y > 0)&&(cPt1.y < 350)){
//vz = 0.75;
}else if((cPt1.y > 400)&&(cPt1.y < 600)){
//vz = -0.75;
}
if((mFaceHeight > 1)&&(mFaceHeight < 200)){
//vx = 0.75;
//vz = 0.75;
}else if((mFaceHeight > 300)&&(mFaceHeight < 600)){
//vx = -0.75;
//vz = -0.75;
}
if(!mNonDronRDebug)
{
if((!ardrone.onGround())&&(mArDroneCommandFlag == false))
{
//time_t now = time(NULL);
//struct tm *pnow = localtime(&now);
//printf("FT:%02d:%02d:%02d X:%03d Y:%03d vx:%02.1f vy:%02.1f vz:%02.1f vr:%02.1f FH:%03d\n",pnow->tm_hour,pnow->tm_min,pnow->tm_sec, cPt1.x,cPt1.y, vx, vy, vz, vr, mFaceHeight);
//ardrone.move3D(vx, vy, vz, vr);
//msleep(30);
}
}
}else
{
//Face Lostモード
if(mFaceLostFlag == false)
{
mFaceLostFlag = true;
if(!mNonDronRDebug)
{
if (!ardrone.onGround())
{
if(mArDroneCommandFlag == false)
{
//ardrone.move3D(0.0, 0.0, 0.0, 0.0);
//printf(" X:%03d Y:%03d vx:%02d vy:%02d vz:%02d vr:%02d FH:%03d\n", 0, 0, 0, 0, 0, 0, 0);
printf("Face Lostモード\n");
//msleep(100);
}
}
}
}
}
//2014.02.22
// height value enable 150 - 400
// x 150 - 600 center:400
// y 150 - 600 center:400
//printf(" x:%02d y:%02d w:%02d h:%02d",faces[0].x,faces[0].y,faces[0].width,faces[0].height);
//printf(" cx:%02d cy:%02d w:%02d h:%02d",cPt1.x,cPt1.y,faces[0].width,faces[0].height);
//printf("\n");
//
IplImage wimage = captureFrameMat;
//static IplImage wimage = grayscaleFrame;
//cvCopy( image, wimage);
image = &wimage;
}
}catch(char *e)
{
printf("%s\n",e);
}
#endif
}
//2014.03.09 add
if((mLeapnot != true)&&(pLeapData.mLeapMode == true)&&(leapController.isConnected()))
{
frame = leapController.frame(); // controller is a Leap::Controller object
hands = frame.hands();
firstHand = hands[0];
pitch_pre = pitch;
pitch = firstHand.direction().pitch();//前p：-0.5 後p： 0.9
pitch = pitch_pre*Para_pre + pitch*Para_cur; //Para_pre:0.80 Para_cur:0.20
yaw_pre = yaw; //左y：-1.0 右y： 0.7
yaw = firstHand.direction().yaw(); //左y：-1.0 右y： 0.7
yaw = yaw_pre*Para_pre + yaw*Para_cur;
roll_pre = roll; //左R： 0.8 右R：-1.0
roll = firstHand.palmNormal().roll(); //左R： 0.8 右R：-1.0
roll = roll_pre*Para_pre + roll*Para_cur;
PosX = frame.pointables().leftmost().tipPosition().x; //左右 左-150 〜 右 150
PosY = frame.pointables().leftmost().tipPosition().y; //上下昇降 下 50 〜 上 300
PosZ = frame.pointables().leftmost().tipPosition().z * (1); //前後 手前-100 〜 奥 100
if(pLeapData.mLeapDebugPrint == true){
printf("%03d XYZ:%03.02f:%03.02f:%03.02f p:%03.02f y:%03.02f r:%03.02f TF:%01i: %i\n",mSoundCommandcounter, PosX,PosY,PosZ,pitch,yaw,roll,(int)mTakOffFlag,mSendCommandcounter);
}
//LeapMotion Value set
//LeapMotionに近づけると TakeOFF
if((PosY > 50) && (PosY < 75) && (mTakOffFlag == true))
{
if(mNonDronDebug == false)
{
if (ardrone.onGround())
{
mTakOffFlag = false;
}else
{
if(mSoundCommandcounter>mSoundCommandOKcounter){
sndPlaySound("..\\..\\src\\resource\\HackathonUser1orimasu.wav", SND_ASYNC);//orimasu
mSoundCommandcounter = 0;
}
if(!mNonDronRDebug) {
ardrone.landing();
}
mTakOffFlag = false;
mSendCommandflag = true;
if(!mNonDronRDebug)
if((pLeapData.mLeapMode == true)&&(mArDroneCommandFlag == false))
ardrone.move3D(0.0, 0.0, 0.0, 0.0);
msleep(250);
}
}
}
if((PosY > 200) && (PosY < 250) && (mTakOffFlag == false))
{
if(mNonDronDebug == false)
{
if (ardrone.onGround())
{
if(pLeapData.mLeapMode == true)
{
mSendCommandflag = true;
if(!mNonDronRDebug)
ardrone.takeoff();
msleep(250);
printf("Leap takeoff\n");
mTakOffFlag = true;
ardrone.move3D(0.0, 0.0, 0.0, 0.0);
//msleep(50);
msleep(100);
}
}
}
}
if((pitch > -0.6) && (pitch < -0.45)){
//前進
//vx = 1.0;
}else if((pitch < 0.9)&&(pitch > 0.5)){
//back
//vx = -1.0;
}
if((roll > 0.5)&&(roll < 0.8)){
//左傾斜
//vy = 1.0;
}else if((roll < -1.0)&&(roll > -1.4)){
//右傾斜
//vy = -1.0;
}else if((vx == 0) &&(vy == 0))
{
//左向き
if((yaw < -0.5)&&(yaw > -0.8)){
vr = 1.0;
}
//右向き
if((yaw > 0.05)&&(yaw < 0.5)){
vr = -1.0;
}
}else
{
vr = 0.0;
}
if(!pLeapData.mLeapDebugPrint == true){
printf("vxyr:%02.01f %02.01f %02.01f: %02.01f %02.01f \n",vx,vy,vr,roll,roll_pre);
}
}//(mLeapnot != true)
//キーコントロール入力
// Take off / Landing
if(mNonDronDebug == false)
{
if (key == ' ')
{
if (ardrone.onGround())
{
if(!mNonDronRDebug)
ardrone.takeoff();
msleep(300);
printf("takeoff\n");
if(mArDroneCommandFlag == false)
{
ardrone.move3D(0.0, 0.0, 0.0, 0.0);
msleep(200);
}
if(mSoundCommandcounter>mSoundCommandOKcounter)
{
sndPlaySound("..\\..\\src\\resource\\HackathonUser1tobimasu.wav", SND_ASYNC);//orimasu
mSoundCommandcounter = 0;
}
mTakOffFlag = true;
mSendCommandflag = true;
//msleep(500);
}else//
{
if(!mNonDronRDebug)// false
{
ardrone.landing();
printf("Landing\n");
msleep(500);
ardrone.move3D(0.0, 0.0, 0.0, 0.0);
msleep(200);
}
if(mSoundCommandcounter>mSoundCommandOKcounter)
{
sndPlaySound("..\\..\\src\\resource\\HackathonUser1orimasu.wav", SND_ASYNC);//orimasu
mSoundCommandcounter = 0;
}
mTakOffFlag = false;
mSendCommandflag = true;
//msleep(500);
}
}//'Space'
}
//printf("* 'Space' -- Takeoff/Landing *\n");
//printf("* 'Up' -- Move forward *\n");
//printf("* 'Down' -- Move backward *\n");
//printf("* 'Left' -- Turn left *\n");
//printf("* 'Right' -- Turn right *\n");
//printf("* 'Q' -- Move upward *\n");
//printf("* 'A' -- Move downward *\n");
// Move
//vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
if (key == 0x260000) vx = 1.0;//Up arrow
if (key == 0x280000) vx = -1.0;//Down arrow key
if (key == 0x250000) vr = 1.0;//Left arrow key
if (key == 0x270000) vr = -1.0;//Right arrow key
if (key == 'q') vz = 1.0;
if (key == 'a') vz = -1.0;
if (key == 'r')
{
//Reset
//ardrone.emergency();
if(mNonDronDebug == true)
{
}else
{
//2014.03.09 add
vx = 0.0;
vy = 0.0;
vz = 0.0;
vr = 0.0;
//ardrone.close();
if ( initdrone(&ardrone) == -1) {
printf("Failed to initialize.\n");
return -1;
}
}
}
// 2014.03.02 add
if((key == 'f')||(key == 'F')){
mFaceDetectMode = !mFaceDetectMode;
printf("Face Mode:%02X Battery = %d%%\n",mFaceDetectMode, ardrone.getBatteryPercentage());
}
if((key == 'l')||(key == 'L')){
pLeapData.mLeapMode = !pLeapData.mLeapMode;
printf("Leap Mode:%02X Battery = %d%%\n",pLeapData.mLeapMode, ardrone.getBatteryPercentage());
}
if((key == 'v')||(key == 'V')){
printf("Btry:%d%% mSendCommandflag:%02d\n", ardrone.getBatteryPercentage(), mSendCommandflag);
}
if((key == '0')||(key == '0')){
printf("Btry:%d%% reset setFlatTrim():%02d\n", ardrone.getBatteryPercentage(), key);
ardrone.setFlatTrim();
msleep(500);
}
if(mNonDronDebug == false)
{
gotoPlaySound(vx, vy, vr, mSoundCommandcounter, mSoundCommandOKcounter);
if((!mNonDronRDebug)&&(!ardrone.onGround()))
{
if((mArDroneCommandFlag == false)&&(MouseARMode == false))
{
ardrone.move3D(vx, vy, vz, vr);
//ardrone.move3D(vx, vy, vz, vr);
//ardrone.move3D(vx, vr, 0.0, vy);
msleep(150);
time_t now = time(NULL);
struct tm *pnow = localtime(&now);
printf("KLT:%02d:%02d:%02d vx:%02.1f vy:%02.1f vz:%02.1f vr:%02.1f \n",pnow->tm_hour,pnow->tm_min,pnow->tm_sec, vx, vy, vz, vr);
}
}
//ardrone.move3D(0, 0, 0, 0);
}
// Change camera
if(mNonDronDebug == false)
{
static int mode = 0;
if((key == 'c')||(key == 'C'))
ardrone.setCamera(++mode%2);
//ardrone.setCamera(++mode%4);
// Display the image
//cvCircle (image,pt1,30,rcolor,2);
IplImage wGrayImage = grayscaleFrame;
cvShowImage ("FaceDetectW", &wGrayImage);
cvShowImage("camera", image);
cvMoveWindow( "FaceDetectW", 600, 0 );
cvMoveWindow( "camera", 50, 0 );
//WindowFromPoint(point(100,200));
if ((key == 's') && (ardrone.getCameraMode() == 1))
{
imgSave("..\\..\\SaveFileName.jpg", &wGrayImage);
}
}
}//while loop
// See you
if(mNonDronDebug == false)
{
ardrone.close();
cvDestroyWindow ("camera");
cvDestroyWindow ("FaceDetectW");
}
return 0;
}
void oleap_bang(t_oleap *x)
{
const Leap::Frame frame = x->leap->frame();
const int64_t frame_id = frame.id();
Leap::Controller controller;
// ignore the same frame
if (frame_id == x->frame_id_save) return;
x->frame_id_save = frame_id;
//outlet_anything(x->outlet, gensym("frame_start"), 0, nil);
char buff[128];
const Leap::HandList hands = frame.hands();
const size_t numHands = hands.count();
const Leap::Hand leftmost = hands.leftmost();
const Leap::Hand rightmost = hands.rightmost();
t_osc_bundle_u *bundle = osc_bundle_u_alloc();//alloc creates memory for and initializes the bundle
controller.enableGesture(Leap::Gesture::TYPE_CIRCLE);
controller.enableGesture(Leap::Gesture::TYPE_KEY_TAP);
controller.enableGesture(Leap::Gesture::TYPE_SCREEN_TAP);
controller.enableGesture(Leap::Gesture::TYPE_SWIPE);
// Get gestures
const Leap::GestureList gestures = frame.gestures();
for (int g = 0; g < gestures.count(); ++g) {
Leap::Gesture gesture = gestures[g];
switch (gesture.type()) {
case Leap::Gesture::TYPE_CIRCLE:
{
Leap::CircleGesture circle = gesture;
std::string clockwiseness;
sprintf(buff,"/gesture/circle/center/x");
oleap_bundleMessage(bundle,buff,circle.center().x);
sprintf(buff,"/gesture/circle/center/y");
oleap_bundleMessage(bundle,buff,circle.center().y);
sprintf(buff,"/gesture/circle/center/z");
oleap_bundleMessage(bundle,buff,circle.center().z);
sprintf(buff,"/gesture/circle/pitch");
oleap_bundleMessage(bundle,buff,circle.center().pitch());
sprintf(buff,"/gesture/circle/yaw");
oleap_bundleMessage(bundle,buff,circle.center().yaw());
sprintf(buff,"/gesture/circle/roll");
oleap_bundleMessage(bundle,buff,circle.center().roll());
sprintf(buff,"/gesture/circle/radius");
oleap_bundleMessage(bundle,buff,circle.radius());
sprintf(buff,"/gesture/circle/duration");
oleap_bundleMessage(bundle,buff,circle.duration());
if (circle.pointable().direction().angleTo(circle.normal()) <= Leap::PI/4) {
clockwiseness = "clockwise";
sprintf(buff,"/gesture/circle/clockwiseness/");
oleap_bundleMessage(bundle,buff,1);
} else {
clockwiseness = "counterclockwise";
sprintf(buff,"/gesture/circle/clockwiseness/");
oleap_bundleMessage(bundle,buff,-1);
}
// Calculate angle swept since last frame
float sweptAngle = 0;
if (circle.state() != Leap::Gesture::STATE_START) {
Leap::CircleGesture previousUpdate = Leap::CircleGesture(controller.frame(1).gesture(circle.id()));
sweptAngle = (circle.progress() - previousUpdate.progress()) * 2 * Leap::PI;
sprintf(buff,"/gesture/circle/angle/sweep");
oleap_bundleMessage(bundle,buff,sweptAngle);
}
}
case Leap::Gesture::TYPE_SWIPE:
{
Leap::SwipeGesture swipe = gesture;
int swipe_id = gesture.id();
int gesture_state = gesture.state();
Leap::Vector swipe_direction = swipe.direction();
float swipe_speed = swipe.speed();
////////////////////////////////Swipe data
sprintf(buff,"/gesture/swipe/direction/x");
oleap_bundleMessage(bundle,buff,swipe_direction.x);
sprintf(buff,"/gesture/swipe/direction/x");
oleap_bundleMessage(bundle,buff,swipe_direction.y);
sprintf(buff,"/gesture/swipe/direction/x");
oleap_bundleMessage(bundle,buff,swipe_direction.z);
sprintf(buff,"/gesture/swipe/position/x");
oleap_bundleMessage(bundle,buff,swipe.position().x);
sprintf(buff,"/gesture/swipe/position/y");
oleap_bundleMessage(bundle,buff,swipe.position().y);
sprintf(buff,"/gesture/swipe/position/z");
oleap_bundleMessage(bundle,buff,swipe.position().z);
sprintf(buff,"/gesture/swipe/pitch");
oleap_bundleMessage(bundle,buff,swipe.position().pitch());
sprintf(buff,"/gesture/swipe/yaw");
oleap_bundleMessage(bundle,buff,swipe.position().yaw());
sprintf(buff,"/gesture/swipe/roll");
oleap_bundleMessage(bundle,buff,swipe.position().roll());
sprintf(buff,"/gesture/swipe/position/start/x");
oleap_bundleMessage(bundle,buff,swipe.startPosition().x);
sprintf(buff,"/gesture/swipe/position/start/y");
oleap_bundleMessage(bundle,buff,swipe.startPosition().y);
sprintf(buff,"/gesture/swipe/position/start/z");
oleap_bundleMessage(bundle,buff,swipe.startPosition().z);
sprintf(buff,"/gesture/swipe/speed");
oleap_bundleMessage(bundle,buff,swipe_speed);
sprintf(buff,"/gesture/swipe/duration");
oleap_bundleMessage(bundle,buff,swipe.duration());
}
case Leap::Gesture::TYPE_KEY_TAP:
{
Leap::KeyTapGesture tap = gesture;
int tap_id = gesture.id();
int tap_state = gesture.state();
Leap::Vector tap_position = tap.position();
Leap::Vector tap_direction = tap.direction();
////////////////////////////////Key tap data
sprintf(buff,"/gesture/tap/down/position/x");
oleap_bundleMessage(bundle,buff,tap_position.x);
sprintf(buff,"/gesture/tap/down/position/y");
oleap_bundleMessage(bundle,buff,tap_position.y);
sprintf(buff,"/gesture/tap/down/position/z");
oleap_bundleMessage(bundle,buff,tap_position.z);
sprintf(buff,"/gesture/tap/down/direction/x");
oleap_bundleMessage(bundle,buff,tap_direction.x);
sprintf(buff,"/gesture/tap/down/direction/y");
oleap_bundleMessage(bundle,buff,tap_direction.y);
sprintf(buff,"/gesture/tap/down/direction/z");
oleap_bundleMessage(bundle,buff,tap_direction.z);
sprintf(buff,"/gesture/tap/down/duration");
oleap_bundleMessage(bundle,buff,tap.duration());
}
case Leap::Gesture::TYPE_SCREEN_TAP:
{
Leap::ScreenTapGesture screentap = gesture;
int screen_tap_id = gesture.id();
int screen_tap_state = gesture.state();
Leap::Vector screentap_position = screentap.position();
Leap::Vector screentap_direction = screentap.direction();
////////////////////////////////Screen tap data
sprintf(buff,"/gesture/tap/forward/position/x");
oleap_bundleMessage(bundle,buff,screentap_position.x);
sprintf(buff,"/gesture/tap/forward/position/y");
oleap_bundleMessage(bundle,buff,screentap_position.y);
sprintf(buff,"/gesture/tap/forward/position/z");
oleap_bundleMessage(bundle,buff,screentap_position.z);
sprintf(buff,"/gesture/tap/forward/direction/x");
oleap_bundleMessage(bundle,buff,screentap_direction.x);
sprintf(buff,"/gesture/tap/forward/direction/y");
oleap_bundleMessage(bundle,buff,screentap_direction.y);
sprintf(buff,"/gesture/tap/forward/direction/z");
oleap_bundleMessage(bundle,buff,screentap_direction.z);
sprintf(buff,"/gesture/tap/forward/duration");
oleap_bundleMessage(bundle,buff,screentap.duration());
}
default:
break;
}
}
sprintf(buff,"/timeStamp");
oleap_bundleMessage(bundle,buff,frame.timestamp());
sprintf(buff,"/Hands");
oleap_bundleMessage(bundle,buff,numHands);
sprintf(buff,"/hand/leftmost/id");
oleap_bundleMessage(bundle,buff,leftmost.id());
sprintf(buff,"/hand/leftmost/palm/positiony/x");
oleap_bundleMessage(bundle,buff,leftmost.palmPosition().x);
sprintf(buff,"/hand/leftmost/palm/positiony/y");
oleap_bundleMessage(bundle,buff,leftmost.palmPosition().y);
sprintf(buff,"/hand/leftmost/palm/positiony/z");
oleap_bundleMessage(bundle,buff,leftmost.palmPosition().z);
sprintf(buff,"/hand/leftmost/direction/x");
oleap_bundleMessage(bundle,buff,leftmost.direction().x);
sprintf(buff,"/hand/leftmost/direction/y");
oleap_bundleMessage(bundle,buff,leftmost.direction().y);
sprintf(buff,"/hand/leftmost/direction/z");
oleap_bundleMessage(bundle,buff,leftmost.direction().z);
sprintf(buff,"/hand/leftmost/pitch");
oleap_bundleMessage(bundle,buff,leftmost.palmPosition().pitch());
sprintf(buff,"/hand/leftmost/yaw");
oleap_bundleMessage(bundle,buff,leftmost.palmPosition().yaw());
sprintf(buff,"/hand/leftmost/roll");
oleap_bundleMessage(bundle,buff,leftmost.palmPosition().roll());
sprintf(buff,"/hand/leftmost/palm/velocity/x");
oleap_bundleMessage(bundle,buff,leftmost.palmVelocity().x);
sprintf(buff,"/hand/leftmost/palm/velocity/y");
oleap_bundleMessage(bundle,buff,leftmost.palmVelocity().y);
sprintf(buff,"/hand/leftmost/palm/velocity/z");
oleap_bundleMessage(bundle,buff,leftmost.palmVelocity().z);
sprintf(buff,"/hand/leftmost/palm/sphere/center/x");
oleap_bundleMessage(bundle,buff,leftmost.sphereCenter().x);
sprintf(buff,"/hand/leftmost/palm/sphere/center/y");
oleap_bundleMessage(bundle,buff,leftmost.sphereCenter().y);
sprintf(buff,"/hand/leftmost/palm/sphere/center/z");
oleap_bundleMessage(bundle,buff,leftmost.sphereCenter().z);
sprintf(buff,"/hand/leftmost/palm/sphere/radius");
oleap_bundleMessage(bundle,buff,leftmost.sphereRadius());
sprintf(buff,"/hand/leftmost/palm/normal/x");
oleap_bundleMessage(bundle,buff,leftmost.palmNormal().x);
sprintf(buff,"/hand/leftmost/palm/normal/y");
oleap_bundleMessage(bundle,buff,leftmost.palmNormal().y);
sprintf(buff,"/hand/leftmost/palm/normal/z");
oleap_bundleMessage(bundle,buff,leftmost.palmNormal().z);
sprintf(buff,"/hand/leftmost/distance/from/rightmost");
oleap_bundleMessage(bundle,buff,leftmost.palmPosition().angleTo(rightmost.palmPosition()));
const Leap::FingerList &fingers = leftmost.fingers();
const size_t numFingers = fingers.count();
for(size_t j = 0; j < numFingers; j++)
{
const Leap::Finger &finger = fingers[j];
const int32_t finger_id = finger.id();
//const Leap::Ray& tip = finger.tip();
const Leap::Vector direction = finger.direction();
const Leap::Vector position = finger.tipPosition();
const Leap::Vector velocity = finger.tipVelocity();
const double width = finger.width();
const double length = finger.length();
const bool isTool = finger.isTool();
sprintf(buff,"/hand/leftmost/finger/%d/hand_id",j+1);
oleap_bundleMessage(bundle,buff,leftmost.id());
sprintf(buff,"/hand/leftmost/finger/%d/finger_id",j+1);
oleap_bundleMessage(bundle,buff,finger.id());
sprintf(buff,"/hand/leftmost/finger/%d/position/x",j+1);
oleap_bundleMessage(bundle,buff,position.x);
sprintf(buff,"/hand/leftmost/finger/%d/position/y",j+1);
oleap_bundleMessage(bundle,buff,position.y);
sprintf(buff,"/hand/leftmost/finger/%d/position/z",j+1);
oleap_bundleMessage(bundle,buff,position.z);
sprintf(buff,"/hand/leftmost/finger/%d/direction/x",j+1);
oleap_bundleMessage(bundle,buff,direction.x);
sprintf(buff,"/hand/leftmost/finger/%d/direction/y",j+1);
oleap_bundleMessage(bundle,buff,direction.y);
sprintf(buff,"/hand/leftmost/finger/%d/direction/z",j+1);
oleap_bundleMessage(bundle,buff,direction.z);
sprintf(buff,"/hand/leftmost/finger/%d/velocity/x",j+1);
oleap_bundleMessage(bundle,buff,velocity.x);
sprintf(buff,"/hand/leftmost/finger/%d/velocity/y",j+1);
oleap_bundleMessage(bundle,buff,velocity.y);
sprintf(buff,"/hand/leftmost/finger/%d/velocity/z",j+1);
oleap_bundleMessage(bundle,buff,velocity.z);
sprintf(buff,"/hand/leftmost/finger/%d/direction/normalized/x",j+1);
oleap_bundleMessage(bundle,buff,direction.normalized().x);
sprintf(buff,"/hand/leftmost/finger/%d/direction/normalized/y",j+1);
oleap_bundleMessage(bundle,buff,direction.normalized().y);
sprintf(buff,"/hand/leftmost/finger/%d/direction/normalized/z",j+1);
oleap_bundleMessage(bundle,buff,direction.normalized().z);
sprintf(buff,"/hand/leftmost/finger/%d/pitch/normalized/",j+1);
oleap_bundleMessage(bundle,buff,direction.normalized().pitch());
sprintf(buff,"/hand/leftmost/finger/%d/yaw/normalized/",j+1);
oleap_bundleMessage(bundle,buff,direction.normalized().yaw());
sprintf(buff,"/hand/leftmost/finger/%d/roll/normalized/",j+1);
oleap_bundleMessage(bundle,buff,direction.normalized().roll());
sprintf(buff,"/hand/leftmost/finger/%d/width",j+1);
oleap_bundleMessage(bundle,buff,width);
sprintf(buff,"/hand/leftmost/finger/%d/length",j+1);
oleap_bundleMessage(bundle,buff,length);
sprintf(buff,"/hand/leftmost/isTool",j+1);
oleap_bundleMessage(bundle,buff,isTool);
for(size_t i = j+1; i < numFingers; i++)
{
sprintf(buff,"/hand/leftmost/finger/%d/distance/to/finger/%d",j+1,i+1);
oleap_bundleMessage(bundle,buff,fingers[j].direction().distanceTo(fingers[i].direction()));
sprintf(buff,"/hand/leftmost/finger/%d/angle/to/finger/%d",j+1,i+1);
oleap_bundleMessage(bundle,buff,fingers[j].direction().angleTo((fingers[i].direction())));
}
}
sprintf(buff,"/hand/rightmost/id");
oleap_bundleMessage(bundle,buff,rightmost.id());
sprintf(buff,"/hand/rightmost/palm/positiony/x");
oleap_bundleMessage(bundle,buff,rightmost.palmPosition().x);
sprintf(buff,"/hand/rightmost/palm/positiony/y");
oleap_bundleMessage(bundle,buff,rightmost.palmPosition().y);
sprintf(buff,"/hand/rightmost/palm/positiony/z");
oleap_bundleMessage(bundle,buff,rightmost.palmPosition().z);
sprintf(buff,"/hand/rightmost/direction/x");
oleap_bundleMessage(bundle,buff,rightmost.direction().x);
sprintf(buff,"/hand/rightmost/direction/y");
oleap_bundleMessage(bundle,buff,rightmost.direction().y);
sprintf(buff,"/hand/rightmost/direction/z");
oleap_bundleMessage(bundle,buff,rightmost.direction().z);
sprintf(buff,"/hand/rightmost/pitch");
oleap_bundleMessage(bundle,buff,rightmost.palmPosition().pitch());
sprintf(buff,"/hand/rightmost/yaw");
oleap_bundleMessage(bundle,buff,rightmost.palmPosition().yaw());
sprintf(buff,"/hand/rightmost/roll");
oleap_bundleMessage(bundle,buff,rightmost.palmPosition().roll());
sprintf(buff,"/hand/rightmost/palm/velocity/x");
oleap_bundleMessage(bundle,buff,rightmost.palmVelocity().x);
sprintf(buff,"/hand/rightmost/palm/velocity/y");
oleap_bundleMessage(bundle,buff,rightmost.palmVelocity().y);
sprintf(buff,"/hand/rightmost/palm/velocity/z");
oleap_bundleMessage(bundle,buff,rightmost.palmVelocity().z);
sprintf(buff,"/hand/rightmost/palm/sphere/center/x");
oleap_bundleMessage(bundle,buff,rightmost.sphereCenter().x);
sprintf(buff,"/hand/rightmost/palm/sphere/center/y");
oleap_bundleMessage(bundle,buff,rightmost.sphereCenter().y);
sprintf(buff,"/hand/rightmost/palm/sphere/center/z");
oleap_bundleMessage(bundle,buff,rightmost.sphereCenter().z);
sprintf(buff,"/hand/rightmost/palm/sphere/radius");
oleap_bundleMessage(bundle,buff,rightmost.sphereRadius());
sprintf(buff,"/hand/rightmost/palm/normal/x");
oleap_bundleMessage(bundle,buff,rightmost.palmNormal().x);
sprintf(buff,"/hand/rightmost/palm/normal/y");
oleap_bundleMessage(bundle,buff,rightmost.palmNormal().y);
sprintf(buff,"/hand/rightmost/palm/normal/z");
oleap_bundleMessage(bundle,buff,rightmost.palmNormal().z);
sprintf(buff,"/hand/rightmost/distance/from/leftmost");
oleap_bundleMessage(bundle,buff,rightmost.palmPosition().angleTo(leftmost.palmPosition()));
const Leap::FingerList &rightMostfingers = rightmost.fingers();
const size_t rightMostnNumFingers = fingers.count();
for(size_t j = 0; j < rightMostnNumFingers; j++)
{
const Leap::Finger &finger = rightMostfingers[j];
const int32_t finger_id = finger.id();
//const Leap::Ray& tip = finger.tip();
const Leap::Vector direction = finger.direction();
const Leap::Vector position = finger.tipPosition();
const Leap::Vector velocity = finger.tipVelocity();
const double width = finger.width();
const double length = finger.length();
const bool isTool = finger.isTool();
sprintf(buff,"/hand/rightmost/finger/%d/hand_id",j+1);
oleap_bundleMessage(bundle,buff,rightmost.id());
sprintf(buff,"/hand/rightmost/finger/%d/finger_id",j+1);
oleap_bundleMessage(bundle,buff,finger.id());
sprintf(buff,"/hand/rightmost/finger/%d/position/x",j+1);
oleap_bundleMessage(bundle,buff,position.x);
sprintf(buff,"/hand/rightmost/finger/%d/position/y",j+1);
oleap_bundleMessage(bundle,buff,position.y);
sprintf(buff,"/hand/rightmost/finger/%d/position/z",j+1);
oleap_bundleMessage(bundle,buff,position.z);
sprintf(buff,"/hand/rightmost/finger/%d/direction/x",j+1);
oleap_bundleMessage(bundle,buff,direction.x);
sprintf(buff,"/hand/rightmost/finger/%d/direction/y",j+1);
oleap_bundleMessage(bundle,buff,direction.y);
sprintf(buff,"/hand/rightmost/finger/%d/direction/z",j+1);
oleap_bundleMessage(bundle,buff,direction.z);
sprintf(buff,"/hand/rightmost/finger/%d/velocity/x",j+1);
oleap_bundleMessage(bundle,buff,velocity.x);
sprintf(buff,"/hand/rightmost/finger/%d/velocity/y",j+1);
oleap_bundleMessage(bundle,buff,velocity.y);
sprintf(buff,"/hand/rightmost/finger/%d/velocity/z",j+1);
oleap_bundleMessage(bundle,buff,velocity.z);
sprintf(buff,"/hand/rightmost/finger/%d/direction/normalized/x",j+1);
oleap_bundleMessage(bundle,buff,direction.normalized().x);
sprintf(buff,"/hand/rightmost/finger/%d/direction/normalized/y",j+1);
oleap_bundleMessage(bundle,buff,direction.normalized().y);
sprintf(buff,"/hand/rightmost/finger/%d/direction/normalized/z",j+1);
oleap_bundleMessage(bundle,buff,direction.normalized().z);
sprintf(buff,"/hand/rightmost/finger/%d/pitch/normalized/",j+1);
oleap_bundleMessage(bundle,buff,direction.normalized().pitch());
sprintf(buff,"/hand/rightmost/finger/%d/yaw/normalized/",j+1);
oleap_bundleMessage(bundle,buff,direction.normalized().yaw());
sprintf(buff,"/hand/rightmost/finger/%d/roll/normalized/",j+1);
oleap_bundleMessage(bundle,buff,direction.normalized().roll());
sprintf(buff,"/hand/rightmost/finger/%d/width",j+1);
oleap_bundleMessage(bundle,buff,width);
sprintf(buff,"/hand/rightmost/finger/%d/length",j+1);
oleap_bundleMessage(bundle,buff,length);
sprintf(buff,"/hand/rightmost/isTool",j+1);
oleap_bundleMessage(bundle,buff,isTool);
for(size_t i = j+1; i < numFingers; i++)
{
sprintf(buff,"/hand/rightmost/finger/%d/distance/to/finger/%d",j+1,i+1);
oleap_bundleMessage(bundle,buff,fingers[j].direction().distanceTo(fingers[i].direction()));
sprintf(buff,"/hand/rightmost/finger/%d/angle/to/finger/%d",j+1,i+1);
oleap_bundleMessage(bundle,buff,fingers[j].direction().angleTo((fingers[i].direction())));
}
}
//////////////////////////
for(size_t i = 0; i < numHands; i++)
{
// Hand
const Leap::Hand &hand = hands[i];
const int32_t hand_id = hand.id();
const Leap::FingerList &fingers = hand.fingers();
const size_t numFingers = fingers.count();
float pitch = hand.direction().pitch();
float yaw = hand.direction().yaw();
float roll = hand.palmNormal().roll();
//Leap::Hand leftmost = ;
//t_osc_bundle_u *bundle, string address, float datum
sprintf(buff,"/hand/%d/id",i+1);
oleap_bundleMessage(bundle,buff,hand_id);
sprintf(buff,"/hand/%d/fingers",i+1);
oleap_bundleMessage(bundle,buff,numFingers);
sprintf(buff,"/hand/%d/pitch",i+1);
oleap_bundleMessage(bundle,buff,pitch);
sprintf(buff,"/hand/%d/yaw",i+1);
oleap_bundleMessage(bundle,buff,yaw);
sprintf(buff,"/hand/%d/roll",i+1);
oleap_bundleMessage(bundle,buff,roll);
for(size_t j = 0; j < numFingers; j++)
{
// Finger
const Leap::Finger &finger = fingers[j];
const int32_t finger_id = finger.id();
//const Leap::Ray& tip = finger.tip();
const Leap::Vector direction = finger.direction();
const Leap::Vector position = finger.tipPosition();
const Leap::Vector velocity = finger.tipVelocity();
const double width = finger.width();
const double length = finger.length();
const bool isTool = finger.isTool();
/*
string names [14]= {“xpos”,”ypos”,”zpos”,”xdir”,”ydir”,”zdir”,”xvel”,”yvel”,”zvel,finger_length”,”istool_mes”};
for(LOOP OVER FINGERS “J”)
{
t_osc_message_u *handdata[14];
for(int i=0;i<14;i++)
{
handdata[i]=osc_message_u_alloc();
osc_message_u_setAddress(handdata[i], “/”+j.toString()+ ”/” +names[i]);
}
}
*/
sprintf(buff,"/hand/%d/finger/%d/hand_id",i+1,j+1);
oleap_bundleMessage(bundle,buff,hand_id);
sprintf(buff,"/hand/%d/finger/%d/finger_id",i+1,j+1);
oleap_bundleMessage(bundle,buff,finger_id);
sprintf(buff,"/hand/%d/finger/%d/position/x",i+1,j+1);
oleap_bundleMessage(bundle,buff,position.x);
sprintf(buff,"/hand/%d/finger/%d/position/y",i+1,j+1);
oleap_bundleMessage(bundle,buff,position.y);
sprintf(buff,"/hand/%d/finger/%d/position/z",i+1,j+1);
oleap_bundleMessage(bundle,buff,position.z);
sprintf(buff,"/hand/%d/finger/%d/direction/x",i+1,j+1);
oleap_bundleMessage(bundle,buff,direction.x);
sprintf(buff,"/hand/%d/finger/%d/direction/y",i+1,j+1);
oleap_bundleMessage(bundle,buff,direction.y);
sprintf(buff,"/hand/%d/finger/%d/direction/z",i+1,j+1);
oleap_bundleMessage(bundle,buff,direction.z);
sprintf(buff,"/hand/%d/finger/%d/velocity/x",i+1,j+1);
oleap_bundleMessage(bundle,buff,velocity.x);
sprintf(buff,"/hand/%d/finger/%d/velocity/y",i+1,j+1);
oleap_bundleMessage(bundle,buff,velocity.y);
sprintf(buff,"/hand/%d/finger/%d/velocity/z",i+1,j+1);
oleap_bundleMessage(bundle,buff,velocity.z);
sprintf(buff,"/hand/%d/finger/%d/width",i+1,j+1);
oleap_bundleMessage(bundle,buff,width);
sprintf(buff,"/hand/%d/finger/%d/length",i+1,j+1);
oleap_bundleMessage(bundle,buff,length);
sprintf(buff,"/hand/%d/tool",i+1,j+1);
oleap_bundleMessage(bundle,buff,isTool);
}
const Leap::Vector position = hand.palmPosition();
const Leap::Vector direction = hand.direction();
const Leap::Vector velocity = hand.palmVelocity();
const Leap::Vector normal = hand.palmNormal();
const Leap::Vector sphereCenter = hand.sphereCenter();
const double sphereRadius = hand.sphereRadius();
///////////////////////////Palm Data!!!
sprintf(buff,"/hand/%d/palm/hand_id",i+1);
oleap_bundleMessage(bundle,buff,hand_id);
sprintf(buff,"/hand/%d/palm/frame_id",i+1);
oleap_bundleMessage(bundle,buff,frame_id);
sprintf(buff,"/hand/%d/palm/position/x",i+1);
oleap_bundleMessage(bundle,buff,position.x);
sprintf(buff,"/hand/%d/palm/position/y",i+1);
oleap_bundleMessage(bundle,buff,position.y);
sprintf(buff,"/hand/%d/palm/position/z",i+1);
oleap_bundleMessage(bundle,buff,position.z);
sprintf(buff,"/hand/%d/palm/direction/x",i+1);
oleap_bundleMessage(bundle,buff,direction.x);
sprintf(buff,"/hand/%d/palm/direction/y",i+1);
oleap_bundleMessage(bundle,buff,direction.y);
sprintf(buff,"/hand/%d/palm/direction/z",i+1);
oleap_bundleMessage(bundle,buff,direction.z);
sprintf(buff,"/hand/%d/palm/velocity/x",i+1);
oleap_bundleMessage(bundle,buff,velocity.x);
sprintf(buff,"/hand/%d/palm/velocity/x",i+1);
oleap_bundleMessage(bundle,buff,velocity.y);
sprintf(buff,"/hand/%d/palm/velocity/z",i+1);
oleap_bundleMessage(bundle,buff,velocity.z);
sprintf(buff,"/hand/%d/palm/normal/x",i+1);
oleap_bundleMessage(bundle,buff,normal.x);
sprintf(buff,"/hand/%d/palm/normal/y",i+1);
oleap_bundleMessage(bundle,buff,normal.y);
sprintf(buff,"/hand/%d/palm/normal/z",i+1);
oleap_bundleMessage(bundle,buff,normal.z);
sprintf(buff,"/hand/%d/sphere/id",i);
oleap_bundleMessage(bundle,buff,hand_id);
sprintf(buff,"/hand/%d/sphere/frame_id",i);
oleap_bundleMessage(bundle,buff,frame_id);
sprintf(buff,"/hand/%d/sphere/center/x",i+1);
oleap_bundleMessage(bundle,buff,sphereCenter.x);
sprintf(buff,"/hand/%d/sphere/center/y",i+1);
oleap_bundleMessage(bundle,buff,sphereCenter.y);
sprintf(buff,"/hand/%d/sphere/center/z",i+1);
oleap_bundleMessage(bundle,buff,sphereCenter.z);
sprintf(buff,"/hand/%d/sphere/radius",i+1);
oleap_bundleMessage(bundle,buff,sphereRadius);
const Leap::PointableList pointables = frame.pointables();
const int count = pointables.count();
for(size_t j = 0; j < count; j++){
sprintf(buff,"/hand/%d/pointable/%d/id",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).id());
sprintf(buff,"/hand/%d/pointable/%d/length",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).length());
sprintf(buff,"/hand/%d/pointable/%d/width",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).width());
sprintf(buff,"/hand/%d/pointable/%d/direction/x",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).direction().x);
sprintf(buff,"/hand/%d/pointable/%d/direction/y",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).direction().y);
sprintf(buff,"/hand/%d/pointable/%d/direction/z",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).direction().z);
sprintf(buff,"/hand/%d/pointable/%d/isFinger",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).isFinger());
sprintf(buff,"/hand/%d/pointable/%d/isTool",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).isTool());
sprintf(buff,"/hand/%d/pointable/%d/position/tip/x",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).tipPosition().x);
sprintf(buff,"/hand/%d/pointable/%d/position/tip/y",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).tipPosition().y);
sprintf(buff,"/hand/%d/pointable/%d/position/tip/z",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).tipPosition().z);
sprintf(buff,"/hand/%d/pointable/%d/velocity/tip/x",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).tipVelocity().x);
sprintf(buff,"/hand/%d/pointable/%d/velocity/tip/y",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).tipVelocity().y);
sprintf(buff,"/hand/%d/pointable/%d/velocity/tip/z",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).tipVelocity().z);
sprintf(buff,"/hand/%d/pointable/%d/position/stabilized/x",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).stabilizedTipPosition().x);
sprintf(buff,"/hand/%d/pointable/%d/position/stabilized/y",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).stabilizedTipPosition().y);
sprintf(buff,"/hand/%d/pointable/%d/position/stabilized/z",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).stabilizedTipPosition().z);
sprintf(buff,"/hand/%d/pointable/%d/touchZone/distance",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).touchDistance());
sprintf(buff,"/hand/%d/pointable/%d/touchZone",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).touchZone());
sprintf(buff,"/hand/%d/pointable/%d/touchZone/touching",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).ZONE_TOUCHING);
sprintf(buff,"/hand/%d/pointable/%d/touchZone/hovering",i+1,j+1);
oleap_bundleMessage(bundle,buff,pointables.operator[](j).ZONE_HOVERING);
}
const Leap::InteractionBox box = frame.interactionBox();
const Leap::Vector center = box.center();
const Leap::Vector normalizedPosition = box.normalizePoint(position);
sprintf(buff,"/hand/%d/interactionBox/depth",i+1);
oleap_bundleMessage(bundle,buff,box.depth());
sprintf(buff,"/hand/%d/interactionBox/center/x",i+1);
oleap_bundleMessage(bundle,buff,center.x);
sprintf(buff,"/hand/%d/interactionBox/center/y",i+1);
oleap_bundleMessage(bundle,buff,center.y);
sprintf(buff,"/hand/%d/interactionBox/center/z",i+1);
oleap_bundleMessage(bundle,buff,center.z);
sprintf(buff,"/hand/%d/interactionBox/position/normalized/x",i+1);
oleap_bundleMessage(bundle,buff,normalizedPosition.x);
sprintf(buff,"/hand/%d/interactionBox/position/normalized/y",i+1);
oleap_bundleMessage(bundle,buff,normalizedPosition.y);
sprintf(buff,"/hand/%d/interactionBox/position/normalized/z",i+1);
oleap_bundleMessage(bundle,buff,normalizedPosition.z);
sprintf(buff,"/hand/%d/interactionBox/width",i+1);
oleap_bundleMessage(bundle,buff,box.width());
sprintf(buff,"/hand/%d/interactionBox/height",i+1);
oleap_bundleMessage(bundle,buff,box.height());
}
long bytes = 0;//length of byte array
char* pointer = NULL;
osc_bundle_u_serialize(bundle, &bytes, &pointer);//& is address of the variable
//post("%ld %p", bytes,pointer);
t_atom out[2];
atom_setlong(out, bytes);
atom_setlong(out+1, (long)pointer);
outlet_anything(x->outlet, gensym("FullPacket"), 2, out);
osc_bundle_u_free(bundle);//get rid of stuff in osc message
osc_mem_free(pointer);//marks pointer address as being free (clear if you want to keep using same)
}
void SampleListener::onFrame(const Leap::Controller& controller) {
// Get the most recent frame and report some basic information
const Leap::Frame frame = controller.frame();
std::cout << "Frame id: " << frame.id()
<< ", timestamp: " << frame.timestamp()
<< ", hands: " << frame.hands().count()
<< ", extended fingers: " << frame.fingers().extended().count()
<< ", tools: " << frame.tools().count()
<< ", gestures: " << frame.gestures().count() << std::endl;
Leap::HandList hands = frame.hands();
for (Leap::HandList::const_iterator hl = hands.begin(); hl != hands.end(); ++hl) {
// Get the first hand
const Leap::Hand hand = *hl;
std::string handType = hand.isLeft() ? "Left hand" : "Right hand";
std::cout << std::string(2, ' ') << handType << ", id: " << hand.id()
<< ", palm position: " << hand.palmPosition() << std::endl;
// Get the hand's normal vector and direction
const Leap::Vector normal = hand.palmNormal();
const Leap::Vector direction = hand.direction();
// Calculate the hand's pitch, roll, and yaw angles
std::cout << std::string(2, ' ') << "pitch: " << direction.pitch() * Leap::RAD_TO_DEG << " degrees, "
<< "roll: " << normal.roll() * Leap::RAD_TO_DEG << " degrees, "
<< "yaw: " << direction.yaw() * Leap::RAD_TO_DEG << " degrees" << std::endl;
// Get the Arm bone
Leap::Arm arm = hand.arm();
std::cout << std::string(2, ' ') << "Arm direction: " << arm.direction()
<< " wrist position: " << arm.wristPosition()
<< " elbow position: " << arm.elbowPosition() << std::endl;
// Get fingers
const Leap::FingerList fingers = hand.fingers();
for (Leap::FingerList::const_iterator fl = fingers.begin(); fl != fingers.end(); ++fl) {
const Leap::Finger finger = *fl;
std::cout << std::string(4, ' ') << fingerNames[finger.type()]
<< " finger, id: " << finger.id()
<< ", length: " << finger.length()
<< "mm, width: " << finger.width() << std::endl;
// Get finger bones
for (int b = 0; b < 4; ++b) {
Leap::Bone::Type boneType = static_cast<Leap::Bone::Type>(b);
Leap::Bone bone = finger.bone(boneType);
std::cout << std::string(6, ' ') << boneNames[boneType]
<< " bone, start: " << bone.prevJoint()
<< ", end: " << bone.nextJoint()
<< ", direction: " << bone.direction() << std::endl;
}
}
}
// Get tools
const Leap::ToolList tools = frame.tools();
for (Leap::ToolList::const_iterator tl = tools.begin(); tl != tools.end(); ++tl) {
const Leap::Tool tool = *tl;
std::cout << std::string(2, ' ') << "Tool, id: " << tool.id()
<< ", position: " << tool.tipPosition()
<< ", direction: " << tool.direction() << std::endl;
}
// Get gestures
const Leap::GestureList gestures = frame.gestures();
for (int g = 0; g < gestures.count(); ++g) {
Leap::Gesture gesture = gestures[g];
switch (gesture.type()) {
case Leap::Gesture::TYPE_CIRCLE:
{
Leap::CircleGesture circle = gesture;
std::string clockwiseness;
if (circle.pointable().direction().angleTo(circle.normal()) <= Leap::PI/2) {
clockwiseness = "clockwise";
} else {
clockwiseness = "counterclockwise";
}
// Calculate angle swept since last frame
float sweptAngle = 0;
if (circle.state() != Leap::Gesture::STATE_START) {
Leap::CircleGesture previousUpdate = Leap::CircleGesture(controller.frame(1).gesture(circle.id()));
sweptAngle = (circle.progress() - previousUpdate.progress()) * 2 * Leap::PI;
}
std::cout << std::string(2, ' ')
<< "Circle id: " << gesture.id()
<< ", state: " << stateNames[gesture.state()]
<< ", progress: " << circle.progress()
<< ", radius: " << circle.radius()
<< ", angle " << sweptAngle * Leap::RAD_TO_DEG
<< ", " << clockwiseness << std::endl;
break;
}
case Leap::Gesture::TYPE_SWIPE:
{
Leap::SwipeGesture swipe = gesture;
std::cout << std::string(2, ' ')
<< "Swipe id: " << gesture.id()
<< ", state: " << stateNames[gesture.state()]
<< ", direction: " << swipe.direction()
<< ", speed: " << swipe.speed() << std::endl;
break;
}
case Leap::Gesture::TYPE_KEY_TAP:
{
Leap::KeyTapGesture tap = gesture;
std::cout << std::string(2, ' ')
<< "Key Tap id: " << gesture.id()
<< ", state: " << stateNames[gesture.state()]
<< ", position: " << tap.position()
<< ", direction: " << tap.direction()<< std::endl;
break;
}
case Leap::Gesture::TYPE_SCREEN_TAP:
{
Leap::ScreenTapGesture screentap = gesture;
std::cout << std::string(2, ' ')
<< "Screen Tap id: " << gesture.id()
<< ", state: " << stateNames[gesture.state()]
<< ", position: " << screentap.position()
<< ", direction: " << screentap.direction()<< std::endl;
break;
}
default:
std::cout << std::string(2, ' ') << "Unknown gesture type." << std::endl;
break;
}
}
if (!frame.hands().isEmpty() || !gestures.isEmpty()) {
std::cout << std::endl;
}
}