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