+++ /dev/null
-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)}
-""")