C++ HandList类代码示例

void AbstractLeapGestureManager::update(){
	cout << "update\n";
	pastFrame = frame;
	frame = controller.frame();
	HandList hands = frame.hands();
		pastLeft = left;
		pastRight = right;
		for (HandList::const_iterator hl = hands.begin(); hl != hands.end(); ++hl){
			Hand hand = *hl;
			if (hand.isLeft()){
				left = hand;

			}
			else{
				right = hand;

			}
		
	}
	
}

void MyListener::onFrame(const Controller& controller) 
{
    const Frame frame = controller.frame();
    const GestureList gestures = frame.gestures();
    HandList hands = frame.hands();
    
    for (HandList::const_iterator hl = hands.begin(); hl != hands.end(); ++hl) {
        const Hand hand = *hl;
        // Smoothing and stabilization is performed in order to make this 
        // value more suitable for interaction with 2D content. The stabilized 
        // position lags behind the palm position by a variable amount, 
        // depending primarily on the speed of movement.
        Vector position = hand.stabilizedPalmPosition();

        if (m_positionChanged)
            m_positionChanged(position[0], position[1], position[2], 
                              frame.fingers().extended().count(), 
                              hand.direction(),
                              hand.palmVelocity());

        if (m_pinch)
            m_pinch(hand.pinchStrength());

        if (m_grab)
            m_grab(hand.grabStrength());
    }

    for (int g = 0; g < gestures.count(); ++g) {
        Gesture gesture = gestures[g];
        switch (gesture.type()) {
        case Gesture::TYPE_KEY_TAP:
        case Gesture::TYPE_SCREEN_TAP:
            if (m_tapped)
                m_tapped();
            break;
        case Gesture::TYPE_SWIPE:
            break;
        }
    }
}

void LeapBrowser::updateCursorLocation()
{
	bool is_index_found = false;

	Frame frame = leap_controller.frame();	
	HandList hands = frame.hands();
	for( int h=0; h < hands.count(); h++ )
	{
		Hand hand = hands[h];
		FingerList fingers = hand.fingers();
		for( int f=0; f < fingers.count(); f++ )
		{
			Finger finger = fingers[f];
			if( finger.type() == Finger::Type::TYPE_INDEX )
			{
				is_index_found = true;

				Vector position = finger.tipPosition();
				cursor_location = math::position(position.x, position.y, position.z) * leap_transform;

				break;
			}
		}
		if( is_index_found )
		{
			break;
		}
	}

	if( !is_index_found )
	{
		cursor_location = math::position(0,0,0);
		is_cursor_located = false;
	}
	else
	{
		is_cursor_located = true;
	}
}

void SampleListener::onFrame(const Controller& controller) {

    // Get the most recent frame and report some basic information
    const Frame frame = controller.frame();
    HandList hands = frame.hands();
    for (HandList::const_iterator hl = hands.begin(); hl != hands.end(); ++hl) {
        // Get the first hand
        const 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 Vector normal = hand.palmNormal();
        const Vector direction = hand.direction();

        // Get fingers
        const FingerList fingers = hand.fingers();
        const Finger finger = *(++fingers.begin()); // get Index finger
        std::cout << std::string(4, ' ') << fingerNames[finger.type()]
            << " finger, id: " << finger.id()
            << ", length: " << finger.length()
            << "mm, width: " << finger.width() << std::endl;

        Bone::Type boneType = static_cast<Bone::Type>(3); // Get distal bone for character recognization
        Bone bone = finger.bone(boneType);
        //output_file << finger.id()
        //<< ", " << bone.nextJoint().x << ", " << bone.nextJoint().y << ", " << bone.nextJoint().z << std::endl;

        // 2D-3D transformation
        if (finger.id() != finger_id) {
            finger_id = finger.id(); // reset the data
            array_counter = 0;
        }
        else array_counter++;
        finger_array_x[array_counter] = bone.nextJoint().x;
        finger_array_y[array_counter] = bone.nextJoint().y;
        finger_array_z[array_counter] = bone.nextJoint().z;
    }
}

