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