소스 검색

Fix: IK calculation can now bisect search at any angle

before this commit it could get clamped if it was less than -180 or more than 180 degrees
now it does the calculation unclamped and wraps
the value before setting.
this is also probably not very efficient, but it works.
Joseph Brandenburg 7 달 전
부모
커밋
31532190b9
1개의 변경된 파일17개의 추가작업 그리고 4개의 파일을 삭제
  1. 17 4
      link_containers.py

+ 17 - 4
link_containers.py

@@ -1188,7 +1188,19 @@ class LinkInverseKinematics(MantisLinkNode):
         while (i<chain_length) and (base_ik_bone.parent):
             base_ik_bone=base_ik_bone.parent
         return base_ik_bone
-
+    
+    # We need to do the calculation in a "full circle", meaning the pole_angle
+    # can go over pi or less than -pi - but the actuall constraint value must
+    # be clamped in that range.
+    # so we simply wrap the value.
+    # not very efficient but it's OK
+    def set_pole_angle(self, angle: float) -> None:
+        from math import pi
+        def wrap(min : float, max : float, value: float) -> float:
+            range = max-min; remainder = value % range
+            if remainder > max: return min + remainder-max
+            else: return remainder
+        self.bObject.pole_angle = wrap(-pi, pi, angle)
     
     def calc_pole_angle_pre(self, c, ik_bone):
         """
@@ -1272,7 +1284,7 @@ class LinkInverseKinematics(MantisLinkNode):
             dot_after=current_knee_direction.dot(knee_direction)
             if dot_after < dot_before: # they are somehow less aligned
                 prPurple("Mantis has gone down an unexpected code path. Please report this as a bug.")
-                angle = -angle; c.pole_angle = angle
+                angle = -angle; self.set_pole_angle(angle)
                 dg.update()
 
         # now we can do a bisect search to find the best value.
@@ -1287,6 +1299,7 @@ class LinkInverseKinematics(MantisLinkNode):
         upper_bounds = alt_angle if alt_angle > angle else angle
         lower_bounds = alt_angle if alt_angle < angle else angle
         i=0
+
         while ( True ):
             if (i>=max_iterations):
                 prOrange(f"IK Pole Angle Set reached max iterations of {i} in {time()-start_time} seconds")
@@ -1296,7 +1309,7 @@ class LinkInverseKinematics(MantisLinkNode):
                 break
             # get the center-point betweeen the bounds
             try_angle = lower_bounds + (upper_bounds-lower_bounds)/2
-            c.pole_angle = try_angle; dg.update()
+            self.set_pole_angle(try_angle); dg.update()
             error=signed_angle((base_ik_bone.tail-center_point), knee_direction, ik_axis)
             if error>0: upper_bounds=try_angle
             if error<0: lower_bounds=try_angle
@@ -1322,7 +1335,7 @@ class LinkInverseKinematics(MantisLinkNode):
             # if not isinstance(my_xf, xFormBone):
             #     raise GraphError(f"ERROR: Pole Target must be ")
             # if c.target != 
-            c.pole_angle = self.calc_pole_angle_pre(c, ik_bone)
+            self.set_pole_angle(self.calc_pole_angle_pre(c, ik_bone))
 
 
         props_sockets = {