def rotate3d(origin, angle_quat, point_in):
# "rotate around a point in 3d space"
# subtract "origin" to move the whole system to rotating around 0,0,0
point = [p - o for p, o in zip(point_in, origin)]
# might need to scale the point down to unit-length???
# i'll do it just to be safe, it couldn't hurt
length = core.my_euclidian_distance(point)
if length != 0:
point = [p / length for p in point]
# set up the math as instructed by math.stackexchange
p_vect = [0] + point
r_prime_vect = core.my_quat_conjugate(angle_quat)
# r_prime_vect = [angle_quat[0], -angle_quat[1], -angle_quat[2], -angle_quat[3]]
# P' = R * P * R'
# P' = H( H(R,P), R')
temp = core.hamilton_product(angle_quat, p_vect)
p_prime_vect = core.hamilton_product(temp, r_prime_vect)
# note that the first element of P' will always be 0
point = p_prime_vect[1:4]
# might need to undo scaling the point down to unit-length???
point = [p * length for p in point]
# re-add "origin" to move the system to where it should have been
point = [p + o for p, o in zip(point, origin)]
return point
def swing_twist_decompose(quat_in, axis): """ Decompose the rotation on to 2 parts. 1. Twist - rotation around the "direction" vector 2. Swing - rotation around axis that is perpendicular to "direction" vector The rotation can be composed back by quat_in = swing * twist has singularity in case of swing_rotation close to 180 degrees rotation. if the input quaternion is of non-unit length, the outputs are non-unit as well otherwise, outputs are both unit output = (swing, twist) """ # vector3 quat_rotation_axis( quat_in.x, quat_in.y, quat_in.z ); // rotation axis # quat rotation axis quat_rotation_axis = quat_in[1:4] # vector3 p = projection( quat_rotation_axis, axis ); // return projection x on to y (parallel component) p = core.my_projection(quat_rotation_axis, axis) # twist.set( p.x, p.y, p.z, quat_in.w ); // but i use them as W X Y Z twist = [quat_in[0], p[0], p[1], p[2]] # twist.normalize(); length = core.my_euclidian_distance(twist) twist = [t / length for t in twist] # swing = quat_in * twist.conjugated(); twist_conjugate = core.my_quat_conjugate(twist) swing = core.hamilton_product(quat_in, twist_conjugate) return swing, twist
def get_corner_sharpness_factor(
quatA: Tuple[float, float, float, float], quatB: Tuple[float, float,
float, float],
quatC: Tuple[float, float, float, float]) -> float:
"""
Calculate a [0.0-1.0] factor indicating how "sharp" the corner is at B.
By "corner" I mean the directional change when A->B stops and B->C begins.
If they are going the same angular "direction", then return 1.0. If they
are going perfectly opposite directions, return 0.0. Otherwise return something
in between.
The option ROTATION_CORNER_SHARPNESS_FACTOR_MODE controls what the transfer
curve looks like from angle to factor.
:param quatA: quaterinon WXYZ for frame A
:param quatB: quaterinon WXYZ for frame B
:param quatC: quaterinon WXYZ for frame C
:return: float [0.0-1.0]
"""
# to compensate for the angle difference, both will be slowed by some amount
# IDENTICAL IMPACT
# first, find the deltas between the quaternions
deltaquat_AB = core.hamilton_product(core.my_quat_conjugate(quatA), quatB)
deltaquat_BC = core.hamilton_product(core.my_quat_conjugate(quatB), quatC)
# to get sensible results below, ignore the "W" component and only use the XYZ components, treat as 3d vector
deltavect_AB = deltaquat_AB[1:4]
deltavect_BC = deltaquat_BC[1:4]
# second, find the angle between these two deltas
# use the plain old "find the angle between two vectors" formula
t = core.my_euclidian_distance(deltavect_AB) * core.my_euclidian_distance(
deltavect_BC)
if t == 0:
# this happens when one vector has a length of 0
ang_d = 0
else:
# technically the clamp shouldn't be necessary but floating point inaccuracy caused it to do math.acos(1.000000002) which crashed lol
shut_up = core.my_dot(deltavect_AB, deltavect_BC) / t
shut_up = core.clamp(shut_up, -1.0, 1.0)
ang_d = math.acos(shut_up)
# print(math.degrees(ang_d))
# if ang = 0, perfectly colinear, factor = 1
# if ang = 180, perfeclty opposite, factor = 0
factor = 1 - (math.degrees(ang_d) / 180)
# print(factor)
# ANGLE_SHARPNESS_FACTORS.append(factor)
if ROTATION_CORNER_SHARPNESS_FACTOR_MODE == 1:
# disabled
out_factor = 1
elif ROTATION_CORNER_SHARPNESS_FACTOR_MODE == 2:
# linear
out_factor = factor
elif ROTATION_CORNER_SHARPNESS_FACTOR_MODE == 3:
# square root
out_factor = math.sqrt(factor)
elif ROTATION_CORNER_SHARPNESS_FACTOR_MODE == 4:
# piecewise floored, (0,.5) to (.5,1)
out_factor = 0.5 + factor
out_factor = core.clamp(out_factor, 0.0, 1.0)
else:
out_factor = 1
out_factor = core.clamp(out_factor, 0.0, 1.0)
return out_factor
def main(moreinfo=True):
# prompt PMX name
core.MY_PRINT_FUNC("Please enter name of PMX input file:")
input_filename_pmx = core.MY_FILEPROMPT_FUNC(".pmx")
pmx = pmxlib.read_pmx(input_filename_pmx, moreinfo=moreinfo)
# get bones
realbones = pmx.bones
# then, make 2 lists: one starting from jp_righttoe, one starting from jp_lefttoe
# start from each "toe" bone (names are known), go parent-find-parent-find until reaching no-parent
bonechain_r = build_bonechain(realbones, jp_righttoe)
bonechain_l = build_bonechain(realbones, jp_lefttoe)
# assert that the bones were found, have correct names, and are in the correct positions
# also verifies that they are direct parent-child with nothing in between
try:
assert bonechain_r[-1].name == jp_righttoe
assert bonechain_r[-2].name == jp_rightfoot
assert bonechain_l[-1].name == jp_lefttoe
assert bonechain_l[-2].name == jp_leftfoot
except AssertionError:
core.MY_PRINT_FUNC(
"ERROR: unexpected structure found for foot/toe bones, verify semistandard names and structure"
)
raise RuntimeError()
# then walk down these 2 lists, add each name to a set: build union of all relevant bones
relevant_bones = set()
for b in bonechain_r + bonechain_l:
relevant_bones.add(b.name)
# check if waist-cancellation bones are in "relevant_bones", print a warning if they are
if jp_left_waistcancel in relevant_bones or jp_right_waistcancel in relevant_bones:
# TODO LOW: i probably could figure out how to support them but this whole script is useless so idgaf
core.MY_PRINT_FUNC(
"Warning: waist-cancellation bones found in the model! These are not supported, tool may produce bad results! Attempting to continue..."
)
# also need to find initial positions of ik bones (names are known)
# build a full parentage-chain for each leg
bonechain_ikr = build_bonechain(realbones, jp_righttoe_ik)
bonechain_ikl = build_bonechain(realbones, jp_lefttoe_ik)
# verify that the ik bones were found, have correct names, and are in the correct positions
try:
assert bonechain_ikr[-1].name == jp_righttoe_ik
assert bonechain_ikr[-2].name == jp_rightfoot_ik
assert bonechain_ikl[-1].name == jp_lefttoe_ik
assert bonechain_ikl[-2].name == jp_leftfoot_ik
except AssertionError:
core.MY_PRINT_FUNC(
"ERROR: unexpected structure found for foot/toe IK bones, verify semistandard names and structure"
)
raise RuntimeError()
# verify that the bonechains are symmetric in length
try:
assert len(bonechain_l) == len(bonechain_r)
assert len(bonechain_ikl) == len(bonechain_ikr)
except AssertionError:
core.MY_PRINT_FUNC(
"ERROR: unexpected structure found, model is not left-right symmetric"
)
raise RuntimeError()
# determine how many levels of parentage, this value "t" should hold the first level where they are no longer shared
t = 0
while bonechain_l[t].name == bonechain_ikl[t].name:
t += 1
# back off one level
lowest_shared_parent = t - 1
# now i am completely done with the bones CSV, all the relevant info has been distilled down to:
# !!! bonechain_r, bonechain_l, bonechain_ikr, bonechain_ikl, relevant_bones
core.MY_PRINT_FUNC("...identified " + str(len(bonechain_l)) +
" bones per leg-chain, " + str(len(relevant_bones)) +
" relevant bones total")
core.MY_PRINT_FUNC("...identified " + str(len(bonechain_ikl)) +
" bones per IK leg-chain")
###################################################################################
# prompt VMD file name
core.MY_PRINT_FUNC("Please enter name of VMD dance input file:")
input_filename_vmd = core.MY_FILEPROMPT_FUNC(".vmd")
nicelist_in = vmdlib.read_vmd(input_filename_vmd, moreinfo=moreinfo)
# check if this VMD uses IK or not, print a warning if it does
any_ik_on = False
for ikdispframe in nicelist_in.ikdispframes:
for ik_bone in ikdispframe.ikbones:
if ik_bone.enable is True:
any_ik_on = True
break
if any_ik_on:
core.MY_PRINT_FUNC(
"Warning: the input VMD already has IK enabled, there is no point in running this script. Attempting to continue..."
)
# reduce down to only the boneframes for the relevant bones
# also build a list of each framenumber with a frame for a bone we care about
relevant_framenums = set()
boneframe_list = []
for boneframe in nicelist_in.boneframes:
if boneframe.name in relevant_bones:
boneframe_list.append(boneframe)
relevant_framenums.add(boneframe.f)
# sort the boneframes by frame number
boneframe_list.sort(key=lambda x: x.f)
# make the relevant framenumbers also an ascending list
relevant_framenums = sorted(list(relevant_framenums))
boneframe_dict = dict()
# now restructure the data from a list to a dictionary, keyed by bone name. also discard excess data when i do
for b in boneframe_list:
if b.name not in boneframe_dict:
boneframe_dict[b.name] = []
# only storing the frame#(1) + position(234) + rotation values(567)
saveme = [b.f, *b.pos, *b.rot]
boneframe_dict[b.name].append(saveme)
core.MY_PRINT_FUNC(
"...running interpolation to rectangularize the frames...")
has_warned = False
# now fill in the blanks by using interpolation, if needed
for key, bone in boneframe_dict.items(): # for each bone,
# start a list of frames generated by interpolation
interpframe_list = []
i = 0
j = 0
while j < len(relevant_framenums): # for each frame it should have,
if i == len(bone):
# if i is beyond end of bone, then copy the values from the last frame and use as a new frame
newframe = [relevant_framenums[j]] + bone[-1][1:7]
interpframe_list.append(newframe)
j += 1
elif bone[i][0] == relevant_framenums[j]: # does it have it?
i += 1
j += 1
else:
# TODO LOW: i could modify this to include my interpolation curve math now that I understand it, but i dont care
if not has_warned:
core.MY_PRINT_FUNC(
"Warning: interpolation is needed but interpolation curves are not fully tested! Assuming linear interpolation..."
)
has_warned = True
# if there is a mismatch then the target framenum is less than the boneframe framenum
# build a frame that has frame# + position(123) + rotation values(456)
newframe = [relevant_framenums[j]]
# if target is less than the current boneframe, interp between here and prev boneframe
for p in range(1, 4):
# interpolate for each position offset
newframe.append(
core.linear_map(bone[i][0], bone[i][p], bone[i - 1][0],
bone[i - 1][p], relevant_framenums[j]))
# rotation interpolation must happen in the quaternion-space
quat1 = core.euler_to_quaternion(bone[i - 1][4:7])
quat2 = core.euler_to_quaternion(bone[i][4:7])
# desired frame is relevant_framenums[j] = d
# available frames are bone[i-1][0] = s and bone[i][0] = e
# percentage = (d - s) / (e - s)
percentage = (relevant_framenums[j] -
bone[i - 1][0]) / (bone[i][0] - bone[i - 1][0])
quat_slerp = core.my_slerp(quat1, quat2, percentage)
euler_slerp = core.quaternion_to_euler(quat_slerp)
newframe += euler_slerp
interpframe_list.append(newframe)
j += 1
bone += interpframe_list
bone.sort(key=core.get1st)
# the dictionary should be fully filled out and rectangular now
for bone in boneframe_dict:
assert len(boneframe_dict[bone]) == len(relevant_framenums)
# now i am completely done reading the VMD file and parsing its data! everything has been distilled down to:
# relevant_framenums, boneframe_dict
###################################################################################
# begin the actual calculations
core.MY_PRINT_FUNC("...beginning forward kinematics computation for " +
str(len(relevant_framenums)) + " frames...")
# output array
ikframe_list = []
# have list of bones, parentage, initial pos
# have list of frames
# now i "run the dance" and build the ik frames
# for each relevant frame,
for I in range(len(relevant_framenums)):
# for each side,
for (thisik, this_chain) in zip([bonechain_ikr, bonechain_ikl],
[bonechain_r, bonechain_l]):
# for each bone in this_chain (ordered, start with root!),
for J in range(len(this_chain)):
# reset the current to be the inital position again
this_chain[J].reset()
# for each bone in this_chain (ordered, start with toe! do children before parents!)
# also, don't read/use root! because the IK are also children of root, they inherit the same root transformations
# count backwards from end to lowest_shared_parent, not including lowest_shared_parent
for J in range(len(this_chain) - 1, lowest_shared_parent, -1):
# get bone J within this_chain, translate to name
name = this_chain[J].name
# get bone [name] at index I: position & rotation
try:
xpos, ypos, zpos, xrot, yrot, zrot = boneframe_dict[name][
I][1:7]
except KeyError:
continue
# apply position offset to self & children
# also resets the currposition when changing frames
for K in range(J, len(this_chain)):
# set this_chain[K].current456 = current456 + position
this_chain[K].xcurr += xpos
this_chain[K].ycurr += ypos
this_chain[K].zcurr += zpos
# apply rotation offset to all children, but not self
_origin = [
this_chain[J].xcurr, this_chain[J].ycurr,
this_chain[J].zcurr
]
_angle = [xrot, yrot, zrot]
_angle_quat = core.euler_to_quaternion(_angle)
for K in range(J, len(this_chain)):
# set this_chain[K].current456 = current rotated around this_chain[J].current456
_point = [
this_chain[K].xcurr, this_chain[K].ycurr,
this_chain[K].zcurr
]
_newpoint = rotate3d(_origin, _angle_quat, _point)
(this_chain[K].xcurr, this_chain[K].ycurr,
this_chain[K].zcurr) = _newpoint
# also rotate the angle of this bone
curr_angle_euler = [
this_chain[K].xrot, this_chain[K].yrot,
this_chain[K].zrot
]
curr_angle_quat = core.euler_to_quaternion(
curr_angle_euler)
new_angle_quat = core.hamilton_product(
_angle_quat, curr_angle_quat)
new_angle_euler = core.quaternion_to_euler(new_angle_quat)
(this_chain[K].xrot, this_chain[K].yrot,
this_chain[K].zrot) = new_angle_euler
pass
pass
# now i have cascaded this frame's pose data down the this_chain
# grab foot/toe (-2 and -1) current position and calculate IK offset from that
# first, foot:
# footikend - footikinit = footikoffset
xfoot = this_chain[-2].xcurr - thisik[-2].xinit
yfoot = this_chain[-2].ycurr - thisik[-2].yinit
zfoot = this_chain[-2].zcurr - thisik[-2].zinit
# save as boneframe to be ultimately formatted for VMD:
# need bonename = (known)
# need frame# = relevantframe#s[I]
# position = calculated
# rotation = 0
# phys = not disabled
# interp = default (20/107)
# # then, foot-angle: just copy the angle that the foot has
if STORE_IK_AS_FOOT_ONLY:
ikframe = [
thisik[-2].name, relevant_framenums[I], xfoot, yfoot,
zfoot, this_chain[-2].xrot, this_chain[-2].yrot,
this_chain[-2].zrot, False
]
else:
ikframe = [
thisik[-2].name, relevant_framenums[I], xfoot, yfoot,
zfoot, 0.0, 0.0, 0.0, False
]
ikframe += [20] * 8
ikframe += [107] * 8
# append the freshly-built frame
ikframe_list.append(ikframe)
if not STORE_IK_AS_FOOT_ONLY:
# then, toe:
# toeikend - toeikinit - footikoffset = toeikoffset
xtoe = this_chain[-1].xcurr - thisik[-1].xinit - xfoot
ytoe = this_chain[-1].ycurr - thisik[-1].yinit - yfoot
ztoe = this_chain[-1].zcurr - thisik[-1].zinit - zfoot
ikframe = [
thisik[-1].name, relevant_framenums[I], xtoe, ytoe, ztoe,
0.0, 0.0, 0.0, False
]
ikframe += [20] * 8
ikframe += [107] * 8
# append the freshly-built frame
ikframe_list.append(ikframe)
# now done with a timeframe for all bones on both sides
# print progress updates
core.print_progress_oneline(I / len(relevant_framenums))
core.MY_PRINT_FUNC(
"...done with forward kinematics computation, now writing output...")
if INCLUDE_IK_ENABLE_FRAME:
# create a single ikdispframe that enables the ik bones at frame 0
ikbones = [
vmdstruct.VmdIkbone(name=jp_rightfoot_ik, enable=True),
vmdstruct.VmdIkbone(name=jp_righttoe_ik, enable=True),
vmdstruct.VmdIkbone(name=jp_leftfoot_ik, enable=True),
vmdstruct.VmdIkbone(name=jp_lefttoe_ik, enable=True)
]
ikdispframe_list = [
vmdstruct.VmdIkdispFrame(f=0, disp=True, ikbones=ikbones)
]
else:
ikdispframe_list = []
core.MY_PRINT_FUNC(
"Warning: IK following will NOT be enabled when this VMD is loaded, you will need enable it manually!"
)
# convert old-style bonelist ikframe_list to new object format
ikframe_list = [
vmdstruct.VmdBoneFrame(name=r[0],
f=r[1],
pos=r[2:5],
rot=r[5:8],
phys_off=r[8],
interp=r[9:]) for r in ikframe_list
]
# build actual VMD object
nicelist_out = vmdstruct.Vmd(
vmdstruct.VmdHeader(2, "SEMISTANDARD-IK-BONES--------"),
ikframe_list, # bone
[], # morph
[], # cam
[], # light
[], # shadow
ikdispframe_list # ikdisp
)
# write out
output_filename_vmd = "%s_ik_from_%s.vmd" % \
(input_filename_vmd[0:-4], core.get_clean_basename(input_filename_pmx))
output_filename_vmd = core.get_unused_file_name(output_filename_vmd)
vmdlib.write_vmd(output_filename_vmd, nicelist_out, moreinfo=moreinfo)
core.MY_PRINT_FUNC("Done!")
return None