0ee3caedcb88e58db594d5175b94222b156f619f
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]
18 def v3_muladds( a
, b
, s
, d
):
23 def v3_muls( a
, s
, d
):
38 def v3_negate( a
, b
):
44 return a
[0]*b
[0] + a
[1]*b
[1] + a
[2]*b
[2]
47 return a
[0]*b
[0] + a
[1]*b
[1]
56 return sqrt( v3_length2(a
) )
59 return sqrt( v2_length2(a
) )
61 def v3_normalize( a
):
62 v3_muls( a
, 1.0 / v3_length( a
), a
)
64 def m3x3_identity( a
):
75 def m4x3_mul( a
, b
, d
):
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
114 def m4x3_mulv( m
, v
, d
):
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]
121 def vg_clampf( a
, minf
, maxf
):
122 return max( min(maxf
,a
), minf
)
139 def ik_track_to( m
, pos
, target
, axis
, fwd
, up
):
142 v3_sub( target
, pos
, dirr
)
144 v3_cross(dirr
,axis
,other
)
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] )
168 def ik_basic( ik
, m1
, m2
, fwd
, up
):
173 v3_sub( ik
.base
, ik
.pole
, v0
)
174 v3_sub( ik
.end
, ik
.pole
, v1
)
176 v3_cross( v0
, v1
, axis
)
179 v3_cross( axis
, v0
, v1
)
181 base
= [ v3_dot(v0
,ik
.base
), v3_dot(v1
,ik
.base
) ]
182 end
= [ v3_dot(v0
,ik
.end
), v3_dot(v1
,ik
.end
) ]
186 v2_sub( end
, base
, delta
)
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
192 knee
[0] = sin(-rot
) * ik
.l1
193 knee
[1] = cos(-rot
) * ik
.l1
196 v3_muladds( ik
.base
, v0
, knee
[0], world_knee
)
197 v3_muladds( world_knee
, v1
, knee
[1], world_knee
)
199 ik_track_to( m1
, ik
.base
, world_knee
, axis
, fwd
, up
)
200 ik_track_to( m2
, world_knee
, ik
.end
, axis
, fwd
, up
)
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]
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]
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
)
223 def make_offset( n1
, n2
, v
):
227 get_location_obj( bpy
.data
.objects
[n1
], a
)
228 get_location_obj( bpy
.data
.objects
[n2
], b
)
232 def get_dist( n1
, n2
):
234 make_offset( n1
, n2
, c
)
235 return v3_length( c
)
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" )
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" )
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" )
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
)
265 # character_eval() clone
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
)
282 m1
= [[0,0,0,0] for _
in range(4)]
283 m2
= [[0,0,0,0] for _
in range(4)]
285 ik_basic( ik_body
, m1
, m2
, k_ikY
, k_ikX
)
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
)
294 bpy
.data
.objects
["_body0"].matrix_world
= m1
295 bpy
.data
.objects
["_body1"].matrix_world
= m2
298 ik_basic( ik_arm_l
, m1
, m2
, k_ikZ
, k_ikY
)
301 bpy
.data
.objects
["_arm_l0"].matrix_world
= m1
302 bpy
.data
.objects
["_arm_l1"].matrix_world
= m2
304 ik_basic( ik_arm_r
, m1
, m2
, k_iknZ
, k_ikY
)
307 bpy
.data
.objects
["_arm_r0"].matrix_world
= m1
308 bpy
.data
.objects
["_arm_r1"].matrix_world
= m2
310 ik_basic( ik_leg_l
, m1
, m2
, k_ikY
, k_iknX
)
313 bpy
.data
.objects
["_leg_l0"].matrix_world
= m1
314 bpy
.data
.objects
["_leg_l1"].matrix_world
= m2
316 ik_basic( ik_leg_r
, m1
, m2
, k_ikY
, k_iknX
)
319 bpy
.data
.objects
["_leg_r0"].matrix_world
= m1
320 bpy
.data
.objects
["_leg_r1"].matrix_world
= m2
323 return '{'+ F
"{v[0]:.4f}f, {v[1]:.4f}f, {v[2]:.4f}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)}