PoseCorrection/backend/analyse_jump.py

import math

import numpy as np


def angle(v1, v2):
    dx1 = v1[2] - v1[0]
    dy1 = v1[3] - v1[1]
    dx2 = v2[2] - v2[0]
    dy2 = v2[3] - v2[1]
    angle1 = math.atan2(dy1, dx1)
    angle1 = round(angle1 * 180.0 / math.pi, 2)
    # print(angle1)
    angle2 = math.atan2(dy2, dx2)
    angle2 = round(angle2 * 180.0 / math.pi, 2)
    # print(angle2)
    if angle1 * angle2 >= 0:
        included_angle = abs(angle1 - angle2)
    else:
        included_angle = abs(angle1) + abs(angle2)
        # if included_angle > 180:
        #    included_angle = 360 - included_angle
    return included_angle


def angle1(v1, v2):
    dx1 = v1[2] - v1[0]
    dy1 = v1[3] - v1[1]
    angle1 = (float)(math.atan2(dy1, dx1))
    angle2 = abs(round(angle1 * 180.0 / math.pi, 2))
    return angle2


# calculate the angle between 3 points under the coordinates
# params: list, item [x,y]
# return: the angle value of b
def cal_angle(point_a, point_b, point_c):
    a_x, b_x, c_x = point_a[0], point_b[0], point_c[0]
    a_y, b_y, c_y = point_a[1], point_b[1], point_c[1]
    a_z, b_z, c_z = 0, 0, 0
    #  m=(x1,y1,z1), n=(x2,y2,z2)
    x1, y1, z1 = (a_x - b_x), (a_y - b_y), (a_z - b_z)
    x2, y2, z2 = (c_x - b_x), (c_y - b_y), (c_z - b_z)

    cos_b = (x1 * x2 + y1 * y2 + z1 * z2) / (
            math.sqrt(x1 ** 2 + y1 ** 2 + z1 ** 2) * (math.sqrt(x2 ** 2 + y2 ** 2 + z2 ** 2)))
    B = math.degrees(math.acos(cos_b))
    return round(B, 2)


def analyse_npy_side_juanfu(name, num, npy_side):
    result = {}
    data = np.load(npy_side)[0]
    j = 0
    print(len(data))
    # print(data)
    if (len(data) != 25):
        return False
    p = "./cap_file/" + name

    file = open("./cap_file/" + name + "/file2.txt", "a+")
    for i in range(0, len(data)):
        file.write(str(data[i]) + "\n")
    file.close()
    nose_x = data[0][0]
    nose_y = data[0][1]
    shoulder_r_x = data[2][0]
    shoulder_r_y = data[2][1]
    elbow_r_x = data[3][0]
    elbow_r_y = data[3][1]
    wrist_r_x = data[4][0]
    wrist_r_y = data[4][1]

    hip_r_x = data[9][0]
    hip_r_y = data[9][1]
    knee_r_x = data[10][0]
    knee_r_y = data[10][1]
    neck_x = data[1][0]
    neck_y = data[1][1]
    ankle_r_x = data[11][0]
    ankle_r_y = data[11][1]
    leg_heigh = data[11][1]
    neck_hip_angle = round(angle1([neck_x, neck_y, hip_r_x, hip_r_y], [0, 0, 0, -1]), 2)
    nose_neck_angle = round(angle1([nose_x, nose_y, neck_x, neck_y], [0, 0, 0, -1]), 2)
    hip_knee_angle = round(angle1([hip_r_x, hip_r_y, knee_r_x, knee_r_y], [0, 0, 0, -1]), 2)
    knee_ankle_angle = round(angle1([knee_r_x, knee_r_y, ankle_r_x, ankle_r_y], [0, 0, 0, -1]), 2)
    result["neck_hip_angle"] = neck_hip_angle
    result["nose_neck_angle"] = nose_neck_angle
    result["hip_knee_angle"] = hip_knee_angle
    result["knee_ankle_angle"] = knee_ankle_angle

    # print(npy_side)
    return result


