diff --git a/rigify/rig_ui_template.py b/rigify/rig_ui_template.py index 2f76d8a4c4056aa691a3540d506a84ce7cc0042d..89a4e544c29e4fcae3cba8cf5993aecb31975a66 100644 --- a/rigify/rig_ui_template.py +++ b/rigify/rig_ui_template.py @@ -21,11 +21,45 @@ UI_SLIDERS = ''' import bpy from mathutils import Matrix, Vector -from math import acos +from math import acos, pi rig_id = "%s" +############################ +## Math utility functions ## +############################ + +def perpendicular_vector(v): + """ Returns a vector that is perpendicular to the one given. + The returned vector is _not_ guaranteed to be normalized. + """ + # Create a vector that is not aligned with v. + # It doesn't matter what vector. Just any vector + # that's guaranteed to not be pointing in the same + # direction. + if abs(v[0]) < abs(v[1]): + tv = Vector((1,0,0)) + else: + tv = Vector((0,1,0)) + + # Use cross prouct to generate a vector perpendicular to + # both tv and (more importantly) v. + return v.cross(tv) + + +def rotation_difference(mat1, mat2): + """ Returns the shortest-path rotational difference between two + matrices. + """ + q1 = mat1.to_quaternion() + q2 = mat2.to_quaternion() + angle = acos(min(1,max(-1,q1.dot(q2)))) * 2 + if angle > pi: + angle = -angle + (2*pi) + return angle + + ######################################### ## "Visual Transform" helper functions ## ######################################### @@ -162,20 +196,8 @@ def match_pole_target(ik_first, ik_last, pole, match_bone, length): # tip of ik_last ikv = b - a - # Create a vector that is not aligned with ikv. - # It doesn't matter what vector. Just any vector - # that's guaranteed to not be pointing in the same - # direction. In this case, we create a unit vector - # on the axis of the smallest component of ikv. - if abs(ikv[0]) < abs(ikv[1]) and abs(ikv[0]) < abs(ikv[2]): - v = Vector((1,0,0)) - elif abs(ikv[1]) < abs(ikv[2]): - v = Vector((0,1,0)) - else: - v = Vector((0,0,1)) - # Get a vector perpendicular to ikv - pv = v.cross(ikv).normalized() * length + pv = perpendicular_vector(ikv).normalized() * length def set_pole(pvi): """ Set pole target's position based on a vector @@ -194,24 +216,20 @@ def match_pole_target(ik_first, ik_last, pole, match_bone, length): set_pole(pv) # Get the rotation difference between ik_first and match_bone - q1 = ik_first.matrix.to_quaternion() - q2 = match_bone.matrix.to_quaternion() - angle = acos(min(1,max(-1,q1.dot(q2)))) * 2 + angle = rotation_difference(ik_first.matrix, match_bone.matrix) + + # Try compensating for the rotation difference in both directions + pv1 = Matrix.Rotation(angle, 4, ikv) * pv + set_pole(pv1) + ang1 = rotation_difference(ik_first.matrix, match_bone.matrix) + + pv2 = Matrix.Rotation(-angle, 4, ikv) * pv + set_pole(pv2) + ang2 = rotation_difference(ik_first.matrix, match_bone.matrix) - # Compensate for the rotation difference - if angle > 0.0001: - pv = Matrix.Rotation(angle, 4, ikv).to_quaternion() * pv - set_pole(pv) - - # Get rotation difference again, to see if we - # compensated in the right direction - q1 = ik_first.matrix.to_quaternion() - q2 = match_bone.matrix.to_quaternion() - angle2 = acos(min(1,max(-1,q1.dot(q2)))) * 2 - if angle2 > 0.0001: - # Compensate in the other direction - pv = Matrix.Rotation((angle*(-2)), 4, ikv).to_quaternion() * pv - set_pole(pv) + # Do the one with the smaller angle + if ang1 < ang2: + set_pole(pv1) def fk2ik_arm(obj, fk, ik):