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