def analyse_npy_side_pingban(name, num, npy_side):
    result = {}
    data = np.load(npy_side)
    j = 0
    if (len(data) < 15):
        return False
    p = "./cap_file/" + name

    file = open("./cap_file/" + name + "/file2.txt", "a+")
    for i in range(0, len(data)):
        file.write(str(data[i]) + "\n")
    file.close()
    shoulder_r_x = data[2][0]
    shoulder_r_y = data[2][1]
    elbow_r_x = data[3][0]
    elbow_r_y = data[3][1]
    wrist_r_x = data[4][0]
    wrist_r_y = data[4][1]

    hip_r_x = data[9][0]
    hip_r_y = data[9][1]
    knee_r_x = data[10][0]
    knee_r_y = data[10][1]
    neck_x = data[1][0]
    neck_y = data[1][1]
    ankle_r_x = data[11][0]
    ankle_r_y = data[11][1]
    bigToe_r_x = data[22][0]
    bigToe_r_y = data[22][1]
    knee_ankle_angle = round(angle1([knee_r_x, knee_r_y, ankle_r_x, ankle_r_y], [0, 0, 0, -1]), 2)
    leg_heigh = data[11][1]
    neck_hip_angle = round(angle1([neck_x, neck_y, hip_r_x, hip_r_y], [0, 0, 0, -1]), 2)
    hip_knee_angle = round(angle1([hip_r_x, hip_r_y, knee_r_x, knee_r_y], [0, 0, 0, -1]), 2)
    s_e_angle = round(angle1([shoulder_r_x, shoulder_r_y, elbow_r_x, elbow_r_y], []), 2)
    e_w_angle = round(angle1([elbow_r_x, elbow_r_y, wrist_r_x, wrist_r_y], []), 2)
    hip_heigh = hip_r_y

    result["knee_ankle_angle"] = knee_ankle_angle
    result["leg_heigh"] = leg_heigh
    result["neck_hip_angle"] = neck_hip_angle
    result["hip_knee_angle"] = hip_knee_angle
    result["hip_heigh"] = hip_heigh
    result["s_e_angle"] = s_e_angle
    result["e_w_angle"] = e_w_angle

    # print(npy_side)
    return result


def analyse_npy_side_jump(name, num, npy_side):
    result = {}
    print("analyse_npy_side_jump====")
    data = np.load(npy_side)[0]
    j = 0
    file = open("./cap_file/" + name + "/file2.txt", "a+")
    file.write(str(num) + "\n")
    for i in range(0, len(data)):
        file.write(str(data[i]) + "\n")
    file.close()

    if len(data) != 25:
        return False
    p = "./cap_file/" + name

    # should-elbow-wrist r2-3-4 l5-6-7
    # neck-hip-knee r1-9-10 l1-12-13
    # hip-knee-ankle r9-10-11 l12-13-14
    # knee-ankle-bigToe r10-11-22 l13-14-19
    result["elbow_right_angle"] = cal_angle(data[2], data[3], data[4])
    result["elbow_left_angle"] = cal_angle(data[5], data[6], data[7])
    result["hip_right_angle"] = cal_angle(data[1], data[9], data[10])
    result["hip_left_angle"] = cal_angle(data[1], data[12], data[13])
    result["knee_right_angle"] = cal_angle(data[9], data[10], data[11])
    result["knee_left_angle"] = cal_angle(data[12], data[13], data[14])
    result["ankle_right_angle"] = cal_angle(data[10], data[11], data[12])
    result["ankle_left_angle"] = cal_angle(data[13], data[14], data[19])
    result["leg_heigh"] = data[11][1]  # ankle
    result["hip_heigh"] = data[9][1]  # hip
    result["wrist_heigh"] = data[4][1]
    result["neck_heigh"] = data[1][1]

    # knee_ankle_angle = round(angle1([knee_r_x, knee_r_y, ankle_r_x, ankle_r_y], [0, 0, 0, -1]), 2)
    # neck_hip_angle = round(angle1([neck_x, neck_y, hip_r_x, hip_r_y], [0, 0, 0, -1]), 2)
    # hip_knee_angle = round(angle1([hip_r_x, hip_r_y, knee_r_x, knee_r_y], [0, 0, 0, -1]), 2)
    # hip_heigh = hip_r_y
    # result["knee_ankle_angle"] = knee_ankle_angle
    # result["leg_heigh"] = leg_heigh
    # result["neck_hip_angle"] = neck_hip_angle
    # result["hip_knee_angle"] = hip_knee_angle
    # result["hip_heigh"] = hip_heigh

    return result


