svn merge -r 13415:13452 https://svn.blender.org/svnroot/bf-blender/trunk/blender
[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[3]={""};
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         pchan= pose->chanbase.first;
1402         for(; pchan; pchan= pchan->next) {
1403                 if(pchan->bone->layer & layer_protected) {
1404                         ListBase proxylocal_constraints = {NULL, NULL};
1405                         pchanp= get_pose_channel(frompose, pchan->name);
1406                         
1407                         /* copy posechannel to temp, but restore important pointers */
1408                         pchanw= *pchanp;
1409                         pchanw.prev= pchan->prev;
1410                         pchanw.next= pchan->next;
1411                         pchanw.parent= pchan->parent;
1412                         pchanw.child= pchan->child;
1413                         pchanw.path= NULL;
1414                         
1415                         /* constraints - proxy constraints are flushed... local ones are added after 
1416                          *      1. extract constraints not from proxy (CONSTRAINT_PROXY_LOCAL) from pchan's constraints
1417                          *      2. copy proxy-pchan's constraints on-to new
1418                          *      3. add extracted local constraints back on top 
1419                          */
1420                         extract_proxylocal_constraints(&proxylocal_constraints, &pchan->constraints);
1421                         copy_constraints(&pchanw.constraints, &pchanp->constraints);
1422                         addlisttolist(&pchanw.constraints, &proxylocal_constraints);
1423                         
1424                         /* constraints - set target ob pointer to own object */
1425                         for (con= pchanw.constraints.first; con; con= con->next) {
1426                                 bConstraintTypeInfo *cti= constraint_get_typeinfo(con);
1427                                 ListBase targets = {NULL, NULL};
1428                                 bConstraintTarget *ct;
1429                                 
1430                                 if (cti && cti->get_constraint_targets) {
1431                                         cti->get_constraint_targets(con, &targets);
1432                                         
1433                                         for (ct= targets.first; ct; ct= ct->next) {
1434                                                 if (ct->tar == from)
1435                                                         ct->tar = ob;
1436                                         }
1437                                         
1438                                         if (cti->flush_constraint_targets)
1439                                                 cti->flush_constraint_targets(con, &targets, 0);
1440                                 }
1441                         }
1442                         
1443                         /* free stuff from current channel */
1444                         if (pchan->path) MEM_freeN(pchan->path);
1445                         free_constraints(&pchan->constraints);
1446                         
1447                         /* the final copy */
1448                         *pchan= pchanw;
1449                 }
1450         }
1451 }
1452
1453 static int rebuild_pose_bone(bPose *pose, Bone *bone, bPoseChannel *parchan, int counter)
1454 {
1455         bPoseChannel *pchan = verify_pose_channel (pose, bone->name);   // verify checks and/or adds
1456
1457         pchan->bone= bone;
1458         pchan->parent= parchan;
1459         
1460         counter++;
1461         
1462         for(bone= bone->childbase.first; bone; bone= bone->next) {
1463                 counter= rebuild_pose_bone(pose, bone, pchan, counter);
1464                 /* for quick detecting of next bone in chain, only b-bone uses it now */
1465                 if(bone->flag & BONE_CONNECTED)
1466                         pchan->child= get_pose_channel(pose, bone->name);
1467         }
1468         
1469         return counter;
1470 }
1471
1472 /* only after leave editmode, duplicating, validating older files, library syncing */
1473 /* NOTE: pose->flag is set for it */
1474 void armature_rebuild_pose(Object *ob, bArmature *arm)
1475 {
1476         Bone *bone;
1477         bPose *pose;
1478         bPoseChannel *pchan, *next;
1479         int counter=0;
1480                 
1481         /* only done here */
1482         if(ob->pose==NULL) ob->pose= MEM_callocN(sizeof(bPose), "new pose");
1483         pose= ob->pose;
1484         
1485         /* clear */
1486         for(pchan= pose->chanbase.first; pchan; pchan= pchan->next) {
1487                 pchan->bone= NULL;
1488                 pchan->child= NULL;
1489         }
1490         
1491         /* first step, check if all channels are there */
1492         for(bone= arm->bonebase.first; bone; bone= bone->next) {
1493                 counter= rebuild_pose_bone(pose, bone, NULL, counter);
1494         }
1495
1496         /* and a check for garbage */
1497         for(pchan= pose->chanbase.first; pchan; pchan= next) {
1498                 next= pchan->next;
1499                 if(pchan->bone==NULL) {
1500                         if(pchan->path)
1501                                 MEM_freeN(pchan->path);
1502                         free_constraints(&pchan->constraints);
1503                         BLI_freelinkN(&pose->chanbase, pchan);
1504                 }
1505         }
1506         // printf("rebuild pose %s, %d bones\n", ob->id.name, counter);
1507         
1508         /* synchronize protected layers with proxy */
1509         if(ob->proxy)
1510                 pose_proxy_synchronize(ob, ob->proxy, arm->layer_protected);
1511         
1512         update_pose_constraint_flags(ob->pose); // for IK detection for example
1513         
1514         /* the sorting */
1515         if(counter>1)
1516                 DAG_pose_sort(ob);
1517         
1518         ob->pose->flag &= ~POSE_RECALC;
1519 }
1520
1521
1522 /* ********************** THE IK SOLVER ******************* */
1523
1524
1525
1526 /* allocates PoseTree, and links that to root bone/channel */
1527 /* Note: detecting the IK chain is duplicate code... in drawarmature.c and in transform_conversions.c */
1528 static void initialize_posetree(struct Object *ob, bPoseChannel *pchan_tip)
1529 {
1530         bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
1531         PoseTree *tree;
1532         PoseTarget *target;
1533         bConstraint *con;
1534         bKinematicConstraint *data;
1535         int a, segcount= 0, size, newsize, *oldparent, parent;
1536         
1537         /* find IK constraint, and validate it */
1538         for(con= pchan_tip->constraints.first; con; con= con->next) {
1539                 if(con->type==CONSTRAINT_TYPE_KINEMATIC) break;
1540         }
1541         if(con==NULL) return;
1542         
1543         data=(bKinematicConstraint*)con->data;
1544         
1545         /* two types of targets */
1546         if(data->flag & CONSTRAINT_IK_AUTO);
1547         else {
1548                 if(con->flag & CONSTRAINT_DISABLE) return;      /* checked in editconstraint.c */
1549                 if(con->enforce == 0.0f) return;
1550                 if(data->tar==NULL) return;
1551                 if(data->tar->type==OB_ARMATURE && data->subtarget[0]==0) return;
1552         }
1553         
1554         /* exclude tip from chain? */
1555         if(!(data->flag & CONSTRAINT_IK_TIP))
1556                 pchan_tip= pchan_tip->parent;
1557         
1558         /* Find the chain's root & count the segments needed */
1559         for (curchan = pchan_tip; curchan; curchan=curchan->parent){
1560                 pchan_root = curchan;
1561                 
1562                 curchan->flag |= POSE_CHAIN;    // don't forget to clear this
1563                 chanlist[segcount]=curchan;
1564                 segcount++;
1565                 
1566                 if(segcount==data->rootbone || segcount>255) break; // 255 is weak
1567         }
1568         if (!segcount) return;
1569
1570         /* setup the chain data */
1571         
1572         /* we make tree-IK, unless all existing targets are in this chain */
1573         for(tree= pchan_root->iktree.first; tree; tree= tree->next) {
1574                 for(target= tree->targets.first; target; target= target->next) {
1575                         curchan= tree->pchan[target->tip];
1576                         if(curchan->flag & POSE_CHAIN)
1577                                 curchan->flag &= ~POSE_CHAIN;
1578                         else
1579                                 break;
1580                 }
1581                 if(target) break;
1582         }
1583
1584         /* create a target */
1585         target= MEM_callocN(sizeof(PoseTarget), "posetarget");
1586         target->con= con;
1587         pchan_tip->flag &= ~POSE_CHAIN;
1588
1589         if(tree==NULL) {
1590                 /* make new tree */
1591                 tree= MEM_callocN(sizeof(PoseTree), "posetree");
1592
1593                 tree->iterations= data->iterations;
1594                 tree->totchannel= segcount;
1595                 tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
1596                 
1597                 tree->pchan= MEM_callocN(segcount*sizeof(void*), "ik tree pchan");
1598                 tree->parent= MEM_callocN(segcount*sizeof(int), "ik tree parent");
1599                 for(a=0; a<segcount; a++) {
1600                         tree->pchan[a]= chanlist[segcount-a-1];
1601                         tree->parent[a]= a-1;
1602                 }
1603                 target->tip= segcount-1;
1604                 
1605                 /* AND! link the tree to the root */
1606                 BLI_addtail(&pchan_root->iktree, tree);
1607         }
1608         else {
1609                 tree->iterations= MAX2(data->iterations, tree->iterations);
1610                 tree->stretch= tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
1611
1612                 /* skip common pose channels and add remaining*/
1613                 size= MIN2(segcount, tree->totchannel);
1614                 for(a=0; a<size && tree->pchan[a]==chanlist[segcount-a-1]; a++);
1615                 parent= a-1;
1616
1617                 segcount= segcount-a;
1618                 target->tip= tree->totchannel + segcount - 1;
1619
1620                 if (segcount > 0) {
1621                         /* resize array */
1622                         newsize= tree->totchannel + segcount;
1623                         oldchan= tree->pchan;
1624                         oldparent= tree->parent;
1625
1626                         tree->pchan= MEM_callocN(newsize*sizeof(void*), "ik tree pchan");
1627                         tree->parent= MEM_callocN(newsize*sizeof(int), "ik tree parent");
1628                         memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel);
1629                         memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel);
1630                         MEM_freeN(oldchan);
1631                         MEM_freeN(oldparent);
1632
1633                         /* add new pose channels at the end, in reverse order */
1634                         for(a=0; a<segcount; a++) {
1635                                 tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1];
1636                                 tree->parent[tree->totchannel+a]= tree->totchannel+a-1;
1637                         }
1638                         tree->parent[tree->totchannel]= parent;
1639                         
1640                         tree->totchannel= newsize;
1641                 }
1642
1643                 /* move tree to end of list, for correct evaluation order */
1644                 BLI_remlink(&pchan_root->iktree, tree);
1645                 BLI_addtail(&pchan_root->iktree, tree);
1646         }
1647
1648         /* add target to the tree */
1649         BLI_addtail(&tree->targets, target);
1650 }
1651
1652 /* called from within the core where_is_pose loop, all animsystems and constraints
1653 were executed & assigned. Now as last we do an IK pass */
1654 static void execute_posetree(Object *ob, PoseTree *tree)
1655 {
1656         float R_parmat[3][3];
1657         float iR_parmat[3][3];
1658         float R_bonemat[3][3];
1659         float goalrot[3][3], goalpos[3];
1660         float rootmat[4][4], imat[4][4];
1661         float goal[4][4], goalinv[4][4];
1662         float irest_basis[3][3], full_basis[3][3];
1663         float end_pose[4][4], world_pose[4][4];
1664         float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch=NULL;
1665         int a, flag, hasstretch=0;
1666         bPoseChannel *pchan;
1667         IK_Segment *seg, *parent, **iktree, *iktarget;
1668         IK_Solver *solver;
1669         PoseTarget *target;
1670         bKinematicConstraint *data, *poleangledata=NULL;
1671         Bone *bone;
1672
1673         if (tree->totchannel == 0)
1674                 return;
1675
1676         iktree= MEM_mallocN(sizeof(void*)*tree->totchannel, "ik tree");
1677
1678         for(a=0; a<tree->totchannel; a++) {
1679                 pchan= tree->pchan[a];
1680                 bone= pchan->bone;
1681
1682                 /* set DoF flag */
1683                 flag= 0;
1684                 if(!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP))
1685                         flag |= IK_XDOF;
1686                 if(!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP))
1687                         flag |= IK_YDOF;
1688                 if(!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP))
1689                         flag |= IK_ZDOF;
1690
1691                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
1692                         flag |= IK_TRANS_YDOF;
1693                         hasstretch = 1;
1694                 }
1695
1696                 seg= iktree[a]= IK_CreateSegment(flag);
1697
1698                 /* find parent */
1699                 if(a == 0)
1700                         parent= NULL;
1701                 else
1702                         parent= iktree[tree->parent[a]];
1703
1704                 IK_SetParent(seg, parent);
1705         
1706                 /* get the matrix that transforms from prevbone into this bone */
1707                 Mat3CpyMat4(R_bonemat, pchan->pose_mat);
1708
1709                 /* gather transformations for this IK segment */
1710
1711                 if (pchan->parent)
1712                         Mat3CpyMat4(R_parmat, pchan->parent->pose_mat);
1713                 else
1714                         Mat3One(R_parmat);
1715
1716                 /* bone offset */
1717                 if (pchan->parent && (a > 0))
1718                         VecSubf(start, pchan->pose_head, pchan->parent->pose_tail);
1719                 else
1720                         /* only root bone (a = 0) has no parent */
1721                         start[0]= start[1]= start[2]= 0.0f;
1722                 
1723                 /* change length based on bone size */
1724                 length= bone->length*VecLength(R_bonemat[1]);
1725
1726                 /* compute rest basis and its inverse */
1727                 Mat3CpyMat3(rest_basis, bone->bone_mat);
1728                 Mat3CpyMat3(irest_basis, bone->bone_mat);
1729                 Mat3Transp(irest_basis);
1730
1731                 /* compute basis with rest_basis removed */
1732                 Mat3Inv(iR_parmat, R_parmat);
1733                 Mat3MulMat3(full_basis, iR_parmat, R_bonemat);
1734                 Mat3MulMat3(basis, irest_basis, full_basis);
1735
1736                 /* basis must be pure rotation */
1737                 Mat3Ortho(basis);
1738
1739                 /* transform offset into local bone space */
1740                 Mat3Ortho(iR_parmat);
1741                 Mat3MulVecfl(iR_parmat, start);
1742
1743                 IK_SetTransform(seg, start, rest_basis, basis, length);
1744
1745                 if (pchan->ikflag & BONE_IK_XLIMIT)
1746                         IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]);
1747                 if (pchan->ikflag & BONE_IK_YLIMIT)
1748                         IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]);
1749                 if (pchan->ikflag & BONE_IK_ZLIMIT)
1750                         IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]);
1751
1752                 IK_SetStiffness(seg, IK_X, pchan->stiffness[0]);
1753                 IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]);
1754                 IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]);
1755
1756                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
1757                         float ikstretch = pchan->ikstretch*pchan->ikstretch;
1758                         IK_SetStiffness(seg, IK_TRANS_Y, MIN2(1.0-ikstretch, 0.999));
1759                         IK_SetLimit(seg, IK_TRANS_Y, 0.001, 1e10);
1760                 }
1761         }
1762
1763         solver= IK_CreateSolver(iktree[0]);
1764
1765         /* set solver goals */
1766
1767         /* first set the goal inverse transform, assuming the root of tree was done ok! */
1768         pchan= tree->pchan[0];
1769         if (pchan->parent)
1770                 /* transform goal by parent mat, so this rotation is not part of the
1771                    segment's basis. otherwise rotation limits do not work on the
1772                    local transform of the segment itself. */
1773                 Mat4CpyMat4(rootmat, pchan->parent->pose_mat);
1774         else
1775                 Mat4One(rootmat);
1776         VECCOPY(rootmat[3], pchan->pose_head);
1777         
1778         Mat4MulMat4 (imat, rootmat, ob->obmat);
1779         Mat4Invert (goalinv, imat);
1780         
1781         for (target=tree->targets.first; target; target=target->next) {
1782                 float polepos[3];
1783                 int poleconstrain= 0;
1784
1785                 data= (bKinematicConstraint*)target->con->data;
1786                 
1787                 /* 1.0=ctime, we pass on object for auto-ik (owner-type here is object, even though
1788                  * strictly speaking, it is a posechannel)
1789                  */
1790                 get_constraint_target_matrix(target->con, 0, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
1791                 
1792                 /* and set and transform goal */
1793                 Mat4MulMat4(goal, rootmat, goalinv);
1794                 
1795                 VECCOPY(goalpos, goal[3]);
1796                 Mat3CpyMat4(goalrot, goal);
1797                 
1798                 /* same for pole vector target */
1799                 if(data->poletar) {
1800                         get_constraint_target_matrix(target->con, 1, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
1801
1802                         if(data->flag & CONSTRAINT_IK_SETANGLE) {
1803                                 /* don't solve IK when we are setting the pole angle */
1804                                 break;
1805                         }
1806                         else {
1807                                 Mat4MulMat4(goal, rootmat, goalinv);
1808                                 VECCOPY(polepos, goal[3]);
1809                                 poleconstrain= 1;
1810
1811                                 if(data->flag & CONSTRAINT_IK_GETANGLE) {
1812                                         poleangledata= data;
1813                                         data->flag &= ~CONSTRAINT_IK_GETANGLE;
1814                                 }
1815                         }
1816                 }
1817
1818                 /* do we need blending? */
1819                 if (target->con->enforce!=1.0) {
1820                         float q1[4], q2[4], q[4];
1821                         float fac= target->con->enforce;
1822                         float mfac= 1.0-fac;
1823                         
1824                         pchan= tree->pchan[target->tip];
1825                         
1826                         /* end effector in world space */
1827                         Mat4CpyMat4(end_pose, pchan->pose_mat);
1828                         VECCOPY(end_pose[3], pchan->pose_tail);
1829                         Mat4MulSerie(world_pose, goalinv, ob->obmat, end_pose, 0, 0, 0, 0, 0);
1830                         
1831                         /* blend position */
1832                         goalpos[0]= fac*goalpos[0] + mfac*world_pose[3][0];
1833                         goalpos[1]= fac*goalpos[1] + mfac*world_pose[3][1];
1834                         goalpos[2]= fac*goalpos[2] + mfac*world_pose[3][2];
1835                         
1836                         /* blend rotation */
1837                         Mat3ToQuat(goalrot, q1);
1838                         Mat4ToQuat(world_pose, q2);
1839                         QuatInterpol(q, q1, q2, mfac);
1840                         QuatToMat3(q, goalrot);
1841                 }
1842                 
1843                 iktarget= iktree[target->tip];
1844                 
1845                 if(data->weight != 0.0) {
1846                         if(poleconstrain)
1847                                 IK_SolverSetPoleVectorConstraint(solver, iktarget, goalpos,
1848                                         polepos, data->poleangle*M_PI/180, (poleangledata == data));
1849                         IK_SolverAddGoal(solver, iktarget, goalpos, data->weight);
1850                 }
1851                 if((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0))
1852                         if((data->flag & CONSTRAINT_IK_AUTO)==0)
1853                                 IK_SolverAddGoalOrientation(solver, iktarget, goalrot,
1854                                         data->orientweight);
1855         }
1856
1857         /* solve */
1858         IK_Solve(solver, 0.0f, tree->iterations);
1859
1860         if(poleangledata)
1861                 poleangledata->poleangle= IK_SolverGetPoleAngle(solver)*180/M_PI;
1862
1863         IK_FreeSolver(solver);
1864
1865         /* gather basis changes */
1866         tree->basis_change= MEM_mallocN(sizeof(float[3][3])*tree->totchannel, "ik basis change");
1867         if(hasstretch)
1868                 ikstretch= MEM_mallocN(sizeof(float)*tree->totchannel, "ik stretch");
1869
1870         for(a=0; a<tree->totchannel; a++) {
1871                 IK_GetBasisChange(iktree[a], tree->basis_change[a]);
1872
1873                 if(hasstretch) {
1874                         /* have to compensate for scaling received from parent */
1875                         float parentstretch, stretch;
1876
1877                         pchan= tree->pchan[a];
1878                         parentstretch= (tree->parent[a] >= 0)? ikstretch[tree->parent[a]]: 1.0;
1879
1880                         if(tree->stretch && (pchan->ikstretch > 0.0)) {
1881                                 float trans[3], length;
1882
1883                                 IK_GetTranslationChange(iktree[a], trans);
1884                                 length= pchan->bone->length*VecLength(pchan->pose_mat[1]);
1885
1886                                 ikstretch[a]= (length == 0.0)? 1.0: (trans[1]+length)/length;
1887                         }
1888                         else
1889                                 ikstretch[a] = 1.0;
1890
1891                         stretch= (parentstretch == 0.0)? 1.0: ikstretch[a]/parentstretch;
1892
1893                         VecMulf(tree->basis_change[a][0], stretch);
1894                         VecMulf(tree->basis_change[a][1], stretch);
1895                         VecMulf(tree->basis_change[a][2], stretch);
1896
1897                 }
1898
1899                 IK_FreeSegment(iktree[a]);
1900         }
1901         
1902         MEM_freeN(iktree);
1903         if(ikstretch) MEM_freeN(ikstretch);
1904 }
1905
1906 void free_posetree(PoseTree *tree)
1907 {
1908         BLI_freelistN(&tree->targets);
1909         if(tree->pchan) MEM_freeN(tree->pchan);
1910         if(tree->parent) MEM_freeN(tree->parent);
1911         if(tree->basis_change) MEM_freeN(tree->basis_change);
1912         MEM_freeN(tree);
1913 }
1914
1915 /* ********************** THE POSE SOLVER ******************* */
1916
1917
1918 /* loc/rot/size to mat4 */
1919 /* used in constraint.c too */
1920 void chan_calc_mat(bPoseChannel *chan)
1921 {
1922         float smat[3][3];
1923         float rmat[3][3];
1924         float tmat[3][3];
1925         
1926         SizeToMat3(chan->size, smat);
1927         
1928         NormalQuat(chan->quat);
1929
1930         QuatToMat3(chan->quat, rmat);
1931         
1932         Mat3MulMat3(tmat, rmat, smat);
1933         
1934         Mat4CpyMat3(chan->chan_mat, tmat);
1935         
1936         /* prevent action channels breaking chains */
1937         /* need to check for bone here, CONSTRAINT_TYPE_ACTION uses this call */
1938         if (chan->bone==NULL || !(chan->bone->flag & BONE_CONNECTED)) {
1939                 VECCOPY(chan->chan_mat[3], chan->loc);
1940         }
1941
1942 }
1943
1944 /* transform from bone(b) to bone(b+1), store in chan_mat */
1945 static void make_dmats(bPoseChannel *pchan)
1946 {
1947         if (pchan->parent) {
1948                 float iR_parmat[4][4];
1949                 Mat4Invert(iR_parmat, pchan->parent->pose_mat);
1950                 Mat4MulMat4(pchan->chan_mat,  pchan->pose_mat, iR_parmat);      // delta mat
1951         }
1952         else Mat4CpyMat4(pchan->chan_mat, pchan->pose_mat);
1953 }
1954
1955 /* applies IK matrix to pchan, IK is done separated */
1956 /* formula: pose_mat(b) = pose_mat(b-1) * diffmat(b-1, b) * ik_mat(b) */
1957 /* to make this work, the diffmats have to be precalculated! Stored in chan_mat */
1958 static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[][3])   // nr = to detect if this is first bone
1959 {
1960         float vec[3], ikmat[4][4];
1961         
1962         Mat4CpyMat3(ikmat, ik_mat);
1963         
1964         if (pchan->parent)
1965                 Mat4MulSerie(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat, ikmat, NULL, NULL, NULL, NULL, NULL);
1966         else 
1967                 Mat4MulMat4(pchan->pose_mat, ikmat, pchan->chan_mat);
1968
1969         /* calculate head */
1970         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
1971         /* calculate tail */
1972         VECCOPY(vec, pchan->pose_mat[1]);
1973         VecMulf(vec, pchan->bone->length);
1974         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
1975
1976         pchan->flag |= POSE_DONE;
1977 }
1978
1979 /* NLA strip modifiers */
1980 static void do_strip_modifiers(Object *armob, Bone *bone, bPoseChannel *pchan)
1981 {
1982         bActionModifier *amod;
1983         bActionStrip *strip, *strip2;
1984         float scene_cfra= G.scene->r.cfra;
1985         int do_modif;
1986
1987         for (strip=armob->nlastrips.first; strip; strip=strip->next) {
1988                 do_modif=0;
1989                 
1990                 if (scene_cfra>=strip->start && scene_cfra<=strip->end)
1991                         do_modif=1;
1992                 
1993                 if ((scene_cfra > strip->end) && (strip->flag & ACTSTRIP_HOLDLASTFRAME)) {
1994                         do_modif=1;
1995                         
1996                         /* if there are any other strips active, ignore modifiers for this strip - 
1997                          * 'hold' option should only hold action modifiers if there are 
1998                          * no other active strips */
1999                         for (strip2=strip->next; strip2; strip2=strip2->next) {
2000                                 if (strip2 == strip) continue;
2001                                 
2002                                 if (scene_cfra>=strip2->start && scene_cfra<=strip2->end) {
2003                                         if (!(strip2->flag & ACTSTRIP_MUTE))
2004                                                 do_modif=0;
2005                                 }
2006                         }
2007                         
2008                         /* if there are any later, activated, strips with 'hold' set, they take precedence, 
2009                          * so ignore modifiers for this strip */
2010                         for (strip2=strip->next; strip2; strip2=strip2->next) {
2011                                 if (scene_cfra < strip2->start) continue;
2012                                 if ((strip2->flag & ACTSTRIP_HOLDLASTFRAME) && !(strip2->flag & ACTSTRIP_MUTE)) {
2013                                         do_modif=0;
2014                                 }
2015                         }
2016                 }
2017                 
2018                 if (do_modif) {
2019                         /* temporal solution to prevent 2 strips accumulating */
2020                         if(scene_cfra==strip->end && strip->next && strip->next->start==scene_cfra)
2021                                 continue;
2022                         
2023                         for(amod= strip->modifiers.first; amod; amod= amod->next) {
2024                                 switch (amod->type) {
2025                                 case ACTSTRIP_MOD_DEFORM:
2026                                 {
2027                                         /* validate first */
2028                                         if(amod->ob && amod->ob->type==OB_CURVE && amod->channel[0]) {
2029                                                 
2030                                                 if( strcmp(pchan->name, amod->channel)==0 ) {
2031                                                         float mat4[4][4], mat3[3][3];
2032                                                         
2033                                                         curve_deform_vector(amod->ob, armob, bone->arm_mat[3], pchan->pose_mat[3], mat3, amod->no_rot_axis);
2034                                                         Mat4CpyMat4(mat4, pchan->pose_mat);
2035                                                         Mat4MulMat34(pchan->pose_mat, mat3, mat4);
2036                                                         
2037                                                 }
2038                                         }
2039                                 }
2040                                         break;
2041                                 case ACTSTRIP_MOD_NOISE:        
2042                                 {
2043                                         if( strcmp(pchan->name, amod->channel)==0 ) {
2044                                                 float nor[3], loc[3], ofs;
2045                                                 float eul[3], size[3], eulo[3], sizeo[3];
2046                                                 
2047                                                 /* calculate turbulance */
2048                                                 ofs = amod->turbul / 200.0f;
2049                                                 
2050                                                 /* make a copy of starting conditions */
2051                                                 VECCOPY(loc, pchan->pose_mat[3]);
2052                                                 Mat4ToEul(pchan->pose_mat, eul);
2053                                                 Mat4ToSize(pchan->pose_mat, size);
2054                                                 VECCOPY(eulo, eul);
2055                                                 VECCOPY(sizeo, size);
2056                                                 
2057                                                 /* apply noise to each set of channels */
2058                                                 if (amod->channels & 4) {
2059                                                         /* for scaling */
2060                                                         nor[0] = BLI_gNoise(amod->noisesize, size[0]+ofs, size[1], size[2], 0, 0) - ofs;
2061                                                         nor[1] = BLI_gNoise(amod->noisesize, size[0], size[1]+ofs, size[2], 0, 0) - ofs;        
2062                                                         nor[2] = BLI_gNoise(amod->noisesize, size[0], size[1], size[2]+ofs, 0, 0) - ofs;
2063                                                         VecAddf(size, size, nor);
2064                                                         
2065                                                         if (sizeo[0] != 0)
2066                                                                 VecMulf(pchan->pose_mat[0], size[0] / sizeo[0]);
2067                                                         if (sizeo[1] != 0)
2068                                                                 VecMulf(pchan->pose_mat[1], size[1] / sizeo[1]);
2069                                                         if (sizeo[2] != 0)
2070                                                                 VecMulf(pchan->pose_mat[2], size[2] / sizeo[2]);
2071                                                 }
2072                                                 if (amod->channels & 2) {
2073                                                         /* for rotation */
2074                                                         nor[0] = BLI_gNoise(amod->noisesize, eul[0]+ofs, eul[1], eul[2], 0, 0) - ofs;
2075                                                         nor[1] = BLI_gNoise(amod->noisesize, eul[0], eul[1]+ofs, eul[2], 0, 0) - ofs;   
2076                                                         nor[2] = BLI_gNoise(amod->noisesize, eul[0], eul[1], eul[2]+ofs, 0, 0) - ofs;
2077                                                         
2078                                                         compatible_eul(nor, eulo);
2079                                                         VecAddf(eul, eul, nor);
2080                                                         compatible_eul(eul, eulo);
2081                                                         
2082                                                         LocEulSizeToMat4(pchan->pose_mat, loc, eul, size);
2083                                                 }
2084                                                 if (amod->channels & 1) {
2085                                                         /* for location */
2086                                                         nor[0] = BLI_gNoise(amod->noisesize, loc[0]+ofs, loc[1], loc[2], 0, 0) - ofs;
2087                                                         nor[1] = BLI_gNoise(amod->noisesize, loc[0], loc[1]+ofs, loc[2], 0, 0) - ofs;   
2088                                                         nor[2] = BLI_gNoise(amod->noisesize, loc[0], loc[1], loc[2]+ofs, 0, 0) - ofs;
2089                                                         
2090                                                         VecAddf(pchan->pose_mat[3], loc, nor);
2091                                                 }
2092                                         }
2093                                 }
2094                                         break;
2095                                 }
2096                         }
2097                 }
2098         }
2099 }
2100
2101
2102 /* The main armature solver, does all constraints excluding IK */
2103 /* pchan is validated, as having bone and parent pointer */
2104 static void where_is_pose_bone(Object *ob, bPoseChannel *pchan, float ctime)
2105 {
2106         Bone *bone, *parbone;
2107         bPoseChannel *parchan;
2108         float vec[3];
2109         
2110         /* set up variables for quicker access below */
2111         bone= pchan->bone;
2112         parbone= bone->parent;
2113         parchan= pchan->parent;
2114         
2115         /* this gives a chan_mat with actions (ipos) results */
2116         chan_calc_mat(pchan);
2117         
2118         /* construct the posemat based on PoseChannels, that we do before applying constraints */
2119         /* pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b) */
2120         
2121         if(parchan) {
2122                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
2123                 
2124                 /* bone transform itself */
2125                 Mat4CpyMat3(offs_bone, bone->bone_mat);
2126                 
2127                 /* The bone's root offset (is in the parent's coordinate system) */
2128                 VECCOPY(offs_bone[3], bone->head);
2129                 
2130                 /* Get the length translation of parent (length along y axis) */
2131                 offs_bone[3][1]+= parbone->length;
2132                 
2133                 /* Compose the matrix for this bone  */
2134                 if(bone->flag & BONE_HINGE) {   // uses restposition rotation, but actual position
2135                         float tmat[4][4];
2136                         
2137                         /* the rotation of the parent restposition */
2138                         Mat4CpyMat4(tmat, parbone->arm_mat);
2139                         
2140                         /* the location of actual parent transform */
2141                         VECCOPY(tmat[3], offs_bone[3]);
2142                         offs_bone[3][0]= offs_bone[3][1]= offs_bone[3][2]= 0.0f;
2143                         Mat4MulVecfl(parchan->pose_mat, tmat[3]);
2144                         
2145                         Mat4MulSerie(pchan->pose_mat, tmat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
2146                 }
2147                 else if(bone->flag & BONE_NO_SCALE) {
2148                         float orthmat[4][4], vec[3];
2149                         
2150                         /* get the official transform, but we only use the vector from it (optimize...) */
2151                         Mat4MulSerie(pchan->pose_mat, parchan->pose_mat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
2152                         VECCOPY(vec, pchan->pose_mat[3]);
2153                         
2154                         /* do this again, but with an ortho-parent matrix */
2155                         Mat4CpyMat4(orthmat, parchan->pose_mat);
2156                         Mat4Ortho(orthmat);
2157                         Mat4MulSerie(pchan->pose_mat, orthmat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
2158                         
2159                         /* copy correct transform */
2160                         VECCOPY(pchan->pose_mat[3], vec);
2161                 }
2162                 else 
2163                         Mat4MulSerie(pchan->pose_mat, parchan->pose_mat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
2164         }
2165         else {
2166                 Mat4MulMat4(pchan->pose_mat, pchan->chan_mat, bone->arm_mat);
2167                 /* only rootbones get the cyclic offset */
2168                 VecAddf(pchan->pose_mat[3], pchan->pose_mat[3], ob->pose->cyclic_offset);
2169         }
2170         
2171         /* do NLA strip modifiers - i.e. curve follow */
2172         do_strip_modifiers(ob, bone, pchan);
2173         
2174         /* Do constraints */
2175         if (pchan->constraints.first) {
2176                 bConstraintOb *cob;
2177                 
2178                 /* local constraints */
2179                 do_constraint_channels(&pchan->constraints, NULL, ctime, 0);
2180                 
2181                 /* make a copy of location of PoseChannel for later */
2182                 VECCOPY(vec, pchan->pose_mat[3]);
2183                 
2184                 /* prepare PoseChannel for Constraint solving 
2185                  * - makes a copy of matrix, and creates temporary struct to use 
2186                  */
2187                 cob= constraints_make_evalob(ob, pchan, CONSTRAINT_OBTYPE_BONE);
2188                 
2189                 /* Solve PoseChannel's Constraints */
2190                 solve_constraints(&pchan->constraints, cob, ctime);     // ctime doesnt alter objects
2191                 
2192                 /* cleanup after Constraint Solving 
2193                  * - applies matrix back to pchan, and frees temporary struct used
2194                  */
2195                 constraints_clear_evalob(cob);
2196                 
2197                 /* prevent constraints breaking a chain */
2198                 if(pchan->bone->flag & BONE_CONNECTED) {
2199                         VECCOPY(pchan->pose_mat[3], vec);
2200                 }
2201         }
2202         
2203         /* calculate head */
2204         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
2205         /* calculate tail */
2206         VECCOPY(vec, pchan->pose_mat[1]);
2207         VecMulf(vec, bone->length);
2208         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
2209 }
2210
2211 /* This only reads anim data from channels, and writes to channels */
2212 /* This is the only function adding poses */
2213 void where_is_pose (Object *ob)
2214 {
2215         bArmature *arm;
2216         Bone *bone;
2217         bPoseChannel *pchan;
2218         float imat[4][4];
2219         float ctime= bsystem_time(ob, (float)G.scene->r.cfra, 0.0);     /* not accurate... */
2220         
2221         arm = get_armature(ob);
2222         
2223         if(arm==NULL) return;
2224         if(ob->pose==NULL || (ob->pose->flag & POSE_RECALC)) 
2225            armature_rebuild_pose(ob, arm);
2226         
2227         /* In restposition we read the data from the bones */
2228         if(ob==G.obedit || (arm->flag & ARM_RESTPOS)) {
2229                 
2230                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2231                         bone= pchan->bone;
2232                         if(bone) {
2233                                 Mat4CpyMat4(pchan->pose_mat, bone->arm_mat);
2234                                 VECCOPY(pchan->pose_head, bone->arm_head);
2235                                 VECCOPY(pchan->pose_tail, bone->arm_tail);
2236                         }
2237                 }
2238         }
2239         else {
2240                 Mat4Invert(ob->imat, ob->obmat);        // imat is needed 
2241
2242                 /* 1. construct the PoseTrees, clear flags */
2243                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2244                         pchan->flag &= ~(POSE_DONE|POSE_CHAIN);
2245                         if(pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints
2246                                 initialize_posetree(ob, pchan); // will attach it to root!
2247                 }
2248                 
2249                 /* 2. the main loop, channels are already hierarchical sorted from root to children */
2250                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2251                         
2252                         /* 3. if we find an IK root, we handle it separated */
2253                         if(pchan->iktree.first) {
2254                                 while(pchan->iktree.first) {
2255                                         PoseTree *tree= pchan->iktree.first;
2256                                         int a;
2257                                         
2258                                         /* 4. walk over the tree for regular solving */
2259                                         for(a=0; a<tree->totchannel; a++) {
2260                                                 if(!(tree->pchan[a]->flag & POSE_DONE)) // successive trees can set the flag
2261                                                         where_is_pose_bone(ob, tree->pchan[a], ctime);
2262                                         }
2263                                         /* 5. execute the IK solver */
2264                                         execute_posetree(ob, tree);
2265                                         
2266                                         /* 6. apply the differences to the channels, 
2267                                                   we need to calculate the original differences first */
2268                                         for(a=0; a<tree->totchannel; a++)
2269                                                 make_dmats(tree->pchan[a]);
2270                                         
2271                                         for(a=0; a<tree->totchannel; a++)
2272                                                 /* sets POSE_DONE */
2273                                                 where_is_ik_bone(tree->pchan[a], tree->basis_change[a]);
2274                                         
2275                                         /* 7. and free */
2276                                         BLI_remlink(&pchan->iktree, tree);
2277                                         free_posetree(tree);
2278                                 }
2279                         }
2280                         else if(!(pchan->flag & POSE_DONE)) {
2281                                 where_is_pose_bone(ob, pchan, ctime);
2282                         }
2283                 }
2284         }
2285                 
2286         /* calculating deform matrices */
2287         for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2288                 if(pchan->bone) {
2289                         Mat4Invert(imat, pchan->bone->arm_mat);
2290                         Mat4MulMat4(pchan->chan_mat, imat, pchan->pose_mat);
2291                 }
2292         }
2293 }