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