ik_emulate.py

   1 import bpy
   2 from math import *
   3
   4 def v2_sub( a, b, d ):
   5    d[0] = a[0]-b[0]
   6    d[1] = a[1]-b[1]
   7
   8 def v3_sub( a, b, d ):
   9    d[0] = a[0]-b[0]
  10    d[1] = a[1]-b[1]
  11    d[2] = a[2]-b[2]
  12
  13 def v3_cross( a, b, d ):
  14    d[0] = a[1]*b[2] - a[2]*b[1]
  15    d[1] = a[2]*b[0] - a[0]*b[2]
  16    d[2] = a[0]*b[1] - a[1]*b[0]
  17
  18 def v3_muladds( a, b, s, d ):
  19    d[0] = a[0]+b[0]*s
  20    d[1] = a[1]+b[1]*s
  21    d[2] = a[2]+b[2]*s
  22
  23 def v3_muls( a, s, d ):
  24    d[0] = a[0]*s
  25    d[1] = a[1]*s
  26    d[2] = a[2]*s
  27
  28 def v3_copy( a, b ):
  29    b[0] = a[0]
  30    b[1] = a[1]
  31    b[2] = a[2]
  32
  33 def v3_zero( a ):
  34    a[0] = 0.0
  35    a[1] = 0.0
  36    a[2] = 0.0
  37
  38 def v3_negate( a, b ):
  39    b[0] = -a[0]
  40    b[1] = -a[1]
  41    b[2] = -a[2]
  42
  43 def v3_dot( a, b ):
  44    return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]
  45
  46 def v2_dot( a, b ):
  47    return a[0]*b[0] + a[1]*b[1]
  48
  49 def v2_length2( a ):
  50    return v2_dot( a, a )
  51
  52 def v3_length2( a ):
  53    return v3_dot( a, a )
  54
  55 def v3_length( a ):
  56    return sqrt( v3_length2(a) )
  57
  58 def v2_length( a ):
  59    return sqrt( v2_length2(a) )
  60
  61 def v3_normalize( a ):
  62    v3_muls( a, 1.0 / v3_length( a ), a )
  63
  64 def m3x3_identity( a ):
  65    a[0][0] = 1.0
  66    a[0][1] = 0.0
  67    a[0][2] = 0.0
  68    a[1][0] = 0.0
  69    a[1][1] = 1.0
  70    a[1][2] = 0.0
  71    a[2][0] = 0.0
  72    a[2][1] = 0.0
  73    a[2][2] = 1.0
  74
  75 def m4x3_mul( a, b, d ):
  76    a00 = a[0][0]
  77    a01 = a[0][1]
  78    a02 = a[0][2]
  79    a10 = a[1][0]
  80    a11 = a[1][1]
  81    a12 = a[1][2]
  82    a20 = a[2][0]
  83    a21 = a[2][1]
  84    a22 = a[2][2]
  85    a30 = a[3][0]
  86    a31 = a[3][1]
  87    a32 = a[3][2]
  88    b00 = b[0][0]
  89    b01 = b[0][1]
  90    b02 = b[0][2]
  91    b10 = b[1][0]
  92    b11 = b[1][1]
  93    b12 = b[1][2]
  94    b20 = b[2][0]
  95    b21 = b[2][1]
  96    b22 = b[2][2]
  97    b30 = b[3][0]
  98    b31 = b[3][1]
  99    b32 = b[3][2]
 100
 101    d[0][0] = a00*b00 + a10*b01 + a20*b02
 102    d[0][1] = a01*b00 + a11*b01 + a21*b02
 103    d[0][2] = a02*b00 + a12*b01 + a22*b02
 104    d[1][0] = a00*b10 + a10*b11 + a20*b12
 105    d[1][1] = a01*b10 + a11*b11 + a21*b12
 106    d[1][2] = a02*b10 + a12*b11 + a22*b12
 107    d[2][0] = a00*b20 + a10*b21 + a20*b22
 108    d[2][1] = a01*b20 + a11*b21 + a21*b22
 109    d[2][2] = a02*b20 + a12*b21 + a22*b22
 110    d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30
 111    d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31
 112    d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32
 113
 114 def m4x3_mulv( m, v, d ):
 115    res = [0,0,0]
 116    res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0]
 117    res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1]
 118    res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2]
 119    v3_copy( res, d )
 120
 121 def vg_clampf( a, minf, maxf ):
 122    return max( min(maxf,a), minf )
 123
 124 class ik_basic_t():
 125    def __init__(_):
 126       _.base = [0,0,0]
 127       _.pole = [0,0,0]
 128       _.end = [0,0,0]
 129       _.l1 = 0
 130       _.l2 = 0
 131
 132 k_ikX = 0
 133 k_iknX = 1
 134 k_ikY = 2
 135 k_iknY = 3
 136 k_ikZ = 4
 137 k_iknZ = 5
 138
 139 def ik_track_to( m, pos, target, axis, fwd, up ):
 140    dirr = [0,0,0]
 141    other = [0,0,0]
 142    v3_sub( target, pos, dirr )
 143    v3_normalize(dirr)
 144    v3_cross(dirr,axis,other)
 145
 146    if fwd == k_ikX and up == k_ikY:
 147       v3_copy( axis, m[0] )
 148       v3_copy( dirr, m[1] )
 149       v3_copy( other, m[2] )
 150    if fwd == k_ikY and up == k_ikX:
 151       v3_negate( axis, m[2] )
 152       v3_copy( dirr, m[1] )
 153       v3_negate( other, m[0] )
 154    if fwd == k_ikY and up == k_iknX:
 155       v3_negate( axis, m[2] )
 156       v3_negate( dirr, m[1] )
 157       v3_copy( other, m[0] )
 158    if fwd == k_ikZ and up == k_ikY:
 159       v3_copy( axis, m[1] )
 160       v3_copy( dirr, m[2] )
 161       v3_negate( other, m[0] )
 162    if fwd == k_iknZ and up == k_ikY:
 163       v3_negate( axis, m[1] )
 164       v3_negate( dirr, m[2] )
 165       v3_negate( other, m[0] )
 166    v3_copy( pos, m[3] )
 167
 168 def ik_basic( ik, m1, m2, fwd, up ):
 169    v0 = [0,0,0]
 170    v1 = [0,0,0]
 171    axis = [0,0,0]
 172
 173    v3_sub( ik.base, ik.pole, v0 )
 174    v3_sub( ik.end, ik.pole, v1 )
 175
 176    v3_cross( v0, v1, axis )
 177    v3_normalize( v0 )
 178    v3_normalize( axis )
 179    v3_cross( axis, v0, v1 )
 180
 181    base = [ v3_dot(v0,ik.base), v3_dot(v1,ik.base) ]
 182    end = [ v3_dot(v0,ik.end), v3_dot(v1,ik.end) ]
 183    knee = [0,0]
 184    delta = [0,0]
 185
 186    v2_sub( end, base, delta )
 187
 188    d = vg_clampf( v2_length(delta), abs(ik.l1-ik.l2), ik.l1+ik.l2-0.00001)
 189    c = acos( (ik.l1*ik.l1 + d*d - ik.l2*ik.l2) / (2.0*ik.l1*d) )
 190    rot = atan2( delta[1], delta[0] ) + c - 3.14159265/2.0
 191
 192    knee[0] = sin(-rot) * ik.l1
 193    knee[1] = cos(-rot) * ik.l1
 194
 195    world_knee = [0,0,0]
 196    v3_muladds( ik.base, v0, knee[0], world_knee )
 197    v3_muladds( world_knee, v1, knee[1], world_knee )
 198
 199    ik_track_to( m1, ik.base, world_knee, axis, fwd, up )
 200    ik_track_to( m2, world_knee, ik.end, axis, fwd, up )
 201
 202 def get_location_obj( obj, v ):
 203    v[0] =  obj.location[0]
 204    v[1] =  obj.location[2]
 205    v[2] = -obj.location[1]
 206
 207 def get_location( obj, v ):
 208    v[0] =  obj.matrix_world.translation[0]
 209    v[1] =  obj.matrix_world.translation[2]
 210    v[2] = -obj.matrix_world.translation[1]
 211
 212 def matrix_ready( a ):
 213    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]]
 214    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]]
 215    m4x3_mul( a, b2gl, a )
 216    m4x3_mul( fixer, a, a )
 217
 218    a[0][3] = 0.0
 219    a[1][3] = 0.0
 220    a[2][3] = 0.0
 221    a[3][3] = 1.0
 222
 223 def make_offset( n1, n2, v ):
 224    a = [0,0,0]
 225    b = [0,0,0]
 226
 227    get_location_obj( bpy.data.objects[n1], a )
 228    get_location_obj( bpy.data.objects[n2], b )
 229
 230    v3_sub( b, a, v )
 231
 232 def get_dist( n1, n2 ):
 233    c = [0,0,0]
 234    make_offset( n1, n2, c )
 235    return v3_length( c )
 236
 237 # Measure distances
 238 ik_body = ik_basic_t()
 239 ik_body.l1 = get_dist( "ch_default_body0", "ch_default_body1" )
 240 ik_body.l2 = get_dist( "ch_default_body1", "ch_default_neck" )
 241
 242 ik_arm_l = ik_basic_t()
 243 ik_arm_l.l1 = get_dist( "ch_default_arm_l0", "ch_default_arm_l1" )
 244 ik_arm_l.l2 = get_dist( "ch_default_arm_l1", "ch_default_hand_l" )
 245 ik_arm_r = ik_basic_t()
 246 ik_arm_r.l1 = get_dist( "ch_default_arm_r0", "ch_default_arm_r1" )
 247 ik_arm_r.l2 = get_dist( "ch_default_arm_r1", "ch_default_hand_r" )
 248
 249 ik_leg_l = ik_basic_t()
 250 ik_leg_l.l1 = get_dist( "ch_default_leg_l0", "ch_default_leg_l1" )
 251 ik_leg_l.l2 = get_dist( "ch_default_leg_l1", "ch_default_foot_l" )
 252 ik_leg_r = ik_basic_t()
 253 ik_leg_r.l1 = get_dist( "ch_default_leg_r0", "ch_default_leg_r1" )
 254 ik_leg_r.l2 = get_dist( "ch_default_leg_r1", "ch_default_foot_r" )
 255
 256 offs_arm_l = [0,0,0]
 257 offs_arm_r = [0,0,0]
 258 offs_leg_l = [0,0,0]
 259 offs_leg_r = [0,0,0]
 260 make_offset( "ch_default_body1", "ch_default_arm_l0", offs_arm_l )
 261 make_offset( "ch_default_body1", "ch_default_arm_r0", offs_arm_r )
 262 make_offset( "ch_default_body0", "ch_default_leg_l0", offs_leg_l )
 263 make_offset( "ch_default_body0", "ch_default_leg_r0", offs_leg_r )
 264
 265 # character_eval() clone
 266 #
 267 cam_pos = [0,0,0]
 268
 269 get_location( bpy.data.objects["BODY0"], ik_body.base )
 270 get_location( bpy.data.objects["BODY1"], ik_body.end )
 271 get_location( bpy.data.objects["POLE"], ik_body.pole )
 272 get_location( bpy.data.objects["FOOT_L"], ik_leg_l.end )
 273 get_location( bpy.data.objects["POLE_LEG_L"], ik_leg_l.pole )
 274 get_location( bpy.data.objects["FOOT_R"], ik_leg_r.end )
 275 get_location( bpy.data.objects["POLE_LEG_R"], ik_leg_r.pole )
 276 get_location( bpy.data.objects["HAND_L"], ik_arm_l.end )
 277 get_location( bpy.data.objects["POLE_ARM_L"], ik_arm_l.pole )
 278 get_location( bpy.data.objects["HAND_R"], ik_arm_r.end )
 279 get_location( bpy.data.objects["POLE_ARM_R"], ik_arm_r.pole )
 280 get_location( bpy.data.objects["CAMERA"], cam_pos )
 281
 282 m1 = [[0,0,0,0] for _ in range(4)]
 283 m2 = [[0,0,0,0] for _ in range(4)]
 284
 285 ik_basic( ik_body, m1, m2, k_ikY, k_ikX )
 286
 287 m4x3_mulv( m2, offs_arm_l, ik_arm_l.base )
 288 m4x3_mulv( m2, offs_arm_r, ik_arm_r.base )
 289 m4x3_mulv( m1, offs_leg_l, ik_leg_l.base )
 290 m4x3_mulv( m1, offs_leg_r, ik_leg_r.base )
 291
 292 matrix_ready(m1)
 293 matrix_ready(m2)
 294 bpy.data.objects["_body0"].matrix_world = m1
 295 bpy.data.objects["_body1"].matrix_world = m2
 296
 297 # Arms and legs ik
 298 ik_basic( ik_arm_l, m1, m2, k_ikZ, k_ikY )
 299 matrix_ready(m1)
 300 matrix_ready(m2)
 301 bpy.data.objects["_arm_l0"].matrix_world = m1
 302 bpy.data.objects["_arm_l1"].matrix_world = m2
 303
 304 ik_basic( ik_arm_r, m1, m2, k_iknZ, k_ikY )
 305 matrix_ready(m1)
 306 matrix_ready(m2)
 307 bpy.data.objects["_arm_r0"].matrix_world = m1
 308 bpy.data.objects["_arm_r1"].matrix_world = m2
 309
 310 ik_basic( ik_leg_l, m1, m2, k_ikY, k_iknX )
 311 matrix_ready(m1)
 312 matrix_ready(m2)
 313 bpy.data.objects["_leg_l0"].matrix_world = m1
 314 bpy.data.objects["_leg_l1"].matrix_world = m2
 315
 316 ik_basic( ik_leg_r, m1, m2, k_ikY, k_iknX )
 317 matrix_ready(m1)
 318 matrix_ready(m2)
 319 bpy.data.objects["_leg_r0"].matrix_world = m1
 320 bpy.data.objects["_leg_r1"].matrix_world = m2
 321
 322 def strv3( v ):
 323    return '{'+ F"{v[0]:.4f}f, {v[1]:.4f}f, {v[2]:.4f}f" + '}'
 324
 325 print( F"""
 326   .b0 =  {strv3(ik_body.base)},
 327   .b1 =  {strv3(ik_body.end)},
 328   .p =   {strv3(ik_body.pole)},
 329   .fr =  {strv3(ik_leg_r.end)},
 330   .fl =  {strv3(ik_leg_l.end)},
 331   .pl =  {strv3(ik_leg_l.pole)},
 332   .pr =  {strv3(ik_leg_r.pole)},
 333   .hl =  {strv3(ik_arm_l.end)},
 334   .hr =  {strv3(ik_arm_r.end)},
 335   .apl = {strv3(ik_arm_l.pole)},
 336   .apr = {strv3(ik_arm_r.pole)},
 337   .cam = {strv3(cam_pos)}
 338 """)