working rotation hands

This commit is contained in:
2021-12-23 15:15:45 +07:00
parent 47d9d7514a
commit 7d0a41dedd
3 changed files with 466 additions and 28 deletions

View File

@@ -1242,7 +1242,7 @@ def rhandconverttranslation(data):
bodyaposeworld(data_apose)
tvec_pose = [data_apose["hand_r"]["translation"]["x"], data_apose["hand_r"]["translation"]["y"], data_apose["hand_r"]["translation"]["z"]]
tvec[0] = tvec[0] - tvec_pose[0]
tvec[1] = tvec[1] - tvec_pose[1]
tvec[1] = -(tvec[1] - tvec_pose[1])
tvec[2] = tvec[2] - tvec_pose[2]
bonetranslation(data, "pinky_4_r", tvec)
@@ -1268,7 +1268,7 @@ def lhandconverttranslation(data):
bodyaposeworld(data_apose)
tvec_pose = [data_apose["hand_l"]["translation"]["x"], data_apose["hand_l"]["translation"]["y"], data_apose["hand_l"]["translation"]["z"]]
tvec[0] = tvec[0] - tvec_pose[0]
tvec[1] = tvec[1] - tvec_pose[1]
tvec[1] = -(tvec[1] - tvec_pose[1])
tvec[2] = tvec[2] - tvec_pose[2]
bonetranslation(data, "pinky_4_l", tvec)
@@ -1920,3 +1920,435 @@ def lhandconvert(lhandlms, data, coef):
data["index_1_l"]["rotation"]["rotx"] = data["hand_l"]["rotation"]["rotx"]
data["index_1_l"]["rotation"]["roty"] = data["hand_l"]["rotation"]["roty"]
data["index_1_l"]["rotation"]["rotz"] = data["hand_l"]["rotation"]["rotz"]
def getVecNorm(vec):
length = np.sqrt(pow(vec[0],2)+pow(vec[1],2)+pow(vec[2],2))
vec_norm = [vec[0]/length,
vec[1]/length,
vec[2]/length]
return vec_norm
def getRotaionVect(vec1, vec2):
vec1_norm = getVecNorm(vec1)
vec2_norm = getVecNorm(vec2)
cr = np.cross(vec1_norm, vec2_norm)
length = np.sqrt(pow(cr[0],2)+pow(cr[1],2)+pow(cr[2],2))
cr[0] /= length
cr[1] /= length
cr[2] /= length
angle = np.arccos(np.dot(vec1_norm, vec2_norm))
return (cr[0],cr[1],cr[2],angle)
def toEuler(x, y, z, angle):
s = np.sin(angle)
c = np.cos(angle)
t = 1-c
if ((x*y*t + z*s) > 0.998):
heading = 2*np.arctan2(x*np.sin(angle/2),np.cos(angle/2))
attitude = np.pi/2
bank = 0
heading *= 180 / np.pi
attitude *= 180 / np.pi
bank *= 180 / np.pi
return (heading, attitude, bank)
if ((x*y*t + z*s) < -0.998):
heading = -2*np.arctan2(x*np.sin(angle/2),np.cos(angle/2))
attitude = -np.pi/2
bank = 0
heading *= 180 / np.pi
attitude *= 180 / np.pi
bank *= 180 / np.pi
return (heading, attitude, bank)
heading = np.arctan2(y * s- x * z * t , 1 - (y*y+ z*z ) * t)
attitude = np.arcsin(x * y * t + z * s)
bank = np.arctan2(x * s - y * z * t , 1 - (x*x + z*z) * t)
heading *= 180 / np.pi
attitude *= 180 / np.pi
bank *= 180 / np.pi
return (heading, attitude, bank)
def lfingerRot(current, child, data, dataxyz):
vec2 = [dataxyz[child][0] - dataxyz[current][0],
dataxyz[child][1] - dataxyz[current][1],
dataxyz[child][2] - dataxyz[current][2]]
vec1 = [1, 0, 0]
rotvec = getRotaionVect(vec1, vec2)
euler = toEuler(rotvec[0], rotvec[1], rotvec[2], rotvec[3])
data[current]["rotation"]["rotx"] = -euler[0]
data[current]["rotation"]["roty"] = euler[1]
data[current]["rotation"]["rotz"] = -90
def lhandRot(current, child, data, dataxyz, support1, support2):
vec2 = [dataxyz[child][0] - dataxyz[current][0],
dataxyz[child][1] - dataxyz[current][1],
dataxyz[child][2] - dataxyz[current][2]]
vec1 = [1, 0, 0]
rotvec = getRotaionVect(vec1, vec2)
euler = toEuler(rotvec[0],rotvec[1],rotvec[2],rotvec[3])
# print(euler)
vec3 = [dataxyz[support2][0] - dataxyz[support1][0],
dataxyz[support2][1] - dataxyz[support1][1],
dataxyz[support2][2] - dataxyz[support1][2]]
vec2_norm = getVecNorm(vec2)
vec3_norm = getVecNorm(vec3)
sup2 = np.cross(vec2_norm, vec3_norm)
sup2_norm = getVecNorm(sup2)
sup1 = [0, 1, 0]
sup1_norm = getVecNorm(sup1)
rotsup = getRotaionVect(sup1_norm, sup2_norm)
euler_sup = toEuler(rotsup[0], rotsup[1], rotsup[2], rotsup[3])
print(euler_sup)
data[current]["rotation"]["rotx"] = -euler[0]
data[current]["rotation"]["roty"] = euler[1]
data[current]["rotation"]["rotz"] = -euler_sup[2]
def lhandrot(lhandlms, data, coef):
dataxyz = {}
rootx = lhandlms[17].x * coef
rooty = -lhandlms[17].z * coef
rootz = -lhandlms[17].y * coef
rootv = 1.0
dataxyz["pinky_2_l"] = [rootx, rooty, rootz, rootv]
rootx = lhandlms[18].x * coef
rooty = -lhandlms[18].z * coef
rootz = -lhandlms[18].y * coef
rootv = 1.0
dataxyz["pinky_3_l"] = [rootx, rooty, rootz, rootv]
rootx = lhandlms[19].x * coef
rooty = -lhandlms[19].z * coef
rootz = -lhandlms[19].y * coef
rootv = 1.0
dataxyz["pinky_4_l"] = [rootx, rooty, rootz, rootv]
rootx = lhandlms[20].x * coef
rooty = -lhandlms[20].z * coef
rootz = -lhandlms[20].y * coef
rootv = 1.0
dataxyz["pinky_5_l"] = [rootx, rooty, rootz, rootv]
lfingerRot("pinky_2_l", "pinky_3_l", data, dataxyz)
lfingerRot("pinky_3_l", "pinky_4_l", data, dataxyz)
lfingerRot("pinky_4_l", "pinky_5_l", data, dataxyz)
rootx = lhandlms[13].x * coef
rooty = -lhandlms[13].z * coef
rootz = -lhandlms[13].y * coef
rootv = 1.0
dataxyz["ring_2_l"] = [rootx, rooty, rootz, rootv]
rootx = lhandlms[14].x * coef
rooty = -lhandlms[14].z * coef
rootz = -lhandlms[14].y * coef
rootv = 1.0
dataxyz["ring_3_l"] = [rootx, rooty, rootz, rootv]
rootx = lhandlms[15].x * coef
rooty = -lhandlms[15].z * coef
rootz = -lhandlms[15].y * coef
rootv = 1.0
dataxyz["ring_4_l"] = [rootx, rooty, rootz, rootv]
rootx = lhandlms[16].x * coef
rooty = -lhandlms[16].z * coef
rootz = -lhandlms[16].y * coef
rootv = 1.0
dataxyz["ring_5_l"] = [rootx, rooty, rootz, rootv]
lfingerRot("ring_2_l", "ring_3_l", data, dataxyz)
lfingerRot("ring_3_l", "ring_4_l", data, dataxyz)
lfingerRot("ring_4_l", "ring_5_l", data, dataxyz)
rootx = lhandlms[9].x * coef
rooty = -lhandlms[9].z * coef
rootz = -lhandlms[9].y * coef
rootv = 1.0
dataxyz["middle_2_l"] = [rootx, rooty, rootz, rootv]
rootx = lhandlms[10].x * coef
rooty = -lhandlms[10].z * coef
rootz = -lhandlms[10].y * coef
rootv = 1.0
dataxyz["middle_3_l"] = [rootx, rooty, rootz, rootv]
rootx = lhandlms[11].x * coef
rooty = -lhandlms[11].z * coef
rootz = -lhandlms[11].y * coef
rootv = 1.0
dataxyz["middle_4_l"] = [rootx, rooty, rootz, rootv]
rootx = lhandlms[12].x * coef
rooty = -lhandlms[12].z * coef
rootz = -lhandlms[12].y * coef
rootv = 1.0
dataxyz["middle_5_l"] = [rootx, rooty, rootz, rootv]
lfingerRot("middle_2_l", "middle_3_l", data, dataxyz)
lfingerRot("middle_3_l", "middle_4_l", data, dataxyz)
lfingerRot("middle_4_l", "middle_5_l", data, dataxyz)
rootx = lhandlms[5].x * coef
rooty = -lhandlms[5].z * coef
rootz = -lhandlms[5].y * coef
rootv = 1.0
dataxyz["index_2_l"] = [rootx, rooty, rootz, rootv]
rootx = lhandlms[6].x * coef
rooty = -lhandlms[6].z * coef
rootz = -lhandlms[6].y * coef
rootv = 1.0
dataxyz["index_3_l"] = [rootx, rooty, rootz, rootv]
rootx = lhandlms[7].x * coef
rooty = -lhandlms[7].z * coef
rootz = -lhandlms[7].y * coef
rootv = 1.0
dataxyz["index_4_l"] = [rootx, rooty, rootz, rootv]
rootx = lhandlms[8].x * coef
rooty = -lhandlms[8].z * coef
rootz = -lhandlms[8].y * coef
rootv = 1.0
dataxyz["index_5_l"] = [rootx, rooty, rootz, rootv]
lfingerRot("index_2_l", "index_3_l", data, dataxyz)
lfingerRot("index_3_l", "index_4_l", data, dataxyz)
lfingerRot("index_4_l", "index_5_l", data, dataxyz)
rootx = lhandlms[1].x * coef
rooty = -lhandlms[1].z * coef
rootz = -lhandlms[1].y * coef
rootv = 1.0
dataxyz["thumb_2_l"] = [rootx, rooty, rootz, rootv]
rootx = lhandlms[2].x * coef
rooty = -lhandlms[2].z * coef
rootz = -lhandlms[2].y * coef
rootv = 1.0
dataxyz["thumb_3_l"] = [rootx, rooty, rootz, rootv]
rootx = lhandlms[3].x * coef
rooty = -lhandlms[3].z * coef
rootz = -lhandlms[3].y * coef
rootv = 1.0
dataxyz["thumb_4_l"] = [rootx, rooty, rootz, rootv]
rootx = lhandlms[4].x * coef
rooty = -lhandlms[4].z * coef
rootz = -lhandlms[4].y * coef
rootv = 1.0
dataxyz["thumb_5_l"] = [rootx, rooty, rootz, rootv]
lfingerRot("thumb_2_l", "thumb_3_l", data, dataxyz)
lfingerRot("thumb_3_l", "thumb_4_l", data, dataxyz)
lfingerRot("thumb_4_l", "thumb_5_l", data, dataxyz)
rootx = lhandlms[0].x * coef
rooty = -lhandlms[0].z * coef
rootz = -lhandlms[0].y * coef
rootv = 1.0
dataxyz["hand_l"] = [rootx, rooty, rootz, rootv]
lhandRot("hand_l", "middle_2_l", data, dataxyz, "pinky_2_l", "index_2_l")
def rhandRot(current, child, data, dataxyz, support1, support2):
vec2 = [dataxyz[child][0] - dataxyz[current][0],
dataxyz[child][1] - dataxyz[current][1],
dataxyz[child][2] - dataxyz[current][2]]
vec1 = [-1, 0, 0]
rotvec = getRotaionVect(vec1, vec2)
euler = toEuler(rotvec[0],rotvec[1],rotvec[2],rotvec[3])
# print(euler)
vec3 = [dataxyz[support2][0] - dataxyz[support1][0],
dataxyz[support2][1] - dataxyz[support1][1],
dataxyz[support2][2] - dataxyz[support1][2]]
vec2_norm = getVecNorm(vec2)
vec3_norm = getVecNorm(vec3)
sup2 = np.cross(vec2_norm, vec3_norm)
sup2_norm = getVecNorm(sup2)
sup1 = [0, 1, 0]
sup1_norm = getVecNorm(sup1)
rotsup = getRotaionVect(sup1_norm, sup2_norm)
euler_sup = toEuler(rotsup[0], rotsup[1], rotsup[2], rotsup[3])
print(euler_sup)
data[current]["rotation"]["rotx"] = -euler[0]
data[current]["rotation"]["roty"] = euler[1]
data[current]["rotation"]["rotz"] = -euler_sup[2]
def rfingerRot(current, child, data, dataxyz):
vec2 = [dataxyz[child][0] - dataxyz[current][0],
dataxyz[child][1] - dataxyz[current][1],
dataxyz[child][2] - dataxyz[current][2]]
vec1 = [-1, 0, 0]
rotvec = getRotaionVect(vec1, vec2)
euler = toEuler(rotvec[0], rotvec[1], rotvec[2], rotvec[3])
data[current]["rotation"]["rotx"] = -euler[0]
data[current]["rotation"]["roty"] = euler[1]
data[current]["rotation"]["rotz"] = 90
def rhandrot(rhandlms, data, coef):
dataxyz = {}
rootx = rhandlms[17].x * coef
rooty = -rhandlms[17].z * coef
rootz = -rhandlms[17].y * coef
rootv = 1.0
dataxyz["pinky_2_r"] = [rootx, rooty, rootz, rootv]
rootx = rhandlms[18].x * coef
rooty = -rhandlms[18].z * coef
rootz = -rhandlms[18].y * coef
rootv = 1.0
dataxyz["pinky_3_r"] = [rootx, rooty, rootz, rootv]
rootx = rhandlms[19].x * coef
rooty = -rhandlms[19].z * coef
rootz = -rhandlms[19].y * coef
rootv = 1.0
dataxyz["pinky_4_r"] = [rootx, rooty, rootz, rootv]
rootx = rhandlms[20].x * coef
rooty = -rhandlms[20].z * coef
rootz = -rhandlms[20].y * coef
rootv = 1.0
dataxyz["pinky_5_r"] = [rootx, rooty, rootz, rootv]
rfingerRot("pinky_2_r", "pinky_3_r", data, dataxyz)
rfingerRot("pinky_3_r", "pinky_4_r", data, dataxyz)
rfingerRot("pinky_4_r", "pinky_5_r", data, dataxyz)
rootx = rhandlms[13].x * coef
rooty = -rhandlms[13].z * coef
rootz = -rhandlms[13].y * coef
rootv = 1.0
dataxyz["ring_2_r"] = [rootx, rooty, rootz, rootv]
rootx = rhandlms[14].x * coef
rooty = -rhandlms[14].z * coef
rootz = -rhandlms[14].y * coef
rootv = 1.0
dataxyz["ring_3_r"] = [rootx, rooty, rootz, rootv]
rootx = rhandlms[15].x * coef
rooty = -rhandlms[15].z * coef
rootz = -rhandlms[15].y * coef
rootv = 1.0
dataxyz["ring_4_r"] = [rootx, rooty, rootz, rootv]
rootx = rhandlms[16].x * coef
rooty = -rhandlms[16].z * coef
rootz = -rhandlms[16].y * coef
rootv = 1.0
dataxyz["ring_5_r"] = [rootx, rooty, rootz, rootv]
rfingerRot("ring_2_r", "ring_3_r", data, dataxyz)
rfingerRot("ring_3_r", "ring_4_r", data, dataxyz)
rfingerRot("ring_4_r", "ring_5_r", data, dataxyz)
rootx = rhandlms[9].x * coef
rooty = -rhandlms[9].z * coef
rootz = -rhandlms[9].y * coef
rootv = 1.0
dataxyz["middle_2_r"] = [rootx, rooty, rootz, rootv]
rootx = rhandlms[10].x * coef
rooty = -rhandlms[10].z * coef
rootz = -rhandlms[10].y * coef
rootv = 1.0
dataxyz["middle_3_r"] = [rootx, rooty, rootz, rootv]
rootx = rhandlms[11].x * coef
rooty = -rhandlms[11].z * coef
rootz = -rhandlms[11].y * coef
rootv = 1.0
dataxyz["middle_4_r"] = [rootx, rooty, rootz, rootv]
rootx = rhandlms[12].x * coef
rooty = -rhandlms[12].z * coef
rootz = -rhandlms[12].y * coef
rootv = 1.0
dataxyz["middle_5_r"] = [rootx, rooty, rootz, rootv]
rfingerRot("middle_2_r", "middle_3_r", data, dataxyz)
rfingerRot("middle_3_r", "middle_4_r", data, dataxyz)
rfingerRot("middle_4_r", "middle_5_r", data, dataxyz)
rootx = rhandlms[5].x * coef
rooty = -rhandlms[5].z * coef
rootz = -rhandlms[5].y * coef
rootv = 1.0
dataxyz["index_2_r"] = [rootx, rooty, rootz, rootv]
rootx = rhandlms[6].x * coef
rooty = -rhandlms[6].z * coef
rootz = -rhandlms[6].y * coef
rootv = 1.0
dataxyz["index_3_r"] = [rootx, rooty, rootz, rootv]
rootx = rhandlms[7].x * coef
rooty = -rhandlms[7].z * coef
rootz = -rhandlms[7].y * coef
rootv = 1.0
dataxyz["index_4_r"] = [rootx, rooty, rootz, rootv]
rootx = rhandlms[8].x * coef
rooty = -rhandlms[8].z * coef
rootz = -rhandlms[8].y * coef
rootv = 1.0
dataxyz["index_5_r"] = [rootx, rooty, rootz, rootv]
rfingerRot("index_2_r", "index_3_r", data, dataxyz)
rfingerRot("index_3_r", "index_4_r", data, dataxyz)
rfingerRot("index_4_r", "index_5_r", data, dataxyz)
rootx = rhandlms[1].x * coef
rooty = -rhandlms[1].z * coef
rootz = -rhandlms[1].y * coef
rootv = 1.0
dataxyz["thumb_2_r"] = [rootx, rooty, rootz, rootv]
rootx = rhandlms[2].x * coef
rooty = -rhandlms[2].z * coef
rootz = -rhandlms[2].y * coef
rootv = 1.0
dataxyz["thumb_3_r"] = [rootx, rooty, rootz, rootv]
rootx = rhandlms[3].x * coef
rooty = -rhandlms[3].z * coef
rootz = -rhandlms[3].y * coef
rootv = 1.0
dataxyz["thumb_4_r"] = [rootx, rooty, rootz, rootv]
rootx = rhandlms[4].x * coef
rooty = -rhandlms[4].z * coef
rootz = -rhandlms[4].y * coef
rootv = 1.0
dataxyz["thumb_5_r"] = [rootx, rooty, rootz, rootv]
rfingerRot("thumb_2_r", "thumb_3_r", data, dataxyz)
rfingerRot("thumb_3_r", "thumb_4_r", data, dataxyz)
rfingerRot("thumb_4_r", "thumb_5_r", data, dataxyz)
rootx = rhandlms[0].x * coef
rooty = -rhandlms[0].z * coef
rootz = -rhandlms[0].y * coef
rootv = 1.0
dataxyz["hand_r"] = [rootx, rooty, rootz, rootv]
rhandRot("hand_r", "middle_2_r", data, dataxyz, "pinky_2_r", "index_2_r")