def analyse_npy_side_gaotaitui(name, num, npy_side):
    result = {}
    data = np.load(npy_side)[0]
    print(data)
    j = 0
    file = open("./cap_file/" + name + "/file2.txt", "a+")
    file.write(str(num) + "\n")
    for i in range(0, len(data)):
        file.write(str(data[i]) + "\n")
    file.close()
    if (len(data) != 25):
        return False
    p = "./cap_file/" + name
    # print(data)
    hip_r_x = data[9][0]
    hip_r_y = data[9][1]
    knee_r_x = data[10][0]
    knee_r_y = data[10][1]

    hip_l_x = data[12][0]
    hip_l_y = data[12][1]
    knee_l_x = data[13][0]
    knee_l_y = data[13][1]
    # print(head_forward_level)
    # upper part of body
    up_risk_level = ""
    up_state = ""
    neck_x = data[1][0]
    neck_y = data[1][1]
    ankle_r_x = data[11][0]
    ankle_r_y = data[11][1]
    ankle_l_x = data[14][0]
    ankle_l_y = data[14][1]
    knee_ankle_r_angle = round(angle1([knee_r_x, knee_r_y, ankle_r_x, ankle_r_y], [0, 0, 0, -1]), 2)  # ϥ�ǵ���
    knee_ankle_l_angle = round(angle1([knee_l_x, knee_l_y, ankle_l_x, ankle_l_y], [0, 0, 0, -1]), 2)  # ϥ�ǵ���

    hip_knee_r_angle = round(angle1([hip_r_x, hip_r_y, knee_r_x, knee_r_y], [0, 0, 0, -1]), 2)
    hip_knee_l_angle = round(angle1([hip_l_x, hip_l_y, knee_l_x, knee_l_y], [0, 0, 0, -1]), 2)
    neck_hip_angle = round(angle1([neck_x, neck_y, hip_r_x, hip_r_y], [0, 0, 0, -1]), 2)
    # hip_knee_angle=round(angle1([hip_r_x, hip_r_y,knee_r_x, knee_r_y], [0, 0, 0, -1]), 2)
    hip_heigh = hip_r_y

    result["knee_ankle_r_angle"] = knee_ankle_r_angle
    result["knee_ankle_l_angle"] = knee_ankle_l_angle
    result["hip_knee_r_angle"] = hip_knee_r_angle
    result["hip_knee_l_angle"] = hip_knee_l_angle
    result["r_d"] = abs(0 - float(hip_knee_r_angle) + float(knee_ankle_r_angle))
    result["l_d"] = abs(0 + float(knee_ankle_l_angle) - float(hip_knee_l_angle))
    return result


def analyse_npy_side_shendun(name, num, npy_side):
    result = {}
    data = np.load(npy_side)[0]
    j = 0

    file = open("./cap_file/" + name + "/file2.txt", "a+")
    file.write(str(num) + "\n")

    if (len(data) != 25):
        return False
    for i in range(0, len(data)):
        file.write(str(data[i]) + "\n")
    file.close()
    p = "./cap_file/" + name
    # print(data)
    hip_r_x = data[9][0]
    hip_r_y = data[9][1]
    knee_r_x = data[10][0]
    knee_r_y = data[10][1]

    hip_l_x = data[12][0]
    hip_l_y = data[12][1]
    knee_l_x = data[13][0]
    knee_l_y = data[13][1]
    # print(head_forward_level)
    # upper part of body
    up_risk_level = ""
    up_state = ""
    neck_x = data[1][0]
    neck_y = data[1][1]
    ankle_r_x = data[11][0]
    ankle_r_y = data[11][1]
    ankle_l_x = data[14][0]
    ankle_l_y = data[14][1]
    knee_ankle_r_angle = round(angle1([knee_r_x, knee_r_y, ankle_r_x, ankle_r_y], [0, 0, 0, -1]), 2)
    knee_ankle_l_angle = round(angle1([knee_l_x, knee_l_y, ankle_l_x, ankle_l_y], [0, 0, 0, -1]), 2)

    hip_knee_r_angle = round(angle1([hip_r_x, hip_r_y, knee_r_x, knee_r_y], [0, 0, 0, -1]), 2)
    hip_knee_l_angle = round(angle1([hip_l_x, hip_l_y, knee_l_x, knee_l_y], [0, 0, 0, -1]), 2)
    neck_hip_angle = round(angle1([neck_x, neck_y, hip_r_x, hip_r_y], [0, 0, 0, -1]), 2)
    # hip_knee_angle=round(angle1([hip_r_x, hip_r_y,knee_r_x, knee_r_y], [0, 0, 0, -1]), 2)
    hip_heigh = hip_r_y

    result["knee_ankle_r_angle"] = knee_ankle_r_angle
    result["knee_ankle_l_angle"] = knee_ankle_l_angle
    result["hip_knee_r_angle"] = hip_knee_r_angle
    result["hip_knee_l_angle"] = hip_knee_l_angle
    result["r_d"] = abs(float(result["hip_knee_r_angle"]) - float(result["knee_ankle_r_angle"]))

    return result