=== Constraints ===
[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                                         if (data->flag & LOCLIKE_X_INVERT)
1624                                                 pchan->loc[0]= FloatLerpf(pchant->loc[0], pchan->loc[0], -fac);
1625                                         if (data->flag & LOCLIKE_Y_INVERT)
1626                                                 pchan->loc[1]= FloatLerpf(pchant->loc[1], pchan->loc[1], -fac);
1627                                         if (data->flag & LOCLIKE_Z_INVERT)
1628                                                 pchan->loc[2]= FloatLerpf(pchant->loc[2], pchan->loc[2], -fac);
1629                                 }
1630                         }
1631                 }
1632                         break;
1633                 case CONSTRAINT_TYPE_ROTLIKE:
1634                 {
1635                         bRotateLikeConstraint *data= con->data;
1636                         if(data->tar && data->subtarget[0]) {
1637                                 bPoseChannel *pchant= get_pose_channel(data->tar->pose, data->subtarget);
1638                                 if(pchant) {
1639                                         if(data->flag != (ROTLIKE_X|ROTLIKE_Y|ROTLIKE_Z)) {
1640                                                 float eul[3], eult[3], euln[3], fac= con->enforce;
1641                                                 
1642                                                 QuatToEul(pchan->quat, eul);
1643                                                 QuatToEul(pchant->quat, eult);
1644                                                 VECCOPY(euln, eul);
1645                                                 if(data->flag & ROTLIKE_X) euln[0]= FloatLerpf(eult[0], eul[0], fac); 
1646                                                 if(data->flag & ROTLIKE_Y) euln[1]= FloatLerpf(eult[1], eul[1], fac);
1647                                                 if(data->flag & ROTLIKE_Z) euln[2]= FloatLerpf(eult[2], eul[2], fac);
1648                                                 if(data->flag & ROTLIKE_X_INVERT) euln[0]= FloatLerpf(eult[0], eul[0], -fac); 
1649                                                 if(data->flag & ROTLIKE_Y_INVERT) euln[1]= FloatLerpf(eult[1], eul[1], -fac);
1650                                                 if(data->flag & ROTLIKE_Z_INVERT) euln[2]= FloatLerpf(eult[2], eul[2], -fac);
1651                                                 compatible_eul(eul, euln);
1652                                                 EulToQuat(euln, pchan->quat);
1653                                         }
1654                                         else {
1655                                                 QuatInterpol(pchan->quat, pchan->quat, pchant->quat, con->enforce);
1656                                         }
1657                                 }
1658                         }
1659                 }
1660                         break;
1661                 case CONSTRAINT_TYPE_SIZELIKE:
1662                 {
1663                         bSizeLikeConstraint *data= con->data;
1664                         float fac= con->enforce;
1665                         
1666                         if(data->tar && data->subtarget[0]) {
1667                                 bPoseChannel *pchant= get_pose_channel(data->tar->pose, data->subtarget);
1668                                 if(pchant) {
1669                                         if (data->flag & SIZELIKE_X)
1670                                                 pchan->size[0]= FloatLerpf(pchant->size[0], pchan->size[0], fac);
1671                                         if (data->flag & SIZELIKE_Y)
1672                                                 pchan->size[1]= FloatLerpf(pchant->size[1], pchan->size[1], fac);
1673                                         if (data->flag & SIZELIKE_Z)
1674                                                 pchan->size[2]= FloatLerpf(pchant->size[2], pchan->size[2], fac);
1675                                 }
1676                         }
1677                 }
1678                         break;
1679                 case CONSTRAINT_TYPE_LOCLIMIT:
1680                 {
1681                         bLocLimitConstraint *data= con->data;
1682                         float fac= con->enforce;
1683                         
1684                         if (data->flag & LIMIT_XMIN) {
1685                                 if(pchan->loc[0] < data->xmin)
1686                                         pchan->loc[0] = FloatLerpf(data->xmin, pchan->loc[0], fac);
1687                         }
1688                         if (data->flag & LIMIT_XMAX) {
1689                                 if (pchan->loc[0] > data->xmax)
1690                                         pchan->loc[0] = FloatLerpf(data->xmax, pchan->loc[0], fac);
1691                         }
1692                         if (data->flag & LIMIT_YMIN) {
1693                                 if(pchan->loc[1] < data->ymin)
1694                                         pchan->loc[1] = FloatLerpf(data->ymin, pchan->loc[1], fac);
1695                         }
1696                         if (data->flag & LIMIT_YMAX) {
1697                                 if (pchan->loc[1] > data->ymax)
1698                                         pchan->loc[1] = FloatLerpf(data->ymax, pchan->loc[1], fac);
1699                         }
1700                         if (data->flag & LIMIT_ZMIN) {
1701                                 if(pchan->loc[2] < data->zmin)
1702                                         pchan->loc[2] = FloatLerpf(data->zmin, pchan->loc[2], fac);
1703                         }
1704                         if (data->flag & LIMIT_ZMAX) {
1705                                 if (pchan->loc[2] > data->zmax)
1706                                         pchan->loc[2] = FloatLerpf(data->zmax, pchan->loc[2], fac);
1707                         }
1708                 }       
1709                         break;
1710                 case CONSTRAINT_TYPE_ROTLIMIT:
1711                 {
1712                         bRotLimitConstraint *data = con->data;
1713                         float eul[3];
1714                         float fac= con->enforce;
1715                         
1716                         QuatToEul(pchan->quat, eul);
1717                         
1718                         /* eulers: radians to degrees! */
1719                         eul[0] = (eul[0] / (2*M_PI) * 360);
1720                         eul[1] = (eul[1] / (2*M_PI) * 360);
1721                         eul[2] = (eul[2] / (2*M_PI) * 360);
1722                         
1723                         /* limiting of euler values... */
1724                         if (data->flag & LIMIT_XROT) {
1725                                 if (eul[0] < data->xmin) 
1726                                         eul[0] = FloatLerpf(data->xmin, eul[0], fac);
1727                                         
1728                                 if (eul[0] > data->xmax)
1729                                         eul[0] = FloatLerpf(data->xmax, eul[0], fac);
1730                         }
1731                         if (data->flag & LIMIT_YROT) {
1732                                 if (eul[1] < data->ymin)
1733                                         eul[1] = FloatLerpf(data->ymin, eul[1], fac);
1734                                         
1735                                 if (eul[1] > data->ymax)
1736                                         eul[1] = FloatLerpf(data->ymax, eul[1], fac);
1737                         }
1738                         if (data->flag & LIMIT_ZROT) {
1739                                 if (eul[2] < data->zmin)
1740                                         eul[2] = FloatLerpf(data->zmin, eul[2], fac);
1741                                         
1742                                 if (eul[2] > data->zmax)
1743                                         eul[2] = FloatLerpf(data->zmax, eul[2], fac);
1744                         }
1745                                 
1746                         /* eulers: degrees to radians ! */
1747                         eul[0] = (eul[0] / 360 * (2*M_PI)); 
1748                         eul[1] = (eul[1] / 360 * (2*M_PI));
1749                         eul[2] = (eul[2] / 360 * (2*M_PI));
1750                         
1751                         /* convert back */
1752                         EulToQuat(eul, pchan->quat);
1753                 }
1754                         break;
1755                 case CONSTRAINT_TYPE_SIZELIMIT:
1756                 {
1757                         bSizeLimitConstraint *data= con->data;
1758                         float fac= con->enforce;
1759                         
1760                         if (data->flag & LIMIT_XMIN) {
1761                                 if(pchan->size[0] < data->xmin)
1762                                         pchan->size[0] = FloatLerpf(data->xmin, pchan->size[0], fac);
1763                         }
1764                         if (data->flag & LIMIT_XMAX) {
1765                                 if (pchan->size[0] > data->xmax)
1766                                         pchan->size[0] = FloatLerpf(data->xmax, pchan->size[0], fac);
1767                         }
1768                         if (data->flag & LIMIT_YMIN) {
1769                                 if(pchan->size[1] < data->ymin)
1770                                         pchan->size[1] = FloatLerpf(data->ymin, pchan->size[1], fac);
1771                         }
1772                         if (data->flag & LIMIT_YMAX) {
1773                                 if (pchan->size[1] > data->ymax)
1774                                         pchan->size[1] = FloatLerpf(data->ymax, pchan->size[1], fac);
1775                         }
1776                         if (data->flag & LIMIT_ZMIN) {
1777                                 if(pchan->size[2] < data->zmin)
1778                                         pchan->size[2] = FloatLerpf(data->zmin, pchan->size[2], fac);
1779                         }
1780                         if (data->flag & LIMIT_ZMAX) {
1781                                 if (pchan->size[2] > data->zmax)
1782                                         pchan->size[2] = FloatLerpf(data->zmax, pchan->size[2], fac);
1783                         }
1784                 }
1785                         break;
1786         }
1787 }
1788
1789 static void do_strip_modifiers(Object *armob, Bone *bone, bPoseChannel *pchan)
1790 {
1791         bActionModifier *amod;
1792         bActionStrip *strip;
1793         float scene_cfra= G.scene->r.cfra;
1794
1795         for (strip=armob->nlastrips.first; strip; strip=strip->next) {
1796                 if(scene_cfra>=strip->start && scene_cfra<=strip->end) {
1797                         
1798                         /* temporal solution to prevent 2 strips accumulating */
1799                         if(scene_cfra==strip->end && strip->next && strip->next->start==scene_cfra)
1800                                 continue;
1801                         
1802                         for(amod= strip->modifiers.first; amod; amod= amod->next) {
1803                                 if(amod->type==ACTSTRIP_MOD_DEFORM) {
1804                                         /* validate first */
1805                                         if(amod->ob && amod->ob->type==OB_CURVE && amod->channel[0]) {
1806                                                 
1807                                                 if( strcmp(pchan->name, amod->channel)==0 ) {
1808                                                         float mat4[4][4], mat3[3][3];
1809                                                         
1810                                                         curve_deform_vector(amod->ob, armob, bone->arm_mat[3], pchan->pose_mat[3], mat3, amod->no_rot_axis);
1811                                                         Mat4CpyMat4(mat4, pchan->pose_mat);
1812                                                         Mat4MulMat34(pchan->pose_mat, mat3, mat4);
1813                                                         
1814                                                 }
1815                                         }
1816                                 }
1817                         }
1818                 }
1819         }
1820 }
1821
1822
1823 /* The main armature solver, does all constraints excluding IK */
1824 /* pchan is validated, as having bone and parent pointer */
1825 static void where_is_pose_bone(Object *ob, bPoseChannel *pchan, float ctime)
1826 {
1827         Bone *bone, *parbone;
1828         bPoseChannel *parchan;
1829         float vec[3], quat[4];
1830         int did_local= 0;       /* copying quaternion should be limited, chan_calc_mat() normalizes quat */
1831         
1832         /* set up variables for quicker access below */
1833         bone= pchan->bone;
1834         parbone= bone->parent;
1835         parchan= pchan->parent;
1836                 
1837         /* Do local constraints, these only work on the channel data (loc rot size) */
1838         QUATCOPY(quat, pchan->quat);
1839         if(pchan->constraints.first) {
1840                 bConstraint *con;
1841                 for(con=pchan->constraints.first; con; con= con->next) {
1842                         if(con->flag & CONSTRAINT_LOCAL) {
1843                                 do_local_constraint(pchan, con);
1844                                 did_local= 1;
1845                         }
1846                 }
1847         }
1848         
1849         /* this gives a chan_mat with actions (ipos) results */
1850         chan_calc_mat(pchan);
1851         
1852         if(did_local) 
1853                 QUATCOPY(pchan->quat, quat);    /* local constraint hack. bad! */
1854         
1855         /* construct the posemat based on PoseChannels, that we do before applying constraints */
1856         /* pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b) */
1857         
1858         if(parchan) {
1859                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
1860                 
1861                 /* bone transform itself */
1862                 Mat4CpyMat3(offs_bone, bone->bone_mat);
1863                 
1864                 /* The bone's root offset (is in the parent's coordinate system) */
1865                 VECCOPY(offs_bone[3], bone->head);
1866                 
1867                 /* Get the length translation of parent (length along y axis) */
1868                 offs_bone[3][1]+= parbone->length;
1869                 
1870                 /* Compose the matrix for this bone  */
1871                 if(bone->flag & BONE_HINGE) {   // uses restposition rotation, but actual position
1872                         float tmat[4][4];
1873                         
1874                         /* the rotation of the parent restposition */
1875                         Mat4CpyMat4(tmat, parbone->arm_mat);
1876                         
1877                         /* the location of actual parent transform */
1878                         VECCOPY(tmat[3], offs_bone[3]);
1879                         offs_bone[3][0]= offs_bone[3][1]= offs_bone[3][2]= 0.0f;
1880                         Mat4MulVecfl(parchan->pose_mat, tmat[3]);
1881                         
1882                         Mat4MulSerie(pchan->pose_mat, tmat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
1883                 }
1884                 else 
1885                         Mat4MulSerie(pchan->pose_mat, parchan->pose_mat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
1886         }
1887         else {
1888                 Mat4MulMat4(pchan->pose_mat, pchan->chan_mat, bone->arm_mat);
1889                 /* only rootbones get the cyclic offset */
1890                 VecAddf(pchan->pose_mat[3], pchan->pose_mat[3], ob->pose->cyclic_offset);
1891         }
1892         
1893         do_strip_modifiers(ob, bone, pchan);
1894         
1895         /* Do constraints */
1896         if(pchan->constraints.first) {
1897                 static Object conOb;
1898                 static int initialized= 0;
1899                 
1900                 VECCOPY(vec, pchan->pose_mat[3]);
1901                 
1902                 /* Build a workob to pass the bone to the constraint solver */
1903                 if(initialized==0) {
1904                         memset(&conOb, 0, sizeof(Object));
1905                         initialized= 1;
1906                 }
1907                 conOb.size[0]= conOb.size[1]= conOb.size[2]= 1.0;
1908                 conOb.data = ob->data;
1909                 conOb.type = ob->type;
1910                 conOb.parent = ob;      // ik solver retrieves the armature that way !?!?!?!
1911                 conOb.pose= ob->pose;                           // needed for retrieving pchan
1912                 conOb.trackflag = ob->trackflag;
1913                 conOb.upflag = ob->upflag;
1914                 
1915                 /* Collect the constraints from the pose (listbase copy) */
1916                 conOb.constraints = pchan->constraints;
1917                 
1918                 /* conOb.obmat takes bone to worldspace */
1919                 Mat4MulMat4 (conOb.obmat, pchan->pose_mat, ob->obmat);
1920                 
1921                 /* Solve */
1922                 solve_constraints (&conOb, TARGET_BONE, (void*)pchan, ctime);   // ctime doesnt alter objects
1923                 
1924                 /* Take out of worldspace */
1925                 Mat4MulMat4 (pchan->pose_mat, conOb.obmat, ob->imat);
1926                 
1927                 /* prevent constraints breaking a chain */
1928                 if(pchan->bone->flag & BONE_CONNECTED)
1929                         VECCOPY(pchan->pose_mat[3], vec);
1930
1931         }
1932         
1933         /* calculate head */
1934         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
1935         /* calculate tail */
1936         VECCOPY(vec, pchan->pose_mat[1]);
1937         VecMulf(vec, bone->length);
1938         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
1939         
1940 }
1941
1942 /* This only reads anim data from channels, and writes to channels */
1943 /* This is the only function adding poses */
1944 void where_is_pose (Object *ob)
1945 {
1946         bArmature *arm;
1947         Bone *bone;
1948         bPoseChannel *pchan;
1949         float imat[4][4];
1950         float ctime= bsystem_time(ob, NULL, (float)G.scene->r.cfra, 0.0);       /* not accurate... */
1951         
1952         arm = get_armature(ob);
1953         
1954         if(arm==NULL) return;
1955         if(ob->pose==NULL || (ob->pose->flag & POSE_RECALC)) 
1956            armature_rebuild_pose(ob, arm);
1957         
1958         /* In restposition we read the data from the bones */
1959         if(ob==G.obedit || (arm->flag & ARM_RESTPOS)) {
1960                 
1961                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1962                         bone= pchan->bone;
1963                         if(bone) {
1964                                 Mat4CpyMat4(pchan->pose_mat, bone->arm_mat);
1965                                 VECCOPY(pchan->pose_head, bone->arm_head);
1966                                 VECCOPY(pchan->pose_tail, bone->arm_tail);
1967                         }
1968                 }
1969         }
1970         else {
1971                 Mat4Invert(ob->imat, ob->obmat);        // imat is needed 
1972
1973                 /* 1. construct the PoseTrees, clear flags */
1974                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1975                         pchan->flag &= ~(POSE_DONE|POSE_CHAIN);
1976                         if(pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints
1977                                 initialize_posetree(ob, pchan); // will attach it to root!
1978                 }
1979                 
1980                 /* 2. the main loop, channels are already hierarchical sorted from root to children */
1981                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1982                         
1983                         /* 3. if we find an IK root, we handle it separated */
1984                         if(pchan->iktree.first) {
1985                                 while(pchan->iktree.first) {
1986                                         PoseTree *tree= pchan->iktree.first;
1987                                         int a;
1988                                         
1989                                         /* 4. walk over the tree for regular solving */
1990                                         for(a=0; a<tree->totchannel; a++) {
1991                                                 if(!(tree->pchan[a]->flag & POSE_DONE)) // successive trees can set the flag
1992                                                         where_is_pose_bone(ob, tree->pchan[a], ctime);
1993                                         }
1994                                         /* 5. execute the IK solver */
1995                                         execute_posetree(ob, tree);
1996                                         
1997                                         /* 6. apply the differences to the channels, 
1998                                                   we need to calculate the original differences first */
1999                                         for(a=0; a<tree->totchannel; a++)
2000                                                 make_dmats(tree->pchan[a]);
2001                                         
2002                                         for(a=0; a<tree->totchannel; a++)
2003                                                 /* sets POSE_DONE */
2004                                                 where_is_ik_bone(tree->pchan[a], tree->basis_change[a]);
2005                                         
2006                                         /* 7. and free */
2007                                         BLI_remlink(&pchan->iktree, tree);
2008                                         free_posetree(tree);
2009                                 }
2010                         }
2011                         else if(!(pchan->flag & POSE_DONE)) {
2012                                 where_is_pose_bone(ob, pchan, ctime);
2013                         }
2014                 }
2015         }
2016                 
2017         /* calculating deform matrices */
2018         for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2019                 if(pchan->bone) {
2020                         Mat4Invert(imat, pchan->bone->arm_mat);
2021                         Mat4MulMat4(pchan->chan_mat, imat, pchan->pose_mat);
2022                 }
2023         }
2024 }