Rigify: fixed two bugs in IK/FK snapping that made it fail in some cases.

1 files changed, 49 insertions, 31 deletions

diff --git a/rigify/rig_ui_template.py b/rigify/rig_ui_template.py
index 2f76d8a4..89a4e544 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):