/* 1. Select a person most likely for gesture
* 2. Wait until triggering conditions are met
* 3. Wait for 0.5 seconds when triggered before starting the reading
* 4. Read while checking the boundaries
* 5. When a rotation is made, and the hands are still again, send a bluetooth signal
* a. at each correct angle reading, measure the time spent at that angle
* b. if the time exceeds some threshhold (1 second), send a signal
* c. if the person wants to continue rotating, he needs to wait a particular amount of time (based on motor rotation speed)
* d. reading to commence again for a new rotation
* 6. Wait until another rotation is made */
/* Constructor: */
public GesturesMasterControl()
{
stopWatch = new Stopwatch();
stopWatch_single_event = new Stopwatch();
stopWatch_trigger = new Stopwatch();
running_gesture = GestureRunningState.Unknown;
right_elbow_general_state = JointGeneralState.Unknown;
left_elbow_general_state = JointGeneralState.Unknown;
right_arm_position = ArmPosition.Unknown;
left_arm_position = ArmPosition.Unknown;
lf_elbow_prev_pos = new Joint[PREV_FRAMES_ARRAY_LENGTH];
rt_elbow_prev_pos = new Joint[PREV_FRAMES_ARRAY_LENGTH];
shoulder_to_elbow_left = new Vector3D();
shoulder_to_elbow_right = new Vector3D();
elbow_to_hand_left = new Vector3D();
elbow_to_hand_right = new Vector3D();
}
public static bool isJointStable(double sensitivity, JointType joint, Joint[] prev_array, int prev_length, ref JointGeneralState elbow_state)
{
bool return_bool = false;
//double avg_standard_dev = calcAverageStandardDev(prev_array, prev_length);
//if (avg_standard_dev < sensitivity)
if (calcFirstLastDiff(prev_array, prev_length, sensitivity))
{
return_bool = true;
elbow_state = JointGeneralState.Still;
TextInformation.insert_main_text_block(joint + " Still", 2);
}
else
{
return_bool = false;
elbow_state = JointGeneralState.Moving;
TextInformation.insert_main_text_block(joint + " Moving", 2);
}
return(return_bool);
}
public void runGestureAnalysis()
{
stopWatch_single_event.Reset();
stopWatch_single_event.Start();
TextInformation.insert_main_text_block("Stopwatch: " + stopWatch.Elapsed, 3);
TextInformation.insert_main_text_block("Running Gesture: " + running_gesture.ToString(), 1);
// I have moved the next 4 blocks of codes out of the if statement...
/* Searching State: CHANGED FROM WRIST TO ELBOW */
Joint left_elbow = body.Joints[JointType.ElbowLeft];
Joint right_elbow = body.Joints[JointType.ElbowRight];
// We make a call to this method to store previous positions:
GestureAuxilaryMethods.managePrevArray(left_elbow, ref lf_elbow_prev_pos, PREV_FRAMES_ARRAY_LENGTH);
GestureAuxilaryMethods.managePrevArray(right_elbow, ref rt_elbow_prev_pos, PREV_FRAMES_ARRAY_LENGTH);
// We make a call to the following methods in order to update the state of elbows:
GestureAuxilaryMethods.isJointStable(stillness_sensitivity, JointType.ElbowLeft, lf_elbow_prev_pos, PREV_FRAMES_ARRAY_LENGTH,
ref left_elbow_general_state);
GestureAuxilaryMethods.isJointStable(stillness_sensitivity, JointType.ElbowRight, rt_elbow_prev_pos, PREV_FRAMES_ARRAY_LENGTH,
ref right_elbow_general_state);
// The if else block below is to see if we are in the searching (for gesture) state or reading (the gesture) state:
if (running_gesture == GestureRunningState.None || running_gesture == GestureRunningState.Unknown)
{
if (left_elbow_general_state == JointGeneralState.Still && right_elbow_general_state == JointGeneralState.Still)
{
// Calling this method to see if a panel rotation gesture is possible:
if (Gesture_PanelRotation.checkPanelRotationGesture(horizontal_stretch_sensitivity, ref left_arm_position, ref right_arm_position))
{
// We can now initialize a PanelRotation class and go on further:
panel_rotation = new Gesture_PanelRotation(left_arm_position, right_arm_position, horizontal_stretch_sensitivity);
panel_rotation.initializeGesture();
// Update the gesture state:
running_gesture = GestureRunningState.PanelRotation;
// Start the stopwatch, we will wait .5 seconds for the gesture reading to begin:
stopWatch_trigger.Start();
}
}
}
else /* Reading State: */
{
if (stopWatch_trigger.ElapsedMilliseconds > 500)
{
int reading_result = 0;
// There is a gesture that is to be read, we need to check which:
if (running_gesture == GestureRunningState.PanelRotation)
{
reading_result = panel_rotation.readArmRotation(left_elbow_general_state, right_elbow_general_state);
}
if (reading_result == -1)
{
// Discontinue the reading of the gesture:
running_gesture = GestureRunningState.None;
// Clear out the previous hand positions arrays:
lf_elbow_prev_pos = new Joint[PREV_FRAMES_ARRAY_LENGTH];
rt_elbow_prev_pos = new Joint[PREV_FRAMES_ARRAY_LENGTH];
// Clear out the statuses of Limbs:
left_elbow_general_state = JointGeneralState.Unknown;
right_elbow_general_state = JointGeneralState.Unknown;
// Reset the stopwatch:
stopWatch_trigger.Reset();
}
}
}
TextInformation.insert_main_text_block("Event: " + stopWatch_single_event.Elapsed, 3);
TextInformation.update_main_text();
}
internal bool boundaryConditions(JointGeneralState left_elbow_general_state, JointGeneralState right_elbow_general_state)
{
bool angle_shoulder_elbow_steady = false, both_arms_same_dir = false, speed_normal = false,
arms_Z_pos_constant = false, both_elbows_same_state = false;
double left_Y_difference = 0, right_Y_difference = 0;
// PREVIOUSLY: went from shoulder to elbow:
// We check to see if the angle between spine shoulder and wrist for both arms are close:
double left_Y_difference_signed = GesturesMasterControl.body.Joints[JointType.WristLeft].Position.Y -
GesturesMasterControl.body.Joints[JointType.SpineShoulder].Position.Y;
double right_Y_difference_signed = GesturesMasterControl.body.Joints[JointType.WristLeft].Position.Y -
GesturesMasterControl.body.Joints[JointType.SpineShoulder].Position.Y;
double left_Y_difference_signed_elbow = GesturesMasterControl.body.Joints[JointType.ElbowLeft].Position.Y -
GesturesMasterControl.body.Joints[JointType.ShoulderLeft].Position.Y;
double right_Y_difference_signed_elbow = GesturesMasterControl.body.Joints[JointType.ElbowRight].Position.Y -
GesturesMasterControl.body.Joints[JointType.ShoulderRight].Position.Y;
left_Y_difference = Math.Abs(left_Y_difference_signed);
right_Y_difference = Math.Abs(right_Y_difference_signed);
//TextInformation.insert_main_text_block("Left Y Difference: " + left_Y_difference.ToString("n4"), 1);
//TextInformation.insert_main_text_block("Right Y Difference: " + right_Y_difference.ToString("n4"), 1);
//TextInformation.insert_main_text_block("Difference Between: " + (left_Y_difference - right_Y_difference).ToString("n4"), 1);
if (Math.Abs(left_Y_difference - right_Y_difference) > angle_difference_sensitivity)
{
angle_shoulder_elbow_steady = false;
}
else
{
angle_shoulder_elbow_steady = true;
}
// Reusing the same variables for the elbow shoulder difference measurement:
left_Y_difference = Math.Abs(left_Y_difference_signed_elbow);
right_Y_difference = Math.Abs(right_Y_difference_signed_elbow);
// If the arms are not stretched out horizontally, we can check to see if both arms are down or up:
if (left_Y_difference > both_arms_down_sensitivity && right_Y_difference > both_arms_down_sensitivity)
{
// Check to see if both variables have the same sign:
if (left_Y_difference_signed_elbow > 0 && right_Y_difference_signed_elbow > 0)
{
TextInformation.insert_main_text_block("IIIIIII", 4);
both_arms_same_dir = true;
}
else if (left_Y_difference_signed_elbow < 0 && right_Y_difference_signed_elbow < 0)
{
TextInformation.insert_main_text_block("OOOOOOO", 4);
both_arms_same_dir = true;
}
else
{
both_arms_same_dir = false;
}
}
// We need to also check to see if the Z distances between shoulder and elbow are close enough:
double left_Z_difference = GesturesMasterControl.body.Joints[JointType.ElbowLeft].Position.Z -
GesturesMasterControl.body.Joints[JointType.ShoulderLeft].Position.Z;
double right_Z_difference = GesturesMasterControl.body.Joints[JointType.ElbowRight].Position.Z -
GesturesMasterControl.body.Joints[JointType.ShoulderRight].Position.Z;
//TextInformation.insert_main_text_block("Left Z Difference: " + left_Z_difference.ToString("n4"), 2);
//TextInformation.insert_main_text_block("Right Z Difference: " + right_Z_difference.ToString("n4"), 2);
left_Z_difference = Math.Abs(left_Z_difference);
right_Z_difference = Math.Abs(right_Z_difference);
if (left_Z_difference > Z_distance_sensitivity || right_Z_difference > Z_distance_sensitivity)
{
arms_Z_pos_constant = false;
}
else
{
arms_Z_pos_constant = true;
}
// Lets check if one elbow is moving and other is still:
// Very hard to make useful... not implemented
if (left_elbow_general_state == JointGeneralState.Moving && right_elbow_general_state == JointGeneralState.Still)
{
both_elbows_same_state = false;
}
else if (left_elbow_general_state == JointGeneralState.Still && right_elbow_general_state == JointGeneralState.Moving)
{
both_elbows_same_state = false;
}
else if (left_elbow_general_state == JointGeneralState.Still && right_elbow_general_state == JointGeneralState.Still)
{
both_elbows_same_state = true;
}
else if (left_elbow_general_state == JointGeneralState.Moving && right_elbow_general_state == JointGeneralState.Moving)
{
both_elbows_same_state = true;
}
// Lets check the speed of rotation from one frame to another:
// Not used due to poor functionality:
double Y_single_frame_diff_left = shoulder_to_elbow_left.Y - left_arm_rotation.previous_frame_vector.Y;
double Y_single_frame_diff_right = shoulder_to_elbow_right.Y - right_arm_rotation.previous_frame_vector.Y;
TextInformation.insert_main_text_block("Left Speed: " + Y_single_frame_diff_left.ToString("n4"), 1);
TextInformation.insert_main_text_block("Right Speed: " + Y_single_frame_diff_right.ToString("n4"), 1);
if (Math.Abs(Y_single_frame_diff_left) < .015 && Math.Abs(Y_single_frame_diff_right) < .015)
{
speed_normal = true;
}
TextInformation.insert_main_text_block("Arms Z Position Constant: " + arms_Z_pos_constant.ToString(), 2);
TextInformation.insert_main_text_block("Both Arms Same Direction: " + both_arms_same_dir.ToString(), 2);
TextInformation.insert_main_text_block("Angle Shoulder Elbow Steady: " + angle_shoulder_elbow_steady.ToString(), 2);
// Combining all booleans:
if (angle_shoulder_elbow_steady == true && both_arms_same_dir == false && arms_Z_pos_constant == true)
{
TextInformation.insert_main_text_block("Boundary Clear", 2);
return(true);
}
else
{
TextInformation.insert_main_text_block("Boundary NOT Clear", 2);
return(false);
}
}
// This method returns 0 if everything went well, -1 if reading of the gesture is stopped:
internal int readArmRotation(JointGeneralState left_elbow_general_state, JointGeneralState right_elbow_general_state)
{
// Moved these two lines out of the if statement:
shoulder_to_elbow_left = GestureAuxilaryMethods.updateLimbVectors(JointType.ShoulderLeft, JointType.ElbowLeft);
shoulder_to_elbow_right = GestureAuxilaryMethods.updateLimbVectors(JointType.ShoulderRight, JointType.ElbowRight);
// Checking the boundary conditions to see if we can actually do the reading:
if (boundaryConditions(left_elbow_general_state, right_elbow_general_state) == true &&
(left_arm_rotation.rot_tracking_state == ArmRotationTrackingState.Tracking ||
left_arm_rotation.rot_tracking_state == ArmRotationTrackingState.Initialized) &&
(right_arm_rotation.rot_tracking_state == ArmRotationTrackingState.Tracking ||
right_arm_rotation.rot_tracking_state == ArmRotationTrackingState.Initialized))
{
left_arm_rotation.rot_tracking_state = ArmRotationTrackingState.Tracking;
right_arm_rotation.rot_tracking_state = ArmRotationTrackingState.Tracking;
}
else
{
left_arm_rotation.rot_tracking_state = ArmRotationTrackingState.NotTracking;
right_arm_rotation.rot_tracking_state = ArmRotationTrackingState.NotTracking;
}
if (left_arm_rotation.rot_tracking_state == ArmRotationTrackingState.Tracking &&
right_arm_rotation.rot_tracking_state == ArmRotationTrackingState.Tracking)
{
updateRotationStatus();
updateArmStatus();
//TextInformation.insert_main_text_block("Left Arm: " + left_arm_rotation.previous_angles[0].ToString(), 2);
//TextInformation.insert_main_text_block("Right Arm: " + right_arm_rotation.previous_angles[0].ToString(), 2);
// Lets stabialize the angle:
left_arm_rotation.stabializeAngle();
right_arm_rotation.stabializeAngle();
TextInformation.insert_main_text_block("Angle Left Arm: " + left_arm_rotation.angle.ToString("n4"), 1);
TextInformation.insert_main_text_block("Angle Right Arm: " + right_arm_rotation.angle.ToString("n4"), 1);
if (left_arm_rotation.enough_time_at_angle == true && right_arm_rotation.enough_time_at_angle == true)
{
TextInformation.insert_main_text_block("Sending Signal", 1);
// Terminating the gesture reading. This might be changed later on:
//left_arm_rotation.rot_tracking_state = ArmRotationTrackingState.NotTracking;
//right_arm_rotation.rot_tracking_state = ArmRotationTrackingState.NotTracking;
}
else
{
TextInformation.insert_main_text_block("NOT Sending Signal", 1);
}
left_arm_rotation.frame_counter++;
right_arm_rotation.frame_counter++;
// Updating previous vectors:
left_arm_rotation.previous_frame_vector = shoulder_to_elbow_left;
right_arm_rotation.previous_frame_vector = shoulder_to_elbow_right;
return(0);
}
else
{
left_arm_rotation.previous_rot_state = ArmRotationalState.NotRotating;
right_arm_rotation.previous_rot_state = ArmRotationalState.NotRotating;
return(-1);
}
}