Filling in branch from trunk
[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, int rest)
408 {
409         static Mat4 bbone_array[MAX_BBONE_SUBDIV];
410         static Mat4 bbone_rest_array[MAX_BBONE_SUBDIV];
411         Mat4 *result_array= (rest)? bbone_rest_array: bbone_array;
412         bPoseChannel *next, *prev;
413         Bone *bone= pchan->bone;
414         float h1[3], h2[3], scale[3], length, hlength1, hlength2, roll1=0.0f, roll2;
415         float mat3[3][3], imat[4][4], posemat[4][4], scalemat[4][4], iscalemat[4][4];
416         float data[MAX_BBONE_SUBDIV+1][4], *fp;
417         int a, doscale= 0;
418
419         length= bone->length;
420
421         if(!rest) {
422                 /* check if we need to take non-uniform bone scaling into account */
423                 scale[0]= VecLength(pchan->pose_mat[0]);
424                 scale[1]= VecLength(pchan->pose_mat[1]);
425                 scale[2]= VecLength(pchan->pose_mat[2]);
426
427                 if(fabs(scale[0] - scale[1]) > 1e-6f || fabs(scale[1] - scale[2]) > 1e-6f) {
428                         Mat4One(scalemat);
429                         scalemat[0][0]= scale[0];
430                         scalemat[1][1]= scale[1];
431                         scalemat[2][2]= scale[2];
432                         Mat4Invert(iscalemat, scalemat);
433
434                         length *= scale[1];
435                         doscale = 1;
436                 }
437         }
438         
439         hlength1= bone->ease1*length*0.390464f;         // 0.5*sqrt(2)*kappa, the handle length for near-perfect circles
440         hlength2= bone->ease2*length*0.390464f;
441         
442         /* evaluate next and prev bones */
443         if(bone->flag & BONE_CONNECTED)
444                 prev= pchan->parent;
445         else
446                 prev= NULL;
447         
448         next= pchan->child;
449         
450         /* find the handle points, since this is inside bone space, the 
451                 first point = (0,0,0)
452                 last point =  (0, length, 0) */
453         
454         if(rest) {
455                 Mat4Invert(imat, pchan->bone->arm_mat);
456         }
457         else if(doscale) {
458                 Mat4CpyMat4(posemat, pchan->pose_mat);
459                 Mat4Ortho(posemat);
460                 Mat4Invert(imat, posemat);
461         }
462         else
463                 Mat4Invert(imat, pchan->pose_mat);
464         
465         if(prev) {
466                 float difmat[4][4], result[3][3], imat3[3][3];
467
468                 /* transform previous point inside this bone space */
469                 if(rest)
470                         VECCOPY(h1, prev->bone->arm_head)
471                 else
472                         VECCOPY(h1, prev->pose_head)
473                 Mat4MulVecfl(imat, h1);
474
475                 if(prev->bone->segments>1) {
476                         /* if previous bone is B-bone too, use average handle direction */
477                         h1[1]-= length;
478                         roll1= 0.0f;
479                 }
480
481                 Normalize(h1);
482                 VecMulf(h1, -hlength1);
483
484                 if(prev->bone->segments==1) {
485                         /* find the previous roll to interpolate */
486                         if(rest)
487                                 Mat4MulMat4(difmat, prev->bone->arm_mat, imat);
488                         else
489                                 Mat4MulMat4(difmat, prev->pose_mat, imat);
490                         Mat3CpyMat4(result, difmat);                            // the desired rotation at beginning of next bone
491                         
492                         vec_roll_to_mat3(h1, 0.0f, mat3);                       // the result of vec_roll without roll
493                         
494                         Mat3Inv(imat3, mat3);
495                         Mat3MulMat3(mat3, result, imat3);                       // the matrix transforming vec_roll to desired roll
496                         
497                         roll1= atan2(mat3[2][0], mat3[2][2]);
498                 }
499         }
500         else {
501                 h1[0]= 0.0f; h1[1]= hlength1; h1[2]= 0.0f;
502                 roll1= 0.0f;
503         }
504         if(next) {
505                 float difmat[4][4], result[3][3], imat3[3][3];
506                 
507                 /* transform next point inside this bone space */
508                 if(rest)
509                         VECCOPY(h2, next->bone->arm_tail)
510                 else
511                         VECCOPY(h2, next->pose_tail)
512                 Mat4MulVecfl(imat, h2);
513                 /* if next bone is B-bone too, use average handle direction */
514                 if(next->bone->segments>1);
515                 else h2[1]-= length;
516                 Normalize(h2);
517                 
518                 /* find the next roll to interpolate as well */
519                 if(rest)
520                         Mat4MulMat4(difmat, next->bone->arm_mat, imat);
521                 else
522                         Mat4MulMat4(difmat, next->pose_mat, imat);
523                 Mat3CpyMat4(result, difmat);                            // the desired rotation at beginning of next bone
524                 
525                 vec_roll_to_mat3(h2, 0.0f, mat3);                       // the result of vec_roll without roll
526                 
527                 Mat3Inv(imat3, mat3);
528                 Mat3MulMat3(mat3, imat3, result);                       // the matrix transforming vec_roll to desired roll
529                 
530                 roll2= atan2(mat3[2][0], mat3[2][2]);
531                 
532                 /* and only now negate handle */
533                 VecMulf(h2, -hlength2);
534         }
535         else {
536                 h2[0]= 0.0f; h2[1]= -hlength2; h2[2]= 0.0f;
537                 roll2= 0.0;
538         }
539
540         /* make curve */
541         if(bone->segments > MAX_BBONE_SUBDIV)
542                 bone->segments= MAX_BBONE_SUBDIV;
543         
544         forward_diff_bezier(0.0, h1[0],         h2[0],                  0.0,            data[0],        MAX_BBONE_SUBDIV, 4);
545         forward_diff_bezier(0.0, h1[1],         length + h2[1], length,         data[0]+1,      MAX_BBONE_SUBDIV, 4);
546         forward_diff_bezier(0.0, h1[2],         h2[2],                  0.0,            data[0]+2,      MAX_BBONE_SUBDIV, 4);
547         forward_diff_bezier(roll1, roll1 + 0.390464f*(roll2-roll1), roll2 - 0.390464f*(roll2-roll1),    roll2,  data[0]+3,      MAX_BBONE_SUBDIV, 4);
548         
549         equalize_bezier(data[0], bone->segments);       // note: does stride 4!
550         
551         /* make transformation matrices for the segments for drawing */
552         for(a=0, fp= data[0]; a<bone->segments; a++, fp+=4) {
553                 VecSubf(h1, fp+4, fp);
554                 vec_roll_to_mat3(h1, fp[3], mat3);              // fp[3] is roll
555
556                 Mat4CpyMat3(result_array[a].mat, mat3);
557                 VECCOPY(result_array[a].mat[3], fp);
558
559                 if(doscale) {
560                         /* correct for scaling when this matrix is used in scaled space */
561                         Mat4MulSerie(result_array[a].mat, iscalemat, result_array[a].mat,
562                                 scalemat, NULL, NULL, NULL, NULL, NULL);
563                 }
564         }
565         
566         return result_array;
567 }
568
569 /* ************ Armature Deform ******************* */
570
571 static void pchan_b_bone_defmats(bPoseChannel *pchan, int use_quaternion, int rest_def)
572 {
573         Bone *bone= pchan->bone;
574         Mat4 *b_bone= b_bone_spline_setup(pchan, 0);
575         Mat4 *b_bone_rest= (rest_def)? NULL: b_bone_spline_setup(pchan, 1);
576         Mat4 *b_bone_mats;
577         DualQuat *b_bone_dual_quats= NULL;
578         float tmat[4][4];
579         int a;
580         
581         /* allocate b_bone matrices and dual quats */
582         b_bone_mats= MEM_mallocN((1+bone->segments)*sizeof(Mat4), "BBone defmats");
583         pchan->b_bone_mats= b_bone_mats;
584
585         if(use_quaternion) {
586                 b_bone_dual_quats= MEM_mallocN((bone->segments)*sizeof(DualQuat), "BBone dqs");
587                 pchan->b_bone_dual_quats= b_bone_dual_quats;
588         }
589         
590         /* first matrix is the inverse arm_mat, to bring points in local bone space
591            for finding out which segment it belongs to */
592         Mat4Invert(b_bone_mats[0].mat, bone->arm_mat);
593
594         /* then we make the b_bone_mats:
595             - first transform to local bone space
596                 - translate over the curve to the bbone mat space
597                 - transform with b_bone matrix
598                 - transform back into global space */
599         Mat4One(tmat);
600
601         for(a=0; a<bone->segments; a++) {
602                 if(b_bone_rest)
603                         Mat4Invert(tmat, b_bone_rest[a].mat);
604                 else
605                         tmat[3][1] = -a*(bone->length/(float)bone->segments);
606
607                 Mat4MulSerie(b_bone_mats[a+1].mat, pchan->chan_mat, bone->arm_mat,
608                         b_bone[a].mat, tmat, b_bone_mats[0].mat, NULL, NULL, NULL);
609
610                 if(use_quaternion)
611                         Mat4ToDQuat(bone->arm_mat, b_bone_mats[a+1].mat, &b_bone_dual_quats[a]);
612         }
613 }
614
615 static void b_bone_deform(bPoseChannel *pchan, Bone *bone, float *co, DualQuat *dq, float defmat[][3])
616 {
617         Mat4 *b_bone= pchan->b_bone_mats;
618         float (*mat)[4]= b_bone[0].mat;
619         float segment, y;
620         int a;
621         
622         /* need to transform co back to bonespace, only need y */
623         y= mat[0][1]*co[0] + mat[1][1]*co[1] + mat[2][1]*co[2] + mat[3][1];
624         
625         /* now calculate which of the b_bones are deforming this */
626         segment= bone->length/((float)bone->segments);
627         a= (int)(y/segment);
628         
629         /* note; by clamping it extends deform at endpoints, goes best with
630            straight joints in restpos. */
631         CLAMP(a, 0, bone->segments-1);
632
633         if(dq) {
634                 DQuatCpyDQuat(dq, &((DualQuat*)pchan->b_bone_dual_quats)[a]);
635         }
636         else {
637                 Mat4MulVecfl(b_bone[a+1].mat, co);
638
639                 if(defmat)
640                         Mat3CpyMat4(defmat, b_bone[a+1].mat);
641         }
642 }
643
644 /* using vec with dist to bone b1 - b2 */
645 float distfactor_to_bone (float vec[3], float b1[3], float b2[3], float rad1, float rad2, float rdist)
646 {
647         float dist=0.0f; 
648         float bdelta[3];
649         float pdelta[3];
650         float hsqr, a, l, rad;
651         
652         VecSubf (bdelta, b2, b1);
653         l = Normalize (bdelta);
654         
655         VecSubf (pdelta, vec, b1);
656         
657         a = bdelta[0]*pdelta[0] + bdelta[1]*pdelta[1] + bdelta[2]*pdelta[2];
658         hsqr = ((pdelta[0]*pdelta[0]) + (pdelta[1]*pdelta[1]) + (pdelta[2]*pdelta[2]));
659         
660         if (a < 0.0F){
661                 /* If we're past the end of the bone, do a spherical field attenuation thing */
662                 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])) ;
663                 rad= rad1;
664         }
665         else if (a > l){
666                 /* If we're past the end of the bone, do a spherical field attenuation thing */
667                 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])) ;
668                 rad= rad2;
669         }
670         else {
671                 dist= (hsqr - (a*a));
672                 
673                 if(l!=0.0f) {
674                         rad= a/l;
675                         rad= rad*rad2 + (1.0-rad)*rad1;
676                 }
677                 else rad= rad1;
678         }
679         
680         a= rad*rad;
681         if(dist < a) 
682                 return 1.0f;
683         else {
684                 l= rad+rdist;
685                 l*= l;
686                 if(rdist==0.0f || dist >= l) 
687                         return 0.0f;
688                 else {
689                         a= sqrt(dist)-rad;
690                         return 1.0-( a*a )/( rdist*rdist );
691                 }
692         }
693 }
694
695 static void pchan_deform_mat_add(bPoseChannel *pchan, float weight, float bbonemat[][3], float mat[][3])
696 {
697         float wmat[3][3];
698
699         if(pchan->bone->segments>1)
700                 Mat3CpyMat3(wmat, bbonemat);
701         else
702                 Mat3CpyMat4(wmat, pchan->chan_mat);
703
704         Mat3MulFloat((float*)wmat, weight);
705         Mat3AddMat3(mat, mat, wmat);
706 }
707
708 static float dist_bone_deform(bPoseChannel *pchan, float *vec, DualQuat *dq, float mat[][3], float *co)
709 {
710         Bone *bone= pchan->bone;
711         float fac, contrib=0.0;
712         float cop[3], bbonemat[3][3];
713         DualQuat bbonedq;
714
715         if(bone==NULL) return 0.0f;
716         
717         VECCOPY (cop, co);
718
719         fac= distfactor_to_bone(cop, bone->arm_head, bone->arm_tail, bone->rad_head, bone->rad_tail, bone->dist);
720         
721         if (fac>0.0) {
722                 
723                 fac*=bone->weight;
724                 contrib= fac;
725                 if(contrib>0.0) {
726                         if(vec) {
727                                 if(bone->segments>1)
728                                         // applies on cop and bbonemat
729                                         b_bone_deform(pchan, bone, cop, NULL, (mat)?bbonemat:NULL);
730                                 else
731                                         Mat4MulVecfl(pchan->chan_mat, cop);
732
733                                 //      Make this a delta from the base position
734                                 VecSubf (cop, cop, co);
735                                 cop[0]*=fac; cop[1]*=fac; cop[2]*=fac;
736                                 VecAddf (vec, vec, cop);
737
738                                 if(mat)
739                                         pchan_deform_mat_add(pchan, fac, bbonemat, mat);
740                         }
741                         else {
742                                 if(bone->segments>1) {
743                                         b_bone_deform(pchan, bone, cop, &bbonedq, NULL);
744                                         DQuatAddWeighted(dq, &bbonedq, fac);
745                                 }
746                                 else
747                                         DQuatAddWeighted(dq, pchan->dual_quat, fac);
748                         }
749                 }
750         }
751         
752         return contrib;
753 }
754
755 static void pchan_bone_deform(bPoseChannel *pchan, float weight, float *vec, DualQuat *dq, float mat[][3], float *co, float *contrib)
756 {
757         float cop[3], bbonemat[3][3];
758         DualQuat bbonedq;
759
760         if (!weight)
761                 return;
762
763         VECCOPY(cop, co);
764
765         if(vec) {
766                 if(pchan->bone->segments>1)
767                         // applies on cop and bbonemat
768                         b_bone_deform(pchan, pchan->bone, cop, NULL, (mat)?bbonemat:NULL);
769                 else
770                         Mat4MulVecfl(pchan->chan_mat, cop);
771                 
772                 vec[0]+=(cop[0]-co[0])*weight;
773                 vec[1]+=(cop[1]-co[1])*weight;
774                 vec[2]+=(cop[2]-co[2])*weight;
775
776                 if(mat)
777                         pchan_deform_mat_add(pchan, weight, bbonemat, mat);
778         }
779         else {
780                 if(pchan->bone->segments>1) {
781                         b_bone_deform(pchan, pchan->bone, cop, &bbonedq, NULL);
782                         DQuatAddWeighted(dq, &bbonedq, weight);
783                 }
784                 else
785                         DQuatAddWeighted(dq, pchan->dual_quat, weight);
786         }
787
788         (*contrib)+=weight;
789 }
790
791 void armature_deform_verts(Object *armOb, Object *target, DerivedMesh *dm,
792                            float (*vertexCos)[3], float (*defMats)[3][3],
793                                                    int numVerts, int deformflag, const char *defgrp_name)
794 {
795         bPoseChannel *pchan, **defnrToPC = NULL;
796         MDeformVert *dverts = NULL;
797         bDeformGroup *dg;
798         DualQuat *dualquats= NULL;
799         float obinv[4][4], premat[4][4], postmat[4][4];
800         int use_envelope = deformflag & ARM_DEF_ENVELOPE;
801         int use_quaternion = deformflag & ARM_DEF_QUATERNION;
802         int bbone_rest_def = deformflag & ARM_DEF_B_BONE_REST;
803         int numGroups = 0;              /* safety for vertexgroup index overflow */
804         int i, target_totvert = 0;      /* safety for vertexgroup overflow */
805         int use_dverts = 0;
806         int armature_def_nr = -1;
807         int totchan;
808
809         if(armOb == G.obedit) return;
810         
811         Mat4Invert(obinv, target->obmat);
812         Mat4CpyMat4(premat, target->obmat);
813         Mat4MulMat4(postmat, armOb->obmat, obinv);
814         Mat4Invert(premat, postmat);
815
816         /* bone defmats are already in the channels, chan_mat */
817         
818         /* initialize B_bone matrices and dual quaternions */
819         if(use_quaternion) {
820                 totchan= BLI_countlist(&armOb->pose->chanbase);
821                 dualquats= MEM_callocN(sizeof(DualQuat)*totchan, "dualquats");
822         }
823
824         totchan= 0;
825         for(pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next) {
826                 if(!(pchan->bone->flag & BONE_NO_DEFORM)) {
827                         if(pchan->bone->segments > 1)
828                                 pchan_b_bone_defmats(pchan, use_quaternion, bbone_rest_def);
829
830                         if(use_quaternion) {
831                                 pchan->dual_quat= &dualquats[totchan++];
832                                 Mat4ToDQuat(pchan->bone->arm_mat, pchan->chan_mat, pchan->dual_quat);
833                         }
834                 }
835         }
836
837         /* get the def_nr for the overall armature vertex group if present */
838         for(i = 0, dg = target->defbase.first; dg; i++, dg = dg->next)
839                 if(defgrp_name && strcmp(defgrp_name, dg->name) == 0)
840                         armature_def_nr = i;
841
842         /* get a vertex-deform-index to posechannel array */
843         if(deformflag & ARM_DEF_VGROUP) {
844                 if(ELEM(target->type, OB_MESH, OB_LATTICE)) {
845                         numGroups = BLI_countlist(&target->defbase);
846                         
847                         if(target->type==OB_MESH) {
848                                 Mesh *me= target->data;
849                                 dverts = me->dvert;
850                                 target_totvert = me->totvert;
851                         }
852                         else {
853                                 Lattice *lt= target->data;
854                                 dverts = lt->dvert;
855                                 if(dverts)
856                                         target_totvert = lt->pntsu*lt->pntsv*lt->pntsw;
857                         }
858                         /* if we have a DerivedMesh, only use dverts if it has them */
859                         if(dm)
860                                 if(dm->getVertData(dm, 0, CD_MDEFORMVERT))
861                                         use_dverts = 1;
862                                 else use_dverts = 0;
863                         else if(dverts) use_dverts = 1;
864
865                         if(use_dverts) {
866                                 defnrToPC = MEM_callocN(sizeof(*defnrToPC) * numGroups,
867                                                         "defnrToBone");
868                                 for(i = 0, dg = target->defbase.first; dg;
869                                     i++, dg = dg->next) {
870                                         defnrToPC[i] = get_pose_channel(armOb->pose, dg->name);
871                                         /* exclude non-deforming bones */
872                                         if(defnrToPC[i]) {
873                                                 if(defnrToPC[i]->bone->flag & BONE_NO_DEFORM)
874                                                         defnrToPC[i]= NULL;
875                                         }
876                                 }
877                         }
878                 }
879         }
880
881         for(i = 0; i < numVerts; i++) {
882                 MDeformVert *dvert;
883                 DualQuat sumdq, *dq = NULL;
884                 float *co = vertexCos[i], dco[3];
885                 float sumvec[3], summat[3][3];
886                 float *vec = NULL, (*smat)[3] = NULL;
887                 float contrib = 0.0f;
888                 float armature_weight = 1.0f; /* default to 1 if no overall def group */
889                 int       j;
890
891                 if(use_quaternion) {
892                         memset(&sumdq, 0, sizeof(DualQuat));
893                         dq= &sumdq;
894                 }
895                 else {
896                         sumvec[0] = sumvec[1] = sumvec[2] = 0.0f;
897                         vec= sumvec;
898
899                         if(defMats) {
900                                 Mat3Clr((float*)summat);
901                                 smat = summat;
902                         }
903                 }
904
905                 if(use_dverts || armature_def_nr >= 0) {
906                         if(dm) dvert = dm->getVertData(dm, i, CD_MDEFORMVERT);
907                         else if(dverts && i < target_totvert) dvert = dverts + i;
908                         else dvert = NULL;
909                 } else
910                         dvert = NULL;
911
912                 if(armature_def_nr >= 0 && dvert) {
913                         armature_weight = 0.0f; /* a def group was given, so default to 0 */
914                         for(j = 0; j < dvert->totweight; j++) {
915                                 if(dvert->dw[j].def_nr == armature_def_nr) {
916                                         armature_weight = dvert->dw[j].weight;
917                                         break;
918                                 }
919                         }
920                 }
921
922                 /* check if there's any  point in calculating for this vert */
923                 if(armature_weight == 0.0f) continue;
924                 
925                 /* Apply the object's matrix */
926                 Mat4MulVecfl(premat, co);
927                 
928                 if(use_dverts && dvert && dvert->totweight) { // use weight groups ?
929                         int deformed = 0;
930                         
931                         for(j = 0; j < dvert->totweight; j++){
932                                 int index = dvert->dw[j].def_nr;
933                                 pchan = index < numGroups?defnrToPC[index]:NULL;
934                                 if(pchan) {
935                                         float weight = dvert->dw[j].weight;
936                                         Bone *bone = pchan->bone;
937
938                                         deformed = 1;
939                                         
940                                         if(bone && bone->flag & BONE_MULT_VG_ENV) {
941                                                 weight *= distfactor_to_bone(co, bone->arm_head,
942                                                                              bone->arm_tail,
943                                                                              bone->rad_head,
944                                                                              bone->rad_tail,
945                                                                              bone->dist);
946                                         }
947                                         pchan_bone_deform(pchan, weight, vec, dq, smat, co, &contrib);
948                                 }
949                         }
950                         /* if there are vertexgroups but not groups with bones
951                          * (like for softbody groups)
952                          */
953                         if(deformed == 0 && use_envelope) {
954                                 for(pchan = armOb->pose->chanbase.first; pchan;
955                                     pchan = pchan->next) {
956                                         if(!(pchan->bone->flag & BONE_NO_DEFORM))
957                                                 contrib += dist_bone_deform(pchan, vec, dq, smat, co);
958                                 }
959                         }
960                 }
961                 else if(use_envelope) {
962                         for(pchan = armOb->pose->chanbase.first; pchan;
963                             pchan = pchan->next) {
964                                 if(!(pchan->bone->flag & BONE_NO_DEFORM))
965                                         contrib += dist_bone_deform(pchan, vec, dq, smat, co);
966                         }
967                 }
968
969                 /* actually should be EPSILON? weight values and contrib can be like 10e-39 small */
970                 if(contrib > 0.0001f) {
971                         if(use_quaternion) {
972                                 DQuatNormalize(dq, contrib, armature_weight);
973
974                                 if(armature_weight != 1.0f) {
975                                         VECCOPY(dco, co);
976                                         DQuatMulVecfl(dq, dco, (defMats)? summat: NULL);
977                                         VecSubf(dco, dco, co);
978                                         VecMulf(dco, armature_weight);
979                                         VecAddf(co, co, dco);
980                                 }
981                                 else
982                                         DQuatMulVecfl(dq, co, (defMats)? summat: NULL);
983
984                                 smat = summat;
985                         }
986                         else {
987                                 VecMulf(vec, armature_weight/contrib);
988                                 VecAddf(co, vec, co);
989                         }
990
991                         if(defMats) {
992                                 float pre[3][3], post[3][3], tmpmat[3][3];
993
994                                 Mat3CpyMat4(pre, premat);
995                                 Mat3CpyMat4(post, postmat);
996                                 Mat3CpyMat3(tmpmat, defMats[i]);
997
998                                 if(!use_quaternion) /* quaternion already is scale corrected */
999                                         Mat3MulFloat((float*)smat, armature_weight/contrib);
1000
1001                                 Mat3MulSerie(defMats[i], tmpmat, pre, smat, post,
1002                                         NULL, NULL, NULL, NULL);
1003                         }
1004                 }
1005                 
1006                 /* always, check above code */
1007                 Mat4MulVecfl(postmat, co);
1008         }
1009
1010         if(dualquats) MEM_freeN(dualquats);
1011         if(defnrToPC) MEM_freeN(defnrToPC);
1012         
1013         /* free B_bone matrices */
1014         for(pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next) {
1015                 if(pchan->b_bone_mats) {
1016                         MEM_freeN(pchan->b_bone_mats);
1017                         pchan->b_bone_mats = NULL;
1018                 }
1019                 if(pchan->b_bone_dual_quats) {
1020                         MEM_freeN(pchan->b_bone_dual_quats);
1021                         pchan->b_bone_dual_quats = NULL;
1022                 }
1023
1024                 pchan->dual_quat = NULL;
1025         }
1026 }
1027
1028 /* ************ END Armature Deform ******************* */
1029
1030 void get_objectspace_bone_matrix (struct Bone* bone, float M_accumulatedMatrix[][4], int root, int posed)
1031 {
1032         Mat4CpyMat4(M_accumulatedMatrix, bone->arm_mat);
1033 }
1034
1035 /* **************** Space to Space API ****************** */
1036
1037 /* Convert World-Space Matrix to Pose-Space Matrix */
1038 void armature_mat_world_to_pose(Object *ob, float inmat[][4], float outmat[][4]) 
1039 {
1040         float obmat[4][4];
1041         
1042         /* prevent crashes */
1043         if (ob==NULL) return;
1044         
1045         /* get inverse of (armature) object's matrix  */
1046         Mat4Invert(obmat, ob->obmat);
1047         
1048         /* multiply given matrix by object's-inverse to find pose-space matrix */
1049         Mat4MulMat4(outmat, obmat, inmat);
1050 }
1051
1052 /* Convert Wolrd-Space Location to Pose-Space Location
1053  * NOTE: this cannot be used to convert to pose-space location of the supplied
1054  *              pose-channel into its local space (i.e. 'visual'-keyframing) 
1055  */
1056 void armature_loc_world_to_pose(Object *ob, float *inloc, float *outloc) 
1057 {
1058         float xLocMat[4][4];
1059         float nLocMat[4][4];
1060         
1061         /* build matrix for location */
1062         Mat4One(xLocMat);
1063         VECCOPY(xLocMat[3], inloc);
1064
1065         /* get bone-space cursor matrix and extract location */
1066         armature_mat_world_to_pose(ob, xLocMat, nLocMat);
1067         VECCOPY(outloc, nLocMat[3]);
1068 }
1069
1070 /* Convert Pose-Space Matrix to Bone-Space Matrix 
1071  * NOTE: this cannot be used to convert to pose-space transforms of the supplied
1072  *              pose-channel into its local space (i.e. 'visual'-keyframing)
1073  */
1074 void armature_mat_pose_to_bone(bPoseChannel *pchan, float inmat[][4], float outmat[][4])
1075 {
1076         float pc_trans[4][4], inv_trans[4][4];
1077         float pc_posemat[4][4], inv_posemat[4][4];
1078         
1079         /* paranoia: prevent crashes with no pose-channel supplied */
1080         if (pchan==NULL) return;
1081         
1082         /* get the inverse matrix of the pchan's transforms */
1083         LocQuatSizeToMat4(pc_trans, pchan->loc, pchan->quat, pchan->size);
1084         Mat4Invert(inv_trans, pc_trans);
1085         
1086         /* Remove the pchan's transforms from it's pose_mat.
1087          * This should leave behind the effects of restpose + 
1088          * parenting + constraints
1089          */
1090         Mat4MulMat4(pc_posemat, inv_trans, pchan->pose_mat);
1091         
1092         /* get the inverse of the leftovers so that we can remove 
1093          * that component from the supplied matrix
1094          */
1095         Mat4Invert(inv_posemat, pc_posemat);
1096         
1097         /* get the new matrix */
1098         Mat4MulMat4(outmat, inmat, inv_posemat);
1099 }
1100
1101 /* Convert Pose-Space Location to Bone-Space Location
1102  * NOTE: this cannot be used to convert to pose-space location of the supplied
1103  *              pose-channel into its local space (i.e. 'visual'-keyframing) 
1104  */
1105 void armature_loc_pose_to_bone(bPoseChannel *pchan, float *inloc, float *outloc) 
1106 {
1107         float xLocMat[4][4];
1108         float nLocMat[4][4];
1109         
1110         /* build matrix for location */
1111         Mat4One(xLocMat);
1112         VECCOPY(xLocMat[3], inloc);
1113
1114         /* get bone-space cursor matrix and extract location */
1115         armature_mat_pose_to_bone(pchan, xLocMat, nLocMat);
1116         VECCOPY(outloc, nLocMat[3]);
1117 }
1118
1119 /* Remove rest-position effects from pose-transform for obtaining
1120  * 'visual' transformation of pose-channel. 
1121  * (used by the Visual-Keyframing stuff)
1122  */
1123 void armature_mat_pose_to_delta(float delta_mat[][4], float pose_mat[][4], float arm_mat[][4])
1124 {
1125         float imat[4][4];
1126  
1127         Mat4Invert(imat, arm_mat);
1128         Mat4MulMat4(delta_mat, pose_mat, imat);
1129 }
1130
1131
1132 /* **************** The new & simple (but OK!) armature evaluation ********* */ 
1133
1134 /*  ****************** And how it works! ****************************************
1135
1136   This is the bone transformation trick; they're hierarchical so each bone(b)
1137   is in the coord system of bone(b-1):
1138
1139   arm_mat(b)= arm_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) 
1140   
1141   -> yoffs is just the y axis translation in parent's coord system
1142   -> d_root is the translation of the bone root, also in parent's coord system
1143
1144   pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b)
1145
1146   we then - in init deform - store the deform in chan_mat, such that:
1147
1148   pose_mat(b)= arm_mat(b) * chan_mat(b)
1149   
1150   *************************************************************************** */
1151 /*  Computes vector and roll based on a rotation. "mat" must
1152      contain only a rotation, and no scaling. */ 
1153 void mat3_to_vec_roll(float mat[][3], float *vec, float *roll) 
1154 {
1155     if (vec)
1156         VecCopyf(vec, mat[1]);
1157
1158     if (roll) {
1159         float vecmat[3][3], vecmatinv[3][3], rollmat[3][3];
1160
1161         vec_roll_to_mat3(mat[1], 0.0f, vecmat);
1162         Mat3Inv(vecmatinv, vecmat);
1163         Mat3MulMat3(rollmat, vecmatinv, mat);
1164
1165         *roll= atan2(rollmat[2][0], rollmat[2][2]);
1166     }
1167 }
1168
1169 /*      Calculates the rest matrix of a bone based
1170         On its vector and a roll around that vector */
1171 void vec_roll_to_mat3(float *vec, float roll, float mat[][3])
1172 {
1173         float   nor[3], axis[3], target[3]={0,1,0};
1174         float   theta;
1175         float   rMatrix[3][3], bMatrix[3][3];
1176         
1177         VECCOPY (nor, vec);
1178         Normalize (nor);
1179         
1180         /*      Find Axis & Amount for bone matrix*/
1181         Crossf (axis,target,nor);
1182
1183         if (Inpf(axis,axis) > 0.0000000000001) {
1184                 /* if nor is *not* a multiple of target ... */
1185                 Normalize (axis);
1186                 
1187                 theta= NormalizedVecAngle2(target, nor);
1188                 
1189                 /*      Make Bone matrix*/
1190                 VecRotToMat3(axis, theta, bMatrix);
1191         }
1192         else {
1193                 /* if nor is a multiple of target ... */
1194                 float updown;
1195                 
1196                 /* point same direction, or opposite? */
1197                 updown = ( Inpf (target,nor) > 0 ) ? 1.0 : -1.0;
1198                 
1199                 /* I think this should work ... */
1200                 bMatrix[0][0]=updown; bMatrix[0][1]=0.0;    bMatrix[0][2]=0.0;
1201                 bMatrix[1][0]=0.0;    bMatrix[1][1]=updown; bMatrix[1][2]=0.0;
1202                 bMatrix[2][0]=0.0;    bMatrix[2][1]=0.0;    bMatrix[2][2]=1.0;
1203         }
1204         
1205         /*      Make Roll matrix*/
1206         VecRotToMat3(nor, roll, rMatrix);
1207         
1208         /*      Combine and output result*/
1209         Mat3MulMat3 (mat, rMatrix, bMatrix);
1210 }
1211
1212
1213 /* recursive part, calculates restposition of entire tree of children */
1214 /* used by exiting editmode too */
1215 void where_is_armature_bone(Bone *bone, Bone *prevbone)
1216 {
1217         float vec[3];
1218         
1219         /* Bone Space */
1220         VecSubf (vec, bone->tail, bone->head);
1221         vec_roll_to_mat3(vec, bone->roll, bone->bone_mat);
1222
1223         bone->length= VecLenf(bone->head, bone->tail);
1224         
1225         /* this is called on old file reading too... */
1226         if(bone->xwidth==0.0) {
1227                 bone->xwidth= 0.1f;
1228                 bone->zwidth= 0.1f;
1229                 bone->segments= 1;
1230         }
1231         
1232         if(prevbone) {
1233                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
1234                 
1235                 /* bone transform itself */
1236                 Mat4CpyMat3(offs_bone, bone->bone_mat);
1237                                 
1238                 /* The bone's root offset (is in the parent's coordinate system) */
1239                 VECCOPY(offs_bone[3], bone->head);
1240
1241                 /* Get the length translation of parent (length along y axis) */
1242                 offs_bone[3][1]+= prevbone->length;
1243                 
1244                 /* Compose the matrix for this bone  */
1245                 Mat4MulMat4(bone->arm_mat, offs_bone, prevbone->arm_mat);
1246         }
1247         else {
1248                 Mat4CpyMat3(bone->arm_mat, bone->bone_mat);
1249                 VECCOPY(bone->arm_mat[3], bone->head);
1250         }
1251         
1252         /* head */
1253         VECCOPY(bone->arm_head, bone->arm_mat[3]);
1254         /* tail is in current local coord system */
1255         VECCOPY(vec, bone->arm_mat[1]);
1256         VecMulf(vec, bone->length);
1257         VecAddf(bone->arm_tail, bone->arm_head, vec);
1258         
1259         /* and the kiddies */
1260         prevbone= bone;
1261         for(bone= bone->childbase.first; bone; bone= bone->next) {
1262                 where_is_armature_bone(bone, prevbone);
1263         }
1264 }
1265
1266 /* updates vectors and matrices on rest-position level, only needed 
1267    after editing armature itself, now only on reading file */
1268 void where_is_armature (bArmature *arm)
1269 {
1270         Bone *bone;
1271         
1272         /* hierarchical from root to children */
1273         for(bone= arm->bonebase.first; bone; bone= bone->next) {
1274                 where_is_armature_bone(bone, NULL);
1275         }
1276 }
1277
1278 /* if bone layer is protected, copy the data from from->pose */
1279 static void pose_proxy_synchronize(Object *ob, Object *from, int layer_protected)
1280 {
1281         bPose *pose= ob->pose, *frompose= from->pose;
1282         bPoseChannel *pchan, *pchanp, pchanw;
1283         bConstraint *con;
1284         
1285         if(frompose==NULL) return;
1286         
1287         /* exception, armature local layer should be proxied too */
1288         if(pose->proxy_layer)
1289                 ((bArmature *)ob->data)->layer= pose->proxy_layer;
1290         
1291         /* clear all transformation values from library */
1292         rest_pose(frompose);
1293         
1294         pchan= pose->chanbase.first;
1295         for(; pchan; pchan= pchan->next) {
1296                 if(pchan->bone->layer & layer_protected) {
1297                         pchanp= get_pose_channel(frompose, pchan->name);
1298                         
1299                         /* copy posechannel to temp, but restore important pointers */
1300                         pchanw= *pchanp;
1301                         pchanw.prev= pchan->prev;
1302                         pchanw.next= pchan->next;
1303                         pchanw.parent= pchan->parent;
1304                         pchanw.child= pchan->child;
1305                         pchanw.path= NULL;
1306                         
1307                         /* constraints, set target ob pointer to own object */
1308                         copy_constraints(&pchanw.constraints, &pchanp->constraints);
1309                         
1310                         for (con= pchanw.constraints.first; con; con= con->next) {
1311                                 bConstraintTypeInfo *cti= constraint_get_typeinfo(con);
1312                                 ListBase targets = {NULL, NULL};
1313                                 bConstraintTarget *ct;
1314                                 
1315                                 if (cti && cti->get_constraint_targets) {
1316                                         cti->get_constraint_targets(con, &targets);
1317                                         
1318                                         for (ct= targets.first; ct; ct= ct->next) {
1319                                                 if (ct->tar == from)
1320                                                         ct->tar = ob;
1321                                         }
1322                                         
1323                                         if (cti->flush_constraint_targets)
1324                                                 cti->flush_constraint_targets(con, &targets, 0);
1325                                 }
1326                         }
1327                         
1328                         /* free stuff from current channel */
1329                         if (pchan->path) MEM_freeN(pchan->path);
1330                         free_constraints(&pchan->constraints);
1331                         
1332                         /* the final copy */
1333                         *pchan= pchanw;
1334                 }
1335         }
1336 }
1337
1338 static int rebuild_pose_bone(bPose *pose, Bone *bone, bPoseChannel *parchan, int counter)
1339 {
1340         bPoseChannel *pchan = verify_pose_channel (pose, bone->name);   // verify checks and/or adds
1341
1342         pchan->bone= bone;
1343         pchan->parent= parchan;
1344         
1345         counter++;
1346         
1347         for(bone= bone->childbase.first; bone; bone= bone->next) {
1348                 counter= rebuild_pose_bone(pose, bone, pchan, counter);
1349                 /* for quick detecting of next bone in chain, only b-bone uses it now */
1350                 if(bone->flag & BONE_CONNECTED)
1351                         pchan->child= get_pose_channel(pose, bone->name);
1352         }
1353         
1354         return counter;
1355 }
1356
1357 /* only after leave editmode, duplicating, validating older files, library syncing */
1358 /* NOTE: pose->flag is set for it */
1359 void armature_rebuild_pose(Object *ob, bArmature *arm)
1360 {
1361         Bone *bone;
1362         bPose *pose;
1363         bPoseChannel *pchan, *next;
1364         int counter=0;
1365                 
1366         /* only done here */
1367         if(ob->pose==NULL) ob->pose= MEM_callocN(sizeof(bPose), "new pose");
1368         pose= ob->pose;
1369         
1370         /* clear */
1371         for(pchan= pose->chanbase.first; pchan; pchan= pchan->next) {
1372                 pchan->bone= NULL;
1373                 pchan->child= NULL;
1374         }
1375         
1376         /* first step, check if all channels are there */
1377         for(bone= arm->bonebase.first; bone; bone= bone->next) {
1378                 counter= rebuild_pose_bone(pose, bone, NULL, counter);
1379         }
1380
1381         /* and a check for garbage */
1382         for(pchan= pose->chanbase.first; pchan; pchan= next) {
1383                 next= pchan->next;
1384                 if(pchan->bone==NULL) {
1385                         if(pchan->path)
1386                                 MEM_freeN(pchan->path);
1387                         free_constraints(&pchan->constraints);
1388                         BLI_freelinkN(&pose->chanbase, pchan);
1389                 }
1390         }
1391         // printf("rebuild pose %s, %d bones\n", ob->id.name, counter);
1392         
1393         /* synchronize protected layers with proxy */
1394         if(ob->proxy)
1395                 pose_proxy_synchronize(ob, ob->proxy, arm->layer_protected);
1396         
1397         update_pose_constraint_flags(ob->pose); // for IK detection for example
1398         
1399         /* the sorting */
1400         if(counter>1)
1401                 DAG_pose_sort(ob);
1402         
1403         ob->pose->flag &= ~POSE_RECALC;
1404 }
1405
1406
1407 /* ********************** THE IK SOLVER ******************* */
1408
1409
1410
1411 /* allocates PoseTree, and links that to root bone/channel */
1412 /* Note: detecting the IK chain is duplicate code... in drawarmature.c and in transform_conversions.c */
1413 static void initialize_posetree(struct Object *ob, bPoseChannel *pchan_tip)
1414 {
1415         bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
1416         PoseTree *tree;
1417         PoseTarget *target;
1418         bConstraint *con;
1419         bKinematicConstraint *data;
1420         int a, segcount= 0, size, newsize, *oldparent, parent;
1421         
1422         /* find IK constraint, and validate it */
1423         for(con= pchan_tip->constraints.first; con; con= con->next) {
1424                 if(con->type==CONSTRAINT_TYPE_KINEMATIC) break;
1425         }
1426         if(con==NULL) return;
1427         
1428         data=(bKinematicConstraint*)con->data;
1429         
1430         /* two types of targets */
1431         if(data->flag & CONSTRAINT_IK_AUTO);
1432         else {
1433                 if(con->flag & CONSTRAINT_DISABLE) return;      /* checked in editconstraint.c */
1434                 if(con->enforce == 0.0f) return;
1435                 if(data->tar==NULL) return;
1436                 if(data->tar->type==OB_ARMATURE && data->subtarget[0]==0) return;
1437         }
1438         
1439         /* exclude tip from chain? */
1440         if(!(data->flag & CONSTRAINT_IK_TIP))
1441                 pchan_tip= pchan_tip->parent;
1442         
1443         /* Find the chain's root & count the segments needed */
1444         for (curchan = pchan_tip; curchan; curchan=curchan->parent){
1445                 pchan_root = curchan;
1446                 
1447                 curchan->flag |= POSE_CHAIN;    // don't forget to clear this
1448                 chanlist[segcount]=curchan;
1449                 segcount++;
1450                 
1451                 if(segcount==data->rootbone || segcount>255) break; // 255 is weak
1452         }
1453         if (!segcount) return;
1454
1455         /* setup the chain data */
1456         
1457         /* we make tree-IK, unless all existing targets are in this chain */
1458         for(tree= pchan_root->iktree.first; tree; tree= tree->next) {
1459                 for(target= tree->targets.first; target; target= target->next) {
1460                         curchan= tree->pchan[target->tip];
1461                         if(curchan->flag & POSE_CHAIN)
1462                                 curchan->flag &= ~POSE_CHAIN;
1463                         else
1464                                 break;
1465                 }
1466                 if(target) break;
1467         }
1468
1469         /* create a target */
1470         target= MEM_callocN(sizeof(PoseTarget), "posetarget");
1471         target->con= con;
1472         pchan_tip->flag &= ~POSE_CHAIN;
1473
1474         if(tree==NULL) {
1475                 /* make new tree */
1476                 tree= MEM_callocN(sizeof(PoseTree), "posetree");
1477
1478                 tree->iterations= data->iterations;
1479                 tree->totchannel= segcount;
1480                 tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
1481                 
1482                 tree->pchan= MEM_callocN(segcount*sizeof(void*), "ik tree pchan");
1483                 tree->parent= MEM_callocN(segcount*sizeof(int), "ik tree parent");
1484                 for(a=0; a<segcount; a++) {
1485                         tree->pchan[a]= chanlist[segcount-a-1];
1486                         tree->parent[a]= a-1;
1487                 }
1488                 target->tip= segcount-1;
1489                 
1490                 /* AND! link the tree to the root */
1491                 BLI_addtail(&pchan_root->iktree, tree);
1492         }
1493         else {
1494                 tree->iterations= MAX2(data->iterations, tree->iterations);
1495                 tree->stretch= tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
1496
1497                 /* skip common pose channels and add remaining*/
1498                 size= MIN2(segcount, tree->totchannel);
1499                 for(a=0; a<size && tree->pchan[a]==chanlist[segcount-a-1]; a++);
1500                 parent= a-1;
1501
1502                 segcount= segcount-a;
1503                 target->tip= tree->totchannel + segcount - 1;
1504
1505                 if (segcount > 0) {
1506                         /* resize array */
1507                         newsize= tree->totchannel + segcount;
1508                         oldchan= tree->pchan;
1509                         oldparent= tree->parent;
1510
1511                         tree->pchan= MEM_callocN(newsize*sizeof(void*), "ik tree pchan");
1512                         tree->parent= MEM_callocN(newsize*sizeof(int), "ik tree parent");
1513                         memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel);
1514                         memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel);
1515                         MEM_freeN(oldchan);
1516                         MEM_freeN(oldparent);
1517
1518                         /* add new pose channels at the end, in reverse order */
1519                         for(a=0; a<segcount; a++) {
1520                                 tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1];
1521                                 tree->parent[tree->totchannel+a]= tree->totchannel+a-1;
1522                         }
1523                         tree->parent[tree->totchannel]= parent;
1524                         
1525                         tree->totchannel= newsize;
1526                 }
1527
1528                 /* move tree to end of list, for correct evaluation order */
1529                 BLI_remlink(&pchan_root->iktree, tree);
1530                 BLI_addtail(&pchan_root->iktree, tree);
1531         }
1532
1533         /* add target to the tree */
1534         BLI_addtail(&tree->targets, target);
1535 }
1536
1537 /* called from within the core where_is_pose loop, all animsystems and constraints
1538 were executed & assigned. Now as last we do an IK pass */
1539 static void execute_posetree(Object *ob, PoseTree *tree)
1540 {
1541         float R_parmat[3][3];
1542         float iR_parmat[3][3];
1543         float R_bonemat[3][3];
1544         float goalrot[3][3], goalpos[3];
1545         float rootmat[4][4], imat[4][4];
1546         float goal[4][4], goalinv[4][4];
1547         float irest_basis[3][3], full_basis[3][3];
1548         float end_pose[4][4], world_pose[4][4];
1549         float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch=NULL;
1550         int a, flag, hasstretch=0;
1551         bPoseChannel *pchan;
1552         IK_Segment *seg, *parent, **iktree, *iktarget;
1553         IK_Solver *solver;
1554         PoseTarget *target;
1555         bKinematicConstraint *data, *poleangledata=NULL;
1556         Bone *bone;
1557
1558         if (tree->totchannel == 0)
1559                 return;
1560
1561         iktree= MEM_mallocN(sizeof(void*)*tree->totchannel, "ik tree");
1562
1563         for(a=0; a<tree->totchannel; a++) {
1564                 pchan= tree->pchan[a];
1565                 bone= pchan->bone;
1566
1567                 /* set DoF flag */
1568                 flag= 0;
1569                 if((pchan->ikflag & BONE_IK_NO_XDOF) == 0)
1570                         flag |= IK_XDOF;
1571                 if((pchan->ikflag & BONE_IK_NO_YDOF) == 0)
1572                         flag |= IK_YDOF;
1573                 if((pchan->ikflag & BONE_IK_NO_ZDOF) == 0)
1574                         flag |= IK_ZDOF;
1575
1576                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
1577                         flag |= IK_TRANS_YDOF;
1578                         hasstretch = 1;
1579                 }
1580
1581                 seg= iktree[a]= IK_CreateSegment(flag);
1582
1583                 /* find parent */
1584                 if(a == 0)
1585                         parent= NULL;
1586                 else
1587                         parent= iktree[tree->parent[a]];
1588
1589                 IK_SetParent(seg, parent);
1590         
1591                 /* get the matrix that transforms from prevbone into this bone */
1592                 Mat3CpyMat4(R_bonemat, pchan->pose_mat);
1593
1594                 /* gather transformations for this IK segment */
1595
1596                 if (pchan->parent)
1597                         Mat3CpyMat4(R_parmat, pchan->parent->pose_mat);
1598                 else
1599                         Mat3One(R_parmat);
1600
1601                 /* bone offset */
1602                 if (pchan->parent && (a > 0))
1603                         VecSubf(start, pchan->pose_head, pchan->parent->pose_tail);
1604                 else
1605                         /* only root bone (a = 0) has no parent */
1606                         start[0]= start[1]= start[2]= 0.0f;
1607                 
1608                 /* change length based on bone size */
1609                 length= bone->length*VecLength(R_bonemat[1]);
1610
1611                 /* compute rest basis and its inverse */
1612                 Mat3CpyMat3(rest_basis, bone->bone_mat);
1613                 Mat3CpyMat3(irest_basis, bone->bone_mat);
1614                 Mat3Transp(irest_basis);
1615
1616                 /* compute basis with rest_basis removed */
1617                 Mat3Inv(iR_parmat, R_parmat);
1618                 Mat3MulMat3(full_basis, iR_parmat, R_bonemat);
1619                 Mat3MulMat3(basis, irest_basis, full_basis);
1620
1621                 /* basis must be pure rotation */
1622                 Mat3Ortho(basis);
1623
1624                 /* transform offset into local bone space */
1625                 Mat3Ortho(iR_parmat);
1626                 Mat3MulVecfl(iR_parmat, start);
1627
1628                 IK_SetTransform(seg, start, rest_basis, basis, length);
1629
1630                 if (pchan->ikflag & BONE_IK_XLIMIT)
1631                         IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]);
1632                 if (pchan->ikflag & BONE_IK_YLIMIT)
1633                         IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]);
1634                 if (pchan->ikflag & BONE_IK_ZLIMIT)
1635                         IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]);
1636
1637                 IK_SetStiffness(seg, IK_X, pchan->stiffness[0]);
1638                 IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]);
1639                 IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]);
1640
1641                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
1642                         float ikstretch = pchan->ikstretch*pchan->ikstretch;
1643                         IK_SetStiffness(seg, IK_TRANS_Y, MIN2(1.0-ikstretch, 0.999));
1644                         IK_SetLimit(seg, IK_TRANS_Y, 0.001, 1e10);
1645                 }
1646         }
1647
1648         solver= IK_CreateSolver(iktree[0]);
1649
1650         /* set solver goals */
1651
1652         /* first set the goal inverse transform, assuming the root of tree was done ok! */
1653         pchan= tree->pchan[0];
1654         if (pchan->parent)
1655                 /* transform goal by parent mat, so this rotation is not part of the
1656                    segment's basis. otherwise rotation limits do not work on the
1657                    local transform of the segment itself. */
1658                 Mat4CpyMat4(rootmat, pchan->parent->pose_mat);
1659         else
1660                 Mat4One(rootmat);
1661         VECCOPY(rootmat[3], pchan->pose_head);
1662         
1663         Mat4MulMat4 (imat, rootmat, ob->obmat);
1664         Mat4Invert (goalinv, imat);
1665         
1666         for (target=tree->targets.first; target; target=target->next) {
1667                 float polepos[3];
1668                 int poleconstrain= 0;
1669
1670                 data= (bKinematicConstraint*)target->con->data;
1671                 
1672                 /* 1.0=ctime, we pass on object for auto-ik (owner-type here is object, even though
1673                  * strictly speaking, it is a posechannel)
1674                  */
1675                 get_constraint_target_matrix(target->con, 0, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
1676                 
1677                 /* and set and transform goal */
1678                 Mat4MulMat4(goal, rootmat, goalinv);
1679                 
1680                 VECCOPY(goalpos, goal[3]);
1681                 Mat3CpyMat4(goalrot, goal);
1682                 
1683                 /* same for pole vector target */
1684                 if(data->poletar) {
1685                         get_constraint_target_matrix(target->con, 1, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
1686
1687                         if(data->flag & CONSTRAINT_IK_SETANGLE) {
1688                                 /* don't solve IK when we are setting the pole angle */
1689                                 break;
1690                         }
1691                         else {
1692                                 Mat4MulMat4(goal, rootmat, goalinv);
1693                                 VECCOPY(polepos, goal[3]);
1694                                 poleconstrain= 1;
1695
1696                                 if(data->flag & CONSTRAINT_IK_GETANGLE) {
1697                                         poleangledata= data;
1698                                         data->flag &= ~CONSTRAINT_IK_GETANGLE;
1699                                 }
1700                         }
1701                 }
1702
1703                 /* do we need blending? */
1704                 if (target->con->enforce!=1.0) {
1705                         float q1[4], q2[4], q[4];
1706                         float fac= target->con->enforce;
1707                         float mfac= 1.0-fac;
1708                         
1709                         pchan= tree->pchan[target->tip];
1710                         
1711                         /* end effector in world space */
1712                         Mat4CpyMat4(end_pose, pchan->pose_mat);
1713                         VECCOPY(end_pose[3], pchan->pose_tail);
1714                         Mat4MulSerie(world_pose, goalinv, ob->obmat, end_pose, 0, 0, 0, 0, 0);
1715                         
1716                         /* blend position */
1717                         goalpos[0]= fac*goalpos[0] + mfac*world_pose[3][0];
1718                         goalpos[1]= fac*goalpos[1] + mfac*world_pose[3][1];
1719                         goalpos[2]= fac*goalpos[2] + mfac*world_pose[3][2];
1720                         
1721                         /* blend rotation */
1722                         Mat3ToQuat(goalrot, q1);
1723                         Mat4ToQuat(world_pose, q2);
1724                         QuatInterpol(q, q1, q2, mfac);
1725                         QuatToMat3(q, goalrot);
1726                 }
1727                 
1728                 iktarget= iktree[target->tip];
1729                 
1730                 if(data->weight != 0.0) {
1731                         if(poleconstrain)
1732                                 IK_SolverSetPoleVectorConstraint(solver, iktarget, goalpos,
1733                                         polepos, data->poleangle*M_PI/180, (poleangledata == data));
1734                         IK_SolverAddGoal(solver, iktarget, goalpos, data->weight);
1735                 }
1736                 if((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0))
1737                         if((data->flag & CONSTRAINT_IK_AUTO)==0)
1738                                 IK_SolverAddGoalOrientation(solver, iktarget, goalrot,
1739                                         data->orientweight);
1740         }
1741
1742         /* solve */
1743         IK_Solve(solver, 0.0f, tree->iterations);
1744
1745         if(poleangledata)
1746                 poleangledata->poleangle= IK_SolverGetPoleAngle(solver)*180/M_PI;
1747
1748         IK_FreeSolver(solver);
1749
1750         /* gather basis changes */
1751         tree->basis_change= MEM_mallocN(sizeof(float[3][3])*tree->totchannel, "ik basis change");
1752         if(hasstretch)
1753                 ikstretch= MEM_mallocN(sizeof(float)*tree->totchannel, "ik stretch");
1754
1755         for(a=0; a<tree->totchannel; a++) {
1756                 IK_GetBasisChange(iktree[a], tree->basis_change[a]);
1757
1758                 if(hasstretch) {
1759                         /* have to compensate for scaling received from parent */
1760                         float parentstretch, stretch;
1761
1762                         pchan= tree->pchan[a];
1763                         parentstretch= (tree->parent[a] >= 0)? ikstretch[tree->parent[a]]: 1.0;
1764
1765                         if(tree->stretch && (pchan->ikstretch > 0.0)) {
1766                                 float trans[3], length;
1767
1768                                 IK_GetTranslationChange(iktree[a], trans);
1769                                 length= pchan->bone->length*VecLength(pchan->pose_mat[1]);
1770
1771                                 ikstretch[a]= (length == 0.0)? 1.0: (trans[1]+length)/length;
1772                         }
1773                         else
1774                                 ikstretch[a] = 1.0;
1775
1776                         stretch= (parentstretch == 0.0)? 1.0: ikstretch[a]/parentstretch;
1777
1778                         VecMulf(tree->basis_change[a][0], stretch);
1779                         VecMulf(tree->basis_change[a][1], stretch);
1780                         VecMulf(tree->basis_change[a][2], stretch);
1781
1782                 }
1783
1784                 IK_FreeSegment(iktree[a]);
1785         }
1786         
1787         MEM_freeN(iktree);
1788         if(ikstretch) MEM_freeN(ikstretch);
1789 }
1790
1791 void free_posetree(PoseTree *tree)
1792 {
1793         BLI_freelistN(&tree->targets);
1794         if(tree->pchan) MEM_freeN(tree->pchan);
1795         if(tree->parent) MEM_freeN(tree->parent);
1796         if(tree->basis_change) MEM_freeN(tree->basis_change);
1797         MEM_freeN(tree);
1798 }
1799
1800 /* ********************** THE POSE SOLVER ******************* */
1801
1802
1803 /* loc/rot/size to mat4 */
1804 /* used in constraint.c too */
1805 void chan_calc_mat(bPoseChannel *chan)
1806 {
1807         float smat[3][3];
1808         float rmat[3][3];
1809         float tmat[3][3];
1810         
1811         SizeToMat3(chan->size, smat);
1812         
1813         NormalQuat(chan->quat);
1814
1815         QuatToMat3(chan->quat, rmat);
1816         
1817         Mat3MulMat3(tmat, rmat, smat);
1818         
1819         Mat4CpyMat3(chan->chan_mat, tmat);
1820         
1821         /* prevent action channels breaking chains */
1822         /* need to check for bone here, CONSTRAINT_TYPE_ACTION uses this call */
1823         if (chan->bone==NULL || !(chan->bone->flag & BONE_CONNECTED)) {
1824                 VECCOPY(chan->chan_mat[3], chan->loc);
1825         }
1826
1827 }
1828
1829 /* transform from bone(b) to bone(b+1), store in chan_mat */
1830 static void make_dmats(bPoseChannel *pchan)
1831 {
1832         if (pchan->parent) {
1833                 float iR_parmat[4][4];
1834                 Mat4Invert(iR_parmat, pchan->parent->pose_mat);
1835                 Mat4MulMat4(pchan->chan_mat,  pchan->pose_mat, iR_parmat);      // delta mat
1836         }
1837         else Mat4CpyMat4(pchan->chan_mat, pchan->pose_mat);
1838 }
1839
1840 /* applies IK matrix to pchan, IK is done separated */
1841 /* formula: pose_mat(b) = pose_mat(b-1) * diffmat(b-1, b) * ik_mat(b) */
1842 /* to make this work, the diffmats have to be precalculated! Stored in chan_mat */
1843 static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[][3])   // nr = to detect if this is first bone
1844 {
1845         float vec[3], ikmat[4][4];
1846         
1847         Mat4CpyMat3(ikmat, ik_mat);
1848         
1849         if (pchan->parent)
1850                 Mat4MulSerie(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat, ikmat, NULL, NULL, NULL, NULL, NULL);
1851         else 
1852                 Mat4MulMat4(pchan->pose_mat, ikmat, pchan->chan_mat);
1853
1854         /* calculate head */
1855         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
1856         /* calculate tail */
1857         VECCOPY(vec, pchan->pose_mat[1]);
1858         VecMulf(vec, pchan->bone->length);
1859         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
1860
1861         pchan->flag |= POSE_DONE;
1862 }
1863
1864 /* NLA strip modifiers */
1865 static void do_strip_modifiers(Object *armob, Bone *bone, bPoseChannel *pchan)
1866 {
1867         bActionModifier *amod;
1868         bActionStrip *strip;
1869         float scene_cfra= G.scene->r.cfra;
1870
1871         for (strip=armob->nlastrips.first; strip; strip=strip->next) {
1872                 if(scene_cfra>=strip->start && scene_cfra<=strip->end) {
1873                         
1874                         /* temporal solution to prevent 2 strips accumulating */
1875                         if(scene_cfra==strip->end && strip->next && strip->next->start==scene_cfra)
1876                                 continue;
1877                         
1878                         for(amod= strip->modifiers.first; amod; amod= amod->next) {
1879                                 switch (amod->type) {
1880                                 case ACTSTRIP_MOD_DEFORM:
1881                                 {
1882                                         /* validate first */
1883                                         if(amod->ob && amod->ob->type==OB_CURVE && amod->channel[0]) {
1884                                                 
1885                                                 if( strcmp(pchan->name, amod->channel)==0 ) {
1886                                                         float mat4[4][4], mat3[3][3];
1887                                                         
1888                                                         curve_deform_vector(amod->ob, armob, bone->arm_mat[3], pchan->pose_mat[3], mat3, amod->no_rot_axis);
1889                                                         Mat4CpyMat4(mat4, pchan->pose_mat);
1890                                                         Mat4MulMat34(pchan->pose_mat, mat3, mat4);
1891                                                         
1892                                                 }
1893                                         }
1894                                 }
1895                                         break;
1896                                 case ACTSTRIP_MOD_NOISE:        
1897                                 {
1898                                         if( strcmp(pchan->name, amod->channel)==0 ) {
1899                                                 float nor[3], loc[3], ofs;
1900                                                 float eul[3], size[3], eulo[3], sizeo[3];
1901                                                 
1902                                                 /* calculate turbulance */
1903                                                 ofs = amod->turbul / 200.0f;
1904                                                 
1905                                                 /* make a copy of starting conditions */
1906                                                 VECCOPY(loc, pchan->pose_mat[3]);
1907                                                 Mat4ToEul(pchan->pose_mat, eul);
1908                                                 Mat4ToSize(pchan->pose_mat, size);
1909                                                 VECCOPY(eulo, eul);
1910                                                 VECCOPY(sizeo, size);
1911                                                 
1912                                                 /* apply noise to each set of channels */
1913                                                 if (amod->channels & 4) {
1914                                                         /* for scaling */
1915                                                         nor[0] = BLI_gNoise(amod->noisesize, size[0]+ofs, size[1], size[2], 0, 0) - ofs;
1916                                                         nor[1] = BLI_gNoise(amod->noisesize, size[0], size[1]+ofs, size[2], 0, 0) - ofs;        
1917                                                         nor[2] = BLI_gNoise(amod->noisesize, size[0], size[1], size[2]+ofs, 0, 0) - ofs;
1918                                                         VecAddf(size, size, nor);
1919                                                         
1920                                                         if (sizeo[0] != 0)
1921                                                                 VecMulf(pchan->pose_mat[0], size[0] / sizeo[0]);
1922                                                         if (sizeo[1] != 0)
1923                                                                 VecMulf(pchan->pose_mat[1], size[1] / sizeo[1]);
1924                                                         if (sizeo[2] != 0)
1925                                                                 VecMulf(pchan->pose_mat[2], size[2] / sizeo[2]);
1926                                                 }
1927                                                 if (amod->channels & 2) {
1928                                                         /* for rotation */
1929                                                         nor[0] = BLI_gNoise(amod->noisesize, eul[0]+ofs, eul[1], eul[2], 0, 0) - ofs;
1930                                                         nor[1] = BLI_gNoise(amod->noisesize, eul[0], eul[1]+ofs, eul[2], 0, 0) - ofs;   
1931                                                         nor[2] = BLI_gNoise(amod->noisesize, eul[0], eul[1], eul[2]+ofs, 0, 0) - ofs;
1932                                                         
1933                                                         compatible_eul(nor, eulo);
1934                                                         VecAddf(eul, eul, nor);
1935                                                         compatible_eul(eul, eulo);
1936                                                         
1937                                                         LocEulSizeToMat4(pchan->pose_mat, loc, eul, size);
1938                                                 }
1939                                                 if (amod->channels & 1) {
1940                                                         /* for location */
1941                                                         nor[0] = BLI_gNoise(amod->noisesize, loc[0]+ofs, loc[1], loc[2], 0, 0) - ofs;
1942                                                         nor[1] = BLI_gNoise(amod->noisesize, loc[0], loc[1]+ofs, loc[2], 0, 0) - ofs;   
1943                                                         nor[2] = BLI_gNoise(amod->noisesize, loc[0], loc[1], loc[2]+ofs, 0, 0) - ofs;
1944                                                         
1945                                                         VecAddf(pchan->pose_mat[3], loc, nor);
1946                                                 }
1947                                         }
1948                                 }
1949                                         break;
1950                                 }
1951                         }
1952                 }
1953         }
1954 }
1955
1956
1957 /* The main armature solver, does all constraints excluding IK */
1958 /* pchan is validated, as having bone and parent pointer */
1959 static void where_is_pose_bone(Object *ob, bPoseChannel *pchan, float ctime)
1960 {
1961         Bone *bone, *parbone;
1962         bPoseChannel *parchan;
1963         float vec[3];
1964         
1965         /* set up variables for quicker access below */
1966         bone= pchan->bone;
1967         parbone= bone->parent;
1968         parchan= pchan->parent;
1969         
1970         /* this gives a chan_mat with actions (ipos) results */
1971         chan_calc_mat(pchan);
1972         
1973         /* construct the posemat based on PoseChannels, that we do before applying constraints */
1974         /* pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b) */
1975         
1976         if(parchan) {
1977                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
1978                 
1979                 /* bone transform itself */
1980                 Mat4CpyMat3(offs_bone, bone->bone_mat);
1981                 
1982                 /* The bone's root offset (is in the parent's coordinate system) */
1983                 VECCOPY(offs_bone[3], bone->head);
1984                 
1985                 /* Get the length translation of parent (length along y axis) */
1986                 offs_bone[3][1]+= parbone->length;
1987                 
1988                 /* Compose the matrix for this bone  */
1989                 if(bone->flag & BONE_HINGE) {   // uses restposition rotation, but actual position
1990                         float tmat[4][4];
1991                         
1992                         /* the rotation of the parent restposition */
1993                         Mat4CpyMat4(tmat, parbone->arm_mat);
1994                         
1995                         /* the location of actual parent transform */
1996                         VECCOPY(tmat[3], offs_bone[3]);
1997                         offs_bone[3][0]= offs_bone[3][1]= offs_bone[3][2]= 0.0f;
1998                         Mat4MulVecfl(parchan->pose_mat, tmat[3]);
1999                         
2000                         Mat4MulSerie(pchan->pose_mat, tmat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
2001                 }
2002                 else 
2003                         Mat4MulSerie(pchan->pose_mat, parchan->pose_mat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
2004         }
2005         else {
2006                 Mat4MulMat4(pchan->pose_mat, pchan->chan_mat, bone->arm_mat);
2007                 /* only rootbones get the cyclic offset */
2008                 VecAddf(pchan->pose_mat[3], pchan->pose_mat[3], ob->pose->cyclic_offset);
2009         }
2010         
2011         /* do NLA strip modifiers - i.e. curve follow */
2012         do_strip_modifiers(ob, bone, pchan);
2013         
2014         /* Do constraints */
2015         if (pchan->constraints.first) {
2016                 bConstraintOb *cob;
2017                 
2018                 /* make a copy of location of PoseChannel for later */
2019                 VECCOPY(vec, pchan->pose_mat[3]);
2020                 
2021                 /* prepare PoseChannel for Constraint solving 
2022                  * - makes a copy of matrix, and creates temporary struct to use 
2023                  */
2024                 cob= constraints_make_evalob(ob, pchan, CONSTRAINT_OBTYPE_BONE);
2025                 
2026                 /* Solve PoseChannel's Constraints */
2027                 solve_constraints(&pchan->constraints, cob, ctime);     // ctime doesnt alter objects
2028                 
2029                 /* cleanup after Constraint Solving 
2030                  * - applies matrix back to pchan, and frees temporary struct used
2031                  */
2032                 constraints_clear_evalob(cob);
2033                 
2034                 /* prevent constraints breaking a chain */
2035                 if(pchan->bone->flag & BONE_CONNECTED) {
2036                         VECCOPY(pchan->pose_mat[3], vec);
2037                 }
2038         }
2039         
2040         /* calculate head */
2041         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
2042         /* calculate tail */
2043         VECCOPY(vec, pchan->pose_mat[1]);
2044         VecMulf(vec, bone->length);
2045         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
2046 }
2047
2048 /* This only reads anim data from channels, and writes to channels */
2049 /* This is the only function adding poses */
2050 void where_is_pose (Object *ob)
2051 {
2052         bArmature *arm;
2053         Bone *bone;
2054         bPoseChannel *pchan;
2055         float imat[4][4];
2056         float ctime= bsystem_time(ob, (float)G.scene->r.cfra, 0.0);     /* not accurate... */
2057         
2058         arm = get_armature(ob);
2059         
2060         if(arm==NULL) return;
2061         if(ob->pose==NULL || (ob->pose->flag & POSE_RECALC)) 
2062            armature_rebuild_pose(ob, arm);
2063         
2064         /* In restposition we read the data from the bones */
2065         if(ob==G.obedit || (arm->flag & ARM_RESTPOS)) {
2066                 
2067                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2068                         bone= pchan->bone;
2069                         if(bone) {
2070                                 Mat4CpyMat4(pchan->pose_mat, bone->arm_mat);
2071                                 VECCOPY(pchan->pose_head, bone->arm_head);
2072                                 VECCOPY(pchan->pose_tail, bone->arm_tail);
2073                         }
2074                 }
2075         }
2076         else {
2077                 Mat4Invert(ob->imat, ob->obmat);        // imat is needed 
2078
2079                 /* 1. construct the PoseTrees, clear flags */
2080                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2081                         pchan->flag &= ~(POSE_DONE|POSE_CHAIN);
2082                         if(pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints
2083                                 initialize_posetree(ob, pchan); // will attach it to root!
2084                 }
2085                 
2086                 /* 2. the main loop, channels are already hierarchical sorted from root to children */
2087                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2088                         
2089                         /* 3. if we find an IK root, we handle it separated */
2090                         if(pchan->iktree.first) {
2091                                 while(pchan->iktree.first) {
2092                                         PoseTree *tree= pchan->iktree.first;
2093                                         int a;
2094                                         
2095                                         /* 4. walk over the tree for regular solving */
2096                                         for(a=0; a<tree->totchannel; a++) {
2097                                                 if(!(tree->pchan[a]->flag & POSE_DONE)) // successive trees can set the flag
2098                                                         where_is_pose_bone(ob, tree->pchan[a], ctime);
2099                                         }
2100                                         /* 5. execute the IK solver */
2101                                         execute_posetree(ob, tree);
2102                                         
2103                                         /* 6. apply the differences to the channels, 
2104                                                   we need to calculate the original differences first */
2105                                         for(a=0; a<tree->totchannel; a++)
2106                                                 make_dmats(tree->pchan[a]);
2107                                         
2108                                         for(a=0; a<tree->totchannel; a++)
2109                                                 /* sets POSE_DONE */
2110                                                 where_is_ik_bone(tree->pchan[a], tree->basis_change[a]);
2111                                         
2112                                         /* 7. and free */
2113                                         BLI_remlink(&pchan->iktree, tree);
2114                                         free_posetree(tree);
2115                                 }
2116                         }
2117                         else if(!(pchan->flag & POSE_DONE)) {
2118                                 where_is_pose_bone(ob, pchan, ctime);
2119                         }
2120                 }
2121         }
2122                 
2123         /* calculating deform matrices */
2124         for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2125                 if(pchan->bone) {
2126                         Mat4Invert(imat, pchan->bone->arm_mat);
2127                         Mat4MulMat4(pchan->chan_mat, imat, pchan->pose_mat);
2128                 }
2129         }
2130 }