2.5
[blender.git] / source / blender / blenkernel / intern / armature.c
1 /**
2  * $Id$
3  *
4  * ***** BEGIN GPL 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 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 <float.h>
34
35 #include "MEM_guardedalloc.h"
36
37 //XXX #include "nla.h"
38
39 #include "BLI_arithb.h"
40 #include "BLI_blenlib.h"
41
42 #include "DNA_armature_types.h"
43 #include "DNA_action_types.h"
44 #include "DNA_constraint_types.h"
45 #include "DNA_mesh_types.h"
46 #include "DNA_lattice_types.h"
47 #include "DNA_meshdata_types.h"
48 #include "DNA_nla_types.h"
49 #include "DNA_object_types.h"
50 #include "DNA_scene_types.h"
51 #include "DNA_view3d_types.h"
52
53 #include "BKE_armature.h"
54 #include "BKE_action.h"
55 #include "BKE_blender.h"
56 #include "BKE_constraint.h"
57 #include "BKE_curve.h"
58 #include "BKE_deform.h"
59 #include "BKE_depsgraph.h"
60 #include "BKE_DerivedMesh.h"
61 #include "BKE_displist.h"
62 #include "BKE_global.h"
63 #include "BKE_library.h"
64 #include "BKE_lattice.h"
65 #include "BKE_main.h"
66 #include "BKE_object.h"
67 #include "BKE_object.h"
68 #include "BKE_utildefines.h"
69
70 //XXX #include "BIF_editdeform.h"
71
72 #include "IK_solver.h"
73
74 #ifdef HAVE_CONFIG_H
75 #include <config.h>
76 #endif
77
78 /*      **************** Generic Functions, data level *************** */
79
80 bArmature *add_armature(char *name)
81 {
82         bArmature *arm;
83         
84         arm= alloc_libblock (&G.main->armature, ID_AR, name);
85         arm->deformflag = ARM_DEF_VGROUP|ARM_DEF_ENVELOPE;
86         arm->layer= 1;
87         return arm;
88 }
89
90
91 void free_boneChildren(Bone *bone)
92
93         Bone *child;
94         
95         if (bone) {
96                 
97                 child=bone->childbase.first;
98                 if (child){
99                         while (child){
100                                 free_boneChildren (child);
101                                 child=child->next;
102                         }
103                         BLI_freelistN (&bone->childbase);
104                 }
105         }
106 }
107
108 void free_bones (bArmature *arm)
109 {
110         Bone *bone;
111         /*      Free children (if any)  */
112         bone= arm->bonebase.first;
113         if (bone) {
114                 while (bone){
115                         free_boneChildren (bone);
116                         bone=bone->next;
117                 }
118         }
119         
120         
121         BLI_freelistN(&arm->bonebase);
122 }
123
124 void free_armature(bArmature *arm)
125 {
126         if (arm) {
127                 /*              unlink_armature(arm);*/
128                 free_bones(arm);
129         }
130 }
131
132 void make_local_armature(bArmature *arm)
133 {
134         int local=0, lib=0;
135         Object *ob;
136         bArmature *newArm;
137         
138         if (arm->id.lib==0)
139                 return;
140         if (arm->id.us==1) {
141                 arm->id.lib= 0;
142                 arm->id.flag= LIB_LOCAL;
143                 new_id(0, (ID*)arm, 0);
144                 return;
145         }
146         
147         if(local && lib==0) {
148                 arm->id.lib= 0;
149                 arm->id.flag= LIB_LOCAL;
150                 new_id(0, (ID *)arm, 0);
151         }
152         else if(local && lib) {
153                 newArm= copy_armature(arm);
154                 newArm->id.us= 0;
155                 
156                 ob= G.main->object.first;
157                 while(ob) {
158                         if(ob->data==arm) {
159                                 
160                                 if(ob->id.lib==0) {
161                                         ob->data= newArm;
162                                         newArm->id.us++;
163                                         arm->id.us--;
164                                 }
165                         }
166                         ob= ob->id.next;
167                 }
168         }
169 }
170
171 static void     copy_bonechildren (Bone* newBone, Bone* oldBone)
172 {
173         Bone    *curBone, *newChildBone;
174         
175         /*      Copy this bone's list*/
176         BLI_duplicatelist(&newBone->childbase, &oldBone->childbase);
177         
178         /*      For each child in the list, update it's children*/
179         newChildBone=newBone->childbase.first;
180         for (curBone=oldBone->childbase.first;curBone;curBone=curBone->next){
181                 newChildBone->parent=newBone;
182                 copy_bonechildren(newChildBone,curBone);
183                 newChildBone=newChildBone->next;
184         }
185 }
186
187 bArmature *copy_armature(bArmature *arm)
188 {
189         bArmature *newArm;
190         Bone            *oldBone, *newBone;
191         
192         newArm= copy_libblock (arm);
193         BLI_duplicatelist(&newArm->bonebase, &arm->bonebase);
194         
195         /*      Duplicate the childrens' lists*/
196         newBone=newArm->bonebase.first;
197         for (oldBone=arm->bonebase.first;oldBone;oldBone=oldBone->next){
198                 newBone->parent=NULL;
199                 copy_bonechildren (newBone, oldBone);
200                 newBone=newBone->next;
201         };
202         
203         return newArm;
204 }
205
206 static Bone *get_named_bone_bonechildren (Bone *bone, const char *name)
207 {
208         Bone *curBone, *rbone;
209         
210         if (!strcmp (bone->name, name))
211                 return bone;
212         
213         for (curBone=bone->childbase.first; curBone; curBone=curBone->next){
214                 rbone=get_named_bone_bonechildren (curBone, name);
215                 if (rbone)
216                         return rbone;
217         }
218         
219         return NULL;
220 }
221
222
223 Bone *get_named_bone (bArmature *arm, const char *name)
224 /*
225         Walk the list until the bone is found
226  */
227 {
228         Bone *bone=NULL, *curBone;
229         
230         if (!arm) return NULL;
231         
232         for (curBone=arm->bonebase.first; curBone; curBone=curBone->next){
233                 bone = get_named_bone_bonechildren (curBone, name);
234                 if (bone)
235                         return bone;
236         }
237         
238         return bone;
239 }
240
241
242 #define IS_SEPARATOR(a) (a=='.' || a==' ' || a=='-' || a=='_')
243
244 /* finds the best possible flipped name. For renaming; check for unique names afterwards */
245 /* if strip_number: removes number extensions */
246 void bone_flip_name (char *name, int strip_number)
247 {
248         int             len;
249         char    prefix[128]={""};       /* The part before the facing */
250         char    suffix[128]={""};       /* The part after the facing */
251         char    replace[128]={""};      /* The replacement string */
252         char    number[128]={""};       /* The number extension string */
253         char    *index=NULL;
254
255         len= strlen(name);
256         if(len<3) return;       // we don't do names like .R or .L
257
258         /* We first check the case with a .### extension, let's find the last period */
259         if(isdigit(name[len-1])) {
260                 index= strrchr(name, '.');      // last occurrance
261                 if (index && isdigit(index[1]) ) {              // doesnt handle case bone.1abc2 correct..., whatever!
262                         if(strip_number==0) 
263                                 strcpy(number, index);
264                         *index= 0;
265                         len= strlen(name);
266                 }
267         }
268
269         strcpy (prefix, name);
270
271         /* first case; separator . - _ with extensions r R l L  */
272         if( IS_SEPARATOR(name[len-2]) ) {
273                 switch(name[len-1]) {
274                         case 'l':
275                                 prefix[len-1]= 0;
276                                 strcpy(replace, "r");
277                                 break;
278                         case 'r':
279                                 prefix[len-1]= 0;
280                                 strcpy(replace, "l");
281                                 break;
282                         case 'L':
283                                 prefix[len-1]= 0;
284                                 strcpy(replace, "R");
285                                 break;
286                         case 'R':
287                                 prefix[len-1]= 0;
288                                 strcpy(replace, "L");
289                                 break;
290                 }
291         }
292         /* case; beginning with r R l L , with separator after it */
293         else if( IS_SEPARATOR(name[1]) ) {
294                 switch(name[0]) {
295                         case 'l':
296                                 strcpy(replace, "r");
297                                 strcpy(suffix, name+1);
298                                 prefix[0]= 0;
299                                 break;
300                         case 'r':
301                                 strcpy(replace, "l");
302                                 strcpy(suffix, name+1);
303                                 prefix[0]= 0;
304                                 break;
305                         case 'L':
306                                 strcpy(replace, "R");
307                                 strcpy(suffix, name+1);
308                                 prefix[0]= 0;
309                                 break;
310                         case 'R':
311                                 strcpy(replace, "L");
312                                 strcpy(suffix, name+1);
313                                 prefix[0]= 0;
314                                 break;
315                 }
316         }
317         else if(len > 5) {
318                 /* hrms, why test for a separator? lets do the rule 'ultimate left or right' */
319                 index = BLI_strcasestr(prefix, "right");
320                 if (index==prefix || index==prefix+len-5) {
321                         if(index[0]=='r') 
322                                 strcpy (replace, "left");
323                         else {
324                                 if(index[1]=='I') 
325                                         strcpy (replace, "LEFT");
326                                 else
327                                         strcpy (replace, "Left");
328                         }
329                         *index= 0;
330                         strcpy (suffix, index+5);
331                 }
332                 else {
333                         index = BLI_strcasestr(prefix, "left");
334                         if (index==prefix || index==prefix+len-4) {
335                                 if(index[0]=='l') 
336                                         strcpy (replace, "right");
337                                 else {
338                                         if(index[1]=='E') 
339                                                 strcpy (replace, "RIGHT");
340                                         else
341                                                 strcpy (replace, "Right");
342                                 }
343                                 *index= 0;
344                                 strcpy (suffix, index+4);
345                         }
346                 }               
347         }
348
349         sprintf (name, "%s%s%s%s", prefix, replace, suffix, number);
350 }
351
352 /* Finds the best possible extension to the name on a particular axis. (For renaming, check for unique names afterwards)
353  * This assumes that bone names are at most 32 chars long!
354  *      strip_number: removes number extensions  (TODO: not used)
355  *      axis: the axis to name on
356  *      head/tail: the head/tail co-ordinate of the bone on the specified axis
357  */
358 void bone_autoside_name (char *name, int strip_number, short axis, float head, float tail)
359 {
360         int             len;
361         char    basename[32]={""};
362         char    extension[5]={""};
363
364         len= strlen(name);
365         if (len == 0) return;
366         strcpy(basename, name);
367         
368         /* Figure out extension to append: 
369          *      - The extension to append is based upon the axis that we are working on.
370          *      - If head happens to be on 0, then we must consider the tail position as well to decide
371          *        which side the bone is on
372          *              -> If tail is 0, then it's bone is considered to be on axis, so no extension should be added
373          *              -> Otherwise, extension is added from perspective of object based on which side tail goes to
374          *      - If head is non-zero, extension is added from perspective of object based on side head is on
375          */
376         if (axis == 2) {
377                 /* z-axis - vertical (top/bottom) */
378                 if (IS_EQ(head, 0)) {
379                         if (tail < 0)
380                                 strcpy(extension, "Bot");
381                         else if (tail > 0)
382                                 strcpy(extension, "Top");
383                 }
384                 else {
385                         if (head < 0)
386                                 strcpy(extension, "Bot");
387                         else
388                                 strcpy(extension, "Top");
389                 }
390         }
391         else if (axis == 1) {
392                 /* y-axis - depth (front/back) */
393                 if (IS_EQ(head, 0)) {
394                         if (tail < 0)
395                                 strcpy(extension, "Fr");
396                         else if (tail > 0)
397                                 strcpy(extension, "Bk");
398                 }
399                 else {
400                         if (head < 0)
401                                 strcpy(extension, "Fr");
402                         else
403                                 strcpy(extension, "Bk");
404                 }
405         }
406         else {
407                 /* x-axis - horizontal (left/right) */
408                 if (IS_EQ(head, 0)) {
409                         if (tail < 0)
410                                 strcpy(extension, "R");
411                         else if (tail > 0)
412                                 strcpy(extension, "L");
413                 }
414                 else {
415                         if (head < 0)
416                                 strcpy(extension, "R");
417                         else if (head > 0)
418                                 strcpy(extension, "L");
419                 }
420         }
421
422         /* Simple name truncation 
423          *      - truncate if there is an extension and it wouldn't be able to fit
424          *      - otherwise, just append to end
425          */
426         if (extension[0]) {
427                 int change = 1;
428                 
429                 while (change) { /* remove extensions */
430                         change = 0;
431                         if (len > 2 && basename[len-2]=='.') {
432                                 if (basename[len-1]=='L' || basename[len-1] == 'R' ) { /* L R */
433                                         basename[len-2] = '\0';
434                                         len-=2;
435                                         change= 1;
436                                 }
437                         } else if (len > 3 && basename[len-3]=='.') {
438                                 if (    (basename[len-2]=='F' && basename[len-1] == 'r') ||     /* Fr */
439                                                 (basename[len-2]=='B' && basename[len-1] == 'k')        /* Bk */
440                                 ) {
441                                         basename[len-3] = '\0';
442                                         len-=3;
443                                         change= 1;
444                                 }
445                         } else if (len > 4 && basename[len-4]=='.') {
446                                 if (    (basename[len-3]=='T' && basename[len-2]=='o' && basename[len-1] == 'p') ||     /* Top */
447                                                 (basename[len-3]=='B' && basename[len-2]=='o' && basename[len-1] == 't')        /* Bot */
448                                 ) {
449                                         basename[len-4] = '\0';
450                                         len-=4;
451                                         change= 1;
452                                 }
453                         }
454                 }
455                 
456                 if ((32 - len) < strlen(extension) + 1) { /* add 1 for the '.' */
457                         strncpy(name, basename, len-strlen(extension));
458                 }
459         }
460
461         sprintf(name, "%s.%s", basename, extension);
462 }
463
464 /* ************* B-Bone support ******************* */
465
466 #define MAX_BBONE_SUBDIV        32
467
468 /* data has MAX_BBONE_SUBDIV+1 interpolated points, will become desired amount with equal distances */
469 static void equalize_bezier(float *data, int desired)
470 {
471         float *fp, totdist, ddist, dist, fac1, fac2;
472         float pdist[MAX_BBONE_SUBDIV+1];
473         float temp[MAX_BBONE_SUBDIV+1][4];
474         int a, nr;
475         
476         pdist[0]= 0.0f;
477         for(a=0, fp= data; a<MAX_BBONE_SUBDIV; a++, fp+=4) {
478                 QUATCOPY(temp[a], fp);
479                 pdist[a+1]= pdist[a]+VecLenf(fp, fp+4);
480         }
481         /* do last point */
482         QUATCOPY(temp[a], fp);
483         totdist= pdist[a];
484         
485         /* go over distances and calculate new points */
486         ddist= totdist/((float)desired);
487         nr= 1;
488         for(a=1, fp= data+4; a<desired; a++, fp+=4) {
489                 
490                 dist= ((float)a)*ddist;
491                 
492                 /* we're looking for location (distance) 'dist' in the array */
493                 while((dist>= pdist[nr]) && nr<MAX_BBONE_SUBDIV) {
494                         nr++;
495                 }
496                 
497                 fac1= pdist[nr]- pdist[nr-1];
498                 fac2= pdist[nr]-dist;
499                 fac1= fac2/fac1;
500                 fac2= 1.0f-fac1;
501                 
502                 fp[0]= fac1*temp[nr-1][0]+ fac2*temp[nr][0];
503                 fp[1]= fac1*temp[nr-1][1]+ fac2*temp[nr][1];
504                 fp[2]= fac1*temp[nr-1][2]+ fac2*temp[nr][2];
505                 fp[3]= fac1*temp[nr-1][3]+ fac2*temp[nr][3];
506         }
507         /* set last point, needed for orientation calculus */
508         QUATCOPY(fp, temp[MAX_BBONE_SUBDIV]);
509 }
510
511 /* returns pointer to static array, filled with desired amount of bone->segments elements */
512 /* this calculation is done  within unit bone space */
513 Mat4 *b_bone_spline_setup(bPoseChannel *pchan, int rest)
514 {
515         static Mat4 bbone_array[MAX_BBONE_SUBDIV];
516         static Mat4 bbone_rest_array[MAX_BBONE_SUBDIV];
517         Mat4 *result_array= (rest)? bbone_rest_array: bbone_array;
518         bPoseChannel *next, *prev;
519         Bone *bone= pchan->bone;
520         float h1[3], h2[3], scale[3], length, hlength1, hlength2, roll1=0.0f, roll2;
521         float mat3[3][3], imat[4][4], posemat[4][4], scalemat[4][4], iscalemat[4][4];
522         float data[MAX_BBONE_SUBDIV+1][4], *fp;
523         int a, doscale= 0;
524
525         length= bone->length;
526
527         if(!rest) {
528                 /* check if we need to take non-uniform bone scaling into account */
529                 scale[0]= VecLength(pchan->pose_mat[0]);
530                 scale[1]= VecLength(pchan->pose_mat[1]);
531                 scale[2]= VecLength(pchan->pose_mat[2]);
532
533                 if(fabs(scale[0] - scale[1]) > 1e-6f || fabs(scale[1] - scale[2]) > 1e-6f) {
534                         Mat4One(scalemat);
535                         scalemat[0][0]= scale[0];
536                         scalemat[1][1]= scale[1];
537                         scalemat[2][2]= scale[2];
538                         Mat4Invert(iscalemat, scalemat);
539
540                         length *= scale[1];
541                         doscale = 1;
542                 }
543         }
544         
545         hlength1= bone->ease1*length*0.390464f;         // 0.5*sqrt(2)*kappa, the handle length for near-perfect circles
546         hlength2= bone->ease2*length*0.390464f;
547         
548         /* evaluate next and prev bones */
549         if(bone->flag & BONE_CONNECTED)
550                 prev= pchan->parent;
551         else
552                 prev= NULL;
553         
554         next= pchan->child;
555         
556         /* find the handle points, since this is inside bone space, the 
557                 first point = (0,0,0)
558                 last point =  (0, length, 0) */
559         
560         if(rest) {
561                 Mat4Invert(imat, pchan->bone->arm_mat);
562         }
563         else if(doscale) {
564                 Mat4CpyMat4(posemat, pchan->pose_mat);
565                 Mat4Ortho(posemat);
566                 Mat4Invert(imat, posemat);
567         }
568         else
569                 Mat4Invert(imat, pchan->pose_mat);
570         
571         if(prev) {
572                 float difmat[4][4], result[3][3], imat3[3][3];
573
574                 /* transform previous point inside this bone space */
575                 if(rest)
576                         VECCOPY(h1, prev->bone->arm_head)
577                 else
578                         VECCOPY(h1, prev->pose_head)
579                 Mat4MulVecfl(imat, h1);
580
581                 if(prev->bone->segments>1) {
582                         /* if previous bone is B-bone too, use average handle direction */
583                         h1[1]-= length;
584                         roll1= 0.0f;
585                 }
586
587                 Normalize(h1);
588                 VecMulf(h1, -hlength1);
589
590                 if(prev->bone->segments==1) {
591                         /* find the previous roll to interpolate */
592                         if(rest)
593                                 Mat4MulMat4(difmat, prev->bone->arm_mat, imat);
594                         else
595                                 Mat4MulMat4(difmat, prev->pose_mat, imat);
596                         Mat3CpyMat4(result, difmat);                            // the desired rotation at beginning of next bone
597                         
598                         vec_roll_to_mat3(h1, 0.0f, mat3);                       // the result of vec_roll without roll
599                         
600                         Mat3Inv(imat3, mat3);
601                         Mat3MulMat3(mat3, result, imat3);                       // the matrix transforming vec_roll to desired roll
602                         
603                         roll1= atan2(mat3[2][0], mat3[2][2]);
604                 }
605         }
606         else {
607                 h1[0]= 0.0f; h1[1]= hlength1; h1[2]= 0.0f;
608                 roll1= 0.0f;
609         }
610         if(next) {
611                 float difmat[4][4], result[3][3], imat3[3][3];
612                 
613                 /* transform next point inside this bone space */
614                 if(rest)
615                         VECCOPY(h2, next->bone->arm_tail)
616                 else
617                         VECCOPY(h2, next->pose_tail)
618                 Mat4MulVecfl(imat, h2);
619                 /* if next bone is B-bone too, use average handle direction */
620                 if(next->bone->segments>1);
621                 else h2[1]-= length;
622                 Normalize(h2);
623                 
624                 /* find the next roll to interpolate as well */
625                 if(rest)
626                         Mat4MulMat4(difmat, next->bone->arm_mat, imat);
627                 else
628                         Mat4MulMat4(difmat, next->pose_mat, imat);
629                 Mat3CpyMat4(result, difmat);                            // the desired rotation at beginning of next bone
630                 
631                 vec_roll_to_mat3(h2, 0.0f, mat3);                       // the result of vec_roll without roll
632                 
633                 Mat3Inv(imat3, mat3);
634                 Mat3MulMat3(mat3, imat3, result);                       // the matrix transforming vec_roll to desired roll
635                 
636                 roll2= atan2(mat3[2][0], mat3[2][2]);
637                 
638                 /* and only now negate handle */
639                 VecMulf(h2, -hlength2);
640         }
641         else {
642                 h2[0]= 0.0f; h2[1]= -hlength2; h2[2]= 0.0f;
643                 roll2= 0.0;
644         }
645
646         /* make curve */
647         if(bone->segments > MAX_BBONE_SUBDIV)
648                 bone->segments= MAX_BBONE_SUBDIV;
649         
650         forward_diff_bezier(0.0, h1[0],         h2[0],                  0.0,            data[0],        MAX_BBONE_SUBDIV, 4);
651         forward_diff_bezier(0.0, h1[1],         length + h2[1], length,         data[0]+1,      MAX_BBONE_SUBDIV, 4);
652         forward_diff_bezier(0.0, h1[2],         h2[2],                  0.0,            data[0]+2,      MAX_BBONE_SUBDIV, 4);
653         forward_diff_bezier(roll1, roll1 + 0.390464f*(roll2-roll1), roll2 - 0.390464f*(roll2-roll1),    roll2,  data[0]+3,      MAX_BBONE_SUBDIV, 4);
654         
655         equalize_bezier(data[0], bone->segments);       // note: does stride 4!
656         
657         /* make transformation matrices for the segments for drawing */
658         for(a=0, fp= data[0]; a<bone->segments; a++, fp+=4) {
659                 VecSubf(h1, fp+4, fp);
660                 vec_roll_to_mat3(h1, fp[3], mat3);              // fp[3] is roll
661
662                 Mat4CpyMat3(result_array[a].mat, mat3);
663                 VECCOPY(result_array[a].mat[3], fp);
664
665                 if(doscale) {
666                         /* correct for scaling when this matrix is used in scaled space */
667                         Mat4MulSerie(result_array[a].mat, iscalemat, result_array[a].mat,
668                                 scalemat, NULL, NULL, NULL, NULL, NULL);
669                 }
670         }
671         
672         return result_array;
673 }
674
675 /* ************ Armature Deform ******************* */
676
677 static void pchan_b_bone_defmats(bPoseChannel *pchan, int use_quaternion, int rest_def)
678 {
679         Bone *bone= pchan->bone;
680         Mat4 *b_bone= b_bone_spline_setup(pchan, 0);
681         Mat4 *b_bone_rest= (rest_def)? NULL: b_bone_spline_setup(pchan, 1);
682         Mat4 *b_bone_mats;
683         DualQuat *b_bone_dual_quats= NULL;
684         float tmat[4][4];
685         int a;
686         
687         /* allocate b_bone matrices and dual quats */
688         b_bone_mats= MEM_mallocN((1+bone->segments)*sizeof(Mat4), "BBone defmats");
689         pchan->b_bone_mats= b_bone_mats;
690
691         if(use_quaternion) {
692                 b_bone_dual_quats= MEM_mallocN((bone->segments)*sizeof(DualQuat), "BBone dqs");
693                 pchan->b_bone_dual_quats= b_bone_dual_quats;
694         }
695         
696         /* first matrix is the inverse arm_mat, to bring points in local bone space
697            for finding out which segment it belongs to */
698         Mat4Invert(b_bone_mats[0].mat, bone->arm_mat);
699
700         /* then we make the b_bone_mats:
701             - first transform to local bone space
702                 - translate over the curve to the bbone mat space
703                 - transform with b_bone matrix
704                 - transform back into global space */
705         Mat4One(tmat);
706
707         for(a=0; a<bone->segments; a++) {
708                 if(b_bone_rest)
709                         Mat4Invert(tmat, b_bone_rest[a].mat);
710                 else
711                         tmat[3][1] = -a*(bone->length/(float)bone->segments);
712
713                 Mat4MulSerie(b_bone_mats[a+1].mat, pchan->chan_mat, bone->arm_mat,
714                         b_bone[a].mat, tmat, b_bone_mats[0].mat, NULL, NULL, NULL);
715
716                 if(use_quaternion)
717                         Mat4ToDQuat(bone->arm_mat, b_bone_mats[a+1].mat, &b_bone_dual_quats[a]);
718         }
719 }
720
721 static void b_bone_deform(bPoseChannel *pchan, Bone *bone, float *co, DualQuat *dq, float defmat[][3])
722 {
723         Mat4 *b_bone= pchan->b_bone_mats;
724         float (*mat)[4]= b_bone[0].mat;
725         float segment, y;
726         int a;
727         
728         /* need to transform co back to bonespace, only need y */
729         y= mat[0][1]*co[0] + mat[1][1]*co[1] + mat[2][1]*co[2] + mat[3][1];
730         
731         /* now calculate which of the b_bones are deforming this */
732         segment= bone->length/((float)bone->segments);
733         a= (int)(y/segment);
734         
735         /* note; by clamping it extends deform at endpoints, goes best with
736            straight joints in restpos. */
737         CLAMP(a, 0, bone->segments-1);
738
739         if(dq) {
740                 DQuatCpyDQuat(dq, &((DualQuat*)pchan->b_bone_dual_quats)[a]);
741         }
742         else {
743                 Mat4MulVecfl(b_bone[a+1].mat, co);
744
745                 if(defmat)
746                         Mat3CpyMat4(defmat, b_bone[a+1].mat);
747         }
748 }
749
750 /* using vec with dist to bone b1 - b2 */
751 float distfactor_to_bone (float vec[3], float b1[3], float b2[3], float rad1, float rad2, float rdist)
752 {
753         float dist=0.0f; 
754         float bdelta[3];
755         float pdelta[3];
756         float hsqr, a, l, rad;
757         
758         VecSubf (bdelta, b2, b1);
759         l = Normalize (bdelta);
760         
761         VecSubf (pdelta, vec, b1);
762         
763         a = bdelta[0]*pdelta[0] + bdelta[1]*pdelta[1] + bdelta[2]*pdelta[2];
764         hsqr = ((pdelta[0]*pdelta[0]) + (pdelta[1]*pdelta[1]) + (pdelta[2]*pdelta[2]));
765         
766         if (a < 0.0F){
767                 /* If we're past the end of the bone, do a spherical field attenuation thing */
768                 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])) ;
769                 rad= rad1;
770         }
771         else if (a > l){
772                 /* If we're past the end of the bone, do a spherical field attenuation thing */
773                 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])) ;
774                 rad= rad2;
775         }
776         else {
777                 dist= (hsqr - (a*a));
778                 
779                 if(l!=0.0f) {
780                         rad= a/l;
781                         rad= rad*rad2 + (1.0-rad)*rad1;
782                 }
783                 else rad= rad1;
784         }
785         
786         a= rad*rad;
787         if(dist < a) 
788                 return 1.0f;
789         else {
790                 l= rad+rdist;
791                 l*= l;
792                 if(rdist==0.0f || dist >= l) 
793                         return 0.0f;
794                 else {
795                         a= sqrt(dist)-rad;
796                         return 1.0-( a*a )/( rdist*rdist );
797                 }
798         }
799 }
800
801 static void pchan_deform_mat_add(bPoseChannel *pchan, float weight, float bbonemat[][3], float mat[][3])
802 {
803         float wmat[3][3];
804
805         if(pchan->bone->segments>1)
806                 Mat3CpyMat3(wmat, bbonemat);
807         else
808                 Mat3CpyMat4(wmat, pchan->chan_mat);
809
810         Mat3MulFloat((float*)wmat, weight);
811         Mat3AddMat3(mat, mat, wmat);
812 }
813
814 static float dist_bone_deform(bPoseChannel *pchan, float *vec, DualQuat *dq, float mat[][3], float *co)
815 {
816         Bone *bone= pchan->bone;
817         float fac, contrib=0.0;
818         float cop[3], bbonemat[3][3];
819         DualQuat bbonedq;
820
821         if(bone==NULL) return 0.0f;
822         
823         VECCOPY (cop, co);
824
825         fac= distfactor_to_bone(cop, bone->arm_head, bone->arm_tail, bone->rad_head, bone->rad_tail, bone->dist);
826         
827         if (fac>0.0) {
828                 
829                 fac*=bone->weight;
830                 contrib= fac;
831                 if(contrib>0.0) {
832                         if(vec) {
833                                 if(bone->segments>1)
834                                         // applies on cop and bbonemat
835                                         b_bone_deform(pchan, bone, cop, NULL, (mat)?bbonemat:NULL);
836                                 else
837                                         Mat4MulVecfl(pchan->chan_mat, cop);
838
839                                 //      Make this a delta from the base position
840                                 VecSubf (cop, cop, co);
841                                 cop[0]*=fac; cop[1]*=fac; cop[2]*=fac;
842                                 VecAddf (vec, vec, cop);
843
844                                 if(mat)
845                                         pchan_deform_mat_add(pchan, fac, bbonemat, mat);
846                         }
847                         else {
848                                 if(bone->segments>1) {
849                                         b_bone_deform(pchan, bone, cop, &bbonedq, NULL);
850                                         DQuatAddWeighted(dq, &bbonedq, fac);
851                                 }
852                                 else
853                                         DQuatAddWeighted(dq, pchan->dual_quat, fac);
854                         }
855                 }
856         }
857         
858         return contrib;
859 }
860
861 static void pchan_bone_deform(bPoseChannel *pchan, float weight, float *vec, DualQuat *dq, float mat[][3], float *co, float *contrib)
862 {
863         float cop[3], bbonemat[3][3];
864         DualQuat bbonedq;
865
866         if (!weight)
867                 return;
868
869         VECCOPY(cop, co);
870
871         if(vec) {
872                 if(pchan->bone->segments>1)
873                         // applies on cop and bbonemat
874                         b_bone_deform(pchan, pchan->bone, cop, NULL, (mat)?bbonemat:NULL);
875                 else
876                         Mat4MulVecfl(pchan->chan_mat, cop);
877                 
878                 vec[0]+=(cop[0]-co[0])*weight;
879                 vec[1]+=(cop[1]-co[1])*weight;
880                 vec[2]+=(cop[2]-co[2])*weight;
881
882                 if(mat)
883                         pchan_deform_mat_add(pchan, weight, bbonemat, mat);
884         }
885         else {
886                 if(pchan->bone->segments>1) {
887                         b_bone_deform(pchan, pchan->bone, cop, &bbonedq, NULL);
888                         DQuatAddWeighted(dq, &bbonedq, weight);
889                 }
890                 else
891                         DQuatAddWeighted(dq, pchan->dual_quat, weight);
892         }
893
894         (*contrib)+=weight;
895 }
896
897 void armature_deform_verts(Object *armOb, Object *target, DerivedMesh *dm,
898                            float (*vertexCos)[3], float (*defMats)[3][3],
899                                                    int numVerts, int deformflag, 
900                                                    float (*prevCos)[3], const char *defgrp_name)
901 {
902         bArmature *arm= armOb->data;
903         bPoseChannel *pchan, **defnrToPC = NULL;
904         MDeformVert *dverts = NULL;
905         bDeformGroup *dg;
906         DualQuat *dualquats= NULL;
907         float obinv[4][4], premat[4][4], postmat[4][4];
908         int use_envelope = deformflag & ARM_DEF_ENVELOPE;
909         int use_quaternion = deformflag & ARM_DEF_QUATERNION;
910         int bbone_rest_def = deformflag & ARM_DEF_B_BONE_REST;
911         int invert_vgroup= deformflag & ARM_DEF_INVERT_VGROUP;
912         int numGroups = 0;              /* safety for vertexgroup index overflow */
913         int i, target_totvert = 0;      /* safety for vertexgroup overflow */
914         int use_dverts = 0;
915         int armature_def_nr = -1;
916         int totchan;
917
918         if(arm->edbo) return;
919         
920         Mat4Invert(obinv, target->obmat);
921         Mat4CpyMat4(premat, target->obmat);
922         Mat4MulMat4(postmat, armOb->obmat, obinv);
923         Mat4Invert(premat, postmat);
924
925         /* bone defmats are already in the channels, chan_mat */
926         
927         /* initialize B_bone matrices and dual quaternions */
928         if(use_quaternion) {
929                 totchan= BLI_countlist(&armOb->pose->chanbase);
930                 dualquats= MEM_callocN(sizeof(DualQuat)*totchan, "dualquats");
931         }
932
933         totchan= 0;
934         for(pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next) {
935                 if(!(pchan->bone->flag & BONE_NO_DEFORM)) {
936                         if(pchan->bone->segments > 1)
937                                 pchan_b_bone_defmats(pchan, use_quaternion, bbone_rest_def);
938
939                         if(use_quaternion) {
940                                 pchan->dual_quat= &dualquats[totchan++];
941                                 Mat4ToDQuat(pchan->bone->arm_mat, pchan->chan_mat, pchan->dual_quat);
942                         }
943                 }
944         }
945
946         /* get the def_nr for the overall armature vertex group if present */
947         for(i = 0, dg = target->defbase.first; dg; i++, dg = dg->next)
948                 if(defgrp_name && strcmp(defgrp_name, dg->name) == 0)
949                         armature_def_nr = i;
950
951         /* get a vertex-deform-index to posechannel array */
952         if(deformflag & ARM_DEF_VGROUP) {
953                 if(ELEM(target->type, OB_MESH, OB_LATTICE)) {
954                         numGroups = BLI_countlist(&target->defbase);
955                         
956                         if(target->type==OB_MESH) {
957                                 Mesh *me= target->data;
958                                 dverts = me->dvert;
959                                 target_totvert = me->totvert;
960                         }
961                         else {
962                                 Lattice *lt= target->data;
963                                 dverts = lt->dvert;
964                                 if(dverts)
965                                         target_totvert = lt->pntsu*lt->pntsv*lt->pntsw;
966                         }
967                         /* if we have a DerivedMesh, only use dverts if it has them */
968                         if(dm)
969                                 if(dm->getVertData(dm, 0, CD_MDEFORMVERT))
970                                         use_dverts = 1;
971                                 else use_dverts = 0;
972                         else if(dverts) use_dverts = 1;
973
974                         if(use_dverts) {
975                                 defnrToPC = MEM_callocN(sizeof(*defnrToPC) * numGroups,
976                                                         "defnrToBone");
977                                 for(i = 0, dg = target->defbase.first; dg;
978                                     i++, dg = dg->next) {
979                                         defnrToPC[i] = get_pose_channel(armOb->pose, dg->name);
980                                         /* exclude non-deforming bones */
981                                         if(defnrToPC[i]) {
982                                                 if(defnrToPC[i]->bone->flag & BONE_NO_DEFORM)
983                                                         defnrToPC[i]= NULL;
984                                         }
985                                 }
986                         }
987                 }
988         }
989
990         for(i = 0; i < numVerts; i++) {
991                 MDeformVert *dvert;
992                 DualQuat sumdq, *dq = NULL;
993                 float *co, dco[3];
994                 float sumvec[3], summat[3][3];
995                 float *vec = NULL, (*smat)[3] = NULL;
996                 float contrib = 0.0f;
997                 float armature_weight = 1.0f;   /* default to 1 if no overall def group */
998                 float prevco_weight = 1.0f;             /* weight for optional cached vertexcos */
999                 int       j;
1000
1001                 if(use_quaternion) {
1002                         memset(&sumdq, 0, sizeof(DualQuat));
1003                         dq= &sumdq;
1004                 }
1005                 else {
1006                         sumvec[0] = sumvec[1] = sumvec[2] = 0.0f;
1007                         vec= sumvec;
1008
1009                         if(defMats) {
1010                                 Mat3Clr((float*)summat);
1011                                 smat = summat;
1012                         }
1013                 }
1014
1015                 if(use_dverts || armature_def_nr >= 0) {
1016                         if(dm) dvert = dm->getVertData(dm, i, CD_MDEFORMVERT);
1017                         else if(dverts && i < target_totvert) dvert = dverts + i;
1018                         else dvert = NULL;
1019                 } else
1020                         dvert = NULL;
1021
1022                 if(armature_def_nr >= 0 && dvert) {
1023                         armature_weight = 0.0f; /* a def group was given, so default to 0 */
1024                         for(j = 0; j < dvert->totweight; j++) {
1025                                 if(dvert->dw[j].def_nr == armature_def_nr) {
1026                                         armature_weight = dvert->dw[j].weight;
1027                                         break;
1028                                 }
1029                         }
1030                         /* hackish: the blending factor can be used for blending with prevCos too */
1031                         if(prevCos) {
1032                                 if(invert_vgroup)
1033                                         prevco_weight= 1.0f-armature_weight;
1034                                 else
1035                                         prevco_weight= armature_weight;
1036                                 armature_weight= 1.0f;
1037                         }
1038                 }
1039
1040                 /* check if there's any  point in calculating for this vert */
1041                 if(armature_weight == 0.0f) continue;
1042                 
1043                 /* get the coord we work on */
1044                 co= prevCos?prevCos[i]:vertexCos[i];
1045                 
1046                 /* Apply the object's matrix */
1047                 Mat4MulVecfl(premat, co);
1048                 
1049                 if(use_dverts && dvert && dvert->totweight) { // use weight groups ?
1050                         int deformed = 0;
1051                         
1052                         for(j = 0; j < dvert->totweight; j++){
1053                                 int index = dvert->dw[j].def_nr;
1054                                 pchan = index < numGroups?defnrToPC[index]:NULL;
1055                                 if(pchan) {
1056                                         float weight = dvert->dw[j].weight;
1057                                         Bone *bone = pchan->bone;
1058
1059                                         deformed = 1;
1060                                         
1061                                         if(bone && bone->flag & BONE_MULT_VG_ENV) {
1062                                                 weight *= distfactor_to_bone(co, bone->arm_head,
1063                                                                              bone->arm_tail,
1064                                                                              bone->rad_head,
1065                                                                              bone->rad_tail,
1066                                                                              bone->dist);
1067                                         }
1068                                         pchan_bone_deform(pchan, weight, vec, dq, smat, co, &contrib);
1069                                 }
1070                         }
1071                         /* if there are vertexgroups but not groups with bones
1072                          * (like for softbody groups)
1073                          */
1074                         if(deformed == 0 && use_envelope) {
1075                                 for(pchan = armOb->pose->chanbase.first; pchan;
1076                                     pchan = pchan->next) {
1077                                         if(!(pchan->bone->flag & BONE_NO_DEFORM))
1078                                                 contrib += dist_bone_deform(pchan, vec, dq, smat, co);
1079                                 }
1080                         }
1081                 }
1082                 else if(use_envelope) {
1083                         for(pchan = armOb->pose->chanbase.first; pchan;
1084                             pchan = pchan->next) {
1085                                 if(!(pchan->bone->flag & BONE_NO_DEFORM))
1086                                         contrib += dist_bone_deform(pchan, vec, dq, smat, co);
1087                         }
1088                 }
1089
1090                 /* actually should be EPSILON? weight values and contrib can be like 10e-39 small */
1091                 if(contrib > 0.0001f) {
1092                         if(use_quaternion) {
1093                                 DQuatNormalize(dq, contrib);
1094
1095                                 if(armature_weight != 1.0f) {
1096                                         VECCOPY(dco, co);
1097                                         DQuatMulVecfl(dq, dco, (defMats)? summat: NULL);
1098                                         VecSubf(dco, dco, co);
1099                                         VecMulf(dco, armature_weight);
1100                                         VecAddf(co, co, dco);
1101                                 }
1102                                 else
1103                                         DQuatMulVecfl(dq, co, (defMats)? summat: NULL);
1104
1105                                 smat = summat;
1106                         }
1107                         else {
1108                                 VecMulf(vec, armature_weight/contrib);
1109                                 VecAddf(co, vec, co);
1110                         }
1111
1112                         if(defMats) {
1113                                 float pre[3][3], post[3][3], tmpmat[3][3];
1114
1115                                 Mat3CpyMat4(pre, premat);
1116                                 Mat3CpyMat4(post, postmat);
1117                                 Mat3CpyMat3(tmpmat, defMats[i]);
1118
1119                                 if(!use_quaternion) /* quaternion already is scale corrected */
1120                                         Mat3MulFloat((float*)smat, armature_weight/contrib);
1121
1122                                 Mat3MulSerie(defMats[i], tmpmat, pre, smat, post,
1123                                         NULL, NULL, NULL, NULL);
1124                         }
1125                 }
1126                 
1127                 /* always, check above code */
1128                 Mat4MulVecfl(postmat, co);
1129                 
1130                 
1131                 /* interpolate with previous modifier position using weight group */
1132                 if(prevCos) {
1133                         float mw= 1.0f - prevco_weight;
1134                         vertexCos[i][0]= prevco_weight*vertexCos[i][0] + mw*co[0];
1135                         vertexCos[i][1]= prevco_weight*vertexCos[i][1] + mw*co[1];
1136                         vertexCos[i][2]= prevco_weight*vertexCos[i][2] + mw*co[2];
1137                 }
1138         }
1139
1140         if(dualquats) MEM_freeN(dualquats);
1141         if(defnrToPC) MEM_freeN(defnrToPC);
1142         
1143         /* free B_bone matrices */
1144         for(pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next) {
1145                 if(pchan->b_bone_mats) {
1146                         MEM_freeN(pchan->b_bone_mats);
1147                         pchan->b_bone_mats = NULL;
1148                 }
1149                 if(pchan->b_bone_dual_quats) {
1150                         MEM_freeN(pchan->b_bone_dual_quats);
1151                         pchan->b_bone_dual_quats = NULL;
1152                 }
1153
1154                 pchan->dual_quat = NULL;
1155         }
1156 }
1157
1158 /* ************ END Armature Deform ******************* */
1159
1160 void get_objectspace_bone_matrix (struct Bone* bone, float M_accumulatedMatrix[][4], int root, int posed)
1161 {
1162         Mat4CpyMat4(M_accumulatedMatrix, bone->arm_mat);
1163 }
1164
1165 /* **************** Space to Space API ****************** */
1166
1167 /* Convert World-Space Matrix to Pose-Space Matrix */
1168 void armature_mat_world_to_pose(Object *ob, float inmat[][4], float outmat[][4]) 
1169 {
1170         float obmat[4][4];
1171         
1172         /* prevent crashes */
1173         if (ob==NULL) return;
1174         
1175         /* get inverse of (armature) object's matrix  */
1176         Mat4Invert(obmat, ob->obmat);
1177         
1178         /* multiply given matrix by object's-inverse to find pose-space matrix */
1179         Mat4MulMat4(outmat, obmat, inmat);
1180 }
1181
1182 /* Convert Wolrd-Space Location to Pose-Space Location
1183  * NOTE: this cannot be used to convert to pose-space location of the supplied
1184  *              pose-channel into its local space (i.e. 'visual'-keyframing) 
1185  */
1186 void armature_loc_world_to_pose(Object *ob, float *inloc, float *outloc) 
1187 {
1188         float xLocMat[4][4];
1189         float nLocMat[4][4];
1190         
1191         /* build matrix for location */
1192         Mat4One(xLocMat);
1193         VECCOPY(xLocMat[3], inloc);
1194
1195         /* get bone-space cursor matrix and extract location */
1196         armature_mat_world_to_pose(ob, xLocMat, nLocMat);
1197         VECCOPY(outloc, nLocMat[3]);
1198 }
1199
1200 /* Convert Pose-Space Matrix to Bone-Space Matrix 
1201  * NOTE: this cannot be used to convert to pose-space transforms of the supplied
1202  *              pose-channel into its local space (i.e. 'visual'-keyframing)
1203  */
1204 void armature_mat_pose_to_bone(bPoseChannel *pchan, float inmat[][4], float outmat[][4])
1205 {
1206         float pc_trans[4][4], inv_trans[4][4];
1207         float pc_posemat[4][4], inv_posemat[4][4];
1208         
1209         /* paranoia: prevent crashes with no pose-channel supplied */
1210         if (pchan==NULL) return;
1211         
1212         /* get the inverse matrix of the pchan's transforms */
1213         if (pchan->rotmode)
1214                 LocEulSizeToMat4(pc_trans, pchan->loc, pchan->eul, pchan->size);
1215         else
1216                 LocQuatSizeToMat4(pc_trans, pchan->loc, pchan->quat, pchan->size);
1217         Mat4Invert(inv_trans, pc_trans);
1218         
1219         /* Remove the pchan's transforms from it's pose_mat.
1220          * This should leave behind the effects of restpose + 
1221          * parenting + constraints
1222          */
1223         Mat4MulMat4(pc_posemat, inv_trans, pchan->pose_mat);
1224         
1225         /* get the inverse of the leftovers so that we can remove 
1226          * that component from the supplied matrix
1227          */
1228         Mat4Invert(inv_posemat, pc_posemat);
1229         
1230         /* get the new matrix */
1231         Mat4MulMat4(outmat, inmat, inv_posemat);
1232 }
1233
1234 /* Convert Pose-Space Location to Bone-Space Location
1235  * NOTE: this cannot be used to convert to pose-space location of the supplied
1236  *              pose-channel into its local space (i.e. 'visual'-keyframing) 
1237  */
1238 void armature_loc_pose_to_bone(bPoseChannel *pchan, float *inloc, float *outloc) 
1239 {
1240         float xLocMat[4][4];
1241         float nLocMat[4][4];
1242         
1243         /* build matrix for location */
1244         Mat4One(xLocMat);
1245         VECCOPY(xLocMat[3], inloc);
1246
1247         /* get bone-space cursor matrix and extract location */
1248         armature_mat_pose_to_bone(pchan, xLocMat, nLocMat);
1249         VECCOPY(outloc, nLocMat[3]);
1250 }
1251
1252 /* Remove rest-position effects from pose-transform for obtaining
1253  * 'visual' transformation of pose-channel. 
1254  * (used by the Visual-Keyframing stuff)
1255  */
1256 void armature_mat_pose_to_delta(float delta_mat[][4], float pose_mat[][4], float arm_mat[][4])
1257 {
1258         float imat[4][4];
1259  
1260         Mat4Invert(imat, arm_mat);
1261         Mat4MulMat4(delta_mat, pose_mat, imat);
1262 }
1263
1264
1265 /* **************** The new & simple (but OK!) armature evaluation ********* */ 
1266
1267 /*  ****************** And how it works! ****************************************
1268
1269   This is the bone transformation trick; they're hierarchical so each bone(b)
1270   is in the coord system of bone(b-1):
1271
1272   arm_mat(b)= arm_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) 
1273   
1274   -> yoffs is just the y axis translation in parent's coord system
1275   -> d_root is the translation of the bone root, also in parent's coord system
1276
1277   pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b)
1278
1279   we then - in init deform - store the deform in chan_mat, such that:
1280
1281   pose_mat(b)= arm_mat(b) * chan_mat(b)
1282   
1283   *************************************************************************** */
1284 /*  Computes vector and roll based on a rotation. "mat" must
1285      contain only a rotation, and no scaling. */ 
1286 void mat3_to_vec_roll(float mat[][3], float *vec, float *roll) 
1287 {
1288     if (vec)
1289         VecCopyf(vec, mat[1]);
1290
1291     if (roll) {
1292         float vecmat[3][3], vecmatinv[3][3], rollmat[3][3];
1293
1294         vec_roll_to_mat3(mat[1], 0.0f, vecmat);
1295         Mat3Inv(vecmatinv, vecmat);
1296         Mat3MulMat3(rollmat, vecmatinv, mat);
1297
1298         *roll= atan2(rollmat[2][0], rollmat[2][2]);
1299     }
1300 }
1301
1302 /*      Calculates the rest matrix of a bone based
1303         On its vector and a roll around that vector */
1304 void vec_roll_to_mat3(float *vec, float roll, float mat[][3])
1305 {
1306         float   nor[3], axis[3], target[3]={0,1,0};
1307         float   theta;
1308         float   rMatrix[3][3], bMatrix[3][3];
1309         
1310         VECCOPY (nor, vec);
1311         Normalize (nor);
1312         
1313         /*      Find Axis & Amount for bone matrix*/
1314         Crossf (axis,target,nor);
1315
1316         if (Inpf(axis,axis) > 0.0000000000001) {
1317                 /* if nor is *not* a multiple of target ... */
1318                 Normalize (axis);
1319                 
1320                 theta= NormalizedVecAngle2(target, nor);
1321                 
1322                 /*      Make Bone matrix*/
1323                 VecRotToMat3(axis, theta, bMatrix);
1324         }
1325         else {
1326                 /* if nor is a multiple of target ... */
1327                 float updown;
1328                 
1329                 /* point same direction, or opposite? */
1330                 updown = ( Inpf (target,nor) > 0 ) ? 1.0 : -1.0;
1331                 
1332                 /* I think this should work ... */
1333                 bMatrix[0][0]=updown; bMatrix[0][1]=0.0;    bMatrix[0][2]=0.0;
1334                 bMatrix[1][0]=0.0;    bMatrix[1][1]=updown; bMatrix[1][2]=0.0;
1335                 bMatrix[2][0]=0.0;    bMatrix[2][1]=0.0;    bMatrix[2][2]=1.0;
1336         }
1337         
1338         /*      Make Roll matrix*/
1339         VecRotToMat3(nor, roll, rMatrix);
1340         
1341         /*      Combine and output result*/
1342         Mat3MulMat3 (mat, rMatrix, bMatrix);
1343 }
1344
1345
1346 /* recursive part, calculates restposition of entire tree of children */
1347 /* used by exiting editmode too */
1348 void where_is_armature_bone(Bone *bone, Bone *prevbone)
1349 {
1350         float vec[3];
1351         
1352         /* Bone Space */
1353         VecSubf (vec, bone->tail, bone->head);
1354         vec_roll_to_mat3(vec, bone->roll, bone->bone_mat);
1355
1356         bone->length= VecLenf(bone->head, bone->tail);
1357         
1358         /* this is called on old file reading too... */
1359         if(bone->xwidth==0.0) {
1360                 bone->xwidth= 0.1f;
1361                 bone->zwidth= 0.1f;
1362                 bone->segments= 1;
1363         }
1364         
1365         if(prevbone) {
1366                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
1367                 
1368                 /* bone transform itself */
1369                 Mat4CpyMat3(offs_bone, bone->bone_mat);
1370                                 
1371                 /* The bone's root offset (is in the parent's coordinate system) */
1372                 VECCOPY(offs_bone[3], bone->head);
1373
1374                 /* Get the length translation of parent (length along y axis) */
1375                 offs_bone[3][1]+= prevbone->length;
1376                 
1377                 /* Compose the matrix for this bone  */
1378                 Mat4MulMat4(bone->arm_mat, offs_bone, prevbone->arm_mat);
1379         }
1380         else {
1381                 Mat4CpyMat3(bone->arm_mat, bone->bone_mat);
1382                 VECCOPY(bone->arm_mat[3], bone->head);
1383         }
1384         
1385         /* head */
1386         VECCOPY(bone->arm_head, bone->arm_mat[3]);
1387         /* tail is in current local coord system */
1388         VECCOPY(vec, bone->arm_mat[1]);
1389         VecMulf(vec, bone->length);
1390         VecAddf(bone->arm_tail, bone->arm_head, vec);
1391         
1392         /* and the kiddies */
1393         prevbone= bone;
1394         for(bone= bone->childbase.first; bone; bone= bone->next) {
1395                 where_is_armature_bone(bone, prevbone);
1396         }
1397 }
1398
1399 /* updates vectors and matrices on rest-position level, only needed 
1400    after editing armature itself, now only on reading file */
1401 void where_is_armature (bArmature *arm)
1402 {
1403         Bone *bone;
1404         
1405         /* hierarchical from root to children */
1406         for(bone= arm->bonebase.first; bone; bone= bone->next) {
1407                 where_is_armature_bone(bone, NULL);
1408         }
1409 }
1410
1411 /* if bone layer is protected, copy the data from from->pose */
1412 static void pose_proxy_synchronize(Object *ob, Object *from, int layer_protected)
1413 {
1414         bPose *pose= ob->pose, *frompose= from->pose;
1415         bPoseChannel *pchan, *pchanp, pchanw;
1416         bConstraint *con;
1417         
1418         if (frompose==NULL) return;
1419         
1420         /* exception, armature local layer should be proxied too */
1421         if (pose->proxy_layer)
1422                 ((bArmature *)ob->data)->layer= pose->proxy_layer;
1423         
1424         /* clear all transformation values from library */
1425         rest_pose(frompose);
1426         
1427         /* copy over all of the proxy's bone groups */
1428                 /* TODO for later - implement 'local' bone groups as for constraints
1429                  *      Note: this isn't trivial, as bones reference groups by index not by pointer, 
1430                  *               so syncing things correctly needs careful attention
1431                  */
1432         BLI_freelistN(&pose->agroups);
1433         BLI_duplicatelist(&pose->agroups, &frompose->agroups);
1434         pose->active_group= frompose->active_group;
1435         
1436         for (pchan= pose->chanbase.first; pchan; pchan= pchan->next) {
1437                 if (pchan->bone->layer & layer_protected) {
1438                         ListBase proxylocal_constraints = {NULL, NULL};
1439                         pchanp= get_pose_channel(frompose, pchan->name);
1440                         
1441                         /* copy posechannel to temp, but restore important pointers */
1442                         pchanw= *pchanp;
1443                         pchanw.prev= pchan->prev;
1444                         pchanw.next= pchan->next;
1445                         pchanw.parent= pchan->parent;
1446                         pchanw.child= pchan->child;
1447                         pchanw.path= NULL;
1448                         
1449                         /* constraints - proxy constraints are flushed... local ones are added after 
1450                          *      1. extract constraints not from proxy (CONSTRAINT_PROXY_LOCAL) from pchan's constraints
1451                          *      2. copy proxy-pchan's constraints on-to new
1452                          *      3. add extracted local constraints back on top 
1453                          */
1454                         extract_proxylocal_constraints(&proxylocal_constraints, &pchan->constraints);
1455                         copy_constraints(&pchanw.constraints, &pchanp->constraints);
1456                         addlisttolist(&pchanw.constraints, &proxylocal_constraints);
1457                         
1458                         /* constraints - set target ob pointer to own object */
1459                         for (con= pchanw.constraints.first; con; con= con->next) {
1460                                 bConstraintTypeInfo *cti= constraint_get_typeinfo(con);
1461                                 ListBase targets = {NULL, NULL};
1462                                 bConstraintTarget *ct;
1463                                 
1464                                 if (cti && cti->get_constraint_targets) {
1465                                         cti->get_constraint_targets(con, &targets);
1466                                         
1467                                         for (ct= targets.first; ct; ct= ct->next) {
1468                                                 if (ct->tar == from)
1469                                                         ct->tar = ob;
1470                                         }
1471                                         
1472                                         if (cti->flush_constraint_targets)
1473                                                 cti->flush_constraint_targets(con, &targets, 0);
1474                                 }
1475                         }
1476                         
1477                         /* free stuff from current channel */
1478                         if (pchan->path) MEM_freeN(pchan->path);
1479                         free_constraints(&pchan->constraints);
1480                         
1481                         /* the final copy */
1482                         *pchan= pchanw;
1483                 }
1484         }
1485 }
1486
1487 static int rebuild_pose_bone(bPose *pose, Bone *bone, bPoseChannel *parchan, int counter)
1488 {
1489         bPoseChannel *pchan = verify_pose_channel (pose, bone->name);   // verify checks and/or adds
1490
1491         pchan->bone= bone;
1492         pchan->parent= parchan;
1493         
1494         counter++;
1495         
1496         for(bone= bone->childbase.first; bone; bone= bone->next) {
1497                 counter= rebuild_pose_bone(pose, bone, pchan, counter);
1498                 /* for quick detecting of next bone in chain, only b-bone uses it now */
1499                 if(bone->flag & BONE_CONNECTED)
1500                         pchan->child= get_pose_channel(pose, bone->name);
1501         }
1502         
1503         return counter;
1504 }
1505
1506 /* only after leave editmode, duplicating, validating older files, library syncing */
1507 /* NOTE: pose->flag is set for it */
1508 void armature_rebuild_pose(Object *ob, bArmature *arm)
1509 {
1510         Bone *bone;
1511         bPose *pose;
1512         bPoseChannel *pchan, *next;
1513         int counter=0;
1514                 
1515         /* only done here */
1516         if(ob->pose==NULL) ob->pose= MEM_callocN(sizeof(bPose), "new pose");
1517         pose= ob->pose;
1518         
1519         /* clear */
1520         for(pchan= pose->chanbase.first; pchan; pchan= pchan->next) {
1521                 pchan->bone= NULL;
1522                 pchan->child= NULL;
1523         }
1524         
1525         /* first step, check if all channels are there */
1526         for(bone= arm->bonebase.first; bone; bone= bone->next) {
1527                 counter= rebuild_pose_bone(pose, bone, NULL, counter);
1528         }
1529
1530         /* and a check for garbage */
1531         for(pchan= pose->chanbase.first; pchan; pchan= next) {
1532                 next= pchan->next;
1533                 if(pchan->bone==NULL) {
1534                         if(pchan->path)
1535                                 MEM_freeN(pchan->path);
1536                         free_constraints(&pchan->constraints);
1537                         BLI_freelinkN(&pose->chanbase, pchan);
1538                 }
1539         }
1540         // printf("rebuild pose %s, %d bones\n", ob->id.name, counter);
1541         
1542         /* synchronize protected layers with proxy */
1543         if(ob->proxy)
1544                 pose_proxy_synchronize(ob, ob->proxy, arm->layer_protected);
1545         
1546         update_pose_constraint_flags(ob->pose); // for IK detection for example
1547         
1548         /* the sorting */
1549         if(counter>1)
1550                 DAG_pose_sort(ob);
1551         
1552         ob->pose->flag &= ~POSE_RECALC;
1553 }
1554
1555
1556 /* ********************** THE IK SOLVER ******************* */
1557
1558
1559
1560 /* allocates PoseTree, and links that to root bone/channel */
1561 /* Note: detecting the IK chain is duplicate code... in drawarmature.c and in transform_conversions.c */
1562 static void initialize_posetree(struct Object *ob, bPoseChannel *pchan_tip)
1563 {
1564         bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
1565         PoseTree *tree;
1566         PoseTarget *target;
1567         bConstraint *con;
1568         bKinematicConstraint *data;
1569         int a, segcount= 0, size, newsize, *oldparent, parent;
1570         
1571         /* find IK constraint, and validate it */
1572         for(con= pchan_tip->constraints.first; con; con= con->next) {
1573                 if(con->type==CONSTRAINT_TYPE_KINEMATIC) {
1574                         data=(bKinematicConstraint*)con->data;
1575                         if (data->flag & CONSTRAINT_IK_AUTO) break;
1576                         if (data->tar==NULL) continue;
1577                         if (data->tar->type==OB_ARMATURE && data->subtarget[0]==0) continue;
1578                         if ((con->flag & CONSTRAINT_DISABLE)==0 && (con->enforce!=0.0)) break;
1579                 }
1580         }
1581         if(con==NULL) return;
1582         
1583         /* exclude tip from chain? */
1584         if(!(data->flag & CONSTRAINT_IK_TIP))
1585                 pchan_tip= pchan_tip->parent;
1586         
1587         /* Find the chain's root & count the segments needed */
1588         for (curchan = pchan_tip; curchan; curchan=curchan->parent){
1589                 pchan_root = curchan;
1590                 
1591                 curchan->flag |= POSE_CHAIN;    // don't forget to clear this
1592                 chanlist[segcount]=curchan;
1593                 segcount++;
1594                 
1595                 if(segcount==data->rootbone || segcount>255) break; // 255 is weak
1596         }
1597         if (!segcount) return;
1598
1599         /* setup the chain data */
1600         
1601         /* we make tree-IK, unless all existing targets are in this chain */
1602         for(tree= pchan_root->iktree.first; tree; tree= tree->next) {
1603                 for(target= tree->targets.first; target; target= target->next) {
1604                         curchan= tree->pchan[target->tip];
1605                         if(curchan->flag & POSE_CHAIN)
1606                                 curchan->flag &= ~POSE_CHAIN;
1607                         else
1608                                 break;
1609                 }
1610                 if(target) break;
1611         }
1612
1613         /* create a target */
1614         target= MEM_callocN(sizeof(PoseTarget), "posetarget");
1615         target->con= con;
1616         pchan_tip->flag &= ~POSE_CHAIN;
1617
1618         if(tree==NULL) {
1619                 /* make new tree */
1620                 tree= MEM_callocN(sizeof(PoseTree), "posetree");
1621
1622                 tree->iterations= data->iterations;
1623                 tree->totchannel= segcount;
1624                 tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
1625                 
1626                 tree->pchan= MEM_callocN(segcount*sizeof(void*), "ik tree pchan");
1627                 tree->parent= MEM_callocN(segcount*sizeof(int), "ik tree parent");
1628                 for(a=0; a<segcount; a++) {
1629                         tree->pchan[a]= chanlist[segcount-a-1];
1630                         tree->parent[a]= a-1;
1631                 }
1632                 target->tip= segcount-1;
1633                 
1634                 /* AND! link the tree to the root */
1635                 BLI_addtail(&pchan_root->iktree, tree);
1636         }
1637         else {
1638                 tree->iterations= MAX2(data->iterations, tree->iterations);
1639                 tree->stretch= tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
1640
1641                 /* skip common pose channels and add remaining*/
1642                 size= MIN2(segcount, tree->totchannel);
1643                 for(a=0; a<size && tree->pchan[a]==chanlist[segcount-a-1]; a++);
1644                 parent= a-1;
1645
1646                 segcount= segcount-a;
1647                 target->tip= tree->totchannel + segcount - 1;
1648
1649                 if (segcount > 0) {
1650                         /* resize array */
1651                         newsize= tree->totchannel + segcount;
1652                         oldchan= tree->pchan;
1653                         oldparent= tree->parent;
1654
1655                         tree->pchan= MEM_callocN(newsize*sizeof(void*), "ik tree pchan");
1656                         tree->parent= MEM_callocN(newsize*sizeof(int), "ik tree parent");
1657                         memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel);
1658                         memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel);
1659                         MEM_freeN(oldchan);
1660                         MEM_freeN(oldparent);
1661
1662                         /* add new pose channels at the end, in reverse order */
1663                         for(a=0; a<segcount; a++) {
1664                                 tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1];
1665                                 tree->parent[tree->totchannel+a]= tree->totchannel+a-1;
1666                         }
1667                         tree->parent[tree->totchannel]= parent;
1668                         
1669                         tree->totchannel= newsize;
1670                 }
1671
1672                 /* move tree to end of list, for correct evaluation order */
1673                 BLI_remlink(&pchan_root->iktree, tree);
1674                 BLI_addtail(&pchan_root->iktree, tree);
1675         }
1676
1677         /* add target to the tree */
1678         BLI_addtail(&tree->targets, target);
1679 }
1680
1681 /* called from within the core where_is_pose loop, all animsystems and constraints
1682 were executed & assigned. Now as last we do an IK pass */
1683 static void execute_posetree(Object *ob, PoseTree *tree)
1684 {
1685         float R_parmat[3][3], identity[3][3];
1686         float iR_parmat[3][3];
1687         float R_bonemat[3][3];
1688         float goalrot[3][3], goalpos[3];
1689         float rootmat[4][4], imat[4][4];
1690         float goal[4][4], goalinv[4][4];
1691         float irest_basis[3][3], full_basis[3][3];
1692         float end_pose[4][4], world_pose[4][4];
1693         float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch=NULL;
1694         float resultinf=0.0f;
1695         int a, flag, hasstretch=0, resultblend=0;
1696         bPoseChannel *pchan;
1697         IK_Segment *seg, *parent, **iktree, *iktarget;
1698         IK_Solver *solver;
1699         PoseTarget *target;
1700         bKinematicConstraint *data, *poleangledata=NULL;
1701         Bone *bone;
1702
1703         if (tree->totchannel == 0)
1704                 return;
1705         
1706         iktree= MEM_mallocN(sizeof(void*)*tree->totchannel, "ik tree");
1707
1708         for(a=0; a<tree->totchannel; a++) {
1709                 pchan= tree->pchan[a];
1710                 bone= pchan->bone;
1711                 
1712                 /* set DoF flag */
1713                 flag= 0;
1714                 if(!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP))
1715                         flag |= IK_XDOF;
1716                 if(!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP))
1717                         flag |= IK_YDOF;
1718                 if(!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP))
1719                         flag |= IK_ZDOF;
1720                 
1721                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
1722                         flag |= IK_TRANS_YDOF;
1723                         hasstretch = 1;
1724                 }
1725                 
1726                 seg= iktree[a]= IK_CreateSegment(flag);
1727                 
1728                 /* find parent */
1729                 if(a == 0)
1730                         parent= NULL;
1731                 else
1732                         parent= iktree[tree->parent[a]];
1733                         
1734                 IK_SetParent(seg, parent);
1735                         
1736                 /* get the matrix that transforms from prevbone into this bone */
1737                 Mat3CpyMat4(R_bonemat, pchan->pose_mat);
1738                 
1739                 /* gather transformations for this IK segment */
1740                 
1741                 if (pchan->parent)
1742                         Mat3CpyMat4(R_parmat, pchan->parent->pose_mat);
1743                 else
1744                         Mat3One(R_parmat);
1745                 
1746                 /* bone offset */
1747                 if (pchan->parent && (a > 0))
1748                         VecSubf(start, pchan->pose_head, pchan->parent->pose_tail);
1749                 else
1750                         /* only root bone (a = 0) has no parent */
1751                         start[0]= start[1]= start[2]= 0.0f;
1752                 
1753                 /* change length based on bone size */
1754                 length= bone->length*VecLength(R_bonemat[1]);
1755                 
1756                 /* compute rest basis and its inverse */
1757                 Mat3CpyMat3(rest_basis, bone->bone_mat);
1758                 Mat3CpyMat3(irest_basis, bone->bone_mat);
1759                 Mat3Transp(irest_basis);
1760                 
1761                 /* compute basis with rest_basis removed */
1762                 Mat3Inv(iR_parmat, R_parmat);
1763                 Mat3MulMat3(full_basis, iR_parmat, R_bonemat);
1764                 Mat3MulMat3(basis, irest_basis, full_basis);
1765                 
1766                 /* basis must be pure rotation */
1767                 Mat3Ortho(basis);
1768                 
1769                 /* transform offset into local bone space */
1770                 Mat3Ortho(iR_parmat);
1771                 Mat3MulVecfl(iR_parmat, start);
1772                 
1773                 IK_SetTransform(seg, start, rest_basis, basis, length);
1774                 
1775                 if (pchan->ikflag & BONE_IK_XLIMIT)
1776                         IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]);
1777                 if (pchan->ikflag & BONE_IK_YLIMIT)
1778                         IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]);
1779                 if (pchan->ikflag & BONE_IK_ZLIMIT)
1780                         IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]);
1781                 
1782                 IK_SetStiffness(seg, IK_X, pchan->stiffness[0]);
1783                 IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]);
1784                 IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]);
1785                 
1786                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
1787                         float ikstretch = pchan->ikstretch*pchan->ikstretch;
1788                         IK_SetStiffness(seg, IK_TRANS_Y, MIN2(1.0-ikstretch, 0.99));
1789                         IK_SetLimit(seg, IK_TRANS_Y, 0.001, 1e10);
1790                 }
1791         }
1792
1793         solver= IK_CreateSolver(iktree[0]);
1794
1795         /* set solver goals */
1796
1797         /* first set the goal inverse transform, assuming the root of tree was done ok! */
1798         pchan= tree->pchan[0];
1799         if (pchan->parent)
1800                 /* transform goal by parent mat, so this rotation is not part of the
1801                    segment's basis. otherwise rotation limits do not work on the
1802                    local transform of the segment itself. */
1803                 Mat4CpyMat4(rootmat, pchan->parent->pose_mat);
1804         else
1805                 Mat4One(rootmat);
1806         VECCOPY(rootmat[3], pchan->pose_head);
1807         
1808         Mat4MulMat4 (imat, rootmat, ob->obmat);
1809         Mat4Invert (goalinv, imat);
1810         
1811         for (target=tree->targets.first; target; target=target->next) {
1812                 float polepos[3];
1813                 int poleconstrain= 0;
1814                 
1815                 data= (bKinematicConstraint*)target->con->data;
1816                 
1817                 /* 1.0=ctime, we pass on object for auto-ik (owner-type here is object, even though
1818                  * strictly speaking, it is a posechannel)
1819                  */
1820                 get_constraint_target_matrix(target->con, 0, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
1821                 
1822                 /* and set and transform goal */
1823                 Mat4MulMat4(goal, rootmat, goalinv);
1824                 
1825                 VECCOPY(goalpos, goal[3]);
1826                 Mat3CpyMat4(goalrot, goal);
1827                 
1828                 /* same for pole vector target */
1829                 if(data->poletar) {
1830                         get_constraint_target_matrix(target->con, 1, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
1831                         
1832                         if(data->flag & CONSTRAINT_IK_SETANGLE) {
1833                                 /* don't solve IK when we are setting the pole angle */
1834                                 break;
1835                         }
1836                         else {
1837                                 Mat4MulMat4(goal, rootmat, goalinv);
1838                                 VECCOPY(polepos, goal[3]);
1839                                 poleconstrain= 1;
1840
1841                                 /* for pole targets, we blend the result of the ik solver
1842                                  * instead of the target position, otherwise we can't get
1843                                  * a smooth transition */
1844                                 resultblend= 1;
1845                                 resultinf= target->con->enforce;
1846                                 
1847                                 if(data->flag & CONSTRAINT_IK_GETANGLE) {
1848                                         poleangledata= data;
1849                                         data->flag &= ~CONSTRAINT_IK_GETANGLE;
1850                                 }
1851                         }
1852                 }
1853
1854                 /* do we need blending? */
1855                 if (!resultblend && target->con->enforce!=1.0) {
1856                         float q1[4], q2[4], q[4];
1857                         float fac= target->con->enforce;
1858                         float mfac= 1.0-fac;
1859                         
1860                         pchan= tree->pchan[target->tip];
1861                         
1862                         /* end effector in world space */
1863                         Mat4CpyMat4(end_pose, pchan->pose_mat);
1864                         VECCOPY(end_pose[3], pchan->pose_tail);
1865                         Mat4MulSerie(world_pose, goalinv, ob->obmat, end_pose, 0, 0, 0, 0, 0);
1866                         
1867                         /* blend position */
1868                         goalpos[0]= fac*goalpos[0] + mfac*world_pose[3][0];
1869                         goalpos[1]= fac*goalpos[1] + mfac*world_pose[3][1];
1870                         goalpos[2]= fac*goalpos[2] + mfac*world_pose[3][2];
1871                         
1872                         /* blend rotation */
1873                         Mat3ToQuat(goalrot, q1);
1874                         Mat4ToQuat(world_pose, q2);
1875                         QuatInterpol(q, q1, q2, mfac);
1876                         QuatToMat3(q, goalrot);
1877                 }
1878                 
1879                 iktarget= iktree[target->tip];
1880                 
1881                 if(data->weight != 0.0) {
1882                         if(poleconstrain)
1883                                 IK_SolverSetPoleVectorConstraint(solver, iktarget, goalpos,
1884                                         polepos, data->poleangle*M_PI/180, (poleangledata == data));
1885                         IK_SolverAddGoal(solver, iktarget, goalpos, data->weight);
1886                 }
1887                 if((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0))
1888                         if((data->flag & CONSTRAINT_IK_AUTO)==0)
1889                                 IK_SolverAddGoalOrientation(solver, iktarget, goalrot,
1890                                         data->orientweight);
1891         }
1892
1893         /* solve */
1894         IK_Solve(solver, 0.0f, tree->iterations);
1895
1896         if(poleangledata)
1897                 poleangledata->poleangle= IK_SolverGetPoleAngle(solver)*180/M_PI;
1898
1899         IK_FreeSolver(solver);
1900
1901         /* gather basis changes */
1902         tree->basis_change= MEM_mallocN(sizeof(float[3][3])*tree->totchannel, "ik basis change");
1903         if(hasstretch)
1904                 ikstretch= MEM_mallocN(sizeof(float)*tree->totchannel, "ik stretch");
1905         
1906         for(a=0; a<tree->totchannel; a++) {
1907                 IK_GetBasisChange(iktree[a], tree->basis_change[a]);
1908                 
1909                 if(hasstretch) {
1910                         /* have to compensate for scaling received from parent */
1911                         float parentstretch, stretch;
1912                         
1913                         pchan= tree->pchan[a];
1914                         parentstretch= (tree->parent[a] >= 0)? ikstretch[tree->parent[a]]: 1.0;
1915                         
1916                         if(tree->stretch && (pchan->ikstretch > 0.0)) {
1917                                 float trans[3], length;
1918                                 
1919                                 IK_GetTranslationChange(iktree[a], trans);
1920                                 length= pchan->bone->length*VecLength(pchan->pose_mat[1]);
1921                                 
1922                                 ikstretch[a]= (length == 0.0)? 1.0: (trans[1]+length)/length;
1923                         }
1924                         else
1925                                 ikstretch[a] = 1.0;
1926                         
1927                         stretch= (parentstretch == 0.0)? 1.0: ikstretch[a]/parentstretch;
1928                         
1929                         VecMulf(tree->basis_change[a][0], stretch);
1930                         VecMulf(tree->basis_change[a][1], stretch);
1931                         VecMulf(tree->basis_change[a][2], stretch);
1932                 }
1933
1934                 if(resultblend && resultinf!=1.0f) {
1935                         Mat3One(identity);
1936                         Mat3BlendMat3(tree->basis_change[a], identity,
1937                                 tree->basis_change[a], resultinf);
1938                 }
1939                 
1940                 IK_FreeSegment(iktree[a]);
1941         }
1942         
1943         MEM_freeN(iktree);
1944         if(ikstretch) MEM_freeN(ikstretch);
1945 }
1946
1947 void free_posetree(PoseTree *tree)
1948 {
1949         BLI_freelistN(&tree->targets);
1950         if(tree->pchan) MEM_freeN(tree->pchan);
1951         if(tree->parent) MEM_freeN(tree->parent);
1952         if(tree->basis_change) MEM_freeN(tree->basis_change);
1953         MEM_freeN(tree);
1954 }
1955
1956 /* ********************** THE POSE SOLVER ******************* */
1957
1958
1959 /* loc/rot/size to mat4 */
1960 /* used in constraint.c too */
1961 void chan_calc_mat(bPoseChannel *chan)
1962 {
1963         float smat[3][3];
1964         float rmat[3][3];
1965         float tmat[3][3];
1966         
1967         /* get scaling matrix */
1968         SizeToMat3(chan->size, smat);
1969         
1970         /* rotations may either be quats or eulers (no rotation modes for now...) */
1971         if (chan->rotmode) {
1972                 /* euler rotations (will cause gimble lock... no rotation order to solve that yet) */
1973                 EulToMat3(chan->eul, rmat);
1974         }
1975         else {
1976                 /* quats are normalised before use to eliminate scaling issues */
1977                 NormalQuat(chan->quat);
1978                 QuatToMat3(chan->quat, rmat);
1979         }
1980         
1981         /* calculate matrix of bone (as 3x3 matrix, but then copy the 4x4) */
1982         Mat3MulMat3(tmat, rmat, smat);
1983         Mat4CpyMat3(chan->chan_mat, tmat);
1984         
1985         /* prevent action channels breaking chains */
1986         /* need to check for bone here, CONSTRAINT_TYPE_ACTION uses this call */
1987         if ((chan->bone==NULL) || !(chan->bone->flag & BONE_CONNECTED)) {
1988                 VECCOPY(chan->chan_mat[3], chan->loc);
1989         }
1990 }
1991
1992 /* transform from bone(b) to bone(b+1), store in chan_mat */
1993 static void make_dmats(bPoseChannel *pchan)
1994 {
1995         if (pchan->parent) {
1996                 float iR_parmat[4][4];
1997                 Mat4Invert(iR_parmat, pchan->parent->pose_mat);
1998                 Mat4MulMat4(pchan->chan_mat,  pchan->pose_mat, iR_parmat);      // delta mat
1999         }
2000         else Mat4CpyMat4(pchan->chan_mat, pchan->pose_mat);
2001 }
2002
2003 /* applies IK matrix to pchan, IK is done separated */
2004 /* formula: pose_mat(b) = pose_mat(b-1) * diffmat(b-1, b) * ik_mat(b) */
2005 /* to make this work, the diffmats have to be precalculated! Stored in chan_mat */
2006 static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[][3])   // nr = to detect if this is first bone
2007 {
2008         float vec[3], ikmat[4][4];
2009         
2010         Mat4CpyMat3(ikmat, ik_mat);
2011         
2012         if (pchan->parent)
2013                 Mat4MulSerie(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat, ikmat, NULL, NULL, NULL, NULL, NULL);
2014         else 
2015                 Mat4MulMat4(pchan->pose_mat, ikmat, pchan->chan_mat);
2016
2017         /* calculate head */
2018         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
2019         /* calculate tail */
2020         VECCOPY(vec, pchan->pose_mat[1]);
2021         VecMulf(vec, pchan->bone->length);
2022         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
2023
2024         pchan->flag |= POSE_DONE;
2025 }
2026
2027 /* NLA strip modifiers */
2028 static void do_strip_modifiers(Scene *scene, Object *armob, Bone *bone, bPoseChannel *pchan)
2029 {
2030         bActionModifier *amod;
2031         bActionStrip *strip, *strip2;
2032         float scene_cfra= scene->r.cfra;
2033         int do_modif;
2034
2035         for (strip=armob->nlastrips.first; strip; strip=strip->next) {
2036                 do_modif=0;
2037                 
2038                 if (scene_cfra>=strip->start && scene_cfra<=strip->end)
2039                         do_modif=1;
2040                 
2041                 if ((scene_cfra > strip->end) && (strip->flag & ACTSTRIP_HOLDLASTFRAME)) {
2042                         do_modif=1;
2043                         
2044                         /* if there are any other strips active, ignore modifiers for this strip - 
2045                          * 'hold' option should only hold action modifiers if there are 
2046                          * no other active strips */
2047                         for (strip2=strip->next; strip2; strip2=strip2->next) {
2048                                 if (strip2 == strip) continue;
2049                                 
2050                                 if (scene_cfra>=strip2->start && scene_cfra<=strip2->end) {
2051                                         if (!(strip2->flag & ACTSTRIP_MUTE))
2052                                                 do_modif=0;
2053                                 }
2054                         }
2055                         
2056                         /* if there are any later, activated, strips with 'hold' set, they take precedence, 
2057                          * so ignore modifiers for this strip */
2058                         for (strip2=strip->next; strip2; strip2=strip2->next) {
2059                                 if (scene_cfra < strip2->start) continue;
2060                                 if ((strip2->flag & ACTSTRIP_HOLDLASTFRAME) && !(strip2->flag & ACTSTRIP_MUTE)) {
2061                                         do_modif=0;
2062                                 }
2063                         }
2064                 }
2065                 
2066                 if (do_modif) {
2067                         /* temporal solution to prevent 2 strips accumulating */
2068                         if(scene_cfra==strip->end && strip->next && strip->next->start==scene_cfra)
2069                                 continue;
2070                         
2071                         for(amod= strip->modifiers.first; amod; amod= amod->next) {
2072                                 switch (amod->type) {
2073                                 case ACTSTRIP_MOD_DEFORM:
2074                                 {
2075                                         /* validate first */
2076                                         if(amod->ob && amod->ob->type==OB_CURVE && amod->channel[0]) {
2077                                                 
2078                                                 if( strcmp(pchan->name, amod->channel)==0 ) {
2079                                                         float mat4[4][4], mat3[3][3];
2080                                                         
2081                                                         curve_deform_vector(scene, amod->ob, armob, bone->arm_mat[3], pchan->pose_mat[3], mat3, amod->no_rot_axis);
2082                                                         Mat4CpyMat4(mat4, pchan->pose_mat);
2083                                                         Mat4MulMat34(pchan->pose_mat, mat3, mat4);
2084                                                         
2085                                                 }
2086                                         }
2087                                 }
2088                                         break;
2089                                 case ACTSTRIP_MOD_NOISE:        
2090                                 {
2091                                         if( strcmp(pchan->name, amod->channel)==0 ) {
2092                                                 float nor[3], loc[3], ofs;
2093                                                 float eul[3], size[3], eulo[3], sizeo[3];
2094                                                 
2095                                                 /* calculate turbulance */
2096                                                 ofs = amod->turbul / 200.0f;
2097                                                 
2098                                                 /* make a copy of starting conditions */
2099                                                 VECCOPY(loc, pchan->pose_mat[3]);
2100                                                 Mat4ToEul(pchan->pose_mat, eul);
2101                                                 Mat4ToSize(pchan->pose_mat, size);
2102                                                 VECCOPY(eulo, eul);
2103                                                 VECCOPY(sizeo, size);
2104                                                 
2105                                                 /* apply noise to each set of channels */
2106                                                 if (amod->channels & 4) {
2107                                                         /* for scaling */
2108                                                         nor[0] = BLI_gNoise(amod->noisesize, size[0]+ofs, size[1], size[2], 0, 0) - ofs;
2109                                                         nor[1] = BLI_gNoise(amod->noisesize, size[0], size[1]+ofs, size[2], 0, 0) - ofs;        
2110                                                         nor[2] = BLI_gNoise(amod->noisesize, size[0], size[1], size[2]+ofs, 0, 0) - ofs;
2111                                                         VecAddf(size, size, nor);
2112                                                         
2113                                                         if (sizeo[0] != 0)
2114                                                                 VecMulf(pchan->pose_mat[0], size[0] / sizeo[0]);
2115                                                         if (sizeo[1] != 0)
2116                                                                 VecMulf(pchan->pose_mat[1], size[1] / sizeo[1]);
2117                                                         if (sizeo[2] != 0)
2118                                                                 VecMulf(pchan->pose_mat[2], size[2] / sizeo[2]);
2119                                                 }
2120                                                 if (amod->channels & 2) {
2121                                                         /* for rotation */
2122                                                         nor[0] = BLI_gNoise(amod->noisesize, eul[0]+ofs, eul[1], eul[2], 0, 0) - ofs;
2123                                                         nor[1] = BLI_gNoise(amod->noisesize, eul[0], eul[1]+ofs, eul[2], 0, 0) - ofs;   
2124                                                         nor[2] = BLI_gNoise(amod->noisesize, eul[0], eul[1], eul[2]+ofs, 0, 0) - ofs;
2125                                                         
2126                                                         compatible_eul(nor, eulo);
2127                                                         VecAddf(eul, eul, nor);
2128                                                         compatible_eul(eul, eulo);
2129                                                         
2130                                                         LocEulSizeToMat4(pchan->pose_mat, loc, eul, size);
2131                                                 }
2132                                                 if (amod->channels & 1) {
2133                                                         /* for location */
2134                                                         nor[0] = BLI_gNoise(amod->noisesize, loc[0]+ofs, loc[1], loc[2], 0, 0) - ofs;
2135                                                         nor[1] = BLI_gNoise(amod->noisesize, loc[0], loc[1]+ofs, loc[2], 0, 0) - ofs;   
2136                                                         nor[2] = BLI_gNoise(amod->noisesize, loc[0], loc[1], loc[2]+ofs, 0, 0) - ofs;
2137                                                         
2138                                                         VecAddf(pchan->pose_mat[3], loc, nor);
2139                                                 }
2140                                         }
2141                                 }
2142                                         break;
2143                                 }
2144                         }
2145                 }
2146         }
2147 }
2148
2149
2150 /* The main armature solver, does all constraints excluding IK */
2151 /* pchan is validated, as having bone and parent pointer */
2152 static void where_is_pose_bone(Scene *scene, Object *ob, bPoseChannel *pchan, float ctime)
2153 {
2154         Bone *bone, *parbone;
2155         bPoseChannel *parchan;
2156         float vec[3];
2157         
2158         /* set up variables for quicker access below */
2159         bone= pchan->bone;
2160         parbone= bone->parent;
2161         parchan= pchan->parent;
2162         
2163         /* this gives a chan_mat with actions (ipos) results */
2164         chan_calc_mat(pchan);
2165         
2166         /* construct the posemat based on PoseChannels, that we do before applying constraints */
2167         /* pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b) */
2168         
2169         if(parchan) {
2170                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
2171                 
2172                 /* bone transform itself */
2173                 Mat4CpyMat3(offs_bone, bone->bone_mat);
2174                 
2175                 /* The bone's root offset (is in the parent's coordinate system) */
2176                 VECCOPY(offs_bone[3], bone->head);
2177                 
2178                 /* Get the length translation of parent (length along y axis) */
2179                 offs_bone[3][1]+= parbone->length;
2180                 
2181                 /* Compose the matrix for this bone  */
2182                 if(bone->flag & BONE_HINGE) {   // uses restposition rotation, but actual position
2183                         float tmat[4][4];
2184                         
2185                         /* the rotation of the parent restposition */
2186                         Mat4CpyMat4(tmat, parbone->arm_mat);
2187                         
2188                         /* the location of actual parent transform */
2189                         VECCOPY(tmat[3], offs_bone[3]);
2190                         offs_bone[3][0]= offs_bone[3][1]= offs_bone[3][2]= 0.0f;
2191                         Mat4MulVecfl(parchan->pose_mat, tmat[3]);
2192                         
2193                         Mat4MulSerie(pchan->pose_mat, tmat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
2194                 }
2195                 else if(bone->flag & BONE_NO_SCALE) {
2196                         float orthmat[4][4], vec[3];
2197                         
2198                         /* get the official transform, but we only use the vector from it (optimize...) */
2199                         Mat4MulSerie(pchan->pose_mat, parchan->pose_mat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
2200                         VECCOPY(vec, pchan->pose_mat[3]);
2201                         
2202                         /* do this again, but with an ortho-parent matrix */
2203                         Mat4CpyMat4(orthmat, parchan->pose_mat);
2204                         Mat4Ortho(orthmat);
2205                         Mat4MulSerie(pchan->pose_mat, orthmat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
2206                         
2207                         /* copy correct transform */
2208                         VECCOPY(pchan->pose_mat[3], vec);
2209                 }
2210                 else 
2211                         Mat4MulSerie(pchan->pose_mat, parchan->pose_mat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
2212         }
2213         else {
2214                 Mat4MulMat4(pchan->pose_mat, pchan->chan_mat, bone->arm_mat);
2215                 
2216                 /* only rootbones get the cyclic offset (unless user doesn't want that) */
2217                 if ((bone->flag & BONE_NO_CYCLICOFFSET) == 0)
2218                         VecAddf(pchan->pose_mat[3], pchan->pose_mat[3], ob->pose->cyclic_offset);
2219         }
2220         
2221         /* do NLA strip modifiers - i.e. curve follow */
2222         do_strip_modifiers(scene, ob, bone, pchan);
2223         
2224         /* Do constraints */
2225         if (pchan->constraints.first) {
2226                 bConstraintOb *cob;
2227                 
2228                 /* local constraints */
2229                 do_constraint_channels(&pchan->constraints, NULL, ctime, 0);
2230                 
2231                 /* make a copy of location of PoseChannel for later */
2232                 VECCOPY(vec, pchan->pose_mat[3]);
2233                 
2234                 /* prepare PoseChannel for Constraint solving 
2235                  * - makes a copy of matrix, and creates temporary struct to use 
2236                  */
2237                 cob= constraints_make_evalob(scene, ob, pchan, CONSTRAINT_OBTYPE_BONE);
2238                 
2239                 /* Solve PoseChannel's Constraints */
2240                 solve_constraints(&pchan->constraints, cob, ctime);     // ctime doesnt alter objects
2241                 
2242                 /* cleanup after Constraint Solving 
2243                  * - applies matrix back to pchan, and frees temporary struct used
2244                  */
2245                 constraints_clear_evalob(cob);
2246                 
2247                 /* prevent constraints breaking a chain */
2248                 if(pchan->bone->flag & BONE_CONNECTED) {
2249                         VECCOPY(pchan->pose_mat[3], vec);
2250                 }
2251         }
2252         
2253         /* calculate head */
2254         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
2255         /* calculate tail */
2256         VECCOPY(vec, pchan->pose_mat[1]);
2257         VecMulf(vec, bone->length);
2258         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
2259 }
2260
2261 /* This only reads anim data from channels, and writes to channels */
2262 /* This is the only function adding poses */
2263 void where_is_pose (Scene *scene, Object *ob)
2264 {
2265         bArmature *arm;
2266         Bone *bone;
2267         bPoseChannel *pchan;
2268         float imat[4][4];
2269         float ctime;
2270         
2271         if(ob->type!=OB_ARMATURE) return;
2272         arm = ob->data;
2273         
2274         if(ELEM(NULL, arm, scene)) return;
2275         if((ob->pose==NULL) || (ob->pose->flag & POSE_RECALC)) 
2276            armature_rebuild_pose(ob, arm);
2277            
2278         ctime= bsystem_time(scene, ob, (float)scene->r.cfra, 0.0);      /* not accurate... */
2279         
2280         /* In editmode or restposition we read the data from the bones */
2281         if(arm->edbo || (arm->flag & ARM_RESTPOS)) {
2282                 
2283                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2284                         bone= pchan->bone;
2285                         if(bone) {
2286                                 Mat4CpyMat4(pchan->pose_mat, bone->arm_mat);
2287                                 VECCOPY(pchan->pose_head, bone->arm_head);
2288                                 VECCOPY(pchan->pose_tail, bone->arm_tail);
2289                         }
2290                 }
2291         }
2292         else {
2293                 Mat4Invert(ob->imat, ob->obmat);        // imat is needed 
2294
2295                 /* 1. construct the PoseTrees, clear flags */
2296                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2297                         pchan->flag &= ~(POSE_DONE|POSE_CHAIN);
2298                         if(pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints
2299                                 initialize_posetree(ob, pchan); // will attach it to root!
2300                 }
2301                 
2302                 /* 2. the main loop, channels are already hierarchical sorted from root to children */
2303                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2304                         
2305                         /* 3. if we find an IK root, we handle it separated */
2306                         if(pchan->iktree.first) {
2307                                 while(pchan->iktree.first) {
2308                                         PoseTree *tree= pchan->iktree.first;
2309                                         int a;
2310                                         
2311                                         /* 4. walk over the tree for regular solving */
2312                                         for(a=0; a<tree->totchannel; a++) {
2313                                                 if(!(tree->pchan[a]->flag & POSE_DONE)) // successive trees can set the flag
2314                                                         where_is_pose_bone(scene, ob, tree->pchan[a], ctime);
2315                                         }
2316                                         /* 5. execute the IK solver */
2317                                         execute_posetree(ob, tree);
2318                                         
2319                                         /* 6. apply the differences to the channels, 
2320                                                   we need to calculate the original differences first */
2321                                         for(a=0; a<tree->totchannel; a++)
2322                                                 make_dmats(tree->pchan[a]);
2323                                         
2324                                         for(a=0; a<tree->totchannel; a++)
2325                                                 /* sets POSE_DONE */
2326                                                 where_is_ik_bone(tree->pchan[a], tree->basis_change[a]);
2327                                         
2328                                         /* 7. and free */
2329                                         BLI_remlink(&pchan->iktree, tree);
2330                                         free_posetree(tree);
2331                                 }
2332                         }
2333                         else if(!(pchan->flag & POSE_DONE)) {
2334                                 where_is_pose_bone(scene, ob, pchan, ctime);
2335                         }
2336                 }
2337         }
2338                 
2339         /* calculating deform matrices */
2340         for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2341                 if(pchan->bone) {
2342                         Mat4Invert(imat, pchan->bone->arm_mat);
2343                         Mat4MulMat4(pchan->chan_mat, imat, pchan->pose_mat);
2344                 }
2345         }
2346 }