X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;ds=sidebyside;f=ik_emulate.py;fp=ik_emulate.py;h=0ee3caedcb88e58db594d5175b94222b156f619f;hb=8dfd1063311df86f24fbad2e221ccc0734695e74;hp=0000000000000000000000000000000000000000;hpb=a1a05787ada52089f30c533fb26b745554c07512;p=carveJwlIkooP6JGAAIwe30JlM.git

diff --git a/ik_emulate.py b/ik_emulate.py
new file mode 100644
index 0000000..0ee3cae
--- /dev/null
+++ b/ik_emulate.py
@@ -0,0 +1,338 @@
+import bpy
+from math import *
+
+def v2_sub( a, b, d ):
+   d[0] = a[0]-b[0]
+   d[1] = a[1]-b[1]
+
+def v3_sub( a, b, d ):
+   d[0] = a[0]-b[0]
+   d[1] = a[1]-b[1]
+   d[2] = a[2]-b[2]
+
+def v3_cross( a, b, d ):
+   d[0] = a[1]*b[2] - a[2]*b[1]
+   d[1] = a[2]*b[0] - a[0]*b[2]
+   d[2] = a[0]*b[1] - a[1]*b[0]
+
+def v3_muladds( a, b, s, d ):
+   d[0] = a[0]+b[0]*s
+   d[1] = a[1]+b[1]*s
+   d[2] = a[2]+b[2]*s
+
+def v3_muls( a, s, d ):
+   d[0] = a[0]*s
+   d[1] = a[1]*s
+   d[2] = a[2]*s
+
+def v3_copy( a, b ):
+   b[0] = a[0]
+   b[1] = a[1]
+   b[2] = a[2]
+
+def v3_zero( a ):
+   a[0] = 0.0
+   a[1] = 0.0
+   a[2] = 0.0
+
+def v3_negate( a, b ):
+   b[0] = -a[0]
+   b[1] = -a[1]
+   b[2] = -a[2]
+
+def v3_dot( a, b ):
+   return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]
+
+def v2_dot( a, b ):
+   return a[0]*b[0] + a[1]*b[1]
+
+def v2_length2( a ):
+   return v2_dot( a, a )
+
+def v3_length2( a ):
+   return v3_dot( a, a )
+
+def v3_length( a ):
+   return sqrt( v3_length2(a) )
+
+def v2_length( a ):
+   return sqrt( v2_length2(a) )
+
+def v3_normalize( a ):
+   v3_muls( a, 1.0 / v3_length( a ), a )
+
+def m3x3_identity( a ):
+   a[0][0] = 1.0
+   a[0][1] = 0.0
+   a[0][2] = 0.0
+   a[1][0] = 0.0
+   a[1][1] = 1.0
+   a[1][2] = 0.0
+   a[2][0] = 0.0
+   a[2][1] = 0.0
+   a[2][2] = 1.0
+
+def m4x3_mul( a, b, d ):
+   a00 = a[0][0]
+   a01 = a[0][1]
+   a02 = a[0][2]
+   a10 = a[1][0]
+   a11 = a[1][1]
+   a12 = a[1][2]
+   a20 = a[2][0]
+   a21 = a[2][1]
+   a22 = a[2][2]
+   a30 = a[3][0]
+   a31 = a[3][1]
+   a32 = a[3][2]
+   b00 = b[0][0]
+   b01 = b[0][1]
+   b02 = b[0][2]
+   b10 = b[1][0]
+   b11 = b[1][1]
+   b12 = b[1][2]
+   b20 = b[2][0]
+   b21 = b[2][1]
+   b22 = b[2][2]
+   b30 = b[3][0]
+   b31 = b[3][1]
+   b32 = b[3][2]
+
+   d[0][0] = a00*b00 + a10*b01 + a20*b02
+   d[0][1] = a01*b00 + a11*b01 + a21*b02
+   d[0][2] = a02*b00 + a12*b01 + a22*b02
+   d[1][0] = a00*b10 + a10*b11 + a20*b12
+   d[1][1] = a01*b10 + a11*b11 + a21*b12
+   d[1][2] = a02*b10 + a12*b11 + a22*b12
+   d[2][0] = a00*b20 + a10*b21 + a20*b22
+   d[2][1] = a01*b20 + a11*b21 + a21*b22
+   d[2][2] = a02*b20 + a12*b21 + a22*b22
+   d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30
+   d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31
+   d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32
+
+def m4x3_mulv( m, v, d ):
+   res = [0,0,0]
+   res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0]
+   res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1]
+   res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2]
+   v3_copy( res, d )
+
+def vg_clampf( a, minf, maxf ):
+   return max( min(maxf,a), minf )
+
+class ik_basic_t():
+   def __init__(_):
+      _.base = [0,0,0]
+      _.pole = [0,0,0]
+      _.end = [0,0,0]
+      _.l1 = 0
+      _.l2 = 0
+
+k_ikX = 0
+k_iknX = 1
+k_ikY = 2
+k_iknY = 3
+k_ikZ = 4
+k_iknZ = 5
+
+def ik_track_to( m, pos, target, axis, fwd, up ):
+   dirr = [0,0,0]
+   other = [0,0,0]
+   v3_sub( target, pos, dirr )
+   v3_normalize(dirr)
+   v3_cross(dirr,axis,other)
+
+   if fwd == k_ikX and up == k_ikY:
+      v3_copy( axis, m[0] )
+      v3_copy( dirr, m[1] )
+      v3_copy( other, m[2] )
+   if fwd == k_ikY and up == k_ikX:
+      v3_negate( axis, m[2] )
+      v3_copy( dirr, m[1] )
+      v3_negate( other, m[0] )
+   if fwd == k_ikY and up == k_iknX:
+      v3_negate( axis, m[2] )
+      v3_negate( dirr, m[1] )
+      v3_copy( other, m[0] )
+   if fwd == k_ikZ and up == k_ikY:
+      v3_copy( axis, m[1] )
+      v3_copy( dirr, m[2] )
+      v3_negate( other, m[0] )
+   if fwd == k_iknZ and up == k_ikY:
+      v3_negate( axis, m[1] )
+      v3_negate( dirr, m[2] )
+      v3_negate( other, m[0] )
+   v3_copy( pos, m[3] )
+
+def ik_basic( ik, m1, m2, fwd, up ):
+   v0 = [0,0,0]
+   v1 = [0,0,0]
+   axis = [0,0,0]
+
+   v3_sub( ik.base, ik.pole, v0 )
+   v3_sub( ik.end, ik.pole, v1 )
+
+   v3_cross( v0, v1, axis )
+   v3_normalize( v0 )
+   v3_normalize( axis )
+   v3_cross( axis, v0, v1 )
+
+   base = [ v3_dot(v0,ik.base), v3_dot(v1,ik.base) ]
+   end = [ v3_dot(v0,ik.end), v3_dot(v1,ik.end) ]
+   knee = [0,0]
+   delta = [0,0]
+   
+   v2_sub( end, base, delta )
+
+   d = vg_clampf( v2_length(delta), abs(ik.l1-ik.l2), ik.l1+ik.l2-0.00001)
+   c = acos( (ik.l1*ik.l1 + d*d - ik.l2*ik.l2) / (2.0*ik.l1*d) )
+   rot = atan2( delta[1], delta[0] ) + c - 3.14159265/2.0
+
+   knee[0] = sin(-rot) * ik.l1
+   knee[1] = cos(-rot) * ik.l1
+
+   world_knee = [0,0,0]
+   v3_muladds( ik.base, v0, knee[0], world_knee )
+   v3_muladds( world_knee, v1, knee[1], world_knee )
+
+   ik_track_to( m1, ik.base, world_knee, axis, fwd, up )
+   ik_track_to( m2, world_knee, ik.end, axis, fwd, up )
+
+def get_location_obj( obj, v ):
+   v[0] =  obj.location[0]
+   v[1] =  obj.location[2]
+   v[2] = -obj.location[1]
+
+def get_location( obj, v ):
+   v[0] =  obj.matrix_world.translation[0]
+   v[1] =  obj.matrix_world.translation[2]
+   v[2] = -obj.matrix_world.translation[1]
+
+def matrix_ready( a ):
+   b2gl =  [[1.0,0.0,0.0],[0.0,0.0,-1.0],[0.0,1.0,0.0],[0.0,0.0,0.0]]
+   fixer = [[1.0,0.0,0.0],[0.0,0.0,1.0],[0.0,-1.0,0.0],[0.0,0.0,0.0]]
+   m4x3_mul( a, b2gl, a )
+   m4x3_mul( fixer, a, a )
+
+   a[0][3] = 0.0
+   a[1][3] = 0.0
+   a[2][3] = 0.0
+   a[3][3] = 1.0
+
+def make_offset( n1, n2, v ):
+   a = [0,0,0]
+   b = [0,0,0]
+
+   get_location_obj( bpy.data.objects[n1], a )
+   get_location_obj( bpy.data.objects[n2], b )
+
+   v3_sub( b, a, v )
+
+def get_dist( n1, n2 ):
+   c = [0,0,0]
+   make_offset( n1, n2, c )
+   return v3_length( c )
+
+# Measure distances
+ik_body = ik_basic_t()
+ik_body.l1 = get_dist( "ch_default_body0", "ch_default_body1" )
+ik_body.l2 = get_dist( "ch_default_body1", "ch_default_neck" )
+
+ik_arm_l = ik_basic_t()
+ik_arm_l.l1 = get_dist( "ch_default_arm_l0", "ch_default_arm_l1" )
+ik_arm_l.l2 = get_dist( "ch_default_arm_l1", "ch_default_hand_l" )
+ik_arm_r = ik_basic_t()
+ik_arm_r.l1 = get_dist( "ch_default_arm_r0", "ch_default_arm_r1" )
+ik_arm_r.l2 = get_dist( "ch_default_arm_r1", "ch_default_hand_r" )
+
+ik_leg_l = ik_basic_t()
+ik_leg_l.l1 = get_dist( "ch_default_leg_l0", "ch_default_leg_l1" )
+ik_leg_l.l2 = get_dist( "ch_default_leg_l1", "ch_default_foot_l" )
+ik_leg_r = ik_basic_t()
+ik_leg_r.l1 = get_dist( "ch_default_leg_r0", "ch_default_leg_r1" )
+ik_leg_r.l2 = get_dist( "ch_default_leg_r1", "ch_default_foot_r" )
+
+offs_arm_l = [0,0,0]
+offs_arm_r = [0,0,0]
+offs_leg_l = [0,0,0]
+offs_leg_r = [0,0,0]
+make_offset( "ch_default_body1", "ch_default_arm_l0", offs_arm_l )
+make_offset( "ch_default_body1", "ch_default_arm_r0", offs_arm_r )
+make_offset( "ch_default_body0", "ch_default_leg_l0", offs_leg_l )
+make_offset( "ch_default_body0", "ch_default_leg_r0", offs_leg_r )
+
+# character_eval() clone
+#
+cam_pos = [0,0,0]
+
+get_location( bpy.data.objects["BODY0"], ik_body.base )
+get_location( bpy.data.objects["BODY1"], ik_body.end )
+get_location( bpy.data.objects["POLE"], ik_body.pole )
+get_location( bpy.data.objects["FOOT_L"], ik_leg_l.end )
+get_location( bpy.data.objects["POLE_LEG_L"], ik_leg_l.pole )
+get_location( bpy.data.objects["FOOT_R"], ik_leg_r.end )
+get_location( bpy.data.objects["POLE_LEG_R"], ik_leg_r.pole )
+get_location( bpy.data.objects["HAND_L"], ik_arm_l.end )
+get_location( bpy.data.objects["POLE_ARM_L"], ik_arm_l.pole )
+get_location( bpy.data.objects["HAND_R"], ik_arm_r.end )
+get_location( bpy.data.objects["POLE_ARM_R"], ik_arm_r.pole )
+get_location( bpy.data.objects["CAMERA"], cam_pos )
+
+m1 = [[0,0,0,0] for _ in range(4)]
+m2 = [[0,0,0,0] for _ in range(4)]
+
+ik_basic( ik_body, m1, m2, k_ikY, k_ikX )
+
+m4x3_mulv( m2, offs_arm_l, ik_arm_l.base )
+m4x3_mulv( m2, offs_arm_r, ik_arm_r.base )
+m4x3_mulv( m1, offs_leg_l, ik_leg_l.base )
+m4x3_mulv( m1, offs_leg_r, ik_leg_r.base )
+
+matrix_ready(m1)
+matrix_ready(m2)
+bpy.data.objects["_body0"].matrix_world = m1
+bpy.data.objects["_body1"].matrix_world = m2
+
+# Arms and legs ik
+ik_basic( ik_arm_l, m1, m2, k_ikZ, k_ikY )
+matrix_ready(m1)
+matrix_ready(m2)
+bpy.data.objects["_arm_l0"].matrix_world = m1
+bpy.data.objects["_arm_l1"].matrix_world = m2
+
+ik_basic( ik_arm_r, m1, m2, k_iknZ, k_ikY )
+matrix_ready(m1)
+matrix_ready(m2)
+bpy.data.objects["_arm_r0"].matrix_world = m1
+bpy.data.objects["_arm_r1"].matrix_world = m2
+
+ik_basic( ik_leg_l, m1, m2, k_ikY, k_iknX )
+matrix_ready(m1)
+matrix_ready(m2)
+bpy.data.objects["_leg_l0"].matrix_world = m1
+bpy.data.objects["_leg_l1"].matrix_world = m2
+
+ik_basic( ik_leg_r, m1, m2, k_ikY, k_iknX )
+matrix_ready(m1)
+matrix_ready(m2)
+bpy.data.objects["_leg_r0"].matrix_world = m1
+bpy.data.objects["_leg_r1"].matrix_world = m2
+
+def strv3( v ):
+   return '{'+ F"{v[0]:.4f}f, {v[1]:.4f}f, {v[2]:.4f}f" + '}'
+
+print( F"""
+  .b0 =  {strv3(ik_body.base)},
+  .b1 =  {strv3(ik_body.end)},
+  .p =   {strv3(ik_body.pole)},
+  .fr =  {strv3(ik_leg_r.end)},
+  .fl =  {strv3(ik_leg_l.end)},
+  .pl =  {strv3(ik_leg_l.pole)},
+  .pr =  {strv3(ik_leg_r.pole)},
+  .hl =  {strv3(ik_arm_l.end)},
+  .hr =  {strv3(ik_arm_r.end)},
+  .apl = {strv3(ik_arm_l.pole)},
+  .apr = {strv3(ik_arm_r.pole)},
+  .cam = {strv3(cam_pos)}
+""")