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