--- /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)}
+""")
projects / carveJwlIkooP6JGAAIwe30JlM.git / blobdiff