IK
[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=0.0f, 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         
1243         if(frompose==NULL) return;
1244         
1245         /* exception, armature local layer should be proxied too */
1246         if(pose->proxy_layer)
1247                 ((bArmature *)ob->data)->layer= pose->proxy_layer;
1248         
1249         /* clear all transformation values from library */
1250         rest_pose(frompose);
1251         
1252         pchan= pose->chanbase.first;
1253         for(; pchan; pchan= pchan->next) {
1254                 if(pchan->bone->layer & layer_protected) {
1255                         pchanp= get_pose_channel(frompose, pchan->name);
1256                         
1257                         /* copy posechannel to temp, but restore important pointers */
1258                         pchanw= *pchanp;
1259                         pchanw.prev= pchan->prev;
1260                         pchanw.next= pchan->next;
1261                         pchanw.parent= pchan->parent;
1262                         pchanw.child= pchan->child;
1263                         pchanw.path= NULL;
1264                         
1265                         /* constraints, set target ob pointer to own object */
1266                         copy_constraints(&pchanw.constraints, &pchanp->constraints);
1267                         
1268                         for (con= pchanw.constraints.first; con; con= con->next) {
1269                                 bConstraintTypeInfo *cti= constraint_get_typeinfo(con);
1270                                 ListBase targets = {NULL, NULL};
1271                                 bConstraintTarget *ct;
1272                                 
1273                                 if (cti && cti->get_constraint_targets) {
1274                                         cti->get_constraint_targets(con, &targets);
1275                                         
1276                                         for (ct= targets.first; ct; ct= ct->next) {
1277                                                 if (ct->tar == from)
1278                                                         ct->tar = ob;
1279                                         }
1280                                         
1281                                         if (cti->flush_constraint_targets)
1282                                                 cti->flush_constraint_targets(con, &targets, 0);
1283                                 }
1284                         }
1285                         
1286                         /* free stuff from current channel */
1287                         if (pchan->path) MEM_freeN(pchan->path);
1288                         free_constraints(&pchan->constraints);
1289                         
1290                         /* the final copy */
1291                         *pchan= pchanw;
1292                 }
1293         }
1294 }
1295
1296 static int rebuild_pose_bone(bPose *pose, Bone *bone, bPoseChannel *parchan, int counter)
1297 {
1298         bPoseChannel *pchan = verify_pose_channel (pose, bone->name);   // verify checks and/or adds
1299
1300         pchan->bone= bone;
1301         pchan->parent= parchan;
1302         
1303         counter++;
1304         
1305         for(bone= bone->childbase.first; bone; bone= bone->next) {
1306                 counter= rebuild_pose_bone(pose, bone, pchan, counter);
1307                 /* for quick detecting of next bone in chain, only b-bone uses it now */
1308                 if(bone->flag & BONE_CONNECTED)
1309                         pchan->child= get_pose_channel(pose, bone->name);
1310         }
1311         
1312         return counter;
1313 }
1314
1315 /* only after leave editmode, duplicating, validating older files, library syncing */
1316 /* NOTE: pose->flag is set for it */
1317 void armature_rebuild_pose(Object *ob, bArmature *arm)
1318 {
1319         Bone *bone;
1320         bPose *pose;
1321         bPoseChannel *pchan, *next;
1322         int counter=0;
1323                 
1324         /* only done here */
1325         if(ob->pose==NULL) ob->pose= MEM_callocN(sizeof(bPose), "new pose");
1326         pose= ob->pose;
1327         
1328         /* clear */
1329         for(pchan= pose->chanbase.first; pchan; pchan= pchan->next) {
1330                 pchan->bone= NULL;
1331                 pchan->child= NULL;
1332         }
1333         
1334         /* first step, check if all channels are there */
1335         for(bone= arm->bonebase.first; bone; bone= bone->next) {
1336                 counter= rebuild_pose_bone(pose, bone, NULL, counter);
1337         }
1338
1339         /* and a check for garbage */
1340         for(pchan= pose->chanbase.first; pchan; pchan= next) {
1341                 next= pchan->next;
1342                 if(pchan->bone==NULL) {
1343                         if(pchan->path)
1344                                 MEM_freeN(pchan->path);
1345                         free_constraints(&pchan->constraints);
1346                         BLI_freelinkN(&pose->chanbase, pchan);
1347                 }
1348         }
1349         // printf("rebuild pose %s, %d bones\n", ob->id.name, counter);
1350         
1351         /* synchronize protected layers with proxy */
1352         if(ob->proxy)
1353                 pose_proxy_synchronize(ob, ob->proxy, arm->layer_protected);
1354         
1355         update_pose_constraint_flags(ob->pose); // for IK detection for example
1356         
1357         /* the sorting */
1358         if(counter>1)
1359                 DAG_pose_sort(ob);
1360         
1361         ob->pose->flag &= ~POSE_RECALC;
1362 }
1363
1364
1365 /* ********************** THE IK SOLVER ******************* */
1366
1367
1368
1369 /* allocates PoseTree, and links that to root bone/channel */
1370 /* Note: detecting the IK chain is duplicate code... in drawarmature.c and in transform_conversions.c */
1371 static void initialize_posetree(struct Object *ob, bPoseChannel *pchan_tip)
1372 {
1373         bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
1374         PoseTree *tree;
1375         PoseTarget *target;
1376         bConstraint *con;
1377         bKinematicConstraint *data;
1378         int a, segcount= 0, size, newsize, *oldparent, parent;
1379         
1380         /* find IK constraint, and validate it */
1381         for(con= pchan_tip->constraints.first; con; con= con->next) {
1382                 if(con->type==CONSTRAINT_TYPE_KINEMATIC) break;
1383         }
1384         if(con==NULL) return;
1385         
1386         data=(bKinematicConstraint*)con->data;
1387         
1388         /* two types of targets */
1389         if(data->flag & CONSTRAINT_IK_AUTO);
1390         else {
1391                 if(con->flag & CONSTRAINT_DISABLE) return;      /* checked in editconstraint.c */
1392                 if(con->enforce == 0.0f) return;
1393                 if(data->tar==NULL) return;
1394                 if(data->tar->type==OB_ARMATURE && data->subtarget[0]==0) return;
1395         }
1396         
1397         /* exclude tip from chain? */
1398         if(!(data->flag & CONSTRAINT_IK_TIP))
1399                 pchan_tip= pchan_tip->parent;
1400         
1401         /* Find the chain's root & count the segments needed */
1402         for (curchan = pchan_tip; curchan; curchan=curchan->parent){
1403                 pchan_root = curchan;
1404                 
1405                 curchan->flag |= POSE_CHAIN;    // don't forget to clear this
1406                 chanlist[segcount]=curchan;
1407                 segcount++;
1408                 
1409                 if(segcount==data->rootbone || segcount>255) break; // 255 is weak
1410         }
1411         if (!segcount) return;
1412
1413         /* setup the chain data */
1414         
1415         /* we make tree-IK, unless all existing targets are in this chain */
1416         for(tree= pchan_root->iktree.first; tree; tree= tree->next) {
1417                 for(target= tree->targets.first; target; target= target->next) {
1418                         curchan= tree->pchan[target->tip];
1419                         if(curchan->flag & POSE_CHAIN)
1420                                 curchan->flag &= ~POSE_CHAIN;
1421                         else
1422                                 break;
1423                 }
1424                 if(target) break;
1425         }
1426
1427         /* create a target */
1428         target= MEM_callocN(sizeof(PoseTarget), "posetarget");
1429         target->con= con;
1430         pchan_tip->flag &= ~POSE_CHAIN;
1431
1432         if(tree==NULL) {
1433                 /* make new tree */
1434                 tree= MEM_callocN(sizeof(PoseTree), "posetree");
1435
1436                 tree->iterations= data->iterations;
1437                 tree->totchannel= segcount;
1438                 tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
1439                 
1440                 tree->pchan= MEM_callocN(segcount*sizeof(void*), "ik tree pchan");
1441                 tree->parent= MEM_callocN(segcount*sizeof(int), "ik tree parent");
1442                 for(a=0; a<segcount; a++) {
1443                         tree->pchan[a]= chanlist[segcount-a-1];
1444                         tree->parent[a]= a-1;
1445                 }
1446                 target->tip= segcount-1;
1447                 
1448                 /* AND! link the tree to the root */
1449                 BLI_addtail(&pchan_root->iktree, tree);
1450         }
1451         else {
1452                 tree->iterations= MAX2(data->iterations, tree->iterations);
1453                 tree->stretch= tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
1454
1455                 /* skip common pose channels and add remaining*/
1456                 size= MIN2(segcount, tree->totchannel);
1457                 for(a=0; a<size && tree->pchan[a]==chanlist[segcount-a-1]; a++);
1458                 parent= a-1;
1459
1460                 segcount= segcount-a;
1461                 target->tip= tree->totchannel + segcount - 1;
1462
1463                 if (segcount > 0) {
1464                         /* resize array */
1465                         newsize= tree->totchannel + segcount;
1466                         oldchan= tree->pchan;
1467                         oldparent= tree->parent;
1468
1469                         tree->pchan= MEM_callocN(newsize*sizeof(void*), "ik tree pchan");
1470                         tree->parent= MEM_callocN(newsize*sizeof(int), "ik tree parent");
1471                         memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel);
1472                         memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel);
1473                         MEM_freeN(oldchan);
1474                         MEM_freeN(oldparent);
1475
1476                         /* add new pose channels at the end, in reverse order */
1477                         for(a=0; a<segcount; a++) {
1478                                 tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1];
1479                                 tree->parent[tree->totchannel+a]= tree->totchannel+a-1;
1480                         }
1481                         tree->parent[tree->totchannel]= parent;
1482                         
1483                         tree->totchannel= newsize;
1484                 }
1485
1486                 /* move tree to end of list, for correct evaluation order */
1487                 BLI_remlink(&pchan_root->iktree, tree);
1488                 BLI_addtail(&pchan_root->iktree, tree);
1489         }
1490
1491         /* add target to the tree */
1492         BLI_addtail(&tree->targets, target);
1493 }
1494
1495 /* called from within the core where_is_pose loop, all animsystems and constraints
1496 were executed & assigned. Now as last we do an IK pass */
1497 static void execute_posetree(Object *ob, PoseTree *tree)
1498 {
1499         float R_parmat[3][3];
1500         float iR_parmat[3][3];
1501         float R_bonemat[3][3];
1502         float goalrot[3][3], goalpos[3];
1503         float rootmat[4][4], imat[4][4];
1504         float goal[4][4], goalinv[4][4];
1505         float irest_basis[3][3], full_basis[3][3];
1506         float end_pose[4][4], world_pose[4][4];
1507         float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch=NULL;
1508         int a, flag, hasstretch=0;
1509         bPoseChannel *pchan;
1510         IK_Segment *seg, *parent, **iktree, *iktarget;
1511         IK_Solver *solver;
1512         PoseTarget *target;
1513         bKinematicConstraint *data, *poleangledata=NULL;
1514         Bone *bone;
1515
1516         if (tree->totchannel == 0)
1517                 return;
1518
1519         iktree= MEM_mallocN(sizeof(void*)*tree->totchannel, "ik tree");
1520
1521         for(a=0; a<tree->totchannel; a++) {
1522                 pchan= tree->pchan[a];
1523                 bone= pchan->bone;
1524
1525                 /* set DoF flag */
1526                 flag= 0;
1527                 if((pchan->ikflag & BONE_IK_NO_XDOF) == 0)
1528                         flag |= IK_XDOF;
1529                 if((pchan->ikflag & BONE_IK_NO_YDOF) == 0)
1530                         flag |= IK_YDOF;
1531                 if((pchan->ikflag & BONE_IK_NO_ZDOF) == 0)
1532                         flag |= IK_ZDOF;
1533
1534                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
1535                         flag |= IK_TRANS_YDOF;
1536                         hasstretch = 1;
1537                 }
1538
1539                 seg= iktree[a]= IK_CreateSegment(flag);
1540
1541                 /* find parent */
1542                 if(a == 0)
1543                         parent= NULL;
1544                 else
1545                         parent= iktree[tree->parent[a]];
1546
1547                 IK_SetParent(seg, parent);
1548         
1549                 /* get the matrix that transforms from prevbone into this bone */
1550                 Mat3CpyMat4(R_bonemat, pchan->pose_mat);
1551
1552                 /* gather transformations for this IK segment */
1553
1554                 if (pchan->parent)
1555                         Mat3CpyMat4(R_parmat, pchan->parent->pose_mat);
1556                 else
1557                         Mat3One(R_parmat);
1558
1559                 /* bone offset */
1560                 if (pchan->parent && (a > 0))
1561                         VecSubf(start, pchan->pose_head, pchan->parent->pose_tail);
1562                 else
1563                         /* only root bone (a = 0) has no parent */
1564                         start[0]= start[1]= start[2]= 0.0f;
1565                 
1566                 /* change length based on bone size */
1567                 length= bone->length*VecLength(R_bonemat[1]);
1568
1569                 /* compute rest basis and its inverse */
1570                 Mat3CpyMat3(rest_basis, bone->bone_mat);
1571                 Mat3CpyMat3(irest_basis, bone->bone_mat);
1572                 Mat3Transp(irest_basis);
1573
1574                 /* compute basis with rest_basis removed */
1575                 Mat3Inv(iR_parmat, R_parmat);
1576                 Mat3MulMat3(full_basis, iR_parmat, R_bonemat);
1577                 Mat3MulMat3(basis, irest_basis, full_basis);
1578
1579                 /* basis must be pure rotation */
1580                 Mat3Ortho(basis);
1581
1582                 /* transform offset into local bone space */
1583                 Mat3Ortho(iR_parmat);
1584                 Mat3MulVecfl(iR_parmat, start);
1585
1586                 IK_SetTransform(seg, start, rest_basis, basis, length);
1587
1588                 if (pchan->ikflag & BONE_IK_XLIMIT)
1589                         IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]);
1590                 if (pchan->ikflag & BONE_IK_YLIMIT)
1591                         IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]);
1592                 if (pchan->ikflag & BONE_IK_ZLIMIT)
1593                         IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]);
1594
1595                 IK_SetStiffness(seg, IK_X, pchan->stiffness[0]);
1596                 IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]);
1597                 IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]);
1598
1599                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
1600                         float ikstretch = pchan->ikstretch*pchan->ikstretch;
1601                         IK_SetStiffness(seg, IK_TRANS_Y, MIN2(1.0-ikstretch, 0.999));
1602                         IK_SetLimit(seg, IK_TRANS_Y, 0.001, 1e10);
1603                 }
1604         }
1605
1606         solver= IK_CreateSolver(iktree[0]);
1607
1608         /* set solver goals */
1609
1610         /* first set the goal inverse transform, assuming the root of tree was done ok! */
1611         pchan= tree->pchan[0];
1612         if (pchan->parent)
1613                 /* transform goal by parent mat, so this rotation is not part of the
1614                    segment's basis. otherwise rotation limits do not work on the
1615                    local transform of the segment itself. */
1616                 Mat4CpyMat4(rootmat, pchan->parent->pose_mat);
1617         else
1618                 Mat4One(rootmat);
1619         VECCOPY(rootmat[3], pchan->pose_head);
1620         
1621         Mat4MulMat4 (imat, rootmat, ob->obmat);
1622         Mat4Invert (goalinv, imat);
1623         
1624         for (target=tree->targets.first; target; target=target->next) {
1625                 float polepos[3];
1626                 int poleconstrain= 0;
1627
1628                 data= (bKinematicConstraint*)target->con->data;
1629                 
1630                 /* 1.0=ctime, we pass on object for auto-ik (owner-type here is object, even though
1631                  * strictly speaking, it is a posechannel)
1632                  */
1633                 get_constraint_target_matrix(target->con, 0, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
1634                 
1635                 /* and set and transform goal */
1636                 Mat4MulMat4(goal, rootmat, goalinv);
1637                 
1638                 VECCOPY(goalpos, goal[3]);
1639                 Mat3CpyMat4(goalrot, goal);
1640                 
1641                 /* same for pole vector target */
1642                 if(data->poletar) {
1643                         get_constraint_target_matrix(target->con, 1, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
1644
1645                         if(data->flag & CONSTRAINT_IK_SETANGLE) {
1646                                 /* don't solve IK when we are setting the pole angle */
1647                                 break;
1648                         }
1649                         else {
1650                                 Mat4MulMat4(goal, rootmat, goalinv);
1651                                 VECCOPY(polepos, goal[3]);
1652                                 poleconstrain= 1;
1653
1654                                 if(data->flag & CONSTRAINT_IK_GETANGLE) {
1655                                         poleangledata= data;
1656                                         data->flag &= ~CONSTRAINT_IK_GETANGLE;
1657                                 }
1658                         }
1659                 }
1660
1661                 /* do we need blending? */
1662                 if (target->con->enforce!=1.0) {
1663                         float q1[4], q2[4], q[4];
1664                         float fac= target->con->enforce;
1665                         float mfac= 1.0-fac;
1666                         
1667                         pchan= tree->pchan[target->tip];
1668                         
1669                         /* end effector in world space */
1670                         Mat4CpyMat4(end_pose, pchan->pose_mat);
1671                         VECCOPY(end_pose[3], pchan->pose_tail);
1672                         Mat4MulSerie(world_pose, goalinv, ob->obmat, end_pose, 0, 0, 0, 0, 0);
1673                         
1674                         /* blend position */
1675                         goalpos[0]= fac*goalpos[0] + mfac*world_pose[3][0];
1676                         goalpos[1]= fac*goalpos[1] + mfac*world_pose[3][1];
1677                         goalpos[2]= fac*goalpos[2] + mfac*world_pose[3][2];
1678                         
1679                         /* blend rotation */
1680                         Mat3ToQuat(goalrot, q1);
1681                         Mat4ToQuat(world_pose, q2);
1682                         QuatInterpol(q, q1, q2, mfac);
1683                         QuatToMat3(q, goalrot);
1684                 }
1685                 
1686                 iktarget= iktree[target->tip];
1687                 
1688                 if(data->weight != 0.0) {
1689                         if(poleconstrain)
1690                                 IK_SolverSetPoleVectorConstraint(solver, iktarget, goalpos,
1691                                         polepos, data->poleangle*M_PI/180, (poleangledata == data));
1692                         IK_SolverAddGoal(solver, iktarget, goalpos, data->weight);
1693                 }
1694                 if((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0))
1695                         if((data->flag & CONSTRAINT_IK_AUTO)==0)
1696                                 IK_SolverAddGoalOrientation(solver, iktarget, goalrot,
1697                                         data->orientweight);
1698         }
1699
1700         /* solve */
1701         IK_Solve(solver, 0.0f, tree->iterations);
1702
1703         if(poleangledata)
1704                 poleangledata->poleangle= IK_SolverGetPoleAngle(solver)*180/M_PI;
1705
1706         IK_FreeSolver(solver);
1707
1708         /* gather basis changes */
1709         tree->basis_change= MEM_mallocN(sizeof(float[3][3])*tree->totchannel, "ik basis change");
1710         if(hasstretch)
1711                 ikstretch= MEM_mallocN(sizeof(float)*tree->totchannel, "ik stretch");
1712
1713         for(a=0; a<tree->totchannel; a++) {
1714                 IK_GetBasisChange(iktree[a], tree->basis_change[a]);
1715
1716                 if(hasstretch) {
1717                         /* have to compensate for scaling received from parent */
1718                         float parentstretch, stretch;
1719
1720                         pchan= tree->pchan[a];
1721                         parentstretch= (tree->parent[a] >= 0)? ikstretch[tree->parent[a]]: 1.0;
1722
1723                         if(tree->stretch && (pchan->ikstretch > 0.0)) {
1724                                 float trans[3], length;
1725
1726                                 IK_GetTranslationChange(iktree[a], trans);
1727                                 length= pchan->bone->length*VecLength(pchan->pose_mat[1]);
1728
1729                                 ikstretch[a]= (length == 0.0)? 1.0: (trans[1]+length)/length;
1730                         }
1731                         else
1732                                 ikstretch[a] = 1.0;
1733
1734                         stretch= (parentstretch == 0.0)? 1.0: ikstretch[a]/parentstretch;
1735
1736                         VecMulf(tree->basis_change[a][0], stretch);
1737                         VecMulf(tree->basis_change[a][1], stretch);
1738                         VecMulf(tree->basis_change[a][2], stretch);
1739
1740                 }
1741
1742                 IK_FreeSegment(iktree[a]);
1743         }
1744         
1745         MEM_freeN(iktree);
1746         if(ikstretch) MEM_freeN(ikstretch);
1747 }
1748
1749 void free_posetree(PoseTree *tree)
1750 {
1751         BLI_freelistN(&tree->targets);
1752         if(tree->pchan) MEM_freeN(tree->pchan);
1753         if(tree->parent) MEM_freeN(tree->parent);
1754         if(tree->basis_change) MEM_freeN(tree->basis_change);
1755         MEM_freeN(tree);
1756 }
1757
1758 /* ********************** THE POSE SOLVER ******************* */
1759
1760
1761 /* loc/rot/size to mat4 */
1762 /* used in constraint.c too */
1763 void chan_calc_mat(bPoseChannel *chan)
1764 {
1765         float smat[3][3];
1766         float rmat[3][3];
1767         float tmat[3][3];
1768         
1769         SizeToMat3(chan->size, smat);
1770         
1771         NormalQuat(chan->quat);
1772
1773         QuatToMat3(chan->quat, rmat);
1774         
1775         Mat3MulMat3(tmat, rmat, smat);
1776         
1777         Mat4CpyMat3(chan->chan_mat, tmat);
1778         
1779         /* prevent action channels breaking chains */
1780         /* need to check for bone here, CONSTRAINT_TYPE_ACTION uses this call */
1781         if (chan->bone==NULL || !(chan->bone->flag & BONE_CONNECTED)) {
1782                 VECCOPY(chan->chan_mat[3], chan->loc);
1783         }
1784
1785 }
1786
1787 /* transform from bone(b) to bone(b+1), store in chan_mat */
1788 static void make_dmats(bPoseChannel *pchan)
1789 {
1790         if (pchan->parent) {
1791                 float iR_parmat[4][4];
1792                 Mat4Invert(iR_parmat, pchan->parent->pose_mat);
1793                 Mat4MulMat4(pchan->chan_mat,  pchan->pose_mat, iR_parmat);      // delta mat
1794         }
1795         else Mat4CpyMat4(pchan->chan_mat, pchan->pose_mat);
1796 }
1797
1798 /* applies IK matrix to pchan, IK is done separated */
1799 /* formula: pose_mat(b) = pose_mat(b-1) * diffmat(b-1, b) * ik_mat(b) */
1800 /* to make this work, the diffmats have to be precalculated! Stored in chan_mat */
1801 static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[][3])   // nr = to detect if this is first bone
1802 {
1803         float vec[3], ikmat[4][4];
1804         
1805         Mat4CpyMat3(ikmat, ik_mat);
1806         
1807         if (pchan->parent)
1808                 Mat4MulSerie(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat, ikmat, NULL, NULL, NULL, NULL, NULL);
1809         else 
1810                 Mat4MulMat4(pchan->pose_mat, ikmat, pchan->chan_mat);
1811
1812         /* calculate head */
1813         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
1814         /* calculate tail */
1815         VECCOPY(vec, pchan->pose_mat[1]);
1816         VecMulf(vec, pchan->bone->length);
1817         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
1818
1819         pchan->flag |= POSE_DONE;
1820 }
1821
1822 /* NLA strip modifiers */
1823 static void do_strip_modifiers(Object *armob, Bone *bone, bPoseChannel *pchan)
1824 {
1825         bActionModifier *amod;
1826         bActionStrip *strip;
1827         float scene_cfra= G.scene->r.cfra;
1828
1829         for (strip=armob->nlastrips.first; strip; strip=strip->next) {
1830                 if(scene_cfra>=strip->start && scene_cfra<=strip->end) {
1831                         
1832                         /* temporal solution to prevent 2 strips accumulating */
1833                         if(scene_cfra==strip->end && strip->next && strip->next->start==scene_cfra)
1834                                 continue;
1835                         
1836                         for(amod= strip->modifiers.first; amod; amod= amod->next) {
1837                                 switch (amod->type) {
1838                                 case ACTSTRIP_MOD_DEFORM:
1839                                 {
1840                                         /* validate first */
1841                                         if(amod->ob && amod->ob->type==OB_CURVE && amod->channel[0]) {
1842                                                 
1843                                                 if( strcmp(pchan->name, amod->channel)==0 ) {
1844                                                         float mat4[4][4], mat3[3][3];
1845                                                         
1846                                                         curve_deform_vector(amod->ob, armob, bone->arm_mat[3], pchan->pose_mat[3], mat3, amod->no_rot_axis);
1847                                                         Mat4CpyMat4(mat4, pchan->pose_mat);
1848                                                         Mat4MulMat34(pchan->pose_mat, mat3, mat4);
1849                                                         
1850                                                 }
1851                                         }
1852                                 }
1853                                         break;
1854                                 case ACTSTRIP_MOD_NOISE:        
1855                                 {
1856                                         if( strcmp(pchan->name, amod->channel)==0 ) {
1857                                                 float nor[3], loc[3], ofs;
1858                                                 float eul[3], size[3], eulo[3], sizeo[3];
1859                                                 
1860                                                 /* calculate turbulance */
1861                                                 ofs = amod->turbul / 200.0f;
1862                                                 
1863                                                 /* make a copy of starting conditions */
1864                                                 VECCOPY(loc, pchan->pose_mat[3]);
1865                                                 Mat4ToEul(pchan->pose_mat, eul);
1866                                                 Mat4ToSize(pchan->pose_mat, size);
1867                                                 VECCOPY(eulo, eul);
1868                                                 VECCOPY(sizeo, size);
1869                                                 
1870                                                 /* apply noise to each set of channels */
1871                                                 if (amod->channels & 4) {
1872                                                         /* for scaling */
1873                                                         nor[0] = BLI_gNoise(amod->noisesize, size[0]+ofs, size[1], size[2], 0, 0) - ofs;
1874                                                         nor[1] = BLI_gNoise(amod->noisesize, size[0], size[1]+ofs, size[2], 0, 0) - ofs;        
1875                                                         nor[2] = BLI_gNoise(amod->noisesize, size[0], size[1], size[2]+ofs, 0, 0) - ofs;
1876                                                         VecAddf(size, size, nor);
1877                                                         
1878                                                         if (sizeo[0] != 0)
1879                                                                 VecMulf(pchan->pose_mat[0], size[0] / sizeo[0]);
1880                                                         if (sizeo[1] != 0)
1881                                                                 VecMulf(pchan->pose_mat[1], size[1] / sizeo[1]);
1882                                                         if (sizeo[2] != 0)
1883                                                                 VecMulf(pchan->pose_mat[2], size[2] / sizeo[2]);
1884                                                 }
1885                                                 if (amod->channels & 2) {
1886                                                         /* for rotation */
1887                                                         nor[0] = BLI_gNoise(amod->noisesize, eul[0]+ofs, eul[1], eul[2], 0, 0) - ofs;
1888                                                         nor[1] = BLI_gNoise(amod->noisesize, eul[0], eul[1]+ofs, eul[2], 0, 0) - ofs;   
1889                                                         nor[2] = BLI_gNoise(amod->noisesize, eul[0], eul[1], eul[2]+ofs, 0, 0) - ofs;
1890                                                         
1891                                                         compatible_eul(nor, eulo);
1892                                                         VecAddf(eul, eul, nor);
1893                                                         compatible_eul(eul, eulo);
1894                                                         
1895                                                         LocEulSizeToMat4(pchan->pose_mat, loc, eul, size);
1896                                                 }
1897                                                 if (amod->channels & 1) {
1898                                                         /* for location */
1899                                                         nor[0] = BLI_gNoise(amod->noisesize, loc[0]+ofs, loc[1], loc[2], 0, 0) - ofs;
1900                                                         nor[1] = BLI_gNoise(amod->noisesize, loc[0], loc[1]+ofs, loc[2], 0, 0) - ofs;   
1901                                                         nor[2] = BLI_gNoise(amod->noisesize, loc[0], loc[1], loc[2]+ofs, 0, 0) - ofs;
1902                                                         
1903                                                         VecAddf(pchan->pose_mat[3], loc, nor);
1904                                                 }
1905                                         }
1906                                 }
1907                                         break;
1908                                 }
1909                         }
1910                 }
1911         }
1912 }
1913
1914
1915 /* The main armature solver, does all constraints excluding IK */
1916 /* pchan is validated, as having bone and parent pointer */
1917 static void where_is_pose_bone(Object *ob, bPoseChannel *pchan, float ctime)
1918 {
1919         Bone *bone, *parbone;
1920         bPoseChannel *parchan;
1921         float vec[3];
1922         
1923         /* set up variables for quicker access below */
1924         bone= pchan->bone;
1925         parbone= bone->parent;
1926         parchan= pchan->parent;
1927         
1928         /* this gives a chan_mat with actions (ipos) results */
1929         chan_calc_mat(pchan);
1930         
1931         /* construct the posemat based on PoseChannels, that we do before applying constraints */
1932         /* pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b) */
1933         
1934         if(parchan) {
1935                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
1936                 
1937                 /* bone transform itself */
1938                 Mat4CpyMat3(offs_bone, bone->bone_mat);
1939                 
1940                 /* The bone's root offset (is in the parent's coordinate system) */
1941                 VECCOPY(offs_bone[3], bone->head);
1942                 
1943                 /* Get the length translation of parent (length along y axis) */
1944                 offs_bone[3][1]+= parbone->length;
1945                 
1946                 /* Compose the matrix for this bone  */
1947                 if(bone->flag & BONE_HINGE) {   // uses restposition rotation, but actual position
1948                         float tmat[4][4];
1949                         
1950                         /* the rotation of the parent restposition */
1951                         Mat4CpyMat4(tmat, parbone->arm_mat);
1952                         
1953                         /* the location of actual parent transform */
1954                         VECCOPY(tmat[3], offs_bone[3]);
1955                         offs_bone[3][0]= offs_bone[3][1]= offs_bone[3][2]= 0.0f;
1956                         Mat4MulVecfl(parchan->pose_mat, tmat[3]);
1957                         
1958                         Mat4MulSerie(pchan->pose_mat, tmat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
1959                 }
1960                 else 
1961                         Mat4MulSerie(pchan->pose_mat, parchan->pose_mat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
1962         }
1963         else {
1964                 Mat4MulMat4(pchan->pose_mat, pchan->chan_mat, bone->arm_mat);
1965                 /* only rootbones get the cyclic offset */
1966                 VecAddf(pchan->pose_mat[3], pchan->pose_mat[3], ob->pose->cyclic_offset);
1967         }
1968         
1969         /* do NLA strip modifiers - i.e. curve follow */
1970         do_strip_modifiers(ob, bone, pchan);
1971         
1972         /* Do constraints */
1973         if (pchan->constraints.first) {
1974                 bConstraintOb *cob;
1975                 
1976                 /* make a copy of location of PoseChannel for later */
1977                 VECCOPY(vec, pchan->pose_mat[3]);
1978                 
1979                 /* prepare PoseChannel for Constraint solving 
1980                  * - makes a copy of matrix, and creates temporary struct to use 
1981                  */
1982                 cob= constraints_make_evalob(ob, pchan, CONSTRAINT_OBTYPE_BONE);
1983                 
1984                 /* Solve PoseChannel's Constraints */
1985                 solve_constraints(&pchan->constraints, cob, ctime);     // ctime doesnt alter objects
1986                 
1987                 /* cleanup after Constraint Solving 
1988                  * - applies matrix back to pchan, and frees temporary struct used
1989                  */
1990                 constraints_clear_evalob(cob);
1991                 
1992                 /* prevent constraints breaking a chain */
1993                 if(pchan->bone->flag & BONE_CONNECTED) {
1994                         VECCOPY(pchan->pose_mat[3], vec);
1995                 }
1996         }
1997         
1998         /* calculate head */
1999         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
2000         /* calculate tail */
2001         VECCOPY(vec, pchan->pose_mat[1]);
2002         VecMulf(vec, bone->length);
2003         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
2004 }
2005
2006 /* This only reads anim data from channels, and writes to channels */
2007 /* This is the only function adding poses */
2008 void where_is_pose (Object *ob)
2009 {
2010         bArmature *arm;
2011         Bone *bone;
2012         bPoseChannel *pchan;
2013         float imat[4][4];
2014         float ctime= bsystem_time(ob, (float)G.scene->r.cfra, 0.0);     /* not accurate... */
2015         
2016         arm = get_armature(ob);
2017         
2018         if(arm==NULL) return;
2019         if(ob->pose==NULL || (ob->pose->flag & POSE_RECALC)) 
2020            armature_rebuild_pose(ob, arm);
2021         
2022         /* In restposition we read the data from the bones */
2023         if(ob==G.obedit || (arm->flag & ARM_RESTPOS)) {
2024                 
2025                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2026                         bone= pchan->bone;
2027                         if(bone) {
2028                                 Mat4CpyMat4(pchan->pose_mat, bone->arm_mat);
2029                                 VECCOPY(pchan->pose_head, bone->arm_head);
2030                                 VECCOPY(pchan->pose_tail, bone->arm_tail);
2031                         }
2032                 }
2033         }
2034         else {
2035                 Mat4Invert(ob->imat, ob->obmat);        // imat is needed 
2036
2037                 /* 1. construct the PoseTrees, clear flags */
2038                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2039                         pchan->flag &= ~(POSE_DONE|POSE_CHAIN);
2040                         if(pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints
2041                                 initialize_posetree(ob, pchan); // will attach it to root!
2042                 }
2043                 
2044                 /* 2. the main loop, channels are already hierarchical sorted from root to children */
2045                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2046                         
2047                         /* 3. if we find an IK root, we handle it separated */
2048                         if(pchan->iktree.first) {
2049                                 while(pchan->iktree.first) {
2050                                         PoseTree *tree= pchan->iktree.first;
2051                                         int a;
2052                                         
2053                                         /* 4. walk over the tree for regular solving */
2054                                         for(a=0; a<tree->totchannel; a++) {
2055                                                 if(!(tree->pchan[a]->flag & POSE_DONE)) // successive trees can set the flag
2056                                                         where_is_pose_bone(ob, tree->pchan[a], ctime);
2057                                         }
2058                                         /* 5. execute the IK solver */
2059                                         execute_posetree(ob, tree);
2060                                         
2061                                         /* 6. apply the differences to the channels, 
2062                                                   we need to calculate the original differences first */
2063                                         for(a=0; a<tree->totchannel; a++)
2064                                                 make_dmats(tree->pchan[a]);
2065                                         
2066                                         for(a=0; a<tree->totchannel; a++)
2067                                                 /* sets POSE_DONE */
2068                                                 where_is_ik_bone(tree->pchan[a], tree->basis_change[a]);
2069                                         
2070                                         /* 7. and free */
2071                                         BLI_remlink(&pchan->iktree, tree);
2072                                         free_posetree(tree);
2073                                 }
2074                         }
2075                         else if(!(pchan->flag & POSE_DONE)) {
2076                                 where_is_pose_bone(ob, pchan, ctime);
2077                         }
2078                 }
2079         }
2080                 
2081         /* calculating deform matrices */
2082         for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2083                 if(pchan->bone) {
2084                         Mat4Invert(imat, pchan->bone->arm_mat);
2085                         Mat4MulMat4(pchan->chan_mat, imat, pchan->pose_mat);
2086                 }
2087         }
2088 }