f40c6ed9ceb5ef1674a79fc42994ec6576268ff6
[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_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(char *name)
86 {
87         bArmature *arm;
88         
89         arm= alloc_libblock (&G.main->armature, ID_AR, name);
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         bDeformGroup *dg;
656         float obinv[4][4], premat[4][4], postmat[4][4];
657         int use_envelope = deformflag & ARM_DEF_ENVELOPE;
658         int numGroups = 0;              /* safety for vertexgroup index overflow */
659         int i, target_totvert = 0;      /* safety for vertexgroup overflow */
660         int use_dverts = 0;
661         int armature_def_nr = -1;
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         Mat4Invert(premat, postmat);
669
670         /* bone defmats are already in the channels, chan_mat */
671         
672         /* initialize B_bone matrices */
673         for(pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next) {
674                 if(!(pchan->bone->flag & BONE_NO_DEFORM))
675                         if(pchan->bone->segments > 1)
676                                 pchan_b_bone_defmats(pchan);
677         }
678
679         /* get the def_nr for the overall armature vertex group if present */
680         for(i = 0, dg = target->defbase.first; dg; i++, dg = dg->next)
681                 if(defgrp_name && strcmp(defgrp_name, dg->name) == 0)
682                         armature_def_nr = i;
683
684         /* get a vertex-deform-index to posechannel array */
685         if(deformflag & ARM_DEF_VGROUP) {
686                 if(ELEM(target->type, OB_MESH, OB_LATTICE)) {
687                         numGroups = BLI_countlist(&target->defbase);
688                         
689                         if(target->type==OB_MESH) {
690                                 Mesh *me= target->data;
691                                 dverts = me->dvert;
692                                 target_totvert = me->totvert;
693                         }
694                         else {
695                                 Lattice *lt= target->data;
696                                 dverts = lt->dvert;
697                                 if(dverts)
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, CD_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.0f;
728                 float armature_weight = 1.0f; /* default to 1 if no overall def group */
729                 int       j;
730
731                 vec[0] = vec[1] = vec[2] = 0.0f;
732
733                 if(use_dverts || armature_def_nr >= 0) {
734                         if(dm) dvert = dm->getVertData(dm, i, CD_MDEFORMVERT);
735                         else if(i < target_totvert) dvert = dverts + i;
736                         else dvert = NULL;
737                 } else
738                         dvert = NULL;
739
740                 if(armature_def_nr >= 0 && dvert) {
741                         armature_weight = 0.0f; /* a def group was given, so default to 0 */
742                         for(j = 0; j < dvert->totweight; j++) {
743                                 if(dvert->dw[j].def_nr == armature_def_nr) {
744                                         armature_weight = dvert->dw[j].weight;
745                                         break;
746                                 }
747                         }
748                 }
749
750                 /* check if there's any  point in calculating for this vert */
751                 if(armature_weight == 0.0f) continue;
752                 
753                 /* Apply the object's matrix */
754                 Mat4MulVecfl(premat, co);
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                 /* actually should be EPSILON? weight values and contrib can be like 10e-39 small */
798                 if(contrib > 0.0001f) {
799                         VecMulf(vec, armature_weight / contrib);
800                         VecAddf(co, vec, co);
801                 }
802                 /* always, check above code */
803                 Mat4MulVecfl(postmat, co);
804         }
805
806         if(defnrToPC) MEM_freeN(defnrToPC);
807         
808         /* free B_bone matrices */
809         for(pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next) {
810                 if(pchan->b_bone_mats) {
811                         MEM_freeN(pchan->b_bone_mats);
812                         pchan->b_bone_mats = NULL;
813                 }
814         }
815 }
816
817 /* ************ END Armature Deform ******************* */
818
819 void get_objectspace_bone_matrix (struct Bone* bone, float M_accumulatedMatrix[][4], int root, int posed)
820 {
821         Mat4CpyMat4(M_accumulatedMatrix, bone->arm_mat);
822 }
823
824 /* **************** Space to Space API ****************** */
825
826 /* Convert World-Space Matrix to Pose-Space Matrix */
827 void armature_mat_world_to_pose(Object *ob, float inmat[][4], float outmat[][4]) 
828 {
829         float obmat[4][4];
830         
831         /* prevent crashes */
832         if (ob==NULL) return;
833         
834         /* get inverse of (armature) object's matrix  */
835         Mat4Invert(obmat, ob->obmat);
836         
837         /* multiply given matrix by object's-inverse to find pose-space matrix */
838         Mat4MulMat4(outmat, obmat, inmat);
839 }
840
841 /* Convert Wolrd-Space Location to Pose-Space Location
842  * NOTE: this cannot be used to convert to pose-space location of the supplied
843  *              pose-channel into its local space (i.e. 'visual'-keyframing) 
844  */
845 void armature_loc_world_to_pose(Object *ob, float *inloc, float *outloc) 
846 {
847         float xLocMat[4][4];
848         float nLocMat[4][4];
849         
850         /* build matrix for location */
851         Mat4One(xLocMat);
852         VECCOPY(xLocMat[3], inloc);
853
854         /* get bone-space cursor matrix and extract location */
855         armature_mat_world_to_pose(ob, xLocMat, nLocMat);
856         VECCOPY(outloc, nLocMat[3]);
857 }
858
859 /* Convert Pose-Space Matrix to Bone-Space Matrix 
860  * NOTE: this cannot be used to convert to pose-space transforms of the supplied
861  *              pose-channel into its local space (i.e. 'visual'-keyframing)
862  */
863 void armature_mat_pose_to_bone(bPoseChannel *pchan, float inmat[][4], float outmat[][4])
864 {
865         float pc_trans[4][4], inv_trans[4][4];
866         float pc_posemat[4][4], inv_posemat[4][4];
867         
868         /* paranoia: prevent crashes with no pose-channel supplied */
869         if (pchan==NULL) return;
870         
871         /* get the inverse matrix of the pchan's transforms */
872         LocQuatSizeToMat4(pc_trans, pchan->loc, pchan->quat, pchan->size);
873         Mat4Invert(inv_trans, pc_trans);
874         
875         /* Remove the pchan's transforms from it's pose_mat.
876          * This should leave behind the effects of restpose + 
877          * parenting + constraints
878          */
879         Mat4MulMat4(pc_posemat, inv_trans, pchan->pose_mat);
880         
881         /* get the inverse of the leftovers so that we can remove 
882          * that component from the supplied matrix
883          */
884         Mat4Invert(inv_posemat, pc_posemat);
885         
886         /* get the new matrix */
887         Mat4MulMat4(outmat, inmat, inv_posemat);
888 }
889
890 /* Convert Pose-Space Location to Bone-Space Location
891  * NOTE: this cannot be used to convert to pose-space location of the supplied
892  *              pose-channel into its local space (i.e. 'visual'-keyframing) 
893  */
894 void armature_loc_pose_to_bone(bPoseChannel *pchan, float *inloc, float *outloc) 
895 {
896         float xLocMat[4][4];
897         float nLocMat[4][4];
898         
899         /* build matrix for location */
900         Mat4One(xLocMat);
901         VECCOPY(xLocMat[3], inloc);
902
903         /* get bone-space cursor matrix and extract location */
904         armature_mat_pose_to_bone(pchan, xLocMat, nLocMat);
905         VECCOPY(outloc, nLocMat[3]);
906 }
907
908 /* Remove rest-position effects from pose-transform for obtaining
909  * 'visual' transformation of pose-channel. 
910  * (used by the Visual-Keyframing stuff)
911  */
912 void armature_mat_pose_to_delta(float delta_mat[][4], float pose_mat[][4], float arm_mat[][4])
913 {
914         float imat[4][4];
915  
916         Mat4Invert(imat, arm_mat);
917         Mat4MulMat4(delta_mat, pose_mat, imat);
918 }
919
920
921 /* **************** The new & simple (but OK!) armature evaluation ********* */ 
922
923 /*  ****************** And how it works! ****************************************
924
925   This is the bone transformation trick; they're hierarchical so each bone(b)
926   is in the coord system of bone(b-1):
927
928   arm_mat(b)= arm_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) 
929   
930   -> yoffs is just the y axis translation in parent's coord system
931   -> d_root is the translation of the bone root, also in parent's coord system
932
933   pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b)
934
935   we then - in init deform - store the deform in chan_mat, such that:
936
937   pose_mat(b)= arm_mat(b) * chan_mat(b)
938   
939   *************************************************************************** */
940 /*  Computes vector and roll based on a rotation. "mat" must
941      contain only a rotation, and no scaling. */ 
942 void mat3_to_vec_roll(float mat[][3], float *vec, float *roll) 
943 {
944      if (vec)
945          VecCopyf(vec, mat[1]);
946
947      if (roll) {
948          float vecmat[3][3], vecmatinv[3][3], rollmat[3][3];
949
950          vec_roll_to_mat3(mat[1], 0.0f, vecmat);
951          Mat3Inv(vecmatinv, vecmat);
952          Mat3MulMat3(rollmat, vecmatinv, mat);
953
954          *roll= atan2(rollmat[2][0], rollmat[2][2]);
955      }
956 }
957
958 /*      Calculates the rest matrix of a bone based
959         On its vector and a roll around that vector */
960 void vec_roll_to_mat3(float *vec, float roll, float mat[][3])
961 {
962         float   nor[3], axis[3], target[3]={0,1,0};
963         float   theta;
964         float   rMatrix[3][3], bMatrix[3][3];
965         
966         VECCOPY (nor, vec);
967         Normalise (nor);
968         
969         /*      Find Axis & Amount for bone matrix*/
970         Crossf (axis,target,nor);
971
972         if (Inpf(axis,axis) > 0.0000000000001) {
973                 /* if nor is *not* a multiple of target ... */
974                 Normalise (axis);
975                 
976                 theta= NormalizedVecAngle2(target, nor);
977                 
978                 /*      Make Bone matrix*/
979                 VecRotToMat3(axis, theta, bMatrix);
980         }
981         else {
982                 /* if nor is a multiple of target ... */
983                 float updown;
984                 
985                 /* point same direction, or opposite? */
986                 updown = ( Inpf (target,nor) > 0 ) ? 1.0 : -1.0;
987                 
988                 /* I think this should work ... */
989                 bMatrix[0][0]=updown; bMatrix[0][1]=0.0;    bMatrix[0][2]=0.0;
990                 bMatrix[1][0]=0.0;    bMatrix[1][1]=updown; bMatrix[1][2]=0.0;
991                 bMatrix[2][0]=0.0;    bMatrix[2][1]=0.0;    bMatrix[2][2]=1.0;
992         }
993         
994         /*      Make Roll matrix*/
995         VecRotToMat3(nor, roll, rMatrix);
996         
997         /*      Combine and output result*/
998         Mat3MulMat3 (mat, rMatrix, bMatrix);
999 }
1000
1001
1002 /* recursive part, calculates restposition of entire tree of children */
1003 /* used by exiting editmode too */
1004 void where_is_armature_bone(Bone *bone, Bone *prevbone)
1005 {
1006         float vec[3];
1007         
1008         /* Bone Space */
1009         VecSubf (vec, bone->tail, bone->head);
1010         vec_roll_to_mat3(vec, bone->roll, bone->bone_mat);
1011
1012         bone->length= VecLenf(bone->head, bone->tail);
1013         
1014         /* this is called on old file reading too... */
1015         if(bone->xwidth==0.0) {
1016                 bone->xwidth= 0.1f;
1017                 bone->zwidth= 0.1f;
1018                 bone->segments= 1;
1019         }
1020         
1021         if(prevbone) {
1022                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
1023                 
1024                 /* bone transform itself */
1025                 Mat4CpyMat3(offs_bone, bone->bone_mat);
1026                                 
1027                 /* The bone's root offset (is in the parent's coordinate system) */
1028                 VECCOPY(offs_bone[3], bone->head);
1029
1030                 /* Get the length translation of parent (length along y axis) */
1031                 offs_bone[3][1]+= prevbone->length;
1032                 
1033                 /* Compose the matrix for this bone  */
1034                 Mat4MulMat4(bone->arm_mat, offs_bone, prevbone->arm_mat);
1035         }
1036         else {
1037                 Mat4CpyMat3(bone->arm_mat, bone->bone_mat);
1038                 VECCOPY(bone->arm_mat[3], bone->head);
1039         }
1040         
1041         /* head */
1042         VECCOPY(bone->arm_head, bone->arm_mat[3]);
1043         /* tail is in current local coord system */
1044         VECCOPY(vec, bone->arm_mat[1]);
1045         VecMulf(vec, bone->length);
1046         VecAddf(bone->arm_tail, bone->arm_head, vec);
1047         
1048         /* and the kiddies */
1049         prevbone= bone;
1050         for(bone= bone->childbase.first; bone; bone= bone->next) {
1051                 where_is_armature_bone(bone, prevbone);
1052         }
1053 }
1054
1055 /* updates vectors and matrices on rest-position level, only needed 
1056    after editing armature itself, now only on reading file */
1057 void where_is_armature (bArmature *arm)
1058 {
1059         Bone *bone;
1060         
1061         /* hierarchical from root to children */
1062         for(bone= arm->bonebase.first; bone; bone= bone->next) {
1063                 where_is_armature_bone(bone, NULL);
1064         }
1065 }
1066
1067 /* if bone layer is protected, copy the data from from->pose */
1068 static void pose_proxy_synchronize(Object *ob, Object *from, int layer_protected)
1069 {
1070         bPose *pose= ob->pose, *frompose= from->pose;
1071         bPoseChannel *pchan, *pchanp, pchanw;
1072         bConstraint *con;
1073         char *str;
1074         
1075         if(frompose==NULL) return;
1076         
1077         /* exception, armature local layer should be proxied too */
1078         if(pose->proxy_layer)
1079                 ((bArmature *)ob->data)->layer= pose->proxy_layer;
1080         
1081         /* clear all transformation values from library */
1082         rest_pose(frompose);
1083         
1084         pchan= pose->chanbase.first;
1085         for(; pchan; pchan= pchan->next) {
1086                 if(pchan->bone->layer & layer_protected) {
1087                         pchanp= get_pose_channel(frompose, pchan->name);
1088                         
1089                         /* copy posechannel to temp, but restore important pointers */
1090                         pchanw= *pchanp;
1091                         pchanw.prev= pchan->prev;
1092                         pchanw.next= pchan->next;
1093                         pchanw.parent= pchan->parent;
1094                         pchanw.child= pchan->child;
1095                         pchanw.path= NULL;
1096                         
1097                         /* constraints, set target ob pointer to own object */
1098                         copy_constraints(&pchanw.constraints, &pchanp->constraints);
1099
1100                         for(con= pchanw.constraints.first; con; con= con->next) {
1101                                 if(from==get_constraint_target(con, &str))
1102                                         set_constraint_target(con, ob, NULL);
1103                         }
1104                         
1105                         /* free stuff from current channel */
1106                         if(pchan->path) MEM_freeN(pchan->path);
1107                         free_constraints(&pchan->constraints);
1108                         
1109                         /* the final copy */
1110                         *pchan= pchanw;
1111                 }
1112         }
1113 }
1114
1115 static int rebuild_pose_bone(bPose *pose, Bone *bone, bPoseChannel *parchan, int counter)
1116 {
1117         bPoseChannel *pchan = verify_pose_channel (pose, bone->name);   // verify checks and/or adds
1118
1119         pchan->bone= bone;
1120         pchan->parent= parchan;
1121         
1122         counter++;
1123         
1124         for(bone= bone->childbase.first; bone; bone= bone->next) {
1125                 counter= rebuild_pose_bone(pose, bone, pchan, counter);
1126                 /* for quick detecting of next bone in chain, only b-bone uses it now */
1127                 if(bone->flag & BONE_CONNECTED)
1128                         pchan->child= get_pose_channel(pose, bone->name);
1129         }
1130         
1131         return counter;
1132 }
1133
1134 /* only after leave editmode, duplicating, validating older files, library syncing */
1135 /* NOTE: pose->flag is set for it */
1136 void armature_rebuild_pose(Object *ob, bArmature *arm)
1137 {
1138         Bone *bone;
1139         bPose *pose;
1140         bPoseChannel *pchan, *next;
1141         int counter=0;
1142                 
1143         /* only done here */
1144         if(ob->pose==NULL) ob->pose= MEM_callocN(sizeof(bPose), "new pose");
1145         pose= ob->pose;
1146         
1147         /* clear */
1148         for(pchan= pose->chanbase.first; pchan; pchan= pchan->next) {
1149                 pchan->bone= NULL;
1150                 pchan->child= NULL;
1151         }
1152         
1153         /* first step, check if all channels are there */
1154         for(bone= arm->bonebase.first; bone; bone= bone->next) {
1155                 counter= rebuild_pose_bone(pose, bone, NULL, counter);
1156         }
1157
1158         /* and a check for garbage */
1159         for(pchan= pose->chanbase.first; pchan; pchan= next) {
1160                 next= pchan->next;
1161                 if(pchan->bone==NULL) {
1162                         if(pchan->path)
1163                                 MEM_freeN(pchan->path);
1164                         free_constraints(&pchan->constraints);
1165                         BLI_freelinkN(&pose->chanbase, pchan);
1166                 }
1167         }
1168         // printf("rebuild pose %s, %d bones\n", ob->id.name, counter);
1169         
1170         /* synchronize protected layers with proxy */
1171         if(ob->proxy)
1172                 pose_proxy_synchronize(ob, ob->proxy, arm->layer_protected);
1173         
1174         update_pose_constraint_flags(ob->pose); // for IK detection for example
1175         
1176         /* the sorting */
1177         if(counter>1)
1178                 DAG_pose_sort(ob);
1179         
1180         ob->pose->flag &= ~POSE_RECALC;
1181 }
1182
1183
1184 /* ********************** THE IK SOLVER ******************* */
1185
1186
1187
1188 /* allocates PoseTree, and links that to root bone/channel */
1189 /* Note: detecting the IK chain is duplicate code... in drawarmature.c and in transform_conversions.c */
1190 static void initialize_posetree(struct Object *ob, bPoseChannel *pchan_tip)
1191 {
1192         bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
1193         PoseTree *tree;
1194         PoseTarget *target;
1195         bConstraint *con;
1196         bKinematicConstraint *data;
1197         int a, segcount= 0, size, newsize, *oldparent, parent;
1198         
1199         /* find IK constraint, and validate it */
1200         for(con= pchan_tip->constraints.first; con; con= con->next) {
1201                 if(con->type==CONSTRAINT_TYPE_KINEMATIC) break;
1202         }
1203         if(con==NULL) return;
1204         
1205         data=(bKinematicConstraint*)con->data;
1206         
1207         /* two types of targets */
1208         if(data->flag & CONSTRAINT_IK_AUTO);
1209         else {
1210                 if(con->flag & CONSTRAINT_DISABLE) return;      /* checked in editconstraint.c */
1211                 if(con->enforce == 0.0f) return;
1212                 if(data->tar==NULL) return;
1213                 if(data->tar->type==OB_ARMATURE && data->subtarget[0]==0) return;
1214         }
1215         
1216         /* exclude tip from chain? */
1217         if(!(data->flag & CONSTRAINT_IK_TIP))
1218                 pchan_tip= pchan_tip->parent;
1219         
1220         /* Find the chain's root & count the segments needed */
1221         for (curchan = pchan_tip; curchan; curchan=curchan->parent){
1222                 pchan_root = curchan;
1223                 
1224                 curchan->flag |= POSE_CHAIN;    // don't forget to clear this
1225                 chanlist[segcount]=curchan;
1226                 segcount++;
1227                 
1228                 if(segcount==data->rootbone || segcount>255) break; // 255 is weak
1229         }
1230         if (!segcount) return;
1231
1232         /* setup the chain data */
1233         
1234         /* we make tree-IK, unless all existing targets are in this chain */
1235         for(tree= pchan_root->iktree.first; tree; tree= tree->next) {
1236                 for(target= tree->targets.first; target; target= target->next) {
1237                         curchan= tree->pchan[target->tip];
1238                         if(curchan->flag & POSE_CHAIN)
1239                                 curchan->flag &= ~POSE_CHAIN;
1240                         else
1241                                 break;
1242                 }
1243                 if(target) break;
1244         }
1245
1246         /* create a target */
1247         target= MEM_callocN(sizeof(PoseTarget), "posetarget");
1248         target->con= con;
1249         pchan_tip->flag &= ~POSE_CHAIN;
1250
1251         if(tree==NULL) {
1252                 /* make new tree */
1253                 tree= MEM_callocN(sizeof(PoseTree), "posetree");
1254
1255                 tree->iterations= data->iterations;
1256                 tree->totchannel= segcount;
1257                 tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
1258                 
1259                 tree->pchan= MEM_callocN(segcount*sizeof(void*), "ik tree pchan");
1260                 tree->parent= MEM_callocN(segcount*sizeof(int), "ik tree parent");
1261                 for(a=0; a<segcount; a++) {
1262                         tree->pchan[a]= chanlist[segcount-a-1];
1263                         tree->parent[a]= a-1;
1264                 }
1265                 target->tip= segcount-1;
1266                 
1267                 /* AND! link the tree to the root */
1268                 BLI_addtail(&pchan_root->iktree, tree);
1269         }
1270         else {
1271                 tree->iterations= MAX2(data->iterations, tree->iterations);
1272                 tree->stretch= tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
1273
1274                 /* skip common pose channels and add remaining*/
1275                 size= MIN2(segcount, tree->totchannel);
1276                 for(a=0; a<size && tree->pchan[a]==chanlist[segcount-a-1]; a++);
1277                 parent= a-1;
1278
1279                 segcount= segcount-a;
1280                 target->tip= tree->totchannel + segcount - 1;
1281
1282                 if (segcount > 0) {
1283                         /* resize array */
1284                         newsize= tree->totchannel + segcount;
1285                         oldchan= tree->pchan;
1286                         oldparent= tree->parent;
1287
1288                         tree->pchan= MEM_callocN(newsize*sizeof(void*), "ik tree pchan");
1289                         tree->parent= MEM_callocN(newsize*sizeof(int), "ik tree parent");
1290                         memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel);
1291                         memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel);
1292                         MEM_freeN(oldchan);
1293                         MEM_freeN(oldparent);
1294
1295                         /* add new pose channels at the end, in reverse order */
1296                         for(a=0; a<segcount; a++) {
1297                                 tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1];
1298                                 tree->parent[tree->totchannel+a]= tree->totchannel+a-1;
1299                         }
1300                         tree->parent[tree->totchannel]= parent;
1301                         
1302                         tree->totchannel= newsize;
1303                 }
1304
1305                 /* move tree to end of list, for correct evaluation order */
1306                 BLI_remlink(&pchan_root->iktree, tree);
1307                 BLI_addtail(&pchan_root->iktree, tree);
1308         }
1309
1310         /* add target to the tree */
1311         BLI_addtail(&tree->targets, target);
1312 }
1313
1314 /* called from within the core where_is_pose loop, all animsystems and constraints
1315 were executed & assigned. Now as last we do an IK pass */
1316 static void execute_posetree(Object *ob, PoseTree *tree)
1317 {
1318         float R_parmat[3][3];
1319         float iR_parmat[3][3];
1320         float R_bonemat[3][3];
1321         float goalrot[3][3], goalpos[3];
1322         float rootmat[4][4], imat[4][4];
1323         float goal[4][4], goalinv[4][4];
1324         float size[3], irest_basis[3][3], full_basis[3][3];
1325         float end_pose[4][4], world_pose[4][4];
1326         float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch=NULL;
1327         int a, flag, hasstretch=0;
1328         bPoseChannel *pchan;
1329         IK_Segment *seg, *parent, **iktree, *iktarget;
1330         IK_Solver *solver;
1331         PoseTarget *target;
1332         bKinematicConstraint *data;
1333         Bone *bone;
1334
1335         if (tree->totchannel == 0)
1336                 return;
1337
1338         iktree= MEM_mallocN(sizeof(void*)*tree->totchannel, "ik tree");
1339
1340         for(a=0; a<tree->totchannel; a++) {
1341                 pchan= tree->pchan[a];
1342                 bone= pchan->bone;
1343
1344                 /* set DoF flag */
1345                 flag= 0;
1346                 if((pchan->ikflag & BONE_IK_NO_XDOF) == 0)
1347                         flag |= IK_XDOF;
1348                 if((pchan->ikflag & BONE_IK_NO_YDOF) == 0)
1349                         flag |= IK_YDOF;
1350                 if((pchan->ikflag & BONE_IK_NO_ZDOF) == 0)
1351                         flag |= IK_ZDOF;
1352
1353                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
1354                         flag |= IK_TRANS_YDOF;
1355                         hasstretch = 1;
1356                 }
1357
1358                 seg= iktree[a]= IK_CreateSegment(flag);
1359
1360                 /* find parent */
1361                 if(a == 0)
1362                         parent= NULL;
1363                 else
1364                         parent= iktree[tree->parent[a]];
1365
1366                 IK_SetParent(seg, parent);
1367         
1368                 /* get the matrix that transforms from prevbone into this bone */
1369                 Mat3CpyMat4(R_bonemat, pchan->pose_mat);
1370
1371                 /* gather transformations for this IK segment */
1372
1373                 if (pchan->parent)
1374                         Mat3CpyMat4(R_parmat, pchan->parent->pose_mat);
1375                 else
1376                         Mat3One(R_parmat);
1377
1378                 /* bone offset */
1379                 if (pchan->parent && (a > 0))
1380                         VecSubf(start, pchan->pose_head, pchan->parent->pose_tail);
1381                 else
1382                         /* only root bone (a = 0) has no parent */
1383                         start[0]= start[1]= start[2]= 0.0f;
1384                 
1385                 /* change length based on bone size */
1386                 length= bone->length*VecLength(R_bonemat[1]);
1387
1388                 /* compute rest basis and its inverse */
1389                 Mat3CpyMat3(rest_basis, bone->bone_mat);
1390                 Mat3CpyMat3(irest_basis, bone->bone_mat);
1391                 Mat3Transp(irest_basis);
1392
1393                 /* compute basis with rest_basis removed */
1394                 Mat3Inv(iR_parmat, R_parmat);
1395                 Mat3MulMat3(full_basis, iR_parmat, R_bonemat);
1396                 Mat3MulMat3(basis, irest_basis, full_basis);
1397
1398                 /* basis must be pure rotation */
1399                 Mat3Ortho(basis);
1400
1401                 /* transform offset into local bone space */
1402                 Mat3Ortho(iR_parmat);
1403                 Mat3MulVecfl(iR_parmat, start);
1404
1405                 IK_SetTransform(seg, start, rest_basis, basis, length);
1406
1407                 if (pchan->ikflag & BONE_IK_XLIMIT)
1408                         IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]);
1409                 if (pchan->ikflag & BONE_IK_YLIMIT)
1410                         IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]);
1411                 if (pchan->ikflag & BONE_IK_ZLIMIT)
1412                         IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]);
1413
1414                 IK_SetStiffness(seg, IK_X, pchan->stiffness[0]);
1415                 IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]);
1416                 IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]);
1417
1418                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
1419                         float ikstretch = pchan->ikstretch*pchan->ikstretch;
1420                         IK_SetStiffness(seg, IK_TRANS_Y, MIN2(1.0-ikstretch, 0.99));
1421                         IK_SetLimit(seg, IK_TRANS_Y, 0.001, 1e10);
1422                 }
1423         }
1424
1425         solver= IK_CreateSolver(iktree[0]);
1426
1427         /* set solver goals */
1428
1429         /* first set the goal inverse transform, assuming the root of tree was done ok! */
1430         pchan= tree->pchan[0];
1431         if (pchan->parent)
1432                 /* transform goal by parent mat, so this rotation is not part of the
1433                    segment's basis. otherwise rotation limits do not work on the
1434                    local transform of the segment itself. */
1435                 Mat4CpyMat4(rootmat, pchan->parent->pose_mat);
1436         else
1437                 Mat4One(rootmat);
1438         VECCOPY(rootmat[3], pchan->pose_head);
1439         
1440         Mat4MulMat4 (imat, rootmat, ob->obmat);
1441         Mat4Invert (goalinv, imat);
1442         
1443         for(target=tree->targets.first; target; target=target->next) {
1444                 data= (bKinematicConstraint*)target->con->data;
1445
1446                 /* 1.0=ctime, we pass on object for auto-ik */
1447                 get_constraint_target_matrix(target->con, TARGET_BONE, ob, rootmat, size, 1.0);
1448
1449                 /* and set and transform goal */
1450                 Mat4MulMat4(goal, rootmat, goalinv);
1451
1452                 VECCOPY(goalpos, goal[3]);
1453                 Mat3CpyMat4(goalrot, goal);
1454
1455                 /* do we need blending? */
1456                 if(target->con->enforce!=1.0) {
1457                         float q1[4], q2[4], q[4];
1458                         float fac= target->con->enforce;
1459                         float mfac= 1.0-fac;
1460                         
1461                         pchan= tree->pchan[target->tip];
1462
1463                         /* end effector in world space */
1464                         Mat4CpyMat4(end_pose, pchan->pose_mat);
1465                         VECCOPY(end_pose[3], pchan->pose_tail);
1466                         Mat4MulSerie(world_pose, goalinv, ob->obmat, end_pose, 0, 0, 0, 0, 0);
1467
1468                         /* blend position */
1469                         goalpos[0]= fac*goalpos[0] + mfac*world_pose[3][0];
1470                         goalpos[1]= fac*goalpos[1] + mfac*world_pose[3][1];
1471                         goalpos[2]= fac*goalpos[2] + mfac*world_pose[3][2];
1472
1473                         /* blend rotation */
1474                         Mat3ToQuat(goalrot, q1);
1475                         Mat4ToQuat(world_pose, q2);
1476                         QuatInterpol(q, q1, q2, mfac);
1477                         QuatToMat3(q, goalrot);
1478                 }
1479
1480                 iktarget= iktree[target->tip];
1481
1482                 if(data->weight != 0.0)
1483                         IK_SolverAddGoal(solver, iktarget, goalpos, data->weight);
1484                 if((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0) && (data->flag & CONSTRAINT_IK_AUTO)==0)
1485                         IK_SolverAddGoalOrientation(solver, iktarget, goalrot, data->orientweight);
1486         }
1487
1488         /* solve */
1489         IK_Solve(solver, 0.0f, tree->iterations);
1490         IK_FreeSolver(solver);
1491
1492         /* gather basis changes */
1493         tree->basis_change= MEM_mallocN(sizeof(float[3][3])*tree->totchannel, "ik basis change");
1494         if(hasstretch)
1495                 ikstretch= MEM_mallocN(sizeof(float)*tree->totchannel, "ik stretch");
1496
1497         for(a=0; a<tree->totchannel; a++) {
1498                 IK_GetBasisChange(iktree[a], tree->basis_change[a]);
1499
1500                 if(hasstretch) {
1501                         /* have to compensate for scaling received from parent */
1502                         float parentstretch, stretch;
1503
1504                         pchan= tree->pchan[a];
1505                         parentstretch= (tree->parent[a] >= 0)? ikstretch[tree->parent[a]]: 1.0;
1506
1507                         if(tree->stretch && (pchan->ikstretch > 0.0)) {
1508                                 float trans[3], length;
1509
1510                                 IK_GetTranslationChange(iktree[a], trans);
1511                                 length= pchan->bone->length*VecLength(pchan->pose_mat[1]);
1512
1513                                 ikstretch[a]= (length == 0.0)? 1.0: (trans[1]+length)/length;
1514                         }
1515                         else
1516                                 ikstretch[a] = 1.0;
1517
1518                         stretch= (parentstretch == 0.0)? 1.0: ikstretch[a]/parentstretch;
1519
1520                         VecMulf(tree->basis_change[a][0], stretch);
1521                         VecMulf(tree->basis_change[a][1], stretch);
1522                         VecMulf(tree->basis_change[a][2], stretch);
1523
1524                 }
1525
1526                 IK_FreeSegment(iktree[a]);
1527         }
1528         
1529         MEM_freeN(iktree);
1530         if(ikstretch) MEM_freeN(ikstretch);
1531 }
1532
1533 void free_posetree(PoseTree *tree)
1534 {
1535         BLI_freelistN(&tree->targets);
1536         if(tree->pchan) MEM_freeN(tree->pchan);
1537         if(tree->parent) MEM_freeN(tree->parent);
1538         if(tree->basis_change) MEM_freeN(tree->basis_change);
1539         MEM_freeN(tree);
1540 }
1541
1542 /* ********************** THE POSE SOLVER ******************* */
1543
1544
1545 /* loc/rot/size to mat4 */
1546 /* used in constraint.c too */
1547 void chan_calc_mat(bPoseChannel *chan)
1548 {
1549         float smat[3][3];
1550         float rmat[3][3];
1551         float tmat[3][3];
1552         
1553         SizeToMat3(chan->size, smat);
1554         
1555         NormalQuat(chan->quat);
1556
1557         QuatToMat3(chan->quat, rmat);
1558         
1559         Mat3MulMat3(tmat, rmat, smat);
1560         
1561         Mat4CpyMat3(chan->chan_mat, tmat);
1562         
1563         /* prevent action channels breaking chains */
1564         /* need to check for bone here, CONSTRAINT_TYPE_ACTION uses this call */
1565         if (chan->bone==NULL || !(chan->bone->flag & BONE_CONNECTED)) {
1566                 VECCOPY(chan->chan_mat[3], chan->loc);
1567         }
1568
1569 }
1570
1571 /* transform from bone(b) to bone(b+1), store in chan_mat */
1572 static void make_dmats(bPoseChannel *pchan)
1573 {
1574         if (pchan->parent) {
1575                 float iR_parmat[4][4];
1576                 Mat4Invert(iR_parmat, pchan->parent->pose_mat);
1577                 Mat4MulMat4(pchan->chan_mat,  pchan->pose_mat, iR_parmat);      // delta mat
1578         }
1579         else Mat4CpyMat4(pchan->chan_mat, pchan->pose_mat);
1580 }
1581
1582 /* applies IK matrix to pchan, IK is done separated */
1583 /* formula: pose_mat(b) = pose_mat(b-1) * diffmat(b-1, b) * ik_mat(b) */
1584 /* to make this work, the diffmats have to be precalculated! Stored in chan_mat */
1585 static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[][3])   // nr = to detect if this is first bone
1586 {
1587         float vec[3], ikmat[4][4];
1588         
1589         Mat4CpyMat3(ikmat, ik_mat);
1590         
1591         if (pchan->parent)
1592                 Mat4MulSerie(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat, ikmat, NULL, NULL, NULL, NULL, NULL);
1593         else 
1594                 Mat4MulMat4(pchan->pose_mat, ikmat, pchan->chan_mat);
1595
1596         /* calculate head */
1597         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
1598         /* calculate tail */
1599         VECCOPY(vec, pchan->pose_mat[1]);
1600         VecMulf(vec, pchan->bone->length);
1601         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
1602
1603         pchan->flag |= POSE_DONE;
1604 }
1605
1606 static void do_local_constraint(bPoseChannel *pchan, bConstraint *con)
1607 {
1608         switch(con->type) {
1609                 case CONSTRAINT_TYPE_LOCLIKE:
1610                 {
1611                         bLocateLikeConstraint *data= con->data;
1612                         float fac= con->enforce;
1613                         
1614                         if(data->tar && data->subtarget[0]) {
1615                                 bPoseChannel *pchant= get_pose_channel(data->tar->pose, data->subtarget);
1616                                 if(pchant) {
1617                                         if (data->flag & LOCLIKE_X)
1618                                                 pchan->loc[0]= FloatLerpf(pchant->loc[0], pchan->loc[0], fac);
1619                                         if (data->flag & LOCLIKE_Y)
1620                                                 pchan->loc[1]= FloatLerpf(pchant->loc[1], pchan->loc[1], fac);
1621                                         if (data->flag & LOCLIKE_Z)
1622                                                 pchan->loc[2]= FloatLerpf(pchant->loc[2], pchan->loc[2], fac);
1623                                 }
1624                         }
1625                 }
1626                         break;
1627                 case CONSTRAINT_TYPE_ROTLIKE:
1628                 {
1629                         bRotateLikeConstraint *data= con->data;
1630                         if(data->tar && data->subtarget[0]) {
1631                                 bPoseChannel *pchant= get_pose_channel(data->tar->pose, data->subtarget);
1632                                 if(pchant) {
1633                                         if(data->flag != (ROTLIKE_X|ROTLIKE_Y|ROTLIKE_Z)) {
1634                                                 float eul[3], eult[3], euln[3], fac= con->enforce;
1635                                                 
1636                                                 QuatToEul(pchan->quat, eul);
1637                                                 QuatToEul(pchant->quat, eult);
1638                                                 VECCOPY(euln, eul);
1639                                                 if(data->flag & ROTLIKE_X) euln[0]= FloatLerpf(eult[0], eul[0], fac); 
1640                                                 if(data->flag & ROTLIKE_Y) euln[1]= FloatLerpf(eult[1], eul[1], fac);
1641                                                 if(data->flag & ROTLIKE_Z) euln[2]= FloatLerpf(eult[2], eul[2], fac);
1642                                                 compatible_eul(eul, euln);
1643                                                 EulToQuat(euln, pchan->quat);
1644                                         }
1645                                         else {
1646                                                 QuatInterpol(pchan->quat, pchan->quat, pchant->quat, con->enforce);
1647                                         }
1648                                 }
1649                         }
1650                 }
1651                         break;
1652                 case CONSTRAINT_TYPE_SIZELIKE:
1653                 {
1654                         bSizeLikeConstraint *data= con->data;
1655                         float fac= con->enforce;
1656                         
1657                         if(data->tar && data->subtarget[0]) {
1658                                 bPoseChannel *pchant= get_pose_channel(data->tar->pose, data->subtarget);
1659                                 if(pchant) {
1660                                         if (data->flag & SIZELIKE_X)
1661                                                 pchan->size[0]= FloatLerpf(pchant->size[0], pchan->size[0], fac);
1662                                         if (data->flag & SIZELIKE_Y)
1663                                                 pchan->size[1]= FloatLerpf(pchant->size[1], pchan->size[1], fac);
1664                                         if (data->flag & SIZELIKE_Z)
1665                                                 pchan->size[2]= FloatLerpf(pchant->size[2], pchan->size[2], fac);
1666                                 }
1667                         }
1668                 }
1669                         break;
1670                 case CONSTRAINT_TYPE_LOCLIMIT:
1671                 {
1672                         bLocLimitConstraint *data= con->data;
1673                         float fac= con->enforce;
1674                         
1675                         if (data->flag & LIMIT_XMIN) {
1676                                 if(pchan->loc[0] < data->xmin)
1677                                         pchan->loc[0] = FloatLerpf(data->xmin, pchan->loc[0], fac);
1678                         }
1679                         if (data->flag & LIMIT_XMAX) {
1680                                 if (pchan->loc[0] > data->xmax)
1681                                         pchan->loc[0] = FloatLerpf(data->xmax, pchan->loc[0], fac);
1682                         }
1683                         if (data->flag & LIMIT_YMIN) {
1684                                 if(pchan->loc[1] < data->ymin)
1685                                         pchan->loc[1] = FloatLerpf(data->ymin, pchan->loc[1], fac);
1686                         }
1687                         if (data->flag & LIMIT_YMAX) {
1688                                 if (pchan->loc[1] > data->ymax)
1689                                         pchan->loc[1] = FloatLerpf(data->ymax, pchan->loc[1], fac);
1690                         }
1691                         if (data->flag & LIMIT_ZMIN) {
1692                                 if(pchan->loc[2] < data->zmin)
1693                                         pchan->loc[2] = FloatLerpf(data->zmin, pchan->loc[2], fac);
1694                         }
1695                         if (data->flag & LIMIT_ZMAX) {
1696                                 if (pchan->loc[2] > data->zmax)
1697                                         pchan->loc[2] = FloatLerpf(data->zmax, pchan->loc[2], fac);
1698                         }
1699                 }       
1700                         break;
1701                 case CONSTRAINT_TYPE_ROTLIMIT:
1702                 {
1703                         bRotLimitConstraint *data = con->data;
1704                         float eul[3];
1705                         float fac= con->enforce;
1706                         
1707                         QuatToEul(pchan->quat, eul);
1708                         
1709                         /* eulers: radians to degrees! */
1710                         eul[0] = (eul[0] / (2*M_PI) * 360);
1711                         eul[1] = (eul[1] / (2*M_PI) * 360);
1712                         eul[2] = (eul[2] / (2*M_PI) * 360);
1713                         
1714                         /* limiting of euler values... */
1715                         if (data->flag & LIMIT_XROT) {
1716                                 if (eul[0] < data->xmin) 
1717                                         eul[0] = FloatLerpf(data->xmin, eul[0], fac);
1718                                         
1719                                 if (eul[0] > data->xmax)
1720                                         eul[0] = FloatLerpf(data->xmax, eul[0], fac);
1721                         }
1722                         if (data->flag & LIMIT_YROT) {
1723                                 if (eul[1] < data->ymin)
1724                                         eul[1] = FloatLerpf(data->ymin, eul[1], fac);
1725                                         
1726                                 if (eul[1] > data->ymax)
1727                                         eul[1] = FloatLerpf(data->ymax, eul[1], fac);
1728                         }
1729                         if (data->flag & LIMIT_ZROT) {
1730                                 if (eul[2] < data->zmin)
1731                                         eul[2] = FloatLerpf(data->zmin, eul[2], fac);
1732                                         
1733                                 if (eul[2] > data->zmax)
1734                                         eul[2] = FloatLerpf(data->zmax, eul[2], fac);
1735                         }
1736                                 
1737                         /* eulers: degrees to radians ! */
1738                         eul[0] = (eul[0] / 360 * (2*M_PI)); 
1739                         eul[1] = (eul[1] / 360 * (2*M_PI));
1740                         eul[2] = (eul[2] / 360 * (2*M_PI));
1741                         
1742                         /* convert back */
1743                         EulToQuat(eul, pchan->quat);
1744                 }
1745                         break;
1746                 case CONSTRAINT_TYPE_SIZELIMIT:
1747                 {
1748                         bSizeLimitConstraint *data= con->data;
1749                         float fac= con->enforce;
1750                         
1751                         if (data->flag & LIMIT_XMIN) {
1752                                 if(pchan->size[0] < data->xmin)
1753                                         pchan->size[0] = FloatLerpf(data->xmin, pchan->size[0], fac);
1754                         }
1755                         if (data->flag & LIMIT_XMAX) {
1756                                 if (pchan->size[0] > data->xmax)
1757                                         pchan->size[0] = FloatLerpf(data->xmax, pchan->size[0], fac);
1758                         }
1759                         if (data->flag & LIMIT_YMIN) {
1760                                 if(pchan->size[1] < data->ymin)
1761                                         pchan->size[1] = FloatLerpf(data->ymin, pchan->size[1], fac);
1762                         }
1763                         if (data->flag & LIMIT_YMAX) {
1764                                 if (pchan->size[1] > data->ymax)
1765                                         pchan->size[1] = FloatLerpf(data->ymax, pchan->size[1], fac);
1766                         }
1767                         if (data->flag & LIMIT_ZMIN) {
1768                                 if(pchan->size[2] < data->zmin)
1769                                         pchan->size[2] = FloatLerpf(data->zmin, pchan->size[2], fac);
1770                         }
1771                         if (data->flag & LIMIT_ZMAX) {
1772                                 if (pchan->size[2] > data->zmax)
1773                                         pchan->size[2] = FloatLerpf(data->zmax, pchan->size[2], fac);
1774                         }
1775                 }
1776                         break;
1777         }
1778 }
1779
1780 static void do_strip_modifiers(Object *armob, Bone *bone, bPoseChannel *pchan)
1781 {
1782         bActionModifier *amod;
1783         bActionStrip *strip;
1784         float scene_cfra= G.scene->r.cfra;
1785
1786         for (strip=armob->nlastrips.first; strip; strip=strip->next) {
1787                 if(scene_cfra>=strip->start && scene_cfra<=strip->end) {
1788                         
1789                         /* temporal solution to prevent 2 strips accumulating */
1790                         if(scene_cfra==strip->end && strip->next && strip->next->start==scene_cfra)
1791                                 continue;
1792                         
1793                         for(amod= strip->modifiers.first; amod; amod= amod->next) {
1794                                 if(amod->type==ACTSTRIP_MOD_DEFORM) {
1795                                         /* validate first */
1796                                         if(amod->ob && amod->ob->type==OB_CURVE && amod->channel[0]) {
1797                                                 
1798                                                 if( strcmp(pchan->name, amod->channel)==0 ) {
1799                                                         float mat4[4][4], mat3[3][3];
1800                                                         
1801                                                         curve_deform_vector(amod->ob, armob, bone->arm_mat[3], pchan->pose_mat[3], mat3, amod->no_rot_axis);
1802                                                         Mat4CpyMat4(mat4, pchan->pose_mat);
1803                                                         Mat4MulMat34(pchan->pose_mat, mat3, mat4);
1804                                                         
1805                                                 }
1806                                         }
1807                                 }
1808                         }
1809                 }
1810         }
1811 }
1812
1813
1814 /* The main armature solver, does all constraints excluding IK */
1815 /* pchan is validated, as having bone and parent pointer */
1816 static void where_is_pose_bone(Object *ob, bPoseChannel *pchan, float ctime)
1817 {
1818         Bone *bone, *parbone;
1819         bPoseChannel *parchan;
1820         float vec[3], quat[4];
1821         int did_local= 0;       /* copying quaternion should be limited, chan_calc_mat() normalizes quat */
1822         
1823         /* set up variables for quicker access below */
1824         bone= pchan->bone;
1825         parbone= bone->parent;
1826         parchan= pchan->parent;
1827                 
1828         /* Do local constraints, these only work on the channel data (loc rot size) */
1829         QUATCOPY(quat, pchan->quat);
1830         if(pchan->constraints.first) {
1831                 bConstraint *con;
1832                 for(con=pchan->constraints.first; con; con= con->next) {
1833                         if(con->flag & CONSTRAINT_LOCAL) {
1834                                 do_local_constraint(pchan, con);
1835                                 did_local= 1;
1836                         }
1837                 }
1838         }
1839         
1840         /* this gives a chan_mat with actions (ipos) results */
1841         chan_calc_mat(pchan);
1842         
1843         if(did_local) 
1844                 QUATCOPY(pchan->quat, quat);    /* local constraint hack. bad! */
1845         
1846         /* construct the posemat based on PoseChannels, that we do before applying constraints */
1847         /* pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b) */
1848         
1849         if(parchan) {
1850                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
1851                 
1852                 /* bone transform itself */
1853                 Mat4CpyMat3(offs_bone, bone->bone_mat);
1854                 
1855                 /* The bone's root offset (is in the parent's coordinate system) */
1856                 VECCOPY(offs_bone[3], bone->head);
1857                 
1858                 /* Get the length translation of parent (length along y axis) */
1859                 offs_bone[3][1]+= parbone->length;
1860                 
1861                 /* Compose the matrix for this bone  */
1862                 if(bone->flag & BONE_HINGE) {   // uses restposition rotation, but actual position
1863                         float tmat[4][4];
1864                         
1865                         /* the rotation of the parent restposition */
1866                         Mat4CpyMat4(tmat, parbone->arm_mat);
1867                         
1868                         /* the location of actual parent transform */
1869                         VECCOPY(tmat[3], offs_bone[3]);
1870                         offs_bone[3][0]= offs_bone[3][1]= offs_bone[3][2]= 0.0f;
1871                         Mat4MulVecfl(parchan->pose_mat, tmat[3]);
1872                         
1873                         Mat4MulSerie(pchan->pose_mat, tmat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
1874                 }
1875                 else 
1876                         Mat4MulSerie(pchan->pose_mat, parchan->pose_mat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
1877         }
1878         else {
1879                 Mat4MulMat4(pchan->pose_mat, pchan->chan_mat, bone->arm_mat);
1880                 /* only rootbones get the cyclic offset */
1881                 VecAddf(pchan->pose_mat[3], pchan->pose_mat[3], ob->pose->cyclic_offset);
1882         }
1883         
1884         do_strip_modifiers(ob, bone, pchan);
1885         
1886         /* Do constraints */
1887         if(pchan->constraints.first) {
1888                 static Object conOb;
1889                 static int initialized= 0;
1890                 
1891                 VECCOPY(vec, pchan->pose_mat[3]);
1892                 
1893                 /* Build a workob to pass the bone to the constraint solver */
1894                 if(initialized==0) {
1895                         memset(&conOb, 0, sizeof(Object));
1896                         initialized= 1;
1897                 }
1898                 conOb.size[0]= conOb.size[1]= conOb.size[2]= 1.0;
1899                 conOb.data = ob->data;
1900                 conOb.type = ob->type;
1901                 conOb.parent = ob;      // ik solver retrieves the armature that way !?!?!?!
1902                 conOb.pose= ob->pose;                           // needed for retrieving pchan
1903                 conOb.trackflag = ob->trackflag;
1904                 conOb.upflag = ob->upflag;
1905                 
1906                 /* Collect the constraints from the pose (listbase copy) */
1907                 conOb.constraints = pchan->constraints;
1908                 
1909                 /* conOb.obmat takes bone to worldspace */
1910                 Mat4MulMat4 (conOb.obmat, pchan->pose_mat, ob->obmat);
1911                 
1912                 /* Solve */
1913                 solve_constraints (&conOb, TARGET_BONE, (void*)pchan, ctime);   // ctime doesnt alter objects
1914                 
1915                 /* Take out of worldspace */
1916                 Mat4MulMat4 (pchan->pose_mat, conOb.obmat, ob->imat);
1917                 
1918                 /* prevent constraints breaking a chain */
1919                 if(pchan->bone->flag & BONE_CONNECTED)
1920                         VECCOPY(pchan->pose_mat[3], vec);
1921
1922         }
1923         
1924         /* calculate head */
1925         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
1926         /* calculate tail */
1927         VECCOPY(vec, pchan->pose_mat[1]);
1928         VecMulf(vec, bone->length);
1929         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
1930         
1931 }
1932
1933 /* This only reads anim data from channels, and writes to channels */
1934 /* This is the only function adding poses */
1935 void where_is_pose (Object *ob)
1936 {
1937         bArmature *arm;
1938         Bone *bone;
1939         bPoseChannel *pchan;
1940         float imat[4][4];
1941         float ctime= bsystem_time(ob, NULL, (float)G.scene->r.cfra, 0.0);       /* not accurate... */
1942         
1943         arm = get_armature(ob);
1944         
1945         if(arm==NULL) return;
1946         if(ob->pose==NULL || (ob->pose->flag & POSE_RECALC)) 
1947            armature_rebuild_pose(ob, arm);
1948         
1949         /* In restposition we read the data from the bones */
1950         if(ob==G.obedit || (arm->flag & ARM_RESTPOS)) {
1951                 
1952                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1953                         bone= pchan->bone;
1954                         if(bone) {
1955                                 Mat4CpyMat4(pchan->pose_mat, bone->arm_mat);
1956                                 VECCOPY(pchan->pose_head, bone->arm_head);
1957                                 VECCOPY(pchan->pose_tail, bone->arm_tail);
1958                         }
1959                 }
1960         }
1961         else {
1962                 Mat4Invert(ob->imat, ob->obmat);        // imat is needed 
1963
1964                 /* 1. construct the PoseTrees, clear flags */
1965                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1966                         pchan->flag &= ~(POSE_DONE|POSE_CHAIN);
1967                         if(pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints
1968                                 initialize_posetree(ob, pchan); // will attach it to root!
1969                 }
1970                 
1971                 /* 2. the main loop, channels are already hierarchical sorted from root to children */
1972                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1973                         
1974                         /* 3. if we find an IK root, we handle it separated */
1975                         if(pchan->iktree.first) {
1976                                 while(pchan->iktree.first) {
1977                                         PoseTree *tree= pchan->iktree.first;
1978                                         int a;
1979                                         
1980                                         /* 4. walk over the tree for regular solving */
1981                                         for(a=0; a<tree->totchannel; a++) {
1982                                                 if(!(tree->pchan[a]->flag & POSE_DONE)) // successive trees can set the flag
1983                                                         where_is_pose_bone(ob, tree->pchan[a], ctime);
1984                                         }
1985                                         /* 5. execute the IK solver */
1986                                         execute_posetree(ob, tree);
1987                                         
1988                                         /* 6. apply the differences to the channels, 
1989                                                   we need to calculate the original differences first */
1990                                         for(a=0; a<tree->totchannel; a++)
1991                                                 make_dmats(tree->pchan[a]);
1992                                         
1993                                         for(a=0; a<tree->totchannel; a++)
1994                                                 /* sets POSE_DONE */
1995                                                 where_is_ik_bone(tree->pchan[a], tree->basis_change[a]);
1996                                         
1997                                         /* 7. and free */
1998                                         BLI_remlink(&pchan->iktree, tree);
1999                                         free_posetree(tree);
2000                                 }
2001                         }
2002                         else if(!(pchan->flag & POSE_DONE)) {
2003                                 where_is_pose_bone(ob, pchan, ctime);
2004                         }
2005                 }
2006         }
2007                 
2008         /* calculating deform matrices */
2009         for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2010                 if(pchan->bone) {
2011                         Mat4Invert(imat, pchan->bone->arm_mat);
2012                         Mat4MulMat4(pchan->chan_mat, imat, pchan->pose_mat);
2013                 }
2014         }
2015 }