diff --git a/skeltrack/skeltrack-skeleton.c b/skeltrack/skeltrack-skeleton.c index 04a6884..10ff247 100644 --- a/skeltrack/skeltrack-skeleton.c +++ b/skeltrack/skeltrack-skeleton.c @@ -1614,10 +1614,63 @@ track_joints (SkeltrackSkeleton *self) head, SKELTRACK_JOINT_ID_HEAD, self->priv->dimension_reduction); + + if (left_shoulder && head && head->z > left_shoulder->z) + { + Node *virtual_left_shoulder = g_slice_new (Node); + + virtual_left_shoulder->x = left_shoulder->x; + virtual_left_shoulder->y = left_shoulder->y; + virtual_left_shoulder->z = centroid->z; + + convert_mm_to_screen_coords (self->priv->buffer_width, + self->priv->buffer_height, + self->priv->dimension_reduction, + virtual_left_shoulder->x, + virtual_left_shoulder->y, + virtual_left_shoulder->z, + (guint *) &virtual_left_shoulder->i, + (guint *) &virtual_left_shoulder->j); + + Node *neighbor = get_closest_torso_node (self->priv->graph, + virtual_left_shoulder, + head); + if (neighbor) + left_shoulder = neighbor; + + g_slice_free (Node, virtual_left_shoulder); + } + set_joint_from_node (&joints, left_shoulder, SKELTRACK_JOINT_ID_LEFT_SHOULDER, self->priv->dimension_reduction); + + if (right_shoulder && head && head->z > right_shoulder->z) + { + Node *virtual_right_shoulder = g_slice_new (Node); + + virtual_right_shoulder->x = right_shoulder->x; + virtual_right_shoulder->y = right_shoulder->y; + virtual_right_shoulder->z = centroid->z; + + convert_mm_to_screen_coords (self->priv->buffer_width, + self->priv->buffer_height, + self->priv->dimension_reduction, + virtual_right_shoulder->x, + virtual_right_shoulder->y, + virtual_right_shoulder->z, + (guint *) &virtual_right_shoulder->i, + (guint *) &virtual_right_shoulder->j); + + Node *neighbor = get_closest_torso_node (self->priv->graph, + virtual_right_shoulder, + centroid); + if (neighbor) + right_shoulder = neighbor; + + g_slice_free (Node, virtual_right_shoulder); + } set_joint_from_node (&joints, right_shoulder, SKELTRACK_JOINT_ID_RIGHT_SHOULDER, diff --git a/skeltrack/skeltrack-util.c b/skeltrack/skeltrack-util.c index a81c838..bbc1f0d 100644 --- a/skeltrack/skeltrack-util.c +++ b/skeltrack/skeltrack-util.c @@ -167,6 +167,43 @@ get_distance (Node *a, Node *b) return sqrt (dx * dx + dy * dy + dz * dz); } +Node * +get_closest_torso_node (GList *node_list, Node *from, Node *head) +{ + Node *closest = NULL; + gint distance = -1; + GList *current_node; + + /* @TODO: Replace this and use closest pair of points + algorithm and ensure O(n log n) instead of brute-force */ + + for (current_node = g_list_first (node_list); + current_node != NULL; + current_node = g_list_next (current_node)) + { + Node *node; + gint current_distance; + node = (Node *) current_node->data; + if (closest == NULL && + node->z >= head->z && + node->y >= from->y) + { + closest = node; + distance = get_distance (node, from); + continue; + } + current_distance = get_distance (node, from); + if (current_distance < distance && + node->z >= head->z && + node->y >= from->y) + { + closest = node; + distance = current_distance; + } + } + return closest; +} + Node * get_closest_node (GList *node_list, Node *from) { diff --git a/skeltrack/skeltrack-util.h b/skeltrack/skeltrack-util.h index b247f4b..540aee7 100644 --- a/skeltrack/skeltrack-util.h +++ b/skeltrack/skeltrack-util.h @@ -55,6 +55,10 @@ Node * get_closest_node_to_joint (GList *extremas, Node * get_closest_node (GList *node_list, Node *from); +Node * get_closest_torso_node (GList *node_list, + Node *from, + Node *head); + Label * get_main_component (GList *node_list, Node *from, gdouble min_normalized_nr_nodes);