0a1787cb58c521fe2e49db7b14f2b62cd798e598
[blender-staging.git] / source / blender / blenkernel / intern / armature.c
1 /**
2  * $Id$
3  *
4  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
5  *
6  * This program is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * as published by the Free Software Foundation; either version 2
9  * of the License, or (at your option) any later version. 
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software Foundation,
18  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
19  *
20  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
21  * All rights reserved.
22  *
23  * Contributor(s): Full recode, Ton Roosendaal, Crete 2005
24  *
25  * ***** END GPL/BL DUAL LICENSE BLOCK *****
26  */
27
28 #include <ctype.h>
29 #include <stdlib.h>
30 #include <math.h>
31 #include <string.h>
32 #include <stdio.h>
33 #include "MEM_guardedalloc.h"
34
35 #include "nla.h"
36
37 #include "BLI_arithb.h"
38 #include "BLI_blenlib.h"
39
40 #include "DNA_armature_types.h"
41 #include "DNA_action_types.h"
42 #include "DNA_constraint_types.h"
43 #include "DNA_mesh_types.h"
44 #include "DNA_lattice_types.h"
45 #include "DNA_meshdata_types.h"
46 #include "DNA_nla_types.h"
47 #include "DNA_object_types.h"
48 #include "DNA_scene_types.h"
49 #include "DNA_view3d_types.h"
50
51 #include "BKE_armature.h"
52 #include "BKE_action.h"
53 #include "BKE_blender.h"
54 #include "BKE_constraint.h"
55 #include "BKE_curve.h"
56 #include "BKE_deform.h"
57 #include "BKE_depsgraph.h"
58 #include "BKE_DerivedMesh.h"
59 #include "BKE_displist.h"
60 #include "BKE_global.h"
61 #include "BKE_library.h"
62 #include "BKE_lattice.h"
63 #include "BKE_main.h"
64 #include "BKE_object.h"
65 #include "BKE_object.h"
66 #include "BKE_utildefines.h"
67
68 #include "BIF_editdeform.h"
69
70 #include "IK_solver.h"
71
72 #ifdef HAVE_CONFIG_H
73 #include <config.h>
74 #endif
75
76 /*      **************** Generic Functions, data level *************** */
77
78 bArmature *get_armature(Object *ob)
79 {
80         if(ob==NULL) return NULL;
81         if(ob->type==OB_ARMATURE) return ob->data;
82         else return NULL;
83 }
84
85 bArmature *add_armature()
86 {
87         bArmature *arm;
88         
89         arm= alloc_libblock (&G.main->armature, ID_AR, "Armature");
90         arm->deformflag = ARM_DEF_VGROUP|ARM_DEF_ENVELOPE;
91         arm->layer= 1;
92         return arm;
93 }
94
95
96 void free_boneChildren(Bone *bone)
97
98         Bone *child;
99         
100         if (bone) {
101                 
102                 child=bone->childbase.first;
103                 if (child){
104                         while (child){
105                                 free_boneChildren (child);
106                                 child=child->next;
107                         }
108                         BLI_freelistN (&bone->childbase);
109                 }
110         }
111 }
112
113 void free_bones (bArmature *arm)
114 {
115         Bone *bone;
116         /*      Free children (if any)  */
117         bone= arm->bonebase.first;
118         if (bone) {
119                 while (bone){
120                         free_boneChildren (bone);
121                         bone=bone->next;
122                 }
123         }
124         
125         
126         BLI_freelistN(&arm->bonebase);
127 }
128
129 void free_armature(bArmature *arm)
130 {
131         if (arm) {
132                 /*              unlink_armature(arm);*/
133                 free_bones(arm);
134         }
135 }
136
137 void make_local_armature(bArmature *arm)
138 {
139         int local=0, lib=0;
140         Object *ob;
141         bArmature *newArm;
142         
143         if (arm->id.lib==0)
144                 return;
145         if (arm->id.us==1) {
146                 arm->id.lib= 0;
147                 arm->id.flag= LIB_LOCAL;
148                 new_id(0, (ID*)arm, 0);
149                 return;
150         }
151         
152         if(local && lib==0) {
153                 arm->id.lib= 0;
154                 arm->id.flag= LIB_LOCAL;
155                 new_id(0, (ID *)arm, 0);
156         }
157         else if(local && lib) {
158                 newArm= copy_armature(arm);
159                 newArm->id.us= 0;
160                 
161                 ob= G.main->object.first;
162                 while(ob) {
163                         if(ob->data==arm) {
164                                 
165                                 if(ob->id.lib==0) {
166                                         ob->data= newArm;
167                                         newArm->id.us++;
168                                         arm->id.us--;
169                                 }
170                         }
171                         ob= ob->id.next;
172                 }
173         }
174 }
175
176 static void     copy_bonechildren (Bone* newBone, Bone* oldBone)
177 {
178         Bone    *curBone, *newChildBone;
179         
180         /*      Copy this bone's list*/
181         duplicatelist (&newBone->childbase, &oldBone->childbase);
182         
183         /*      For each child in the list, update it's children*/
184         newChildBone=newBone->childbase.first;
185         for (curBone=oldBone->childbase.first;curBone;curBone=curBone->next){
186                 newChildBone->parent=newBone;
187                 copy_bonechildren(newChildBone,curBone);
188                 newChildBone=newChildBone->next;
189         }
190 }
191
192 bArmature *copy_armature(bArmature *arm)
193 {
194         bArmature *newArm;
195         Bone            *oldBone, *newBone;
196         
197         newArm= copy_libblock (arm);
198         duplicatelist(&newArm->bonebase, &arm->bonebase);
199         
200         /*      Duplicate the childrens' lists*/
201         newBone=newArm->bonebase.first;
202         for (oldBone=arm->bonebase.first;oldBone;oldBone=oldBone->next){
203                 newBone->parent=NULL;
204                 copy_bonechildren (newBone, oldBone);
205                 newBone=newBone->next;
206         };
207         
208         return newArm;
209 }
210
211 static Bone *get_named_bone_bonechildren (Bone *bone, const char *name)
212 {
213         Bone *curBone, *rbone;
214         
215         if (!strcmp (bone->name, name))
216                 return bone;
217         
218         for (curBone=bone->childbase.first; curBone; curBone=curBone->next){
219                 rbone=get_named_bone_bonechildren (curBone, name);
220                 if (rbone)
221                         return rbone;
222         }
223         
224         return NULL;
225 }
226
227
228 Bone *get_named_bone (bArmature *arm, const char *name)
229 /*
230         Walk the list until the bone is found
231  */
232 {
233         Bone *bone=NULL, *curBone;
234         
235         if (!arm) return NULL;
236         
237         for (curBone=arm->bonebase.first; curBone; curBone=curBone->next){
238                 bone = get_named_bone_bonechildren (curBone, name);
239                 if (bone)
240                         return bone;
241         }
242         
243         return bone;
244 }
245
246
247 #define IS_SEPARATOR(a) (a=='.' || a==' ' || a=='-' || a=='_')
248
249 /* finds the best possible flipped name. For renaming; check for unique names afterwards */
250 /* if strip_number: removes number extensions */
251 void bone_flip_name (char *name, int strip_number)
252 {
253         int             len;
254         char    prefix[128]={""};       /* The part before the facing */
255         char    suffix[128]={""};       /* The part after the facing */
256         char    replace[128]={""};      /* The replacement string */
257         char    number[128]={""};       /* The number extension string */
258         char    *index=NULL;
259
260         len= strlen(name);
261         if(len<3) return;       // we don't do names like .R or .L
262
263         /* We first check the case with a .### extension, let's find the last period */
264         if(isdigit(name[len-1])) {
265                 index= strrchr(name, '.');      // last occurrance
266                 if (index && isdigit(index[1]) ) {              // doesnt handle case bone.1abc2 correct..., whatever!
267                         if(strip_number==0) 
268                                 strcpy(number, index);
269                         *index= 0;
270                         len= strlen(name);
271                 }
272         }
273
274         strcpy (prefix, name);
275
276         /* first case; separator . - _ with extensions r R l L  */
277         if( IS_SEPARATOR(name[len-2]) ) {
278                 switch(name[len-1]) {
279                         case 'l':
280                                 prefix[len-1]= 0;
281                                 strcpy(replace, "r");
282                                 break;
283                         case 'r':
284                                 prefix[len-1]= 0;
285                                 strcpy(replace, "l");
286                                 break;
287                         case 'L':
288                                 prefix[len-1]= 0;
289                                 strcpy(replace, "R");
290                                 break;
291                         case 'R':
292                                 prefix[len-1]= 0;
293                                 strcpy(replace, "L");
294                                 break;
295                 }
296         }
297         /* case; beginning with r R l L , with separator after it */
298         else if( IS_SEPARATOR(name[1]) ) {
299                 switch(name[0]) {
300                         case 'l':
301                                 strcpy(replace, "r");
302                                 strcpy(suffix, name+1);
303                                 prefix[0]= 0;
304                                 break;
305                         case 'r':
306                                 strcpy(replace, "l");
307                                 strcpy(suffix, name+1);
308                                 prefix[0]= 0;
309                                 break;
310                         case 'L':
311                                 strcpy(replace, "R");
312                                 strcpy(suffix, name+1);
313                                 prefix[0]= 0;
314                                 break;
315                         case 'R':
316                                 strcpy(replace, "L");
317                                 strcpy(suffix, name+1);
318                                 prefix[0]= 0;
319                                 break;
320                 }
321         }
322         else if(len > 5) {
323                 /* hrms, why test for a separator? lets do the rule 'ultimate left or right' */
324                 index = BLI_strcasestr(prefix, "right");
325                 if (index==prefix || index==prefix+len-5) {
326                         if(index[0]=='r') 
327                                 strcpy (replace, "left");
328                         else {
329                                 if(index[1]=='I') 
330                                         strcpy (replace, "LEFT");
331                                 else
332                                         strcpy (replace, "Left");
333                         }
334                         *index= 0;
335                         strcpy (suffix, index+5);
336                 }
337                 else {
338                         index = BLI_strcasestr(prefix, "left");
339                         if (index==prefix || index==prefix+len-4) {
340                                 if(index[0]=='l') 
341                                         strcpy (replace, "right");
342                                 else {
343                                         if(index[1]=='E') 
344                                                 strcpy (replace, "RIGHT");
345                                         else
346                                                 strcpy (replace, "Right");
347                                 }
348                                 *index= 0;
349                                 strcpy (suffix, index+4);
350                         }
351                 }               
352         }
353
354         sprintf (name, "%s%s%s%s", prefix, replace, suffix, number);
355 }
356
357
358 /* ************* B-Bone support ******************* */
359
360 #define MAX_BBONE_SUBDIV        32
361
362 /* data has MAX_BBONE_SUBDIV+1 interpolated points, will become desired amount with equal distances */
363 static void equalize_bezier(float *data, int desired)
364 {
365         float *fp, totdist, ddist, dist, fac1, fac2;
366         float pdist[MAX_BBONE_SUBDIV+1];
367         float temp[MAX_BBONE_SUBDIV+1][4];
368         int a, nr;
369         
370         pdist[0]= 0.0f;
371         for(a=0, fp= data; a<MAX_BBONE_SUBDIV; a++, fp+=4) {
372                 QUATCOPY(temp[a], fp);
373                 pdist[a+1]= pdist[a]+VecLenf(fp, fp+4);
374         }
375         /* do last point */
376         QUATCOPY(temp[a], fp);
377         totdist= pdist[a];
378         
379         /* go over distances and calculate new points */
380         ddist= totdist/((float)desired);
381         nr= 1;
382         for(a=1, fp= data+4; a<desired; a++, fp+=4) {
383                 
384                 dist= ((float)a)*ddist;
385                 
386                 /* we're looking for location (distance) 'dist' in the array */
387                 while((dist>= pdist[nr]) && nr<MAX_BBONE_SUBDIV) {
388                         nr++;
389                 }
390                 
391                 fac1= pdist[nr]- pdist[nr-1];
392                 fac2= pdist[nr]-dist;
393                 fac1= fac2/fac1;
394                 fac2= 1.0f-fac1;
395                 
396                 fp[0]= fac1*temp[nr-1][0]+ fac2*temp[nr][0];
397                 fp[1]= fac1*temp[nr-1][1]+ fac2*temp[nr][1];
398                 fp[2]= fac1*temp[nr-1][2]+ fac2*temp[nr][2];
399                 fp[3]= fac1*temp[nr-1][3]+ fac2*temp[nr][3];
400         }
401         /* set last point, needed for orientation calculus */
402         QUATCOPY(fp, temp[MAX_BBONE_SUBDIV]);
403 }
404
405 /* returns pointer to static array, filled with desired amount of bone->segments elements */
406 /* this calculation is done  within unit bone space */
407 Mat4 *b_bone_spline_setup(bPoseChannel *pchan)
408 {
409         static Mat4 bbone_array[MAX_BBONE_SUBDIV];
410         bPoseChannel *next, *prev;
411         Bone *bone= pchan->bone;
412         float h1[3], h2[3], length, hlength1, hlength2, roll;
413         float mat3[3][3], imat[4][4];
414         float data[MAX_BBONE_SUBDIV+1][4], *fp;
415         int a;
416         
417         length= bone->length;
418         hlength1= bone->ease1*length*0.390464f;         // 0.5*sqrt(2)*kappa, the handle length for near-perfect circles
419         hlength2= bone->ease2*length*0.390464f;
420         
421         /* evaluate next and prev bones */
422         if(bone->flag & BONE_CONNECTED)
423                 prev= pchan->parent;
424         else
425                 prev= NULL;
426         
427         next= pchan->child;
428         
429         /* find the handle points, since this is inside bone space, the 
430                 first point = (0,0,0)
431                 last point =  (0, length, 0) */
432         
433         Mat4Invert(imat, pchan->pose_mat);
434         
435         if(prev) {
436                 /* transform previous point inside this bone space */
437                 VECCOPY(h1, prev->pose_head);
438                 Mat4MulVecfl(imat, h1);
439                 /* if previous bone is B-bone too, use average handle direction */
440                 if(prev->bone->segments>1) h1[1]-= length;
441                 Normalise(h1);
442                 VecMulf(h1, -hlength1);
443         }
444         else {
445                 h1[0]= 0.0f; h1[1]= hlength1; h1[2]= 0.0f;
446         }
447         if(next) {
448                 float difmat[4][4], result[3][3], imat3[3][3];
449                 
450                 /* transform next point inside this bone space */
451                 VECCOPY(h2, next->pose_tail);
452                 Mat4MulVecfl(imat, h2);
453                 /* if next bone is B-bone too, use average handle direction */
454                 if(next->bone->segments>1);
455                 else h2[1]-= length;
456                 
457                 /* find the next roll to interpolate as well */
458                 Mat4MulMat4(difmat, next->pose_mat, imat);
459                 Mat3CpyMat4(result, difmat);                            // the desired rotation at beginning of next bone
460                 
461                 vec_roll_to_mat3(h2, 0.0f, mat3);                       // the result of vec_roll without roll
462                 
463                 Mat3Inv(imat3, mat3);
464                 Mat3MulMat3(mat3, imat3, result);                       // the matrix transforming vec_roll to desired roll
465                 
466                 roll= atan2(mat3[2][0], mat3[2][2]);
467                 
468                 /* and only now negate handle */
469                 Normalise(h2);
470                 VecMulf(h2, -hlength2);
471                 
472         }
473         else {
474                 h2[0]= 0.0f; h2[1]= -hlength2; h2[2]= 0.0f;
475                 roll= 0.0;
476         }
477         
478         /* make curve */
479         if(bone->segments > MAX_BBONE_SUBDIV)
480                 bone->segments= MAX_BBONE_SUBDIV;
481         
482         forward_diff_bezier(0.0, h1[0],         h2[0],                  0.0,            data[0],        MAX_BBONE_SUBDIV, 4);
483         forward_diff_bezier(0.0, h1[1],         length + h2[1], length,         data[0]+1,      MAX_BBONE_SUBDIV, 4);
484         forward_diff_bezier(0.0, h1[2],         h2[2],                  0.0,            data[0]+2,      MAX_BBONE_SUBDIV, 4);
485         forward_diff_bezier(0.0, 0.390464f*roll, (1.0f-0.390464f)*roll, roll,   data[0]+3,      MAX_BBONE_SUBDIV, 4);
486         
487         equalize_bezier(data[0], bone->segments);       // note: does stride 4!
488         
489         /* make transformation matrices for the segments for drawing */
490         for(a=0, fp= data[0]; a<bone->segments; a++, fp+=4) {
491                 VecSubf(h1, fp+4, fp);
492                 vec_roll_to_mat3(h1, fp[3], mat3);              // fp[3] is roll
493                 Mat4CpyMat3(bbone_array[a].mat, mat3);
494                 VECCOPY(bbone_array[a].mat[3], fp);
495         }
496         
497         return bbone_array;
498 }
499
500 /* ************ Armature Deform ******************* */
501
502 static void pchan_b_bone_defmats(bPoseChannel *pchan)
503 {
504         Bone *bone= pchan->bone;
505         Mat4 *b_bone= b_bone_spline_setup(pchan);
506         Mat4 *b_bone_mats;
507         int a;
508         
509         pchan->b_bone_mats=b_bone_mats= MEM_mallocN((1+bone->segments)*sizeof(Mat4), "BBone defmats");
510         
511         /* first matrix is the inverse arm_mat, to bring points in local bone space */
512         Mat4Invert(b_bone_mats[0].mat, bone->arm_mat);
513         
514         /* then we multiply the bbone_mats with arm_mat */
515         for(a=0; a<bone->segments; a++) {
516                 Mat4MulMat4(b_bone_mats[a+1].mat, b_bone[a].mat, bone->arm_mat);
517         }
518 }
519
520 static void b_bone_deform(bPoseChannel *pchan, Bone *bone, float *defpos)
521 {
522         Mat4 *b_bone= pchan->b_bone_mats;
523         float segment;
524         int a;
525         
526         /* need to transform defpos back to bonespace */
527         Mat4MulVecfl(b_bone[0].mat, defpos);
528         
529         /* now calculate which of the b_bones are deforming this */
530         segment= bone->length/((float)bone->segments);
531         a= (int) (defpos[1]/segment);
532         
533         /* note; by clamping it extends deform at endpoints, goes best with straight joints in restpos. */
534         CLAMP(a, 0, bone->segments-1);
535
536         /* since the bbone mats translate from (0.0.0) on the curve, we subtract */
537         defpos[1] -= ((float)a)*segment;
538         
539         Mat4MulVecfl(b_bone[a+1].mat, defpos);
540 }
541
542 /* using vec with dist to bone b1 - b2 */
543 float distfactor_to_bone (float vec[3], float b1[3], float b2[3], float rad1, float rad2, float rdist)
544 {
545         float dist=0.0f; 
546         float bdelta[3];
547         float pdelta[3];
548         float hsqr, a, l, rad;
549         
550         VecSubf (bdelta, b2, b1);
551         l = Normalise (bdelta);
552         
553         VecSubf (pdelta, vec, b1);
554         
555         a = bdelta[0]*pdelta[0] + bdelta[1]*pdelta[1] + bdelta[2]*pdelta[2];
556         hsqr = ((pdelta[0]*pdelta[0]) + (pdelta[1]*pdelta[1]) + (pdelta[2]*pdelta[2]));
557         
558         if (a < 0.0F){
559                 /* If we're past the end of the bone, do a spherical field attenuation thing */
560                 dist= ((b1[0]-vec[0])*(b1[0]-vec[0]) +(b1[1]-vec[1])*(b1[1]-vec[1]) +(b1[2]-vec[2])*(b1[2]-vec[2])) ;
561                 rad= rad1;
562         }
563         else if (a > l){
564                 /* If we're past the end of the bone, do a spherical field attenuation thing */
565                 dist= ((b2[0]-vec[0])*(b2[0]-vec[0]) +(b2[1]-vec[1])*(b2[1]-vec[1]) +(b2[2]-vec[2])*(b2[2]-vec[2])) ;
566                 rad= rad2;
567         }
568         else {
569                 dist= (hsqr - (a*a));
570                 
571                 if(l!=0.0f) {
572                         rad= a/l;
573                         rad= rad*rad2 + (1.0-rad)*rad1;
574                 }
575                 else rad= rad1;
576         }
577         
578         a= rad*rad;
579         if(dist < a) 
580                 return 1.0f;
581         else {
582                 l= rad+rdist;
583                 l*= l;
584                 if(rdist==0.0f || dist >= l) 
585                         return 0.0f;
586                 else {
587                         a= sqrt(dist)-rad;
588                         return 1.0-( a*a )/( rdist*rdist );
589                 }
590         }
591 }
592
593 static float dist_bone_deform(bPoseChannel *pchan, float *vec, float *co)
594 {
595         Bone *bone= pchan->bone;
596         float   fac;
597         float   cop[3];
598         float   contrib=0.0;
599
600         if(bone==NULL) return 0.0f;
601         
602         VECCOPY (cop, co);
603
604         fac= distfactor_to_bone(cop, bone->arm_head, bone->arm_tail, bone->rad_head, bone->rad_tail, bone->dist);
605         
606         if (fac>0.0){
607                 
608                 fac*=bone->weight;
609                 contrib= fac;
610                 if(contrib>0.0) {
611
612                         VECCOPY (cop, co);
613                         
614                         if(bone->segments>1)
615                                 b_bone_deform(pchan, bone, cop);        // applies on cop
616                         
617                         Mat4MulVecfl(pchan->chan_mat, cop);
618                         
619                         VecSubf (cop, cop, co); //      Make this a delta from the base position
620                         cop[0]*=fac; cop[1]*=fac; cop[2]*=fac;
621                         VecAddf (vec, vec, cop);
622                 }
623         }
624         
625         return contrib;
626 }
627
628 static void pchan_bone_deform(bPoseChannel *pchan, float weight, float *vec, float *co, float *contrib)
629 {
630         float   cop[3];
631
632         if (!weight)
633                 return;
634
635         VECCOPY (cop, co);
636         
637         if(pchan->bone->segments>1)
638                 b_bone_deform(pchan, pchan->bone, cop); // applies on cop
639         
640         Mat4MulVecfl(pchan->chan_mat, cop);
641         
642         vec[0]+=(cop[0]-co[0])*weight;
643         vec[1]+=(cop[1]-co[1])*weight;
644         vec[2]+=(cop[2]-co[2])*weight;
645
646         (*contrib)+=weight;
647 }
648
649 void armature_deform_verts(Object *armOb, Object *target, DerivedMesh *dm,
650                            float (*vertexCos)[3], int numVerts, int deformflag,
651                            const char *defgrp_name)
652 {
653         bPoseChannel *pchan, **defnrToPC = NULL;
654         MDeformVert *dverts = NULL;
655         float obinv[4][4], premat[4][4], postmat[4][4];
656         int use_envelope = deformflag & ARM_DEF_ENVELOPE;
657         int numGroups = 0;              /* safety for vertexgroup index overflow */
658         int i, target_totvert = 0;      /* safety for vertexgroup overflow */
659         int use_dverts = 0;
660         int armature_def_nr = -1;
661         bDeformGroup *dg;
662
663         if(armOb == G.obedit) return;
664         
665         Mat4Invert(obinv, target->obmat);
666         Mat4CpyMat4(premat, target->obmat);
667         Mat4MulMat4(postmat, armOb->obmat, obinv);
668
669         Mat4Invert(premat, postmat);
670
671         /* bone defmats are already in the channels, chan_mat */
672         
673         /* initialize B_bone matrices */
674         for(pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next) {
675                 if(!(pchan->bone->flag & BONE_NO_DEFORM))
676                         if(pchan->bone->segments > 1)
677                                 pchan_b_bone_defmats(pchan);
678         }
679
680         /* get the def_nr for the overall armature vertex group if present */
681         for(i = 0, dg = target->defbase.first; dg; i++, dg = dg->next)
682                 if(defgrp_name && strcmp(defgrp_name, dg->name) == 0)
683                         armature_def_nr = i;
684
685         /* get a vertex-deform-index to posechannel array */
686         if(deformflag & ARM_DEF_VGROUP) {
687                 if(ELEM(target->type, OB_MESH, OB_LATTICE)) {
688                         numGroups = BLI_countlist(&target->defbase);
689                         
690                         if(target->type==OB_MESH) {
691                                 Mesh *me= target->data;
692                                 dverts = me->dvert;
693                                 target_totvert = me->totvert;
694                         }
695                         else {
696                                 Lattice *lt= target->data;
697                                 dverts = lt->dvert;
698                                 target_totvert = lt->pntsu*lt->pntsv*lt->pntsw;
699                         }
700                         /* if we have a DerivedMesh, only use dverts if it has them */
701                         if(dm)
702                                 if(dm->getVertData(dm, 0, LAYERTYPE_MDEFORMVERT))
703                                         use_dverts = 1;
704                                 else use_dverts = 0;
705                         else if(dverts) use_dverts = 1;
706
707                         if(use_dverts) {
708                                 defnrToPC = MEM_callocN(sizeof(*defnrToPC) * numGroups,
709                                                         "defnrToBone");
710                                 for(i = 0, dg = target->defbase.first; dg;
711                                     i++, dg = dg->next) {
712                                         defnrToPC[i] = get_pose_channel(armOb->pose, dg->name);
713                                         /* exclude non-deforming bones */
714                                         if(defnrToPC[i]) {
715                                                 if(defnrToPC[i]->bone->flag & BONE_NO_DEFORM)
716                                                         defnrToPC[i]= NULL;
717                                         }
718                                 }
719                         }
720                 }
721         }
722
723         for(i = 0; i < numVerts; i++) {
724                 MDeformVert *dvert;
725                 float *co = vertexCos[i];
726                 float   vec[3];
727                 float   contrib = 0.0;
728                 int             j;
729                 float armature_weight = 1; /* default to 1 if no overall def group */
730
731                 vec[0] = vec[1] = vec[2] = 0;
732
733                 /* Apply the object's matrix */
734                 Mat4MulVecfl(premat, co);
735                 
736                 if(use_dverts || armature_def_nr >= 0) {
737                         if(dm) dvert = dm->getVertData(dm, i, LAYERTYPE_MDEFORMVERT);
738                         else if(i < target_totvert) dvert = dverts + i;
739                         else dvert = NULL;
740                 } else
741                         dvert = NULL;
742
743                 if(armature_def_nr >= 0 && dvert) {
744                         armature_weight = 0; /* a def group was given, so default to 0 */
745                         for(j = 0; j < dvert->totweight; j++) {
746                                 if(dvert->dw[j].def_nr == armature_def_nr) {
747                                         armature_weight = dvert->dw[j].weight;
748                                         break;
749                                 }
750                         }
751                 }
752
753                 /* check if there's any  point in calculating for this vert */
754                 if(armature_weight == 0) continue;
755                 
756                 if(use_dverts && dvert && dvert->totweight) { // use weight groups ?
757                         int deformed = 0;
758                         
759                         for(j = 0; j < dvert->totweight; j++){
760                                 int index = dvert->dw[j].def_nr;
761                                 pchan = index < numGroups?defnrToPC[index]:NULL;
762                                 if(pchan) {
763                                         float weight = dvert->dw[j].weight;
764                                         Bone *bone = pchan->bone;
765                                         
766                                         deformed = 1;
767                                         
768                                         if(bone && bone->flag & BONE_MULT_VG_ENV) {
769                                                 weight *= distfactor_to_bone(co, bone->arm_head,
770                                                                              bone->arm_tail,
771                                                                              bone->rad_head,
772                                                                              bone->rad_tail,
773                                                                              bone->dist);
774                                         }
775                                         pchan_bone_deform(pchan, weight, vec, co, &contrib);
776                                 }
777                         }
778                         /* if there are vertexgroups but not groups with bones
779                          * (like for softbody groups)
780                          */
781                         if(deformed == 0 && use_envelope) {
782                                 for(pchan = armOb->pose->chanbase.first; pchan;
783                                     pchan = pchan->next) {
784                                         if(!(pchan->bone->flag & BONE_NO_DEFORM))
785                                                 contrib += dist_bone_deform(pchan, vec, co);
786                                 }
787                         }
788                 }
789                 else if(use_envelope) {
790                         for(pchan = armOb->pose->chanbase.first; pchan;
791                             pchan = pchan->next) {
792                                 if(!(pchan->bone->flag & BONE_NO_DEFORM))
793                                         contrib += dist_bone_deform(pchan, vec, co);
794                         }
795                 }
796
797                 if(contrib > 0.0)
798                         VecMulf(vec, armature_weight / contrib);
799
800                 VecAddf(co, vec, co);
801                 Mat4MulVecfl(postmat, co);
802         }
803
804         if(defnrToPC) MEM_freeN(defnrToPC);
805         
806         /* free B_bone matrices */
807         for(pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next) {
808                 if(pchan->b_bone_mats) {
809                         MEM_freeN(pchan->b_bone_mats);
810                         pchan->b_bone_mats = NULL;
811                 }
812         }
813 }
814
815 /* ************ END Armature Deform ******************* */
816
817 void get_objectspace_bone_matrix (struct Bone* bone, float M_accumulatedMatrix[][4], int root, int posed)
818 {
819         Mat4CpyMat4(M_accumulatedMatrix, bone->arm_mat);
820 }
821
822
823 /* **************** The new & simple (but OK!) armature evaluation ********* */ 
824
825 /*  ****************** And how it works! ****************************************
826
827   This is the bone transformation trick; they're hierarchical so each bone(b)
828   is in the coord system of bone(b-1):
829
830   arm_mat(b)= arm_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) 
831   
832   -> yoffs is just the y axis translation in parent's coord system
833   -> d_root is the translation of the bone root, also in parent's coord system
834
835   pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b)
836
837   we then - in init deform - store the deform in chan_mat, such that:
838
839   pose_mat(b)= arm_mat(b) * chan_mat(b)
840   
841   *************************************************************************** */
842 /*  Computes vector and roll based on a rotation. "mat" must
843      contain only a rotation, and no scaling. */ 
844 void mat3_to_vec_roll(float mat[][3], float *vec, float *roll) {
845      if (vec)
846          VecCopyf(vec, mat[1]);
847
848      if (roll) {
849          float vecmat[3][3], vecmatinv[3][3], rollmat[3][3];
850
851          vec_roll_to_mat3(mat[1], 0.0f, vecmat);
852          Mat3Inv(vecmatinv, vecmat);
853          Mat3MulMat3(rollmat, vecmatinv, mat);
854
855          *roll= atan2(rollmat[2][0], rollmat[2][2]);
856      }
857 }
858
859 /*      Calculates the rest matrix of a bone based
860         On its vector and a roll around that vector */
861 void vec_roll_to_mat3(float *vec, float roll, float mat[][3])
862 {
863         float   nor[3], axis[3], target[3]={0,1,0};
864         float   theta;
865         float   rMatrix[3][3], bMatrix[3][3];
866         
867         VECCOPY (nor, vec);
868         Normalise (nor);
869         
870         /*      Find Axis & Amount for bone matrix*/
871         Crossf (axis,target,nor);
872         
873         if (Inpf(axis,axis) > 0.0000000000001) {
874                 /* if nor is *not* a multiple of target ... */
875                 Normalise (axis);
876                 theta=(float) acos (Inpf (target,nor));
877                 
878                 /*      Make Bone matrix*/
879                 VecRotToMat3(axis, theta, bMatrix);
880         }
881         else {
882                 /* if nor is a multiple of target ... */
883                 float updown;
884                 
885                 /* point same direction, or opposite? */
886                 updown = ( Inpf (target,nor) > 0 ) ? 1.0 : -1.0;
887                 
888                 /* I think this should work ... */
889                 bMatrix[0][0]=updown; bMatrix[0][1]=0.0;    bMatrix[0][2]=0.0;
890                 bMatrix[1][0]=0.0;    bMatrix[1][1]=updown; bMatrix[1][2]=0.0;
891                 bMatrix[2][0]=0.0;    bMatrix[2][1]=0.0;    bMatrix[2][2]=1.0;
892         }
893         
894         /*      Make Roll matrix*/
895         VecRotToMat3(nor, roll, rMatrix);
896         
897         /*      Combine and output result*/
898         Mat3MulMat3 (mat, rMatrix, bMatrix);
899 }
900
901
902 /* recursive part, calculates restposition of entire tree of children */
903 /* used by exiting editmode too */
904 void where_is_armature_bone(Bone *bone, Bone *prevbone)
905 {
906         float vec[3];
907         
908         /* Bone Space */
909         VecSubf (vec, bone->tail, bone->head);
910         vec_roll_to_mat3(vec, bone->roll, bone->bone_mat);
911
912         bone->length= VecLenf(bone->head, bone->tail);
913         
914         /* this is called on old file reading too... */
915         if(bone->xwidth==0.0) {
916                 bone->xwidth= 0.1f;
917                 bone->zwidth= 0.1f;
918                 bone->segments= 1;
919         }
920         
921         if(prevbone) {
922                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
923                 
924                 /* bone transform itself */
925                 Mat4CpyMat3(offs_bone, bone->bone_mat);
926                                 
927                 /* The bone's root offset (is in the parent's coordinate system) */
928                 VECCOPY(offs_bone[3], bone->head);
929
930                 /* Get the length translation of parent (length along y axis) */
931                 offs_bone[3][1]+= prevbone->length;
932                 
933                 /* Compose the matrix for this bone  */
934                 Mat4MulMat4(bone->arm_mat, offs_bone, prevbone->arm_mat);
935         }
936         else {
937                 Mat4CpyMat3(bone->arm_mat, bone->bone_mat);
938                 VECCOPY(bone->arm_mat[3], bone->head);
939         }
940         
941         /* head */
942         VECCOPY(bone->arm_head, bone->arm_mat[3]);
943         /* tail is in current local coord system */
944         VECCOPY(vec, bone->arm_mat[1]);
945         VecMulf(vec, bone->length);
946         VecAddf(bone->arm_tail, bone->arm_head, vec);
947         
948         /* and the kiddies */
949         prevbone= bone;
950         for(bone= bone->childbase.first; bone; bone= bone->next) {
951                 where_is_armature_bone(bone, prevbone);
952         }
953 }
954
955 /* updates vectors and matrices on rest-position level, only needed 
956    after editing armature itself, now only on reading file */
957 void where_is_armature (bArmature *arm)
958 {
959         Bone *bone;
960         
961         /* hierarchical from root to children */
962         for(bone= arm->bonebase.first; bone; bone= bone->next) {
963                 where_is_armature_bone(bone, NULL);
964         }
965 }
966
967 /* if bone layer is protected, copy the data from proxy->pose */
968 static void pose_proxy_synchronize(Object *ob, Object *proxy, int layer_protected)
969 {
970         bPose *pose= ob->pose, *proxypose= proxy->pose;
971         bPoseChannel *pchan, *pchanp, pchanw;
972         bConstraint *con;
973         char *str;
974         
975         if(proxypose==NULL) return;
976         
977         /* clear all transformation values from library */
978         rest_pose(proxypose);
979         
980         pchan= pose->chanbase.first;
981         pchanp= proxypose->chanbase.first;
982         for(; pchan && pchanp; pchan= pchan->next, pchanp= pchanp->next) {
983                 if(pchan->bone->layer & layer_protected) {
984                         
985                         /* copy posechannel to temp, but restore important pointers */
986                         pchanw= *pchanp;
987                         pchanw.prev= pchan->prev;
988                         pchanw.next= pchan->next;
989                         pchanw.parent= pchan->parent;
990                         pchanw.child= pchan->child;
991                         pchanw.path= NULL;
992                         
993                         /* constraints, set target ob pointer to own object */
994                         copy_constraints(&pchanw.constraints, &pchanp->constraints);
995
996                         for(con= pchanw.constraints.first; con; con= con->next) {
997                                 if(proxy==get_constraint_target(con, &str))
998                                         set_constraint_target(con, ob, NULL);
999                         }
1000                         
1001                         /* free stuff from current channel */
1002                         if(pchan->path) MEM_freeN(pchan->path);
1003                         free_constraints(&pchan->constraints);
1004                         
1005                         /* the final copy */
1006                         *pchan= pchanw;
1007                 }
1008         }
1009 }
1010
1011 static int rebuild_pose_bone(bPose *pose, Bone *bone, bPoseChannel *parchan, int counter)
1012 {
1013         bPoseChannel *pchan = verify_pose_channel (pose, bone->name);   // verify checks and/or adds
1014
1015         pchan->bone= bone;
1016         pchan->parent= parchan;
1017         
1018         counter++;
1019         
1020         for(bone= bone->childbase.first; bone; bone= bone->next) {
1021                 counter= rebuild_pose_bone(pose, bone, pchan, counter);
1022                 /* for quick detecting of next bone in chain, only b-bone uses it now */
1023                 if(bone->flag & BONE_CONNECTED)
1024                         pchan->child= get_pose_channel(pose, bone->name);
1025         }
1026         
1027         return counter;
1028 }
1029
1030 /* only after leave editmode, duplicating, validating older files, library syncing */
1031 /* NOTE: pose->flag is set for it */
1032 void armature_rebuild_pose(Object *ob, bArmature *arm)
1033 {
1034         Bone *bone;
1035         bPose *pose;
1036         bPoseChannel *pchan, *next;
1037         int counter=0;
1038                 
1039         /* only done here */
1040         if(ob->pose==NULL) ob->pose= MEM_callocN(sizeof(bPose), "new pose");
1041         pose= ob->pose;
1042         
1043         /* clear */
1044         for(pchan= pose->chanbase.first; pchan; pchan= pchan->next) {
1045                 pchan->bone= NULL;
1046                 pchan->child= NULL;
1047         }
1048         
1049         /* first step, check if all channels are there */
1050         for(bone= arm->bonebase.first; bone; bone= bone->next) {
1051                 counter= rebuild_pose_bone(pose, bone, NULL, counter);
1052         }
1053
1054         /* and a check for garbage */
1055         for(pchan= pose->chanbase.first; pchan; pchan= next) {
1056                 next= pchan->next;
1057                 if(pchan->bone==NULL) {
1058                         if(pchan->path)
1059                                 MEM_freeN(pchan->path);
1060                         free_constraints(&pchan->constraints);
1061                         BLI_freelinkN(&pose->chanbase, pchan);
1062                 }
1063         }
1064 //      printf("rebuild pose %s, %d bones\n", ob->id.name, counter);
1065         
1066         /* synchronize protected layers with proxy */
1067         if(OB_IS_PROXY(ob))
1068                 pose_proxy_synchronize(ob, ob->proxy, arm->layer_protected);
1069         
1070         update_pose_constraint_flags(ob->pose); // for IK detection for example
1071         
1072         /* the sorting */
1073         if(counter>1)
1074                 DAG_pose_sort(ob);
1075         
1076         ob->pose->flag &= ~POSE_RECALC;
1077 }
1078
1079
1080 /* ********************** THE IK SOLVER ******************* */
1081
1082
1083
1084 /* allocates PoseTree, and links that to root bone/channel */
1085 /* Note: detecting the IK chain is duplicate code... in drawarmature.c and in transform_conversions.c */
1086 static void initialize_posetree(struct Object *ob, bPoseChannel *pchan_tip)
1087 {
1088         bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
1089         PoseTree *tree;
1090         PoseTarget *target;
1091         bConstraint *con;
1092         bKinematicConstraint *data;
1093         int a, segcount= 0, size, newsize, *oldparent, parent;
1094         
1095         /* find IK constraint, and validate it */
1096         for(con= pchan_tip->constraints.first; con; con= con->next) {
1097                 if(con->type==CONSTRAINT_TYPE_KINEMATIC) break;
1098         }
1099         if(con==NULL) return;
1100         
1101         data=(bKinematicConstraint*)con->data;
1102         
1103         /* two types of targets */
1104         if(data->flag & CONSTRAINT_IK_AUTO);
1105         else {
1106                 if(con->flag & CONSTRAINT_DISABLE) return;      /* checked in editconstraint.c */
1107                 if(data->tar==NULL) return;
1108                 if(data->tar->type==OB_ARMATURE && data->subtarget[0]==0) return;
1109         }
1110         
1111         /* exclude tip from chain? */
1112         if(!(data->flag & CONSTRAINT_IK_TIP))
1113                 pchan_tip= pchan_tip->parent;
1114         
1115         /* Find the chain's root & count the segments needed */
1116         for (curchan = pchan_tip; curchan; curchan=curchan->parent){
1117                 pchan_root = curchan;
1118                 
1119                 curchan->flag |= POSE_CHAIN;    // don't forget to clear this
1120                 chanlist[segcount]=curchan;
1121                 segcount++;
1122                 
1123                 if(segcount==data->rootbone || segcount>255) break; // 255 is weak
1124         }
1125         if (!segcount) return;
1126
1127         /* setup the chain data */
1128         
1129         /* we make tree-IK, unless all existing targets are in this chain */
1130         for(tree= pchan_root->iktree.first; tree; tree= tree->next) {
1131                 for(target= tree->targets.first; target; target= target->next) {
1132                         curchan= tree->pchan[target->tip];
1133                         if(curchan->flag & POSE_CHAIN)
1134                                 curchan->flag &= ~POSE_CHAIN;
1135                         else
1136                                 break;
1137                 }
1138                 if(target) break;
1139         }
1140
1141         /* create a target */
1142         target= MEM_callocN(sizeof(PoseTarget), "posetarget");
1143         target->con= con;
1144         pchan_tip->flag &= ~POSE_CHAIN;
1145
1146         if(tree==NULL) {
1147                 /* make new tree */
1148                 tree= MEM_callocN(sizeof(PoseTree), "posetree");
1149
1150                 tree->iterations= data->iterations;
1151                 tree->totchannel= segcount;
1152                 tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
1153                 
1154                 tree->pchan= MEM_callocN(segcount*sizeof(void*), "ik tree pchan");
1155                 tree->parent= MEM_callocN(segcount*sizeof(int), "ik tree parent");
1156                 for(a=0; a<segcount; a++) {
1157                         tree->pchan[a]= chanlist[segcount-a-1];
1158                         tree->parent[a]= a-1;
1159                 }
1160                 target->tip= segcount-1;
1161                 
1162                 /* AND! link the tree to the root */
1163                 BLI_addtail(&pchan_root->iktree, tree);
1164         }
1165         else {
1166                 tree->iterations= MAX2(data->iterations, tree->iterations);
1167                 tree->stretch= tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
1168
1169                 /* skip common pose channels and add remaining*/
1170                 size= MIN2(segcount, tree->totchannel);
1171                 for(a=0; a<size && tree->pchan[a]==chanlist[segcount-a-1]; a++);
1172                 parent= a-1;
1173
1174                 segcount= segcount-a;
1175                 target->tip= tree->totchannel + segcount - 1;
1176
1177                 if (segcount > 0) {
1178                         /* resize array */
1179                         newsize= tree->totchannel + segcount;
1180                         oldchan= tree->pchan;
1181                         oldparent= tree->parent;
1182
1183                         tree->pchan= MEM_callocN(newsize*sizeof(void*), "ik tree pchan");
1184                         tree->parent= MEM_callocN(newsize*sizeof(int), "ik tree parent");
1185                         memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel);
1186                         memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel);
1187                         MEM_freeN(oldchan);
1188                         MEM_freeN(oldparent);
1189
1190                         /* add new pose channels at the end, in reverse order */
1191                         for(a=0; a<segcount; a++) {
1192                                 tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1];
1193                                 tree->parent[tree->totchannel+a]= tree->totchannel+a-1;
1194                         }
1195                         tree->parent[tree->totchannel]= parent;
1196                         
1197                         tree->totchannel= newsize;
1198                 }
1199
1200                 /* move tree to end of list, for correct evaluation order */
1201                 BLI_remlink(&pchan_root->iktree, tree);
1202                 BLI_addtail(&pchan_root->iktree, tree);
1203         }
1204
1205         /* add target to the tree */
1206         BLI_addtail(&tree->targets, target);
1207 }
1208
1209 /* called from within the core where_is_pose loop, all animsystems and constraints
1210 were executed & assigned. Now as last we do an IK pass */
1211 static void execute_posetree(Object *ob, PoseTree *tree)
1212 {
1213         float R_parmat[3][3];
1214         float iR_parmat[3][3];
1215         float R_bonemat[3][3];
1216         float goalrot[3][3], goalpos[3];
1217         float rootmat[4][4], imat[4][4];
1218         float goal[4][4], goalinv[4][4];
1219         float size[3], irest_basis[3][3], full_basis[3][3];
1220         float end_pose[4][4], world_pose[4][4];
1221         float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch=NULL;
1222         int a, flag, hasstretch=0;
1223         bPoseChannel *pchan;
1224         IK_Segment *seg, *parent, **iktree, *iktarget;
1225         IK_Solver *solver;
1226         PoseTarget *target;
1227         bKinematicConstraint *data;
1228         Bone *bone;
1229
1230         if (tree->totchannel == 0)
1231                 return;
1232
1233         iktree= MEM_mallocN(sizeof(void*)*tree->totchannel, "ik tree");
1234
1235         for(a=0; a<tree->totchannel; a++) {
1236                 pchan= tree->pchan[a];
1237                 bone= pchan->bone;
1238
1239                 /* set DoF flag */
1240                 flag= 0;
1241                 if((pchan->ikflag & BONE_IK_NO_XDOF) == 0)
1242                         flag |= IK_XDOF;
1243                 if((pchan->ikflag & BONE_IK_NO_YDOF) == 0)
1244                         flag |= IK_YDOF;
1245                 if((pchan->ikflag & BONE_IK_NO_ZDOF) == 0)
1246                         flag |= IK_ZDOF;
1247
1248                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
1249                         flag |= IK_TRANS_YDOF;
1250                         hasstretch = 1;
1251                 }
1252
1253                 seg= iktree[a]= IK_CreateSegment(flag);
1254
1255                 /* find parent */
1256                 if(a == 0)
1257                         parent= NULL;
1258                 else
1259                         parent= iktree[tree->parent[a]];
1260
1261                 IK_SetParent(seg, parent);
1262         
1263                 /* get the matrix that transforms from prevbone into this bone */
1264                 Mat3CpyMat4(R_bonemat, pchan->pose_mat);
1265
1266                 /* gather transformations for this IK segment */
1267
1268                 if (pchan->parent)
1269                         Mat3CpyMat4(R_parmat, pchan->parent->pose_mat);
1270                 else
1271                         Mat3One(R_parmat);
1272
1273                 /* bone offset */
1274                 if (pchan->parent && (a > 0))
1275                         VecSubf(start, pchan->pose_head, pchan->parent->pose_tail);
1276                 else
1277                         /* only root bone (a = 0) has no parent */
1278                         start[0]= start[1]= start[2]= 0.0f;
1279                 
1280                 /* change length based on bone size */
1281                 length= bone->length*VecLength(R_bonemat[1]);
1282
1283                 /* compute rest basis and its inverse */
1284                 Mat3CpyMat3(rest_basis, bone->bone_mat);
1285                 Mat3CpyMat3(irest_basis, bone->bone_mat);
1286                 Mat3Transp(irest_basis);
1287
1288                 /* compute basis with rest_basis removed */
1289                 Mat3Inv(iR_parmat, R_parmat);
1290                 Mat3MulMat3(full_basis, iR_parmat, R_bonemat);
1291                 Mat3MulMat3(basis, irest_basis, full_basis);
1292
1293                 /* basis must be pure rotation */
1294                 Mat3Ortho(basis);
1295
1296                 /* transform offset into local bone space */
1297                 Mat3Ortho(iR_parmat);
1298                 Mat3MulVecfl(iR_parmat, start);
1299
1300                 IK_SetTransform(seg, start, rest_basis, basis, length);
1301
1302                 if (pchan->ikflag & BONE_IK_XLIMIT)
1303                         IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]);
1304                 if (pchan->ikflag & BONE_IK_YLIMIT)
1305                         IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]);
1306                 if (pchan->ikflag & BONE_IK_ZLIMIT)
1307                         IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]);
1308
1309                 IK_SetStiffness(seg, IK_X, pchan->stiffness[0]);
1310                 IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]);
1311                 IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]);
1312
1313                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
1314                         float ikstretch = pchan->ikstretch*pchan->ikstretch;
1315                         IK_SetStiffness(seg, IK_TRANS_Y, MIN2(1.0-ikstretch, 0.99));
1316                         IK_SetLimit(seg, IK_TRANS_Y, 0.001, 1e10);
1317                 }
1318         }
1319
1320         solver= IK_CreateSolver(iktree[0]);
1321
1322         /* set solver goals */
1323
1324         /* first set the goal inverse transform, assuming the root of tree was done ok! */
1325         pchan= tree->pchan[0];
1326         if (pchan->parent)
1327                 /* transform goal by parent mat, so this rotation is not part of the
1328                    segment's basis. otherwise rotation limits do not work on the
1329                    local transform of the segment itself. */
1330                 Mat4CpyMat4(rootmat, pchan->parent->pose_mat);
1331         else
1332                 Mat4One(rootmat);
1333         VECCOPY(rootmat[3], pchan->pose_head);
1334         
1335         Mat4MulMat4 (imat, rootmat, ob->obmat);
1336         Mat4Invert (goalinv, imat);
1337         
1338         for(target=tree->targets.first; target; target=target->next) {
1339                 data= (bKinematicConstraint*)target->con->data;
1340
1341                 /* 1.0=ctime, we pass on object for auto-ik */
1342                 get_constraint_target_matrix(target->con, TARGET_BONE, ob, rootmat, size, 1.0);
1343
1344                 /* and set and transform goal */
1345                 Mat4MulMat4(goal, rootmat, goalinv);
1346
1347                 VECCOPY(goalpos, goal[3]);
1348                 Mat3CpyMat4(goalrot, goal);
1349
1350                 /* do we need blending? */
1351                 if(target->con->enforce!=1.0) {
1352                         float q1[4], q2[4], q[4];
1353                         float fac= target->con->enforce;
1354                         float mfac= 1.0-fac;
1355                         
1356                         pchan= tree->pchan[target->tip];
1357
1358                         /* end effector in world space */
1359                         Mat4CpyMat4(end_pose, pchan->pose_mat);
1360                         VECCOPY(end_pose[3], pchan->pose_tail);
1361                         Mat4MulSerie(world_pose, goalinv, ob->obmat, end_pose, 0, 0, 0, 0, 0);
1362
1363                         /* blend position */
1364                         goalpos[0]= fac*goalpos[0] + mfac*world_pose[3][0];
1365                         goalpos[1]= fac*goalpos[1] + mfac*world_pose[3][1];
1366                         goalpos[2]= fac*goalpos[2] + mfac*world_pose[3][2];
1367
1368                         /* blend rotation */
1369                         Mat3ToQuat(goalrot, q1);
1370                         Mat4ToQuat(world_pose, q2);
1371                         QuatInterpol(q, q1, q2, mfac);
1372                         QuatToMat3(q, goalrot);
1373                 }
1374
1375                 iktarget= iktree[target->tip];
1376
1377                 if(data->weight != 0.0)
1378                         IK_SolverAddGoal(solver, iktarget, goalpos, data->weight);
1379                 if((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0) && (data->flag & CONSTRAINT_IK_AUTO)==0)
1380                         IK_SolverAddGoalOrientation(solver, iktarget, goalrot, data->orientweight);
1381         }
1382
1383         /* solve */
1384         IK_Solve(solver, 0.0f, tree->iterations);
1385         IK_FreeSolver(solver);
1386
1387         /* gather basis changes */
1388         tree->basis_change= MEM_mallocN(sizeof(float[3][3])*tree->totchannel, "ik basis change");
1389         if(hasstretch)
1390                 ikstretch= MEM_mallocN(sizeof(float)*tree->totchannel, "ik stretch");
1391
1392         for(a=0; a<tree->totchannel; a++) {
1393                 IK_GetBasisChange(iktree[a], tree->basis_change[a]);
1394
1395                 if(hasstretch) {
1396                         /* have to compensate for scaling received from parent */
1397                         float parentstretch, stretch;
1398
1399                         pchan= tree->pchan[a];
1400                         parentstretch= (tree->parent[a] >= 0)? ikstretch[tree->parent[a]]: 1.0;
1401
1402                         if(tree->stretch && (pchan->ikstretch > 0.0)) {
1403                                 float trans[3], length;
1404
1405                                 IK_GetTranslationChange(iktree[a], trans);
1406                                 length= pchan->bone->length*VecLength(pchan->pose_mat[1]);
1407
1408                                 ikstretch[a]= (length == 0.0)? 1.0: (trans[1]+length)/length;
1409                         }
1410                         else
1411                                 ikstretch[a] = 1.0;
1412
1413                         stretch= (parentstretch == 0.0)? 1.0: ikstretch[a]/parentstretch;
1414
1415                         VecMulf(tree->basis_change[a][0], stretch);
1416                         VecMulf(tree->basis_change[a][1], stretch);
1417                         VecMulf(tree->basis_change[a][2], stretch);
1418
1419                 }
1420
1421                 IK_FreeSegment(iktree[a]);
1422         }
1423         
1424         MEM_freeN(iktree);
1425         if(ikstretch) MEM_freeN(ikstretch);
1426 }
1427
1428 void free_posetree(PoseTree *tree)
1429 {
1430         BLI_freelistN(&tree->targets);
1431         if(tree->pchan) MEM_freeN(tree->pchan);
1432         if(tree->parent) MEM_freeN(tree->parent);
1433         if(tree->basis_change) MEM_freeN(tree->basis_change);
1434         MEM_freeN(tree);
1435 }
1436
1437 /* ********************** THE POSE SOLVER ******************* */
1438
1439
1440 /* loc/rot/size to mat4 */
1441 /* used in constraint.c too */
1442 void chan_calc_mat(bPoseChannel *chan)
1443 {
1444         float smat[3][3];
1445         float rmat[3][3];
1446         float tmat[3][3];
1447         
1448         SizeToMat3(chan->size, smat);
1449         
1450         NormalQuat(chan->quat);
1451
1452         QuatToMat3(chan->quat, rmat);
1453         
1454         Mat3MulMat3(tmat, rmat, smat);
1455         
1456         Mat4CpyMat3(chan->chan_mat, tmat);
1457         
1458         /* prevent action channels breaking chains */
1459         /* need to check for bone here, CONSTRAINT_TYPE_ACTION uses this call */
1460         if (chan->bone==NULL || !(chan->bone->flag & BONE_CONNECTED)) {
1461                 VECCOPY(chan->chan_mat[3], chan->loc);
1462         }
1463
1464 }
1465
1466 /* transform from bone(b) to bone(b+1), store in chan_mat */
1467 static void make_dmats(bPoseChannel *pchan)
1468 {
1469         if (pchan->parent) {
1470                 float iR_parmat[4][4];
1471                 Mat4Invert(iR_parmat, pchan->parent->pose_mat);
1472                 Mat4MulMat4(pchan->chan_mat,  pchan->pose_mat, iR_parmat);      // delta mat
1473         }
1474         else Mat4CpyMat4(pchan->chan_mat, pchan->pose_mat);
1475 }
1476
1477 /* applies IK matrix to pchan, IK is done separated */
1478 /* formula: pose_mat(b) = pose_mat(b-1) * diffmat(b-1, b) * ik_mat(b) */
1479 /* to make this work, the diffmats have to be precalculated! Stored in chan_mat */
1480 static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[][3])   // nr = to detect if this is first bone
1481 {
1482         float vec[3], ikmat[4][4];
1483         
1484         Mat4CpyMat3(ikmat, ik_mat);
1485         
1486         if (pchan->parent)
1487                 Mat4MulSerie(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat, ikmat, NULL, NULL, NULL, NULL, NULL);
1488         else 
1489                 Mat4MulMat4(pchan->pose_mat, ikmat, pchan->chan_mat);
1490
1491         /* calculate head */
1492         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
1493         /* calculate tail */
1494         VECCOPY(vec, pchan->pose_mat[1]);
1495         VecMulf(vec, pchan->bone->length);
1496         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
1497
1498         pchan->flag |= POSE_DONE;
1499 }
1500
1501 static void do_local_constraint(bPoseChannel *pchan, bConstraint *con)
1502 {
1503         switch(con->type) {
1504                 case CONSTRAINT_TYPE_LOCLIKE:
1505                 {
1506                         bLocateLikeConstraint *data= con->data;
1507                         
1508                         if(data->tar && data->subtarget[0]) {
1509                                 bPoseChannel *pchant= get_pose_channel(data->tar->pose, data->subtarget);
1510                                 if(pchant) {
1511                                         if (data->flag & LOCLIKE_X)
1512                                                 pchan->loc[0]= pchant->loc[0];
1513                                         if (data->flag & LOCLIKE_Y)
1514                                                 pchan->loc[1]= pchant->loc[1];
1515                                         if (data->flag & LOCLIKE_Z)
1516                                                 pchan->loc[2]= pchant->loc[2];
1517                                 }
1518                         }
1519                 }
1520                         break;
1521                 case CONSTRAINT_TYPE_ROTLIKE:
1522                 {
1523                         bRotateLikeConstraint *data= con->data;
1524                         if(data->tar && data->subtarget[0]) {
1525                                 bPoseChannel *pchant= get_pose_channel(data->tar->pose, data->subtarget);
1526                                 if(pchant) {
1527                                         if(data->flag != (ROTLIKE_X|ROTLIKE_Y|ROTLIKE_Z)) {
1528                                                 float eul[3], eult[3], euln[3], fac= con->enforce;
1529                                                 
1530                                                 QuatToEul(pchan->quat, eul);
1531                                                 QuatToEul(pchant->quat, eult);
1532                                                 VECCOPY(euln, eul);
1533                                                 if(data->flag & ROTLIKE_X) euln[0]= fac*eult[0] + (1.0f-fac)*eul[0];
1534                                                 if(data->flag & ROTLIKE_Y) euln[1]= fac*eult[1] + (1.0f-fac)*eul[1];
1535                                                 if(data->flag & ROTLIKE_Z) euln[2]= fac*eult[2] + (1.0f-fac)*eul[2];
1536                                                 compatible_eul(eul, euln);
1537                                                 EulToQuat(euln, pchan->quat);
1538                                         }
1539                                         else {
1540                                                 QuatInterpol(pchan->quat, pchan->quat, pchant->quat, con->enforce);
1541                                         }
1542                                 }
1543                         }
1544                 }
1545                         break;
1546                 case CONSTRAINT_TYPE_LOCLIMIT:
1547                 {
1548                         bLocLimitConstraint *data= con->data;
1549                         
1550                         /* Aligorith: don't know whether this function really evaluates constraints, but here goes anyways */
1551                         if (data->flag & LIMIT_XMIN) {
1552                                 if(pchan->loc[0] < data->xmin)
1553                                         pchan->loc[0] = data->xmin;
1554                         }
1555                         if (data->flag & LIMIT_XMAX) {
1556                                 if (pchan->loc[0] > data->xmax)
1557                                         pchan->loc[0] = data->xmax;
1558                         }
1559                         if (data->flag & LIMIT_YMIN) {
1560                                 if(pchan->loc[1] < data->ymin)
1561                                         pchan->loc[1] = data->ymin;
1562                         }
1563                         if (data->flag & LIMIT_YMAX) {
1564                                 if (pchan->loc[1] > data->ymax)
1565                                         pchan->loc[1] = data->ymax;
1566                         }
1567                         if (data->flag & LIMIT_ZMIN) {
1568                                 if(pchan->loc[2] < data->zmin)
1569                                         pchan->loc[2] = data->zmin;
1570                         }
1571                         if (data->flag & LIMIT_ZMAX) {
1572                                 if (pchan->loc[2] > data->zmax)
1573                                         pchan->loc[2] = data->zmax;
1574                         }
1575                 }       
1576                         break;
1577                 case CONSTRAINT_TYPE_ROTLIMIT:
1578                 {
1579                         bRotLimitConstraint *data = con->data;
1580                         float eul[3];
1581                         
1582                         /*Aligorith:  don't know whether this function is really for evaluating constraints, but here goes anyways */
1583                         
1584                         QuatToEul(pchan->quat, eul);
1585                         
1586                         /* eulers: radians to degrees! */
1587                         eul[0] = (eul[0] / (2*M_PI) * 360);
1588                         eul[1] = (eul[1] / (2*M_PI) * 360);
1589                         eul[2] = (eul[2] / (2*M_PI) * 360);
1590                         
1591                         /* limiting of euler values... */
1592                         if (data->flag & LIMIT_XROT) {
1593                                 if (eul[0] < data->xmin) 
1594                                         eul[0] = data->xmin;
1595                                         
1596                                 if (eul[0] > data->xmax)
1597                                         eul[0] = data->xmax;
1598                         }
1599                         if (data->flag & LIMIT_YROT) {
1600                                 if (eul[1] < data->ymin)
1601                                         eul[1] = data->ymin;
1602                                         
1603                                 if (eul[1] > data->ymax)
1604                                         eul[1] = data->ymax;
1605                         }
1606                         if (data->flag & LIMIT_ZROT) {
1607                                 if (eul[2] < data->zmin)
1608                                         eul[2] = data->zmin;
1609                                         
1610                                 if (eul[2] > data->zmax)
1611                                         eul[2] = data->zmax;
1612                         }
1613                                 
1614                         /* eulers: degrees to radians ! */
1615                         eul[0] = (eul[0] / 360 * (2*M_PI)); 
1616                         eul[1] = (eul[1] / 360 * (2*M_PI));
1617                         eul[2] = (eul[2] / 360 * (2*M_PI));
1618                         
1619                         /* convert back */
1620                         EulToQuat(eul, pchan->quat);
1621                 }
1622                         break;
1623         }
1624 }
1625
1626 static void do_strip_modifiers(Object *armob, Bone *bone, bPoseChannel *pchan)
1627 {
1628         bActionModifier *amod;
1629         bActionStrip *strip;
1630         float scene_cfra= G.scene->r.cfra;
1631
1632         for (strip=armob->nlastrips.first; strip; strip=strip->next) {
1633                 if(scene_cfra>=strip->start && scene_cfra<=strip->end) {
1634                         
1635                         /* temporal solution to prevent 2 strips accumulating */
1636                         if(scene_cfra==strip->end && strip->next && strip->next->start==scene_cfra)
1637                                 continue;
1638                         
1639                         for(amod= strip->modifiers.first; amod; amod= amod->next) {
1640                                 if(amod->type==ACTSTRIP_MOD_DEFORM) {
1641                                         /* validate first */
1642                                         if(amod->ob && amod->ob->type==OB_CURVE && amod->channel[0]) {
1643                                                 
1644                                                 if( strcmp(pchan->name, amod->channel)==0 ) {
1645                                                         float mat4[4][4], mat3[3][3];
1646                                                         
1647                                                         curve_deform_vector(amod->ob, armob, bone->arm_mat[3], pchan->pose_mat[3], mat3, amod->no_rot_axis);
1648                                                         Mat4CpyMat4(mat4, pchan->pose_mat);
1649                                                         Mat4MulMat34(pchan->pose_mat, mat3, mat4);
1650                                                         
1651                                                 }
1652                                         }
1653                                 }
1654                         }
1655                 }
1656         }
1657 }
1658
1659
1660 /* The main armature solver, does all constraints excluding IK */
1661 /* pchan is validated, as having bone and parent pointer */
1662 static void where_is_pose_bone(Object *ob, bPoseChannel *pchan, float ctime)
1663 {
1664         Bone *bone, *parbone;
1665         bPoseChannel *parchan;
1666         float vec[3], quat[4];
1667         int did_local= 0;       /* copying quaternion should be limited, chan_calc_mat() normalizes quat */
1668         
1669         /* set up variables for quicker access below */
1670         bone= pchan->bone;
1671         parbone= bone->parent;
1672         parchan= pchan->parent;
1673                 
1674         /* Do local constraints, these only work on the channel data (loc rot size) */
1675         QUATCOPY(quat, pchan->quat);
1676         if(pchan->constraints.first) {
1677                 bConstraint *con;
1678                 for(con=pchan->constraints.first; con; con= con->next) {
1679                         if(con->flag & CONSTRAINT_LOCAL) {
1680                                 do_local_constraint(pchan, con);
1681                                 did_local= 1;
1682                         }
1683                 }
1684         }
1685         
1686         /* this gives a chan_mat with actions (ipos) results */
1687         chan_calc_mat(pchan);
1688         
1689         if(did_local) 
1690                 QUATCOPY(pchan->quat, quat);    /* local constraint hack. bad! */
1691         
1692         /* construct the posemat based on PoseChannels, that we do before applying constraints */
1693         /* pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b) */
1694         
1695         if(parchan) {
1696                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
1697                 
1698                 /* bone transform itself */
1699                 Mat4CpyMat3(offs_bone, bone->bone_mat);
1700                 
1701                 /* The bone's root offset (is in the parent's coordinate system) */
1702                 VECCOPY(offs_bone[3], bone->head);
1703                 
1704                 /* Get the length translation of parent (length along y axis) */
1705                 offs_bone[3][1]+= parbone->length;
1706                 
1707                 /* Compose the matrix for this bone  */
1708                 if(bone->flag & BONE_HINGE) {   // uses restposition rotation, but actual position
1709                         float tmat[4][4];
1710                         
1711                         /* the rotation of the parent restposition */
1712                         Mat4CpyMat4(tmat, parbone->arm_mat);
1713                         
1714                         /* the location of actual parent transform */
1715                         VECCOPY(tmat[3], offs_bone[3]);
1716                         offs_bone[3][0]= offs_bone[3][1]= offs_bone[3][2]= 0.0f;
1717                         Mat4MulVecfl(parchan->pose_mat, tmat[3]);
1718                         
1719                         Mat4MulSerie(pchan->pose_mat, tmat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
1720                 }
1721                 else 
1722                         Mat4MulSerie(pchan->pose_mat, parchan->pose_mat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
1723         }
1724         else {
1725                 Mat4MulMat4(pchan->pose_mat, pchan->chan_mat, bone->arm_mat);
1726                 /* only rootbones get the cyclic offset */
1727                 VecAddf(pchan->pose_mat[3], pchan->pose_mat[3], ob->pose->cyclic_offset);
1728         }
1729         
1730         do_strip_modifiers(ob, bone, pchan);
1731         
1732         /* Do constraints */
1733         if(pchan->constraints.first) {
1734                 static Object conOb;
1735                 static int initialized= 0;
1736                 
1737                 VECCOPY(vec, pchan->pose_mat[3]);
1738                 
1739                 /* Build a workob to pass the bone to the constraint solver */
1740                 if(initialized==0) {
1741                         memset(&conOb, 0, sizeof(Object));
1742                         initialized= 1;
1743                 }
1744                 conOb.size[0]= conOb.size[1]= conOb.size[2]= 1.0;
1745                 conOb.data = ob->data;
1746                 conOb.type = ob->type;
1747                 conOb.parent = ob;      // ik solver retrieves the armature that way !?!?!?!
1748                 conOb.pose= ob->pose;                           // needed for retrieving pchan
1749                 conOb.trackflag = ob->trackflag;
1750                 conOb.upflag = ob->upflag;
1751                 
1752                 /* Collect the constraints from the pose (listbase copy) */
1753                 conOb.constraints = pchan->constraints;
1754                 
1755                 /* conOb.obmat takes bone to worldspace */
1756                 Mat4MulMat4 (conOb.obmat, pchan->pose_mat, ob->obmat);
1757                 
1758                 /* Solve */
1759                 solve_constraints (&conOb, TARGET_BONE, (void*)pchan, ctime);   // ctime doesnt alter objects
1760                 
1761                 /* Take out of worldspace */
1762                 Mat4MulMat4 (pchan->pose_mat, conOb.obmat, ob->imat);
1763                 
1764                 /* prevent constraints breaking a chain */
1765                 if(pchan->bone->flag & BONE_CONNECTED)
1766                         VECCOPY(pchan->pose_mat[3], vec);
1767
1768         }
1769         
1770         /* calculate head */
1771         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
1772         /* calculate tail */
1773         VECCOPY(vec, pchan->pose_mat[1]);
1774         VecMulf(vec, bone->length);
1775         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
1776         
1777 }
1778
1779 /* This only reads anim data from channels, and writes to channels */
1780 /* This is the only function adding poses */
1781 void where_is_pose (Object *ob)
1782 {
1783         bArmature *arm;
1784         Bone *bone;
1785         bPoseChannel *pchan;
1786         float imat[4][4];
1787         float ctime= bsystem_time(ob, NULL, (float)G.scene->r.cfra, 0.0);       /* not accurate... */
1788         
1789         arm = get_armature(ob);
1790         
1791         if(arm==NULL) return;
1792         if(ob->pose==NULL || (ob->pose->flag & POSE_RECALC)) 
1793            armature_rebuild_pose(ob, arm);
1794         
1795         /* In restposition we read the data from the bones */
1796         if(ob==G.obedit || (arm->flag & ARM_RESTPOS)) {
1797                 
1798                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1799                         bone= pchan->bone;
1800                         if(bone) {
1801                                 Mat4CpyMat4(pchan->pose_mat, bone->arm_mat);
1802                                 VECCOPY(pchan->pose_head, bone->arm_head);
1803                                 VECCOPY(pchan->pose_tail, bone->arm_tail);
1804                         }
1805                 }
1806         }
1807         else {
1808                 Mat4Invert(ob->imat, ob->obmat);        // imat is needed 
1809
1810                 /* 1. construct the PoseTrees, clear flags */
1811                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1812                         pchan->flag &= ~(POSE_DONE|POSE_CHAIN);
1813                         if(pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints
1814                                 initialize_posetree(ob, pchan); // will attach it to root!
1815                 }
1816                 
1817                 /* 2. the main loop, channels are already hierarchical sorted from root to children */
1818                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1819                         
1820                         /* 3. if we find an IK root, we handle it separated */
1821                         if(pchan->iktree.first) {
1822                                 while(pchan->iktree.first) {
1823                                         PoseTree *tree= pchan->iktree.first;
1824                                         int a;
1825                                         
1826                                         /* 4. walk over the tree for regular solving */
1827                                         for(a=0; a<tree->totchannel; a++) {
1828                                                 if(!(tree->pchan[a]->flag & POSE_DONE)) // successive trees can set the flag
1829                                                         where_is_pose_bone(ob, tree->pchan[a], ctime);
1830                                         }
1831                                         /* 5. execute the IK solver */
1832                                         execute_posetree(ob, tree);
1833                                         
1834                                         /* 6. apply the differences to the channels, 
1835                                                   we need to calculate the original differences first */
1836                                         for(a=0; a<tree->totchannel; a++)
1837                                                 make_dmats(tree->pchan[a]);
1838                                         
1839                                         for(a=0; a<tree->totchannel; a++)
1840                                                 /* sets POSE_DONE */
1841                                                 where_is_ik_bone(tree->pchan[a], tree->basis_change[a]);
1842                                         
1843                                         /* 7. and free */
1844                                         BLI_remlink(&pchan->iktree, tree);
1845                                         free_posetree(tree);
1846                                 }
1847                         }
1848                         else if(!(pchan->flag & POSE_DONE)) {
1849                                 where_is_pose_bone(ob, pchan, ctime);
1850                         }
1851                 }
1852         }
1853                 
1854         /* calculating deform matrices */
1855         for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1856                 if(pchan->bone) {
1857                         Mat4Invert(imat, pchan->bone->arm_mat);
1858                         Mat4MulMat4(pchan->chan_mat, imat, pchan->pose_mat);
1859                 }
1860         }
1861 }