void Leap::LeapListener::onFrame( const Controller& controller )
{
	Frame frame = controller.frame();
	HandList hands = frame.hands();
	Leap::DirectionDetector::Direction direction;
	//bool handExtended;
	Hand leftHand;
	Hand rightHand;

	//jurik
	//takin just first gesture (gestures are defined for each finger)
	Gesture gesture = frame.gestures()[0];


	if ( arMode ) {
		for ( int i=0; i< hands.count(); ++i ) {
			if ( hands[i].isRight() ) {
				rightHand = hands[i];
			}
			else {
				leftHand = hands[i];
			}
		}
		leapActions->updateARHands( leftHand,rightHand );
	}
	else {
		for ( int i=0; i< hands.count(); ++i ) {
			if ( hands[i].isRight() ) {
				direction = Leap::DirectionDetector::getPalmDirection( hands[i] );
				//using cameramanipulator
				//leapActions->changeViewAngle( direction );
				//using pickhandler class
				leapActions->rotateAruco( direction );

				if ( gesture.type() == Gesture::TYPE_KEY_TAP ) {
					leapActions->scaleNodes( true );
				}
			}
			else {
				direction = Leap::DirectionDetector::getPalmDirection( hands[i] );
				//leapActions.changeViewAngle( direction );
				leapActions->scaleEdges( direction );
				if ( gesture.type() == Gesture::TYPE_KEY_TAP ) {
					leapActions->scaleNodes( false );
				}

				/*handExtended = Leap::FingerPositionDetector::isHandExtended( hands[i] );
				if ( handExtended ) {
					leapActions->startMovingForward();
				}
				else {
					leapActions->stopMovingForward();
				}*/
			}
		}
	}

	//std::cout << "id: " << frame.id();
	/*
	    const GestureList gestures = frame.gestures();
	      for (int g = 0; g < gestures.count(); ++g) {
	        Gesture gesture = gestures[g];

	        HandList hands = gesture.hands();
	        Hand firstHand = hands[0];

	        switch (gesture.type()) {
	          case Gesture::TYPE_CIRCLE:
	          {
				leapActions->zoomGraph(gesture);
	            break;
	          }
	          case Gesture::TYPE_SWIPE:
	          {
	            if(firstHand.isRight()){
					if(leapActions->isCameraMoving)
						leapActions->moveCamera(gesture);
	                else
					  leapActions->rotateGraph(gesture);
	            }
	            break;
	          }
	          case Gesture::TYPE_KEY_TAP:
	          {
	            if(firstHand.isLeft())
					leapActions->onKeyTap(gesture);
	            break;
	          }
	          case Gesture::TYPE_SCREEN_TAP:
	          {
				leapActions->onScreenTap(gesture);
	            break;
	          }
	          default:
	            qDebug() << "Unknown gesture type.";
	            break;
	        }
	      }*/


//.........这里部分代码省略.........

void LeapListener::onFrame(const Controller & controller) {
    // Get the most recent frame and report some basic information
    const 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;

    HandList hands = frame.hands();
    for (HandList::const_iterator hl = hands.begin(); hl != hands.end(); ++hl) {
        // Get the first hand
        const 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 Vector normal = hand.palmNormal();
        const Vector direction = hand.direction();

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

        // Get the Arm bone
        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 FingerList fingers = hand.fingers();
        for (FingerList::const_iterator fl = fingers.begin(); fl != fingers.end(); ++fl) {
            const 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) {
                Bone::Type boneType = static_cast<Bone::Type>(b);
                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 ToolList tools = frame.tools();
    for (ToolList::const_iterator tl = tools.begin(); tl != tools.end(); ++tl) {
        const Tool tool = *tl;
        std::cout << std::string(2, ' ') <<  "Tool, id: " << tool.id()
        << ", position: " << tool.tipPosition()
        << ", direction: " << tool.direction() << std::endl;
    }

    // Get gestures
    const GestureList gestures = frame.gestures();
    for (int g = 0; g < gestures.count(); ++g) {
        Gesture gesture = gestures[g];

        switch (gesture.type()) {
            case Gesture::TYPE_CIRCLE: {
                CircleGesture circle = gesture;
                std::string clockwiseness;

                if (circle.pointable().direction().angleTo(circle.normal()) <= PI/2) {
                    clockwiseness = "clockwise";
                } else {
                    clockwiseness = "counterclockwise";
                }

                // Calculate angle swept since last frame
                float sweptAngle = 0;
                if (circle.state() != Gesture::STATE_START) {
                    CircleGesture previousUpdate = CircleGesture(controller.frame(1).gesture(circle.id()));
                    sweptAngle = (circle.progress() - previousUpdate.progress()) * 2 * PI;
                }
                std::cout << std::string(2, ' ')
                << "Circle id: " << gesture.id()
                << ", state: " << stateNames[gesture.state()]
                << ", progress: " << circle.progress()
                << ", radius: " << circle.radius()
                << ", angle " << sweptAngle * RAD_TO_DEG
                <<  ", " << clockwiseness << std::endl;
                break;
            }
            case Gesture::TYPE_SWIPE: {
                SwipeGesture swipe = gesture;
                std::cout << std::string(2, ' ')
                << "Swipe id: " << gesture.id()
                << ", state: " << stateNames[gesture.state()]
                << ", direction: " << swipe.direction()
                << ", speed: " << swipe.speed() << std::endl;
//.........这里部分代码省略.........

//metodo en bucle que actualiza la informacion de frecuencia, volumen y handsGl
void leapMotion::onFrame(const Controller& controller) {
	
	const Frame frame = controller.frame();

	int j = 0;
	float mejor = 1000;
	float mejorVol = 1000;
	int nada=-1000;
	Vector vNada = Vector(nada,nada,nada);
	Matrix mNada = Matrix(vNada,vNada,vNada);

	HandList hands = frame.hands();
	numManos = hands.count();

	for(int i=0; i<numManos; i++){
	
		palma[i] = hands[i].palmPosition();
		//anchuraPalmas[i] = hands[i].palmWidth();
		direccionMano[i] = hands[i].basis();

		float distVol=palma[i].y;

		if((palma[i].x/50)<-4 && (palma[i].x/50)>-7 && distVol<=mejorVol) mejorVol=distVol;
		
		//if(distVol<=mejorVol) mejorVol=distVol;

		for (int f = 0; f < hands[i].fingers().count(); f++) {
			Finger finger = hands[i].fingers()[f];
      
			for (int b = 0; b < 4; b++) {
				Bone bone = finger.bone(static_cast<Leap::Bone::Type>(b));
				handsGl[i*20+f*4+b]=bone.nextJoint();

				float dist=sqrt(pow(4.125-(handsGl[i*20+f*4+b].x/50),2)+pow((-0.75)-(handsGl[i*20+f*4+b].z/50),2));
				if(dist<=mejor) mejor=dist;

				centroHuesos[i*20+f*4+b]=bone.nextJoint();
				direccionHuesos[i*20+f*4+b]=bone.basis();
				longitudHuesos[i*20+f*4+b]=bone.length();
				anchuraHuesos[i*20+f*4+b]=bone.width()/2;
			}
		}
	}

	for(int i=numManos*20;i<40;i++) handsGl[i] = vNada;
	for(int i=numManos; i<2; i++) palma[i] = vNada;
	//for(int i=numManos; i<2; i++) anchuraPalmas[i] = 0;
	for(int i=numManos; i<2; i++) direccionMano[i] = mNada;
	for(int i=numManos*20; i<40; i++) direccionHuesos[i] = mNada;
	for(int i=numManos*20; i<40; i++) centroHuesos[i] = vNada;
	for(int i=numManos*20; i<40; i++) longitudHuesos[i] = 0;
	for(int i=numManos*20; i<40; i++) anchuraHuesos[i] = 0;

	if(mejor<=0.1) freq=7902.13; //se supone que al tocar la antena del theremin, se produce una frecuencia de sonido muy alta
	else if(mejor<=5){
		freq=4000*(1-mejor/5);
	}
	else freq=0;

	if(mejorVol>=250 && mejorVol<=350) volumen=1*((mejorVol-250)/100);
	else if(mejorVol<250) volumen=0;
	else volumen=1;
	
}

void SampleListener::onFrame(const Controller& controller)
{
    bool thand, index = false;

    // Get the most recent frame and report some basic information
    const Frame frame = controller.frame();

    // Receive Hand List
    HandList hands = frame.hands();

    // For all hands
    for (HandList::const_iterator hl = hands.begin(); hl != hands.end(); ++hl)
    {
        // Get the first hand
        const Hand hand         = *hl;

        // Only for right hand
        if( hand.isRight() )
        {
            // Get fingers
            const FingerList fingers = hand.fingers();
            // For all fingers in the list
            for (FingerList::const_iterator fl = fingers.begin(); fl != fingers.end(); ++fl)
            {
                      Bone      bone;
                const Finger    finger = *fl;

                // If thamb or Index finger
                if( finger.type() == Finger::TYPE_THUMB )
                {
                    // Receive nided bone
                    bone        = finger.bone(Bone::TYPE_DISTAL);
                    m_vThand    = bone.center();

                    thand = true;
                }
                else if( finger.type() == Finger::TYPE_INDEX )
                {
                    // Receive nided bone
                    bone        = finger.bone(Bone::TYPE_DISTAL);
                    m_vIndex    = bone.center();

                    index = true;
                }
            }
        }
    }

    bool newState;
    if( !thand || !index )
    {
        newState = false;
    }
    else
    {
        float distance = m_vThand.distanceTo(m_vIndex);
        qDebug () << "Distance: " << distance;

        if( distance < 40 )
        {
            newState = true;
        }
        else
        {
            newState = false;
        }
    }

    if( newState != m_bLastState )
    {
        Q_EMIT releySignal( newState );
        m_bLastState = newState;
    }
}

void LeapListener::onFrame(const Controller &controller){
	const Frame frame = controller.frame();
	HandList hands = frame.hands();

	std::vector<HandModel> myhands;
	BoneModel mybone;
	HandModel myhand;
	//printf("frame: %d\n", frame.id());
	float scale = 0.025;
	float disp = 3.5f;
	bool record = true;
	for (int i = 0; i < hands.count(); i++){
		FingerList fingers = hands[i].fingers();
		myhand.bones.clear();
		myhand.references.clear();

		Vector handPosition = hands[i].palmPosition();
		handPosition *= scale;
		handPosition.y -= disp;

		Vector wrist = hands[i].wristPosition();
		wrist *= scale;
		wrist.y -= disp;

		myhand.palmPosition = handPosition;
		myhand.palmNormal = hands[i].palmNormal();
		myhand.direction = hands[i].direction();

		for (int j = 0; j < fingers.count(); j++){
			Bone bone;
			Bone::Type boneType;

			Vector currentPosition = fingers[j].tipPosition();
			currentPosition *= scale;
			currentPosition.y -= disp;

			Vector lastPosition = m_lastFrame.finger(fingers[j].id()).tipPosition();
			lastPosition *= scale;
			lastPosition.y -= disp;

			Vector diff = currentPosition - lastPosition;

			//printf("%f, %f, %f\n", abs(diff.x), abs(diff.y), abs(diff.z));
			
			//if (abs(diff.x) > 0.2 || abs(diff.y) > 0.2 || abs(diff.z) > 0.2) record = false;
			if (abs(diff.x) < 0.0001 || abs(diff.y) < 0.0001 || abs(diff.z) < 0.0001) record = false;

			for (int k = 0; k < 4; k++){
				boneType = static_cast<Bone::Type>(k);
				bone = fingers[j].bone(boneType);

				if (fingers[j].type() == Finger::Type::TYPE_THUMB && k == 0) continue;

				Vector prevPos = bone.prevJoint();
				prevPos *= scale;
				prevPos.y -= disp;

				Vector nextPos = bone.nextJoint();
				nextPos *= scale;
				nextPos.y -= disp;

				mybone.direction = nextPos - prevPos;
				mybone.position = (prevPos + nextPos) / 2;
				mybone.prevJoint = prevPos;
				mybone.nextJoint = nextPos;
				mybone.length = bone.length() * scale;

				if (boneType == Bone::Type::TYPE_PROXIMAL){
					if (fingers[j].type() == Finger::Type::TYPE_THUMB){
						myhand.thumb = prevPos;
						//myhand.references.push_back(nextPos);
					}
					else{
						myhand.references.push_back(prevPos);
					}
				}
				
				myhand.bones.push_back(mybone);
			}
		}

		myhand.base = myhand.references[3] - myhand.references[0];
		Vector disp = myhand.base.normalized() * 0.3f;
		myhand.references[3] += disp;
		myhand.references[0] -= disp;

		//for (int i = 1; i < 5; i++){
		//	printf("%f\n", myhand.references[i - 1].distanceTo(myhand.references[i]));
		//}

		myhands.push_back(myhand);
	}

	if (record){
		m_hands.clear();
		for (int i = 0; i < myhands.size(); i++){
			m_hands.push_back(myhands[i]);
		}
	}

//.........这里部分代码省略.........

int main(int argc, char** argv)
{
	struct sockaddr_in si_other;
	int s, slen = sizeof(si_other);
	char buf[BUFLEN];
	char message[BUFLEN];
	WSADATA wsa;

	//Initialise winsock
	printf("\nInitialising Winsock...");
	if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0)
	{
		printf("Failed. Error Code : %d", WSAGetLastError());
		exit(EXIT_FAILURE);
	}
	printf("Initialised.\n");

	//create socket
	//if ((s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == SOCKET_ERROR)
	if ((s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == INVALID_SOCKET)
	{
		printf("socket() failed with error code : %d", WSAGetLastError());
		exit(EXIT_FAILURE);
	}
	char str[INET_ADDRSTRLEN];
	//setup address structure
	memset((char *)&si_other, 0, sizeof(si_other));
	si_other.sin_family = AF_INET;
	si_other.sin_port = htons(PORT);
	//si_other.sin_addr.S_un.S_addr = inet_pton(AF_INET, SERVER, &(si_other.sin_addr));
	inet_pton(AF_INET, SERVER, &(si_other.sin_addr.S_un.S_addr));
	/*if (inet_pton(AF_INET, SERVER, &(si_other.sin_addr.S_un.S_addr)))
	{
		inet_ntop(AF_INET, &(si_other.sin_addr.S_un.S_addr), str, INET_ADDRSTRLEN);
		std::cout << ("%s\n", str);
	}*/

	// We catch any exceptions that might occur below -- see the catch statement for more details.
	try {

		// First, we create a Hub with our application identifier. Be sure not to use the com.example namespace when
		// publishing your application. The Hub provides access to one or more Myos.
		myo::Hub hub("om.example.emg-data-sample");

		std::cout << "Attempting to find a Myo..." << std::endl;

		// Next, we attempt to find a Myo to use. If a Myo is already paired in Myo Connect, this will return that Myo
		// immediately.
		// waitForMyo() takes a timeout value in milliseconds. In this case we will try to find a Myo for 10 seconds, and
		// if that fails, the function will return a null pointer.
		myo::Myo* myo = hub.waitForMyo(10000);

		// If waitForMyo() returned a null pointer, we failed to find a Myo, so exit with an error message.
		if (!myo) {
			throw std::runtime_error("Unable to find a Myo!");
		}

		// We've found a Myo.
		std::cout << "Connected to a Myo armband!" << std::endl << std::endl;

		// Next we enable EMG streaming on the found Myo.
		myo->setStreamEmg(myo::Myo::streamEmgEnabled);

		// Create a sample listener and controller for Leap Motion
		SampleListener listener;
		Controller controller;

		// Next we construct an instance of our DeviceListener, so that we can register it with the Hub.
		DataCollector collector;
		double timeElasped = 0.000;
		const double minMax[10] = { 32, 85, 36, 100, 37, 107, 36, 100, 36, 90 }; //T.I.M.R.P

		// Hub::addListener() takes the address of any object whose class inherits from DeviceListener, and will cause
		// Hub::run() to send events to all registered device listeners.
		hub.addListener(&collector);

		//controller.addListener(listener);

		if (argc > 1 && strcmp(argv[1], "--bg") == 0)
			controller.setPolicy(Leap::Controller::POLICY_BACKGROUND_FRAMES);
		
		myfile << std::fixed;
		myfile << std::setprecision(2);

		// Finally we enter our main loop.
		while (1) {
			//collector.tic();
			// In each iteration of our main loop, we run the Myo event loop for a set number of milliseconds.
			// In this case, we wish to update our display 50 times a second, so we run for 1000/20 milliseconds.
			hub.run(1000 / 100);
			// After processing events, we call the print() member function we defined above to print out the values we've
			// obtained from any events that have occurred.
			collector.print();
			int i = 0;
			int j = 1;
			int h = 0;
			double fingDis[5];
			const Frame frame = controller.frame();
			HandList hands = frame.hands();
			for (HandList::const_iterator hl = hands.begin(); hl != hands.end(); ++hl) {
//.........这里部分代码省略.........

void SampleListener::onFrame(const Controller& controller) {
	//tictoc_stack.push(clock());
	// Get the most recent frame and report some basic information
	const Frame frame = controller.frame();

	HandList hands = frame.hands();
	for (HandList::const_iterator hl = hands.begin(); hl != hands.end(); ++hl) {
		// Get the first hand
		const Hand hand = *hl;
		std::string handType = hand.isLeft() ? "Left hand" : "Right hand";
		
		const Vector normal = hand.palmNormal();
		const Vector direction = hand.direction();

		// Get the Arm bone
		Arm arm = hand.arm();

		// Get fingers
		const FingerList fingers = hand.fingers();
		for (FingerList::const_iterator fl = fingers.begin(); fl != fingers.end(); ++fl) {
			const Finger finger = *fl;
	
			myfile << std::string(4, ' ') << fingerNames[finger.type()]
				<< ": " << hand.palmPosition().distanceTo(finger.tipPosition());

			// Get finger bones
			for (int b = 0; b < 4; ++b) {
				Bone::Type boneType = static_cast<Bone::Type>(b);
				Bone bone = finger.bone(boneType);
			}
		}
	}

	// Get tools
	const ToolList tools = frame.tools();
	for (ToolList::const_iterator tl = tools.begin(); tl != tools.end(); ++tl) {
		const Tool tool = *tl;
	}

	// Get gestures
	const GestureList gestures = frame.gestures();
	for (int g = 0; g < gestures.count(); ++g) {
		Gesture gesture = gestures[g];

		switch (gesture.type()) {
		case Gesture::TYPE_CIRCLE:
		{
			CircleGesture circle = gesture;
			std::string clockwiseness;

			if (circle.pointable().direction().angleTo(circle.normal()) <= PI / 2) {
				clockwiseness = "clockwise";
			}
			else {
				clockwiseness = "counterclockwise";
			}

			// Calculate angle swept since last frame
			float sweptAngle = 0;
			if (circle.state() != Gesture::STATE_START) {
				CircleGesture previousUpdate = CircleGesture(controller.frame(1).gesture(circle.id()));
				sweptAngle = (circle.progress() - previousUpdate.progress()) * 2 * PI;
			}
			break;
		}
		case Gesture::TYPE_SWIPE:
		{
			SwipeGesture swipe = gesture;
			break;
		}
		case Gesture::TYPE_KEY_TAP:
		{
			KeyTapGesture tap = gesture;
			break;
		}
		case Gesture::TYPE_SCREEN_TAP:
		{
			ScreenTapGesture screentap = gesture;
			break;
		}
		default:
			break;
		}
	}

	if (!frame.hands().isEmpty() || !gestures.isEmpty()) {
		std::cout << std::endl;
	}
	myfile << " Time elapsed: "
		<< ((double)(clock() - tictoc_stack.top())) / CLOCKS_PER_SEC;
	tictoc_stack.pop();
	myfile << endl;

}

/* ----------------------------------------------------------------------------------------- */
void CListener::onFrame(const Controller& controller) {
  usleep(timeout);        // timeout to move the arm close to real time

  // Get the most recent frame and report some basic information
  const Frame frame = controller.frame();

  if(frame.hands().count() == 0)    // do nothing if no hand is involved
    return;

  if(frame.hands().count() > 1) {     // do nothing if more than one hand is involved
    //std::cout << "More than one hand detected, returning to center position" << std::endl;
    //rarm.set_to_mid();
    return;
  }
  
  // ------------------------ MAIN LOGIC
  // ------------------------
  // GRIPPER
  new_pos[GRIPPER] = Robot_arm::ranges[GRIPPER][MAX] - frame.fingers().extended().count() * Robot_arm::gripper_offset;  // move GRIPPER in safe ranges
  //std::cout <<  "new_pos[GRIPPER]: " << new_pos[GRIPPER] << std::endl;

  HandList hands = frame.hands();
  for (HandList::const_iterator hl = hands.begin(); hl != hands.end(); ++hl) {
    // Get the first hand
    const Hand hand = *hl;
    Arm arm = hand.arm();
    const Vector direction = hand.direction();
    const Vector normal = hand.palmNormal();

    // ------------------------------
    // ELBOW
    new_pos[ELBOW] = Robot_arm::ranges[ELBOW][MAX] - int(Robot_arm::elbow_offset * hand.palmPosition()[Y]);
    //std::cout <<  "new_pos[ELBOW]: " << new_pos[ELBOW] << std::endl;


    // ------------------------------
    // BASE
    if(arm.direction()[X] < 0)
      new_pos[BASE] = Robot_arm::ranges[BASE][CENTER] - int(Robot_arm::base_offset * fabs(arm.direction()[X]));
    else
      new_pos[BASE] = Robot_arm::ranges[BASE][CENTER] + int(Robot_arm::base_offset * fabs(arm.direction()[X]));
    //std::cout <<  "new_pos[BASE]: " << new_pos[BASE] << std::endl;
 

    // ------------------------------
    // WRIST ROTATE
    if(direction.pitch() * RAD_TO_DEG < 0)
      new_pos[WRIST_ROTATE] = Robot_arm::ranges[WRIST_ROTATE][CENTER] - int(Robot_arm::wrist_rotate_offset * fabs(direction.pitch() * RAD_TO_DEG));
    else
      new_pos[WRIST_ROTATE] = Robot_arm::ranges[WRIST_ROTATE][CENTER] + int(Robot_arm::wrist_rotate_offset * fabs(direction.pitch() * RAD_TO_DEG));
    //std::cout <<  "new_pos[WRIST_ROTATE]: " << new_pos[WRIST_ROTATE] << std::endl;


    // ------------------------------
    // WRIST
    if(normal.roll() * RAD_TO_DEG < 0)
      new_pos[WRIST] = Robot_arm::ranges[WRIST][CENTER] + int(Robot_arm::wrist_offset * fabs(normal.roll() * RAD_TO_DEG));
    else
      new_pos[WRIST] = Robot_arm::ranges[WRIST][CENTER] - int(Robot_arm::wrist_offset * fabs(normal.roll() * RAD_TO_DEG));
    //std::cout <<  "new_pos[WRIST]: " << new_pos[WRIST] << std::endl;


    // ------------------------------
    // SHOULDER
    new_pos[SHOULDER] = Robot_arm::ranges[SHOULDER][MIN] + int(Robot_arm::shoulder_offset * arm.elbowPosition()[Z]);
    //std::cout <<  "new_pos[SHOULDER]: " << new_pos[SHOULDER] << std::endl;
  }

  // filtration
  if(filter != 0) {
    int * tmp_pos = rarm.get_pos();

    for(int i = 0; i < Robot_arm::num_servos; i++) {
      if(fabs((tmp_pos[i] - new_pos[i]) / (double)filter_const) > filter) {      // move only if greater than filter
        rarm.move(new_pos);
        break;
      }
    }
    return;
  }

  rarm.move(new_pos);

}

void LeapInput::onFrame(const Controller& controller)
{
	Frame frame = controller.frame();
	HandList hands = frame.hands();
	this->hands = hands.count();
	if(hands.count() == 2)
	{
		Hand leftHand = hands.leftmost();
		Hand rightHand = hands.rightmost();
		leftHandY = leftHand.palmPosition().y;
		rightHandY = rightHand.palmPosition().y;
		leftFingers = leftHand.fingers().count();
		rightFingers = rightHand.fingers().count();

		float threshold = tanf(5* 3.141592f / 180.0f);
		float maxTan = tanf(45 * 3.141592f / 180.0f);
		if(leftHand.palmPosition().x != rightHand.palmPosition().x){
			float tanValue = fabs((leftHand.palmPosition().y - rightHand.palmPosition().y) / (leftHand.palmPosition().x - rightHand.palmPosition().x));
			if(tanValue > threshold){
				curve = tanValue  / maxTan;
				if(curve > 1)
					curve = 1;
			}else
				curve = 0;
		}else
			curve = 0;
	}
	else
	{
		leftHandY = rightHandY = 0;
	}

	boost = false;
	GestureList gestureList = frame.gestures();
	for(GestureList::const_iterator i = gestureList.begin(); i != gestureList.end(); ++i)
	{
		Gesture gesture = *i;
		if(gesture.state() != Gesture::State::STATE_INVALID)
		{
			switch(gesture.type())
			{
			case Gesture::Type::TYPE_SCREEN_TAP:
				boost = true;
				break;
			}
			
		}
	}

	back = false;
	if(hands.count() == 2 && leftFingers == 1 && rightFingers == 1 &&
		hands.leftmost().fingers()[0].direction().z > 0 &&
		hands.rightmost().fingers()[0].direction().z > 0)
	{
		back = true;
	}

	//accelGesture = brakeGesture = false;
	//GestureList gestureList = frame.gestures();
	//for(GestureList::const_iterator i = gestureList.begin(); i != gestureList.end(); ++i)
	//{
	//	Gesture gesture = *i;
	//	if(gesture.state() != Gesture::State::STATE_INVALID)
	//	{
	//		if(typeid(gesture) == typeid(MyGesture))
	//		{
	//			accelGesture = true;
	//		}
	//		
	//	}
	//}
}

void LeapHander::frame(pugi::xml_node &frameNode){
	Leap::Frame currentFrame = m_sampleListener.frame();
	m_lock.lock();
	frameNode.append_attribute("id").set_value(currentFrame.id());
	pugi::xml_node handList = frameNode.append_child("hands");
	HandList hands = currentFrame.hands();
	
	for (HandList::const_iterator hl = hands.begin(); hl != hands.end(); ++hl) {
		// Get the first hand
		const Hand hand = *hl;
		pugi::xml_node handNode = handList.append_child("hand");
		handNode.append_attribute("id").set_value(hand.id());
		std::string handType;
		if (hand.isLeft()) {
			handType = "Left";
		}
		else {
			handType = "Right";
		}
		handNode.append_attribute("type").set_value(handType.c_str());
		
		pugi::xml_node positionNode = handNode.append_child("position");
		positionToXml(positionNode, hand.palmPosition());
		
		/*pugi::xml_node normalNode = handNode.append_child("normal");
		positionToXml(normalNode, hand.palmNormal());

		pugi::xml_node directionNode = handNode.append_child("direction");
		positionToXml(directionNode, hand.direction());

		pugi::xml_node rotationNode = handNode.append_child("basis");
		rotationToXml(rotationNode, hand.basis());*/
		//// Get fingers
		pugi::xml_node fingerList = handNode.append_child("fingers");
		const FingerList fingers = hand.fingers();
		for (FingerList::const_iterator fl = fingers.begin(); fl != fingers.end(); ++fl) {
			const Finger finger = *fl;
			pugi::xml_node fingerNode = fingerList.append_child("finger");
			fingerNode.append_attribute("id").set_value(finger.id());
			fingerNode.append_attribute("name").set_value(fingerNames[finger.type()].c_str());
			pugi::xml_node boneList = fingerNode.append_child("bones");
			

			// Get finger bones
			for (int b = 0; b < 4; ++b) {
				Bone::Type boneType = static_cast<Bone::Type>(b);
				Bone bone = finger.bone(boneType);
				pugi::xml_node boneNode = boneList.append_child("bone");
				boneNode.append_attribute("length").set_value(bone.length());
				boneNode.append_attribute("name").set_value(boneNames[boneType].c_str());

				pugi::xml_node prevJoint = boneNode.append_child("prevJoint");
				positionToXml(prevJoint, bone.prevJoint());

				pugi::xml_node nextJoint = boneNode.append_child("nextJoint");
				positionToXml(nextJoint, bone.nextJoint());

				/*pugi::xml_node rotation = boneNode.append_child("basis");
				rotationToXml(rotation, bone.basis());*/
			}
		}
	}
	m_lock.unlock();
}

void MangoListener::onFrame(const Controller& controller) {
  const Frame frame = controller.frame();
    HandList hands = frame.hands();
    int extendedFingers = 0;
    for (HandList::const_iterator hl = hands.begin(); hl != hands.end(); ++hl) {
        const Hand hand = *hl;
        for (int i = 0; i < hand.fingers().count(); i++)
        {
            Finger finger = hand.fingers()[i];
            if(finger.isExtended()) extendedFingers++;
        }

        if (!onGesture && !frame.hands().isEmpty() && frame.hands().count() == 1 && extendedFingers == 0)
        {
            preGestureCounter++;
            std::cout << preGestureCounter << std::endl;
            if (preGestureCounter > MAX_PREGESTURE && frameCount == 0)
            {
                onGesture = true;
                preGestureCounter = 0;
                frameCount++;
                note.show();
            }
        }
        else if (preGestureCounter > 0)
        {
            preGestureCounter--;
        }
    }
        if (onGesture && frameCount < MAX_FRAMECOUNT) {

            switch (extendedFingers)
            {
                case 1:
                {
                    std::string command = commands.getCommand("FING3");
                    std::cout << command << std::endl;
                    break;
                }
                case 2:
                {
                    break;
                }
                case 3:
                {
                    break;
                }
                default:
                {
                    break;
                }
            }

            frameCount++;
            const GestureList gestures = frame.gestures();
            for (int g = 0; g < gestures.count(); ++g) {
                Gesture gesture = gestures[g]; //need to move finger detection in here too
                switch (gesture.type()) {
                    case Gesture::TYPE_CIRCLE:
                    {
                        CircleGesture circle = gesture;
                        std::string clockwiseness; //probably simplfy to a bool

                        if (circle.pointable().direction().angleTo(circle.normal()) <= PI/4)
                        {
                          clockwiseness = "clockwise";
                          std::cout << "CIRCLE CLOCKWISE" << std::endl;
                        }
                        else
                        {
                          clockwiseness = "counterclockwise";
                          std::cout << "CIRCLE COUNTERCLOCKWISE" << std::endl;
                        }
                        break;
                    }
                    case Gesture::TYPE_SWIPE:
                    {
                        SwipeGesture swipe = gesture;
                        std::cout << "SWIPE" << std::endl;
                        break;
                    }
                    case Gesture::TYPE_KEY_TAP:
                    {
                        KeyTapGesture tap = gesture;
                        std::cout << "KEY TAP" << std::endl;
                        break;
                    }
                    case Gesture::TYPE_SCREEN_TAP:
                    {
                        ScreenTapGesture screentap = gesture;
                        std::cout << "SCREEN TAP" << std::endl;
                        break;
                    }
                    default:
                    {
                        std::cout << std::string(2, ' ')  << "Unknown gesture type." << std::endl;
                        break;
                    }
                }
                float seconds = gesture.durationSeconds();
//.........这里部分代码省略.........