fdata get_finger_positions()
{
    Leap::Frame frame = control.frame();
    Leap::FingerList fingers = frame.fingers();
    Leap::ToolList tools = frame.tools();
    Leap::HandList hands = frame.hands();

    //std::vector<std::pair<cl_float4, int>> positions;

    fdata hand_data;


    int p = 0;

    for(int i=0; i<40; i++)
    {
        hand_data.fingers[i] = 0.0f;
    }

    ///will explode if more than 2
    for(int i=0; i<hands.count(); i++)
    {
        const Leap::Hand hand = hands[i];
        Leap::FingerList h_fingers = hand.fingers();

        float grab_strength = hand.grabStrength();

        hand_data.grab_confidence[i] = grab_strength;

        for(int j=0; j<h_fingers.count(); j++)
        {
            const Leap::Finger finger = h_fingers[j];

            float mfingerposx = finger.tipPosition().x;
            float mfingerposy = finger.tipPosition().y;
            float mfingerposz = finger.tipPosition().z;

            //cl_float4 ps = {mfingerposx, mfingerposy, mfingerposz, 0.0f};
            //cl_float4 ps = {mfingerposx, mfingerposy, mfingerposz, 0.0f};

            int id = finger.id();

            hand_data.fingers[p++] = mfingerposx;
            hand_data.fingers[p++] = mfingerposy;
            hand_data.fingers[p++] = mfingerposz;
            hand_data.fingers[p++] = 0.0f;

            //positions.push_back(std::pair<cl_float4, int>(ps, id));

        }
    }

    return hand_data;
}
// This controller mode just dumps out a bunch of the Leap Motion device data, which can then be
// analyzed for other use.
void LLLMImpl::modeDumpDebugInfo(Leap::HandList & hands)
{
	S32 numHands = hands.count();		
	if (numHands == 1)
	{
		// Get the first hand
		Leap::Hand hand = hands[0];

		// Check if the hand has any fingers
		Leap::FingerList finger_list = hand.fingers();
		S32 num_fingers = finger_list.count();

		if (num_fingers >= 1) 
		{	// Calculate the hand's average finger tip position
			Leap::Vector pos(0, 0, 0);
			Leap::Vector direction(0, 0, 0);
			for (size_t i = 0; i < num_fingers; ++i) 
			{
				Leap::Finger finger = finger_list[i];
				pos += finger.tipPosition();
				direction += finger.direction();

				// Lots of log spam
				LL_INFOS("LeapMotion") << "Finger " << i << " string is " << finger.toString() << LL_ENDL;
			}
			pos = Leap::Vector(pos.x/num_fingers, pos.y/num_fingers, pos.z/num_fingers);
			direction = Leap::Vector(direction.x/num_fingers, direction.y/num_fingers, direction.z/num_fingers);

			LL_INFOS("LeapMotion") << "Hand has " << num_fingers << " fingers with average tip position"
				<< " (" << pos.x << ", " << pos.y << ", " << pos.z << ")" 
				<< " direction (" << direction.x << ", " << direction.y << ", " << direction.z << ")" 
				<< LL_ENDL;

		}

		Leap::Vector palm_pos = hand.palmPosition();
		Leap::Vector palm_normal = hand.palmNormal();
		LL_INFOS("LeapMotion") << "Palm pos " << palm_pos.x
			<< ", " <<  palm_pos.y
			<< ", " <<  palm_pos.z
			<< ".   Normal: " << palm_normal.x
			<< ", " << palm_normal.y
			<< ", " << palm_normal.z
			<< LL_ENDL;

		F32 ball_radius = (F32) hand.sphereRadius();
		Leap::Vector ball_center = hand.sphereCenter();
		LL_INFOS("LeapMotion") << "Ball pos " << ball_center.x
			<< ", " << ball_center.y
			<< ", " << ball_center.z
			<< ", radius " << ball_radius
			<< LL_ENDL;
	}	// dump_out_data
}
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 jester::LeapMotionImpl::setFingerInJointData(Leap::Finger finger, Bone::BoneId bone) {
	std::pair<Bone::JointId, Bone::JointId> joints = Bone::BoneToJointsMap.find(bone)->second;
	glm::vec3 fingerTip(finger.tipPosition()[0], finger.tipPosition()[1], finger.tipPosition()[2]);
	glm::vec3 fingerDir(finger.direction()[0], finger.direction()[1], finger.direction()[2]);
	float fingerLength = finger.length();
	JointFusionData fingerStartData;
	JointFusionData fingerEndData;

	fingerDir = glm::normalize(fingerDir);
	glm::vec3 fingerStart = fingerTip - fingerLength * fingerDir;

	glm::vec3 boneStart = glm::vec3(fingerStart / LeapMeasurmentScalingFactor);
	glm::vec3 boneEnd = glm::vec3(fingerTip / LeapMeasurmentScalingFactor);
	fingerStartData.position = boneStart;
	fingerEndData.position = boneEnd;
	fingerStartData.id = joints.first;
	fingerEndData.id = joints.second;
	fingerStartData.confidence = fingerEndData.confidence = LeapConfidence;

	kJointData.insert(std::pair<Bone::JointId, JointFusionData>(joints.first, fingerStartData));
	kJointData.insert(std::pair<Bone::JointId, JointFusionData>(joints.second, fingerEndData));
}
// This mode tries to move the avatar and camera in Second Life.   It's pretty rough and needs a lot of work
void LLLMImpl::modeMoveAndCamTest1(Leap::HandList & hands)
{
	S32 numHands = hands.count();		
	if (numHands == 1)
	{
		// Get the first hand
		Leap::Hand hand = hands[0];

		// Check if the hand has any fingers
		Leap::FingerList finger_list = hand.fingers();
		S32 num_fingers = finger_list.count();

		F32 orbit_rate = 0.f;

		Leap::Vector pos(0, 0, 0);
		for (size_t i = 0; i < num_fingers; ++i) 
		{
			Leap::Finger finger = finger_list[i];
			pos += finger.tipPosition();
		}
		pos = Leap::Vector(pos.x/num_fingers, pos.y/num_fingers, pos.z/num_fingers);

		if (num_fingers == 1)
		{	// 1 finger - move avatar
			if (pos.x < -LM_DEAD_ZONE)
			{	// Move left
				gAgent.moveLeftNudge(1.f);
			}
			else if (pos.x > LM_DEAD_ZONE)
			{
				gAgent.moveLeftNudge(-1.f);
			}
			
			/*
			if (pos.z < -LM_DEAD_ZONE)
			{
				gAgent.moveAtNudge(1.f);
			}
			else if (pos.z > LM_DEAD_ZONE)
			{	
				gAgent.moveAtNudge(-1.f);
			} */

			if (pos.y < -LM_DEAD_ZONE)
			{
				gAgent.moveYaw(-1.f);
			}
			else if (pos.y > LM_DEAD_ZONE)
			{
				gAgent.moveYaw(1.f);
			}
		}	// end 1 finger
		else if (num_fingers == 2)
		{	// 2 fingers - move camera around
			// X values run from about -170 to +170
			if (pos.x < -LM_DEAD_ZONE)
			{	// Camera rotate left
				gAgentCamera.unlockView();
				orbit_rate = (llabs(pos.x) - LM_DEAD_ZONE) / LM_ORBIT_RATE_FACTOR;
				gAgentCamera.setOrbitLeftKey(orbit_rate);
			}
			else if (pos.x > LM_DEAD_ZONE)
			{
				gAgentCamera.unlockView();
				orbit_rate = (pos.x - LM_DEAD_ZONE) / LM_ORBIT_RATE_FACTOR;
				gAgentCamera.setOrbitRightKey(orbit_rate);
			}
			if (pos.z < -LM_DEAD_ZONE)
			{	// Camera zoom in
				gAgentCamera.unlockView();
				orbit_rate = (llabs(pos.z) - LM_DEAD_ZONE) / LM_ORBIT_RATE_FACTOR;
				gAgentCamera.setOrbitInKey(orbit_rate);
			}
			else if (pos.z > LM_DEAD_ZONE)
			{	// Camera zoom out
				gAgentCamera.unlockView();
				orbit_rate = (pos.z - LM_DEAD_ZONE) / LM_ORBIT_RATE_FACTOR;
				gAgentCamera.setOrbitOutKey(orbit_rate);
			}

			if (pos.y < -LM_DEAD_ZONE)
			{	// Camera zoom in
				gAgentCamera.unlockView();
				orbit_rate = (llabs(pos.y) - LM_DEAD_ZONE) / LM_ORBIT_RATE_FACTOR;
				gAgentCamera.setOrbitUpKey(orbit_rate);
			}
			else if (pos.y > LM_DEAD_ZONE)
			{	// Camera zoom out
				gAgentCamera.unlockView();
				orbit_rate = (pos.y - LM_DEAD_ZONE) / LM_ORBIT_RATE_FACTOR;
				gAgentCamera.setOrbitDownKey(orbit_rate);
			}
		}	// end 2 finger
	}
}
// This mode tries to detect simple hand motion and either triggers an avatar gesture or 
// sends a chat message into SL in response.   It is very rough, hard-coded for detecting 
// a hand wave (a SL gesture) or the wiggling-thumb gun trigger (a chat message sent to a
// special version of the popgun).
void LLLMImpl::modeGestureDetection1(Leap::HandList & hands)
{
	static S32 trigger_direction = -1;

	S32 numHands = hands.count();
	if (numHands == 1)
	{
		// Get the first hand
		Leap::Hand hand = hands[0];

		// Check if the hand has any fingers
		Leap::FingerList finger_list = hand.fingers();
		S32 num_fingers = finger_list.count();
		static S32 last_num_fingers = 0;

		if (num_fingers == 1)
		{	// One finger ... possibly reset the 
			Leap::Finger finger = finger_list[0];
			Leap::Vector finger_dir = finger.direction();

			// Negative Z is into the screen - check that it's the largest component
			S32 abs_z_dir = llabs(finger_dir.z);
			if (finger_dir.z < -0.5 &&
				abs_z_dir > llabs(finger_dir.x) &&
				abs_z_dir > llabs(finger_dir.y))
			{
				Leap::Vector finger_pos = finger.tipPosition();
				Leap::Vector finger_vel = finger.tipVelocity(); 
				LL_INFOS("LeapMotion") << "finger direction is " << finger_dir.x << ", " << finger_dir.y << ", " << finger_dir.z
					<< ", position " << finger_pos.x << ", " << finger_pos.y << ", " << finger_pos.z 
					<< ", velocity " << finger_vel.x << ", " << finger_vel.y << ", " << finger_vel.z 
					<< LL_ENDL;
			}

			if (trigger_direction != -1)
			{
				LL_INFOS("LeapMotion") << "Reset trigger_direction - one finger" << LL_ENDL;
				trigger_direction = -1;
			}
		}
		else if (num_fingers == 2)
		{
			Leap::Finger barrel_finger = finger_list[0];
			Leap::Vector barrel_finger_dir = barrel_finger.direction();

			// Negative Z is into the screen - check that it's the largest component
			F32 abs_z_dir = llabs(barrel_finger_dir.z);
			if (barrel_finger_dir.z < -0.5f &&
				abs_z_dir > llabs(barrel_finger_dir.x) &&
				abs_z_dir > llabs(barrel_finger_dir.y))
			{
				Leap::Finger thumb_finger = finger_list[1];
				Leap::Vector thumb_finger_dir = thumb_finger.direction();
				Leap::Vector thumb_finger_pos = thumb_finger.tipPosition();
				Leap::Vector thumb_finger_vel = thumb_finger.tipVelocity();

				if ((thumb_finger_dir.x < barrel_finger_dir.x) )
				{	// Trigger gunfire
					if (trigger_direction < 0 &&		// Haven't fired
						thumb_finger_vel.x > 50.f &&	// Moving into screen
						thumb_finger_vel.z < -50.f &&
						mChatMsgTimer.checkExpirationAndReset(LLLEAP_CHAT_MSG_INTERVAL))
					{
						// Chat message looks like "/2343 LM2 gunfire"
						std::string gesture_chat_msg("/2343 LM2 gunfire");
						//LLNearbyChatBar::sendChatFromViewer(gesture_chat_msg, CHAT_TYPE_SHOUT, FALSE);
						trigger_direction = 1;
						LL_INFOS("LeapMotion") << "Sent gunfire chat" << LL_ENDL;
					}
					else if (trigger_direction > 0 &&	// Have fired, need to pull thumb back
						thumb_finger_vel.x < -50.f &&
						thumb_finger_vel.z > 50.f)		// Moving out of screen
					{
						trigger_direction = -1;
						LL_INFOS("LeapMotion") << "Reset trigger_direction" << LL_ENDL;
					}
				}
			}
			else if (trigger_direction != -1)
			{
				LL_INFOS("LeapMotion") << "Reset trigger_direction - hand pos" << LL_ENDL;
				trigger_direction = -1;
			}
		}
		else if (num_fingers == 5 &&
			num_fingers == last_num_fingers)
		{
			if (mGestureTimer.checkExpirationAndReset(LLLEAP_GESTURE_INTERVAL))
			{
				// figure out a gesture to trigger
				std::string gestureString("/overhere");
				LLGestureMgr::instance().triggerAndReviseString( gestureString );
			}
		}
		
		last_num_fingers = num_fingers;
	}
}
bool right_hand_finger_x_coordinate_comparator (Leap::Finger a, Leap::Finger b) { return a.tipPosition()[0] < b.tipPosition()[0]; }
Exemple #8
0
int main()
{
	//Leap Motion Vairables
	Leap::Controller controller;
	Leap::Frame frame;
	Leap::HandList hands;
	Leap::Hand h1;
	Leap::FingerList fingers;
	Leap::Finger index;
	Leap::Finger thumb;
	Leap::PointableList pointables;
	float indexX = 0, indexY = 0, indexZ = 0, thumbX = 0, thumbY = 0, thumbZ = 0, sum = 0;
	unsigned long cycles = 0;

	// TCP Variables
	WSADATA wsaData;
	SOCKET connectSocket = INVALID_SOCKET;

	struct addrinfo* result = NULL;
	struct addrinfo* ptr = NULL;
	struct addrinfo  hints;

	char cSendBuf[512][512];
	char sSendBuf[512];

	int iResult;
	int recvBufLen = DEFAULT_BUFLEN;

	int i = 0;

	iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);
	if (iResult != 0) {
		printf("WSAStartup failed with error: %d\n", iResult);
		return 1;
	}

	// Initialize all address info to 0 to start.
	SecureZeroMemory(&hints, sizeof(hints));
	hints.ai_family = AF_UNSPEC;     // Doesn't matter if we use IPV4 or IPV6
	hints.ai_socktype = SOCK_STREAM; // TCP Stream sockets
	hints.ai_protocol = IPPROTO_TCP;

	// Resolve the server address and port
	iResult = getaddrinfo("127.0.0.1", DEFAULT_PORT, &hints, &result);
	if (iResult != 0) {
		printf("getaddrinfo failed with error: %d\n", iResult);
		WSACleanup();
		return 1;
	}

	// Attempt to connect to an address until one succeeds
	for (ptr = result; ptr != NULL; ptr = ptr->ai_next) {

		// create a socket for connecting to the server
		connectSocket = socket(ptr->ai_family, ptr->ai_socktype, ptr->ai_protocol);

		if (connectSocket == INVALID_SOCKET) {
			printf("socket failed with error: %ld\n", WSAGetLastError());
			WSACleanup();
			return 1;
		}

		// Connect to the server
		iResult = connect(connectSocket, ptr->ai_addr, (int)ptr->ai_addrlen);
		if (iResult == SOCKET_ERROR) {
			closesocket(connectSocket);
			connectSocket = INVALID_SOCKET;
			continue;
		}

		break;
	}

	// Deallocate the address info
	freeaddrinfo(result);

	if (connectSocket == INVALID_SOCKET) {
		printf("Unable to connect to server!\n");
		WSACleanup();
		return 1;
	}

	// Setup serial port connection and needed variables.
	SerialPort.Open(PORT_NUM, BAUD);

	Controller[20].value = 9;	//Verification Byte sent to make sure everything else ends up in the right location
	FillByteSize();

	while (true)
	{
		cycles++;
		UpdateControllerState();	//Updates all values on the controller
		WORD wButtons = g_Controllers[CONTROLLER1].state.Gamepad.wButtons;

		//Stores all of the values from the controller into the controller structure
		Controller[0].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbRX;
		Controller[1].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbRY;
		Controller[2].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbLX;
		Controller[3].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbLY;
		Controller[4].value = (g_Controllers[CONTROLLER1].state.Gamepad.bRightTrigger);
		Controller[5].value = (g_Controllers[CONTROLLER1].state.Gamepad.bLeftTrigger);
		Controller[6].value = (wButtons & XINPUT_GAMEPAD_RIGHT_THUMB);
		Controller[7].value = (wButtons & XINPUT_GAMEPAD_LEFT_THUMB);
		Controller[8].value = (wButtons & XINPUT_GAMEPAD_RIGHT_SHOULDER);
		Controller[9].value = (wButtons & XINPUT_GAMEPAD_LEFT_SHOULDER);
		Controller[10].value = (wButtons & XINPUT_GAMEPAD_DPAD_UP);
		Controller[11].value = (wButtons & XINPUT_GAMEPAD_DPAD_DOWN);
		Controller[12].value = (wButtons & XINPUT_GAMEPAD_DPAD_LEFT);
		Controller[13].value = (wButtons & XINPUT_GAMEPAD_DPAD_RIGHT);
		Controller[14].value = (wButtons & XINPUT_GAMEPAD_A);
		Controller[15].value = (wButtons & XINPUT_GAMEPAD_B);
		Controller[16].value = (wButtons & XINPUT_GAMEPAD_Y);
		Controller[17].value = (wButtons & XINPUT_GAMEPAD_X);
		Controller[18].value = (wButtons & XINPUT_GAMEPAD_START);
		Controller[19].value = (wButtons & XINPUT_GAMEPAD_BACK);

		CheckDeadZone();

		if (controller.isConnected() == true)
		{ 
			sum = 0;
			frame = controller.frame();
			hands = frame.hands();
			h1 = hands[0];
			fingers = frame.fingers();
			thumb = fingers[0];
			index = fingers[1];
			pointables = frame.pointables();

			Leapvalues[0].value = h1.palmVelocity().x;
			Leapvalues[1].value = h1.palmVelocity().y;
			Leapvalues[2].value = h1.palmVelocity().z;

			Leapvalues[3].value = h1.direction().pitch()*Leap::RAD_TO_DEG;
			Leapvalues[4].value = h1.direction().yaw()*Leap::RAD_TO_DEG;
			Leapvalues[5].value = h1.direction().roll()*Leap::RAD_TO_DEG;

			indexX = index.tipPosition().x;
			indexY = index.tipPosition().y;
			indexZ = index.tipPosition().z;

			thumbX = thumb.tipPosition().x;
			thumbY = thumb.tipPosition().y;
			thumbZ = thumb.tipPosition().z;

			Leapvalues[6].value = sqrt(pow((indexX - thumbX), 2) + pow((indexY - thumbY), 2) + pow((indexZ - thumbZ), 2));

			leapConnected = true;
			CheckLeapDeadZone();

		}

		for (i = 6; i < NUMBER_OF_BUTTONS; i++)	//DO NOT SET TO <= NUMBER_OF_BUTTONS, NOT A MISTAKE. Verification bit should always keep its value
		{
			{
				Controller[i].value = AnalogToDigital(Controller[i].value);	//converts all of the button presses on the controller to a binary value
			}
		}

		//turns all of the numerical values into buffers that can be passed to the arduino
		for (i = 0; i <= NUMBER_OF_BUTTONS; i++)
		{
			_itoa_s(Controller[i].value, Controller[i].passedValue, 10);
		}

		/*
		for (i = 0; i < NUMBER_OF_BUTTONS; i++) {
			_itoa_s(Controller[i].value, cSendBuf[i], 10);
			cSendBuf[i][strlen(cSendBuf[i])] = '\0';

			iResult = send(connectSocket, cSendBuf[0], (int)strlen(cSendBuf[0]), 0);

			printf("String sent: %s\n", cSendBuf[0]);

			// Check for errors
			if (iResult == SOCKET_ERROR) {
				printf("send failed with error: %d\n", WSAGetLastError());
				closesocket(connectSocket);
				WSACleanup();
				return 1;
			}
		}

		// Try to send the packet
		iResult = send(connectSocket, "\n", (int)strlen("\n"), 0);

		//printf("String sent: %s\n", sendBuf);

		// Check for errors
		if (iResult == SOCKET_ERROR) {
			printf("send failed with error: %d\n", WSAGetLastError());
			closesocket(connectSocket);
			WSACleanup();
			return 1;
		}*/

		if (leapConnected = true)
		{
			for (i = 0; i < NUMBER_OF_LEAP_INPUTS; i++)
			{
				_itoa_s(Leapvalues[i].value, Leapvalues[i].passedValue, 10);
			}
		}

		/*Values recieved in this order:
		0) YAW
		1) PITCH
		2) ROLL
		3) ACCELERATION ON X AXIS
		4) ACCELERATION ON Y AXIS
		5) ACCELERATION ON Z AXIS
		6) SONAR SENSOR DISTANCE (IN METERS)
		*/
		if (SendData() == 1)
		{
			for (i = 0; i < 7; i++)
			{
				while (SerialPort.ReadDataWaiting() < 3)
				{
				}

				if (i > 2 && i < 6)
					SerialPort.ReadData(received[i], 5);

				else
					SerialPort.ReadData(received[i], 4);

				std::cout << received[i] << ' ';

				// Added this 6.25.14
				strcpy(sSendBuf, received[i]);
				sSendBuf[strlen(sSendBuf)] = '\0';
				iResult = send(connectSocket, sSendBuf, (int)strlen(sSendBuf), 0);

				printf("String sent: %s\n", sSendBuf);

				// Check for errors
				if (iResult == SOCKET_ERROR) {
					printf("send failed with error: %d\n", WSAGetLastError());
					closesocket(connectSocket);
					WSACleanup();
					return 1;
				}

				// Try to send the packet
				iResult = send(connectSocket, "\n", (int)strlen("\n"), 0);

				//printf("String sent: %s\n", sendBuf);

				// Check for errors
				if (iResult == SOCKET_ERROR) {
					printf("send failed with error: %d\n", WSAGetLastError());
					closesocket(connectSocket);
					WSACleanup();
					return 1;
				}
			}
		}
			
		
		
		//std::cout << recieved[0];
		printf("\t%d", cycles);
		printf("\n");
		//Sleep(500);  <<'\t' << recieved[1]

		
	}

	closesocket(connectSocket);
	WSACleanup();
	return 0;
}
Exemple #9
0
//--------------------------------------------------------------
void testApp::update(){
    Leap::Vector pNormal;
    
    Leap::Frame frame = leapController.frame();
    Leap::HandList hands = frame.hands();
    
    Leap::Vector pt0;
    Leap::Vector pt1;
    
    if (!hands.isEmpty()) {
        fingerPos.clear();
        sphereSize.clear();
        sphereNorm.clear();
        spherePos.clear();
        //ofLogNotice("hand detected");
        
        //----------------------------------- data collection -------------------------------------------------------------
        
        for (int i = 0; i<hands.count(); i++) {
            if (i>1) break;
            Leap::Hand tempHand = hands[i];
            Leap::FingerList tempfinger = tempHand.fingers();
            for (int j = 0; j <= tempfinger.count(); j++) {
                ofVec3f pt;
                Leap::Finger finger = hands[i].fingers()[j];
                Leap::Vector tempPT=finger.tipPosition();
                pt.x=tempPT.x;pt.y=tempPT.y;pt.z=tempPT.z;
                fingerPos.push_back(pt);
            }
            pt0 = tempHand.palmNormal();
            Leap::Vector center = tempHand.sphereCenter();
            ofVec3f sp; sp.x = center.x; sp.y = center.y; sp.z = center.z;
            float r = tempHand.sphereRadius();
            spherePos.push_back(sp);
            sphereSize.push_back(r);
            sphereNorm.push_back(pt0);
            ofLogNotice("hand " +ofToString(i) + "normal", ofToString(pt0.x) + " " + ofToString(pt0.y) + " " + ofToString(pt0.z));
            ofLogNotice("hand " + ofToString(i) + "center ", ofToString(sp.x) + " " + ofToString(sp.y) + " " + ofToString(sp.z));
            
        }
        
        //---------------------------------- state machine ------------------------------------------------------------------
        if(phase1==true && phase2 == false && phase3==false && phase4 == false && phase5==false && phase6 == false && phase7 == false && (!fingerPos.empty()))
        {
            phase2 = true;
            state = 1;
        }
        
        if (phase2 == true && (sphereNorm.size()>=2)) {
            pt0 = sphereNorm[0];
            pt1 = sphereNorm[1];
            if (abs(abs(pt0.x)-abs(pt1.x))<0.04 && abs(abs(pt0.y)-abs(pt1.y))<0.04) {
                phase3 = true;
                phase2 = false;
            }
        }
        
        
        
    }
//    ofLogNotice("phase1: ", ofToString(phase1));
//    ofLogNotice("phase2: ", ofToString(phase2));
//    ofLogNotice("phase3: ", ofToString(phase3));
//    ofLogNotice("phase4: ", ofToString(phase4));
//    ofLogNotice("phase5: ", ofToString(phase5));
//    ofLogNotice("phase6: ", ofToString(phase6));
//    ofLogNotice("phase7: ", ofToString(phase7));
    
    oldFrame = frame;
    preId = frame.id();
    
    
}
Exemple #10
0
int main()
{
	Leap::Controller controller;
	Leap::Frame frame;
	Leap::HandList hands;
	Leap::Hand h1;
	Leap::FingerList fingers;
	Leap::Finger index;
	Leap::Finger thumb;
	Leap::PointableList pointables;
	float indexX = 0, indexY = 0, indexZ = 0, thumbX = 0, thumbY = 0, thumbZ = 0, sum = 0, supersample[20];

	int i = 0;

	// Setup serial port connection and needed variables.
	HANDLE hSerial = CreateFile(L"COM6", GENERIC_WRITE | GENERIC_READ, 0, 0, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0);

	if (hSerial != INVALID_HANDLE_VALUE)
	{
		printf("Port opened! \n");

		DCB dcbSerialParams;
		GetCommState(hSerial, &dcbSerialParams);

		dcbSerialParams.BaudRate = CBR_14400;
		dcbSerialParams.ByteSize = 8;
		dcbSerialParams.Parity = NOPARITY;
		dcbSerialParams.StopBits = ONESTOPBIT;

		SetCommState(hSerial, &dcbSerialParams);

		Sleep(1000);
	}

	else
	{
		if (GetLastError() == ERROR_FILE_NOT_FOUND)
		{
			printf("Serial port doesn't exist! \n");
		}

		printf("Error while setting up serial port! \n");
	}

	Controller[20].value = 9;	//Verification Byte sent to make sure everything else ends up in the right location
	FillByteSize();

	while (true)
	{
		UpdateControllerState();	//Updates all values on the controller
		WORD wButtons = g_Controllers[CONTROLLER1].state.Gamepad.wButtons;

		//Stores all of the values from the controller into the controller structure
		Controller[0].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbRX;
		Controller[1].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbRY;
		Controller[2].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbLX;
		Controller[3].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbLY;
		Controller[4].value = (g_Controllers[CONTROLLER1].state.Gamepad.bRightTrigger);
		Controller[5].value = (g_Controllers[CONTROLLER1].state.Gamepad.bLeftTrigger);
		Controller[6].value = (wButtons & XINPUT_GAMEPAD_RIGHT_THUMB);
		Controller[7].value = (wButtons & XINPUT_GAMEPAD_LEFT_THUMB);
		Controller[8].value = (wButtons & XINPUT_GAMEPAD_RIGHT_SHOULDER);
		Controller[9].value = (wButtons & XINPUT_GAMEPAD_LEFT_SHOULDER);
		Controller[10].value = (wButtons & XINPUT_GAMEPAD_DPAD_UP);
		Controller[11].value = (wButtons & XINPUT_GAMEPAD_DPAD_DOWN);
		Controller[12].value = (wButtons & XINPUT_GAMEPAD_DPAD_LEFT);
		Controller[13].value = (wButtons & XINPUT_GAMEPAD_DPAD_RIGHT);
		Controller[14].value = (wButtons & XINPUT_GAMEPAD_A);
		Controller[15].value = (wButtons & XINPUT_GAMEPAD_B);
		Controller[16].value = (wButtons & XINPUT_GAMEPAD_Y);
		Controller[17].value = (wButtons & XINPUT_GAMEPAD_X);
		Controller[18].value = (wButtons & XINPUT_GAMEPAD_START);
		Controller[19].value = (wButtons & XINPUT_GAMEPAD_BACK);

		CheckDeadZone();

		if (controller.isConnected() == true)
		{
			sum = 0;
			frame = controller.frame();
			hands = frame.hands();
			h1 = hands[0];
			fingers = frame.fingers();
			thumb = fingers[0];
			index = fingers[1];
			pointables = frame.pointables();

			Leapvalues[0].value = h1.palmVelocity().x;
			Leapvalues[1].value = h1.palmVelocity().y;
			Leapvalues[2].value = h1.palmVelocity().z;
			Leapvalues[3].value = h1.direction().pitch()*Leap::RAD_TO_DEG;
			Leapvalues[4].value = h1.direction().yaw()*Leap::RAD_TO_DEG;

			indexX = index.tipPosition().x;
			indexY = index.tipPosition().y;
			indexZ = index.tipPosition().z;

			thumbX = thumb.tipPosition().x;
			thumbY = thumb.tipPosition().y;
			thumbZ = thumb.tipPosition().z;

			Leapvalues[5].value = sqrt(pow((indexX - thumbX), 2) + pow((indexY - thumbY), 2) + pow((indexZ - thumbZ), 2));

			leapConnected = true;
			CheckLeapDeadZone();
		}



		for (i = 6; i < NUMBER_OF_BUTTONS; i++)	//DO NOT SET TO <= NUMBER_OF_BUTTONS, NOT A MISTAKE. Verification bit should always keep its value
		{
			{
				Controller[i].value = AnalogToDigital(Controller[i].value);	//converts all of the button presses on the controller to a binary value
			}
		}

		//turns all of the numerical values into buffers that can be passed to the arduino
		for (i = 0; i <= NUMBER_OF_BUTTONS; i++)
		{
			_itoa_s(Controller[i].value, Controller[i].passedValue, 10);
		}

		for (i = 0; i < NUMBER_OF_LEAP_INPUTS; i++)
		{
			_itoa_s(Leapvalues[i].value, Leapvalues[i].passedValue, 10);
		}

		SendData(hSerial);

		std::cout << Controller[8].value << std::endl;
	}
	return 0;
}
Exemple #11
0
void LeapManager::nextFrame(Avatar& avatar) {
    // Apply the frame data directly to the avatar.
    Hand& hand = avatar.getHand();
    
    // If we actually get valid Leap data, this will be set to true;
    bool gotRealData = false;

    if (controllersExist()) {
        _listener->onFrame(*_controller);
    }

#ifndef LEAP_STUBS
    if (controllersExist()) {
        gotRealData = true;
        // First, see which palms and fingers are still valid.
        Leap::Frame& frame = _listener->lastFrame;
        
        // Note that this is O(n^2) at worst, but n is very small.

        // After this many frames of no data, assume the digit is lost.
        const int assumeLostAfterFrameCount = 10;

        // Increment our frame data counters
        for (size_t i = 0; i < hand.getNumPalms(); ++i) {
            PalmData& palm = hand.getPalms()[i];
            palm.incrementFramesWithoutData();
            if (palm.getFramesWithoutData() > assumeLostAfterFrameCount) {
                palm.setActive(false);
            }
            for (size_t f = 0; f < palm.getNumFingers(); ++f) {
                FingerData& finger = palm.getFingers()[f];
                finger.incrementFramesWithoutData();
                if (finger.getFramesWithoutData() > assumeLostAfterFrameCount) {
                    finger.setActive(false);
                }
            }
        }

        size_t numLeapHands = frame.hands().count();
        std::vector<PalmData*> palmAssignment(numLeapHands);
        
        // Look for matches
        for (size_t index = 0; index < numLeapHands; ++index) {
            PalmData* takeoverCandidate = NULL;
            palmAssignment[index] = NULL;
            Leap::Hand leapHand = frame.hands()[index];
            int id = leapHand.id();
            if (leapHand.isValid()) {
                for (size_t i = 0; i < hand.getNumPalms() && palmAssignment[index] == NULL; ++i) {
                    PalmData& palm = hand.getPalms()[i];
                    if (palm.getLeapID() == id) {
                        // Found hand with the same ID. We're set!
                        palmAssignment[index] = &palm;
                        palm.resetFramesWithoutData();
                    }
                    else if (palm.getFramesWithoutData() > assumeLostAfterFrameCount) {
                        takeoverCandidate = &palm;
                    }
                }
                if (palmAssignment[index] == NULL) {
                    palmAssignment[index] = takeoverCandidate;
                }
                if (palmAssignment[index] == NULL) {
                    palmAssignment[index] = &hand.addNewPalm();
                }
            }
        }
        
        // Apply the assignments
        for (size_t index = 0; index < numLeapHands; ++index) {
            if (palmAssignment[index]) {
                Leap::Hand leapHand = frame.hands()[index];
                PalmData& palm = *(palmAssignment[index]);

                palm.resetFramesWithoutData();
                palm.setLeapID(leapHand.id());
                palm.setActive(true);
                const Leap::Vector pos = leapHand.palmPosition();
                const Leap::Vector normal = leapHand.palmNormal();
                palm.setRawPosition(glm::vec3(pos.x, pos.y, pos.z));
                palm.setRawNormal(glm::vec3(normal.x, normal.y, normal.z));
            }
        }

        // Look for fingers per palm
        for (size_t i = 0; i < hand.getNumPalms(); ++i) {
            PalmData& palm = hand.getPalms()[i];
            if (palm.isActive()) {
                Leap::Hand leapHand = frame.hand(palm.getLeapID());
                if (leapHand.isValid()) {
                    int numLeapFingers = leapHand.fingers().count();
                    std::vector<FingerData*> fingerAssignment(numLeapFingers);

                    
                    // Look for matches
                    for (size_t index = 0; index < numLeapFingers; ++index) {
                        FingerData* takeoverCandidate = NULL;
                        fingerAssignment[index] = NULL;
                        Leap::Finger leapFinger = leapHand.fingers()[index];
                        int id = leapFinger.id();
                        if (leapFinger.isValid()) {
                            for (size_t f = 0; f < palm.getNumFingers() && fingerAssignment[index] == NULL; ++f) {
                                FingerData& finger = palm.getFingers()[f];
                                if (finger.getLeapID() == id) {
                                    // Found hand with the same ID. We're set!
                                    fingerAssignment[index] = &finger;
                                }
                                else if (finger.getFramesWithoutData() > assumeLostAfterFrameCount) {
                                    takeoverCandidate = &finger;
                                }
                            }
                            // If we didn't find a match, but we found an unused finger, us it.
                            if (fingerAssignment[index] == NULL) {
                                fingerAssignment[index] = takeoverCandidate;
                            }
                        }
                    }

                    // Apply the assignments
                    for (size_t index = 0; index < numLeapFingers; ++index) {
                        if (fingerAssignment[index]) {
                            Leap::Finger leapFinger = leapHand.fingers()[index];
                            FingerData& finger = *(fingerAssignment[index]);
                            
                            finger.resetFramesWithoutData();
                            finger.setLeapID(leapFinger.id());
                            finger.setActive(true);
#ifdef USE_STABILIZED_DATA
                            const Leap::Vector tip = leapFinger.stabilizedTipPosition();
#else
                            const Leap::Vector tip = leapFinger.tipPosition();
#endif
                            const Leap::Vector root = tip - leapFinger.direction() * leapFinger.length();
                            finger.setRawTipPosition(glm::vec3(tip.x, tip.y, tip.z));
                            finger.setRawRootPosition(glm::vec3(root.x, root.y, root.z));
                        }
                    }
                }
            }
        }
    }
#endif
    if (!gotRealData) {
        if (_doFakeFingers) {
            // There's no real Leap data and we need to fake it.
            for (size_t i = 0; i < hand.getNumPalms(); ++i) {
                static const glm::vec3 fakeHandOffsets[] = {
                    glm::vec3( -500.0f, 50.0f, 50.0f),
                    glm::vec3(    0.0f, 50.0f, 50.0f)
                };
                static const glm::vec3 fakeHandFingerMirrors[] = {
                    glm::vec3( -1.0f, 1.0f, 1.0f),
                    glm::vec3(  1.0f, 1.0f, 1.0f)
                };
                static const glm::vec3 fakeFingerPositions[] = {
                    glm::vec3( -60.0f, 0.0f, -40.0f),
                    glm::vec3( -20.0f, 0.0f, -60.0f),
                    glm::vec3(  20.0f, 0.0f, -60.0f),
                    glm::vec3(  60.0f, 0.0f, -40.0f),
                    glm::vec3( -50.0f, 0.0f,  30.0f)
                };

                PalmData& palm = hand.getPalms()[i];
                palm.setActive(true);
                // Simulated data
                
                palm.setRawPosition(glm::vec3( 0.0f, 0.0f, 0.0f) + fakeHandOffsets[i]);
                palm.setRawNormal(glm::vec3(0.0f, 1.0f, 0.0f));

                for (size_t f = 0; f < palm.getNumFingers(); ++f) {
                    FingerData& finger = palm.getFingers()[f];
                    finger.setActive(true);
                    const float tipScale = 1.5f;
                    const float rootScale = 0.75f;
                    glm::vec3 fingerPos = fakeFingerPositions[f] * fakeHandFingerMirrors[i];
                    finger.setRawTipPosition(fingerPos * tipScale + fakeHandOffsets[i]);
                    finger.setRawRootPosition(fingerPos * rootScale + fakeHandOffsets[i]);
                }
            }
        }
        else {
            // Just deactivate everything.
            for (size_t i = 0; i < hand.getNumPalms(); ++i) {
                PalmData& palm = hand.getPalms()[i];
                palm.setActive(false);
                for (size_t f = 0; f < palm.getNumFingers(); ++f) {
                    FingerData& finger = palm.getFingers()[f];
                    finger.setActive(false);
                }
            }
        }
    }
    hand.updateFingerTrails();
}