2.5 - Animation Fixes + More Porting work in Action Editor
[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         bArmature *arm= armOb->data;
910         bPoseChannel *pchan, **defnrToPC = NULL;
911         MDeformVert *dverts = NULL;
912         bDeformGroup *dg;
913         DualQuat *dualquats= NULL;
914         float obinv[4][4], premat[4][4], postmat[4][4];
915         int use_envelope = deformflag & ARM_DEF_ENVELOPE;
916         int use_quaternion = deformflag & ARM_DEF_QUATERNION;
917         int bbone_rest_def = deformflag & ARM_DEF_B_BONE_REST;
918         int invert_vgroup= deformflag & ARM_DEF_INVERT_VGROUP;
919         int numGroups = 0;              /* safety for vertexgroup index overflow */
920         int i, target_totvert = 0;      /* safety for vertexgroup overflow */
921         int use_dverts = 0;
922         int armature_def_nr = -1;
923         int totchan;
924
925         if(arm->edbo) return;
926         
927         Mat4Invert(obinv, target->obmat);
928         Mat4CpyMat4(premat, target->obmat);
929         Mat4MulMat4(postmat, armOb->obmat, obinv);
930         Mat4Invert(premat, postmat);
931
932         /* bone defmats are already in the channels, chan_mat */
933         
934         /* initialize B_bone matrices and dual quaternions */
935         if(use_quaternion) {
936                 totchan= BLI_countlist(&armOb->pose->chanbase);
937                 dualquats= MEM_callocN(sizeof(DualQuat)*totchan, "dualquats");
938         }
939
940         totchan= 0;
941         for(pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next) {
942                 if(!(pchan->bone->flag & BONE_NO_DEFORM)) {
943                         if(pchan->bone->segments > 1)
944                                 pchan_b_bone_defmats(pchan, use_quaternion, bbone_rest_def);
945
946                         if(use_quaternion) {
947                                 pchan->dual_quat= &dualquats[totchan++];
948                                 Mat4ToDQuat(pchan->bone->arm_mat, pchan->chan_mat, pchan->dual_quat);
949                         }
950                 }
951         }
952
953         /* get the def_nr for the overall armature vertex group if present */
954         for(i = 0, dg = target->defbase.first; dg; i++, dg = dg->next)
955                 if(defgrp_name && strcmp(defgrp_name, dg->name) == 0)
956                         armature_def_nr = i;
957
958         /* get a vertex-deform-index to posechannel array */
959         if(deformflag & ARM_DEF_VGROUP) {
960                 if(ELEM(target->type, OB_MESH, OB_LATTICE)) {
961                         numGroups = BLI_countlist(&target->defbase);
962                         
963                         if(target->type==OB_MESH) {
964                                 Mesh *me= target->data;
965                                 dverts = me->dvert;
966                                 target_totvert = me->totvert;
967                         }
968                         else {
969                                 Lattice *lt= target->data;
970                                 dverts = lt->dvert;
971                                 if(dverts)
972                                         target_totvert = lt->pntsu*lt->pntsv*lt->pntsw;
973                         }
974                         /* if we have a DerivedMesh, only use dverts if it has them */
975                         if(dm)
976                                 if(dm->getVertData(dm, 0, CD_MDEFORMVERT))
977                                         use_dverts = 1;
978                                 else use_dverts = 0;
979                         else if(dverts) use_dverts = 1;
980
981                         if(use_dverts) {
982                                 defnrToPC = MEM_callocN(sizeof(*defnrToPC) * numGroups,
983                                                         "defnrToBone");
984                                 for(i = 0, dg = target->defbase.first; dg;
985                                     i++, dg = dg->next) {
986                                         defnrToPC[i] = get_pose_channel(armOb->pose, dg->name);
987                                         /* exclude non-deforming bones */
988                                         if(defnrToPC[i]) {
989                                                 if(defnrToPC[i]->bone->flag & BONE_NO_DEFORM)
990                                                         defnrToPC[i]= NULL;
991                                         }
992                                 }
993                         }
994                 }
995         }
996
997         for(i = 0; i < numVerts; i++) {
998                 MDeformVert *dvert;
999                 DualQuat sumdq, *dq = NULL;
1000                 float *co, dco[3];
1001                 float sumvec[3], summat[3][3];
1002                 float *vec = NULL, (*smat)[3] = NULL;
1003                 float contrib = 0.0f;
1004                 float armature_weight = 1.0f;   /* default to 1 if no overall def group */
1005                 float prevco_weight = 1.0f;             /* weight for optional cached vertexcos */
1006                 int       j;
1007
1008                 if(use_quaternion) {
1009                         memset(&sumdq, 0, sizeof(DualQuat));
1010                         dq= &sumdq;
1011                 }
1012                 else {
1013                         sumvec[0] = sumvec[1] = sumvec[2] = 0.0f;
1014                         vec= sumvec;
1015
1016                         if(defMats) {
1017                                 Mat3Clr((float*)summat);
1018                                 smat = summat;
1019                         }
1020                 }
1021
1022                 if(use_dverts || armature_def_nr >= 0) {
1023                         if(dm) dvert = dm->getVertData(dm, i, CD_MDEFORMVERT);
1024                         else if(dverts && i < target_totvert) dvert = dverts + i;
1025                         else dvert = NULL;
1026                 } else
1027                         dvert = NULL;
1028
1029                 if(armature_def_nr >= 0 && dvert) {
1030                         armature_weight = 0.0f; /* a def group was given, so default to 0 */
1031                         for(j = 0; j < dvert->totweight; j++) {
1032                                 if(dvert->dw[j].def_nr == armature_def_nr) {
1033                                         armature_weight = dvert->dw[j].weight;
1034                                         break;
1035                                 }
1036                         }
1037                         /* hackish: the blending factor can be used for blending with prevCos too */
1038                         if(prevCos) {
1039                                 if(invert_vgroup)
1040                                         prevco_weight= 1.0f-armature_weight;
1041                                 else
1042                                         prevco_weight= armature_weight;
1043                                 armature_weight= 1.0f;
1044                         }
1045                 }
1046
1047                 /* check if there's any  point in calculating for this vert */
1048                 if(armature_weight == 0.0f) continue;
1049                 
1050                 /* get the coord we work on */
1051                 co= prevCos?prevCos[i]:vertexCos[i];
1052                 
1053                 /* Apply the object's matrix */
1054                 Mat4MulVecfl(premat, co);
1055                 
1056                 if(use_dverts && dvert && dvert->totweight) { // use weight groups ?
1057                         int deformed = 0;
1058                         
1059                         for(j = 0; j < dvert->totweight; j++){
1060                                 int index = dvert->dw[j].def_nr;
1061                                 pchan = index < numGroups?defnrToPC[index]:NULL;
1062                                 if(pchan) {
1063                                         float weight = dvert->dw[j].weight;
1064                                         Bone *bone = pchan->bone;
1065
1066                                         deformed = 1;
1067                                         
1068                                         if(bone && bone->flag & BONE_MULT_VG_ENV) {
1069                                                 weight *= distfactor_to_bone(co, bone->arm_head,
1070                                                                              bone->arm_tail,
1071                                                                              bone->rad_head,
1072                                                                              bone->rad_tail,
1073                                                                              bone->dist);
1074                                         }
1075                                         pchan_bone_deform(pchan, weight, vec, dq, smat, co, &contrib);
1076                                 }
1077                         }
1078                         /* if there are vertexgroups but not groups with bones
1079                          * (like for softbody groups)
1080                          */
1081                         if(deformed == 0 && use_envelope) {
1082                                 for(pchan = armOb->pose->chanbase.first; pchan;
1083                                     pchan = pchan->next) {
1084                                         if(!(pchan->bone->flag & BONE_NO_DEFORM))
1085                                                 contrib += dist_bone_deform(pchan, vec, dq, smat, co);
1086                                 }
1087                         }
1088                 }
1089                 else if(use_envelope) {
1090                         for(pchan = armOb->pose->chanbase.first; pchan;
1091                             pchan = pchan->next) {
1092                                 if(!(pchan->bone->flag & BONE_NO_DEFORM))
1093                                         contrib += dist_bone_deform(pchan, vec, dq, smat, co);
1094                         }
1095                 }
1096
1097                 /* actually should be EPSILON? weight values and contrib can be like 10e-39 small */
1098                 if(contrib > 0.0001f) {
1099                         if(use_quaternion) {
1100                                 DQuatNormalize(dq, contrib);
1101
1102                                 if(armature_weight != 1.0f) {
1103                                         VECCOPY(dco, co);
1104                                         DQuatMulVecfl(dq, dco, (defMats)? summat: NULL);
1105                                         VecSubf(dco, dco, co);
1106                                         VecMulf(dco, armature_weight);
1107                                         VecAddf(co, co, dco);
1108                                 }
1109                                 else
1110                                         DQuatMulVecfl(dq, co, (defMats)? summat: NULL);
1111
1112                                 smat = summat;
1113                         }
1114                         else {
1115                                 VecMulf(vec, armature_weight/contrib);
1116                                 VecAddf(co, vec, co);
1117                         }
1118
1119                         if(defMats) {
1120                                 float pre[3][3], post[3][3], tmpmat[3][3];
1121
1122                                 Mat3CpyMat4(pre, premat);
1123                                 Mat3CpyMat4(post, postmat);
1124                                 Mat3CpyMat3(tmpmat, defMats[i]);
1125
1126                                 if(!use_quaternion) /* quaternion already is scale corrected */
1127                                         Mat3MulFloat((float*)smat, armature_weight/contrib);
1128
1129                                 Mat3MulSerie(defMats[i], tmpmat, pre, smat, post,
1130                                         NULL, NULL, NULL, NULL);
1131                         }
1132                 }
1133                 
1134                 /* always, check above code */
1135                 Mat4MulVecfl(postmat, co);
1136                 
1137                 
1138                 /* interpolate with previous modifier position using weight group */
1139                 if(prevCos) {
1140                         float mw= 1.0f - prevco_weight;
1141                         vertexCos[i][0]= prevco_weight*vertexCos[i][0] + mw*co[0];
1142                         vertexCos[i][1]= prevco_weight*vertexCos[i][1] + mw*co[1];
1143                         vertexCos[i][2]= prevco_weight*vertexCos[i][2] + mw*co[2];
1144                 }
1145         }
1146
1147         if(dualquats) MEM_freeN(dualquats);
1148         if(defnrToPC) MEM_freeN(defnrToPC);
1149         
1150         /* free B_bone matrices */
1151         for(pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next) {
1152                 if(pchan->b_bone_mats) {
1153                         MEM_freeN(pchan->b_bone_mats);
1154                         pchan->b_bone_mats = NULL;
1155                 }
1156                 if(pchan->b_bone_dual_quats) {
1157                         MEM_freeN(pchan->b_bone_dual_quats);
1158                         pchan->b_bone_dual_quats = NULL;
1159                 }
1160
1161                 pchan->dual_quat = NULL;
1162         }
1163 }
1164
1165 /* ************ END Armature Deform ******************* */
1166
1167 void get_objectspace_bone_matrix (struct Bone* bone, float M_accumulatedMatrix[][4], int root, int posed)
1168 {
1169         Mat4CpyMat4(M_accumulatedMatrix, bone->arm_mat);
1170 }
1171
1172 /* **************** Space to Space API ****************** */
1173
1174 /* Convert World-Space Matrix to Pose-Space Matrix */
1175 void armature_mat_world_to_pose(Object *ob, float inmat[][4], float outmat[][4]) 
1176 {
1177         float obmat[4][4];
1178         
1179         /* prevent crashes */
1180         if (ob==NULL) return;
1181         
1182         /* get inverse of (armature) object's matrix  */
1183         Mat4Invert(obmat, ob->obmat);
1184         
1185         /* multiply given matrix by object's-inverse to find pose-space matrix */
1186         Mat4MulMat4(outmat, obmat, inmat);
1187 }
1188
1189 /* Convert Wolrd-Space Location to Pose-Space Location
1190  * NOTE: this cannot be used to convert to pose-space location of the supplied
1191  *              pose-channel into its local space (i.e. 'visual'-keyframing) 
1192  */
1193 void armature_loc_world_to_pose(Object *ob, float *inloc, float *outloc) 
1194 {
1195         float xLocMat[4][4];
1196         float nLocMat[4][4];
1197         
1198         /* build matrix for location */
1199         Mat4One(xLocMat);
1200         VECCOPY(xLocMat[3], inloc);
1201
1202         /* get bone-space cursor matrix and extract location */
1203         armature_mat_world_to_pose(ob, xLocMat, nLocMat);
1204         VECCOPY(outloc, nLocMat[3]);
1205 }
1206
1207 /* Convert Pose-Space Matrix to Bone-Space Matrix 
1208  * NOTE: this cannot be used to convert to pose-space transforms of the supplied
1209  *              pose-channel into its local space (i.e. 'visual'-keyframing)
1210  */
1211 void armature_mat_pose_to_bone(bPoseChannel *pchan, float inmat[][4], float outmat[][4])
1212 {
1213         float pc_trans[4][4], inv_trans[4][4];
1214         float pc_posemat[4][4], inv_posemat[4][4];
1215         
1216         /* paranoia: prevent crashes with no pose-channel supplied */
1217         if (pchan==NULL) return;
1218         
1219         /* get the inverse matrix of the pchan's transforms */
1220         if (pchan->rotmode)
1221                 LocEulSizeToMat4(pc_trans, pchan->loc, pchan->eul, pchan->size);
1222         else
1223                 LocQuatSizeToMat4(pc_trans, pchan->loc, pchan->quat, pchan->size);
1224         Mat4Invert(inv_trans, pc_trans);
1225         
1226         /* Remove the pchan's transforms from it's pose_mat.
1227          * This should leave behind the effects of restpose + 
1228          * parenting + constraints
1229          */
1230         Mat4MulMat4(pc_posemat, inv_trans, pchan->pose_mat);
1231         
1232         /* get the inverse of the leftovers so that we can remove 
1233          * that component from the supplied matrix
1234          */
1235         Mat4Invert(inv_posemat, pc_posemat);
1236         
1237         /* get the new matrix */
1238         Mat4MulMat4(outmat, inmat, inv_posemat);
1239 }
1240
1241 /* Convert Pose-Space Location to Bone-Space Location
1242  * NOTE: this cannot be used to convert to pose-space location of the supplied
1243  *              pose-channel into its local space (i.e. 'visual'-keyframing) 
1244  */
1245 void armature_loc_pose_to_bone(bPoseChannel *pchan, float *inloc, float *outloc) 
1246 {
1247         float xLocMat[4][4];
1248         float nLocMat[4][4];
1249         
1250         /* build matrix for location */
1251         Mat4One(xLocMat);
1252         VECCOPY(xLocMat[3], inloc);
1253
1254         /* get bone-space cursor matrix and extract location */
1255         armature_mat_pose_to_bone(pchan, xLocMat, nLocMat);
1256         VECCOPY(outloc, nLocMat[3]);
1257 }
1258
1259 /* Remove rest-position effects from pose-transform for obtaining
1260  * 'visual' transformation of pose-channel. 
1261  * (used by the Visual-Keyframing stuff)
1262  */
1263 void armature_mat_pose_to_delta(float delta_mat[][4], float pose_mat[][4], float arm_mat[][4])
1264 {
1265         float imat[4][4];
1266  
1267         Mat4Invert(imat, arm_mat);
1268         Mat4MulMat4(delta_mat, pose_mat, imat);
1269 }
1270
1271
1272 /* **************** The new & simple (but OK!) armature evaluation ********* */ 
1273
1274 /*  ****************** And how it works! ****************************************
1275
1276   This is the bone transformation trick; they're hierarchical so each bone(b)
1277   is in the coord system of bone(b-1):
1278
1279   arm_mat(b)= arm_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) 
1280   
1281   -> yoffs is just the y axis translation in parent's coord system
1282   -> d_root is the translation of the bone root, also in parent's coord system
1283
1284   pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b)
1285
1286   we then - in init deform - store the deform in chan_mat, such that:
1287
1288   pose_mat(b)= arm_mat(b) * chan_mat(b)
1289   
1290   *************************************************************************** */
1291 /*  Computes vector and roll based on a rotation. "mat" must
1292      contain only a rotation, and no scaling. */ 
1293 void mat3_to_vec_roll(float mat[][3], float *vec, float *roll) 
1294 {
1295     if (vec)
1296         VecCopyf(vec, mat[1]);
1297
1298     if (roll) {
1299         float vecmat[3][3], vecmatinv[3][3], rollmat[3][3];
1300
1301         vec_roll_to_mat3(mat[1], 0.0f, vecmat);
1302         Mat3Inv(vecmatinv, vecmat);
1303         Mat3MulMat3(rollmat, vecmatinv, mat);
1304
1305         *roll= atan2(rollmat[2][0], rollmat[2][2]);
1306     }
1307 }
1308
1309 /*      Calculates the rest matrix of a bone based
1310         On its vector and a roll around that vector */
1311 void vec_roll_to_mat3(float *vec, float roll, float mat[][3])
1312 {
1313         float   nor[3], axis[3], target[3]={0,1,0};
1314         float   theta;
1315         float   rMatrix[3][3], bMatrix[3][3];
1316         
1317         VECCOPY (nor, vec);
1318         Normalize (nor);
1319         
1320         /*      Find Axis & Amount for bone matrix*/
1321         Crossf (axis,target,nor);
1322
1323         if (Inpf(axis,axis) > 0.0000000000001) {
1324                 /* if nor is *not* a multiple of target ... */
1325                 Normalize (axis);
1326                 
1327                 theta= NormalizedVecAngle2(target, nor);
1328                 
1329                 /*      Make Bone matrix*/
1330                 VecRotToMat3(axis, theta, bMatrix);
1331         }
1332         else {
1333                 /* if nor is a multiple of target ... */
1334                 float updown;
1335                 
1336                 /* point same direction, or opposite? */
1337                 updown = ( Inpf (target,nor) > 0 ) ? 1.0 : -1.0;
1338                 
1339                 /* I think this should work ... */
1340                 bMatrix[0][0]=updown; bMatrix[0][1]=0.0;    bMatrix[0][2]=0.0;
1341                 bMatrix[1][0]=0.0;    bMatrix[1][1]=updown; bMatrix[1][2]=0.0;
1342                 bMatrix[2][0]=0.0;    bMatrix[2][1]=0.0;    bMatrix[2][2]=1.0;
1343         }
1344         
1345         /*      Make Roll matrix*/
1346         VecRotToMat3(nor, roll, rMatrix);
1347         
1348         /*      Combine and output result*/
1349         Mat3MulMat3 (mat, rMatrix, bMatrix);
1350 }
1351
1352
1353 /* recursive part, calculates restposition of entire tree of children */
1354 /* used by exiting editmode too */
1355 void where_is_armature_bone(Bone *bone, Bone *prevbone)
1356 {
1357         float vec[3];
1358         
1359         /* Bone Space */
1360         VecSubf (vec, bone->tail, bone->head);
1361         vec_roll_to_mat3(vec, bone->roll, bone->bone_mat);
1362
1363         bone->length= VecLenf(bone->head, bone->tail);
1364         
1365         /* this is called on old file reading too... */
1366         if(bone->xwidth==0.0) {
1367                 bone->xwidth= 0.1f;
1368                 bone->zwidth= 0.1f;
1369                 bone->segments= 1;
1370         }
1371         
1372         if(prevbone) {
1373                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
1374                 
1375                 /* bone transform itself */
1376                 Mat4CpyMat3(offs_bone, bone->bone_mat);
1377                                 
1378                 /* The bone's root offset (is in the parent's coordinate system) */
1379                 VECCOPY(offs_bone[3], bone->head);
1380
1381                 /* Get the length translation of parent (length along y axis) */
1382                 offs_bone[3][1]+= prevbone->length;
1383                 
1384                 /* Compose the matrix for this bone  */
1385                 Mat4MulMat4(bone->arm_mat, offs_bone, prevbone->arm_mat);
1386         }
1387         else {
1388                 Mat4CpyMat3(bone->arm_mat, bone->bone_mat);
1389                 VECCOPY(bone->arm_mat[3], bone->head);
1390         }
1391         
1392         /* head */
1393         VECCOPY(bone->arm_head, bone->arm_mat[3]);
1394         /* tail is in current local coord system */
1395         VECCOPY(vec, bone->arm_mat[1]);
1396         VecMulf(vec, bone->length);
1397         VecAddf(bone->arm_tail, bone->arm_head, vec);
1398         
1399         /* and the kiddies */
1400         prevbone= bone;
1401         for(bone= bone->childbase.first; bone; bone= bone->next) {
1402                 where_is_armature_bone(bone, prevbone);
1403         }
1404 }
1405
1406 /* updates vectors and matrices on rest-position level, only needed 
1407    after editing armature itself, now only on reading file */
1408 void where_is_armature (bArmature *arm)
1409 {
1410         Bone *bone;
1411         
1412         /* hierarchical from root to children */
1413         for(bone= arm->bonebase.first; bone; bone= bone->next) {
1414                 where_is_armature_bone(bone, NULL);
1415         }
1416 }
1417
1418 /* if bone layer is protected, copy the data from from->pose */
1419 static void pose_proxy_synchronize(Object *ob, Object *from, int layer_protected)
1420 {
1421         bPose *pose= ob->pose, *frompose= from->pose;
1422         bPoseChannel *pchan, *pchanp, pchanw;
1423         bConstraint *con;
1424         
1425         if (frompose==NULL) return;
1426         
1427         /* exception, armature local layer should be proxied too */
1428         if (pose->proxy_layer)
1429                 ((bArmature *)ob->data)->layer= pose->proxy_layer;
1430         
1431         /* clear all transformation values from library */
1432         rest_pose(frompose);
1433         
1434         /* copy over all of the proxy's bone groups */
1435                 /* TODO for later - implement 'local' bone groups as for constraints
1436                  *      Note: this isn't trivial, as bones reference groups by index not by pointer, 
1437                  *               so syncing things correctly needs careful attention
1438                  */
1439         BLI_freelistN(&pose->agroups);
1440         BLI_duplicatelist(&pose->agroups, &frompose->agroups);
1441         pose->active_group= frompose->active_group;
1442         
1443         for (pchan= pose->chanbase.first; pchan; pchan= pchan->next) {
1444                 if (pchan->bone->layer & layer_protected) {
1445                         ListBase proxylocal_constraints = {NULL, NULL};
1446                         pchanp= get_pose_channel(frompose, pchan->name);
1447                         
1448                         /* copy posechannel to temp, but restore important pointers */
1449                         pchanw= *pchanp;
1450                         pchanw.prev= pchan->prev;
1451                         pchanw.next= pchan->next;
1452                         pchanw.parent= pchan->parent;
1453                         pchanw.child= pchan->child;
1454                         pchanw.path= NULL;
1455                         
1456                         /* constraints - proxy constraints are flushed... local ones are added after 
1457                          *      1. extract constraints not from proxy (CONSTRAINT_PROXY_LOCAL) from pchan's constraints
1458                          *      2. copy proxy-pchan's constraints on-to new
1459                          *      3. add extracted local constraints back on top 
1460                          */
1461                         extract_proxylocal_constraints(&proxylocal_constraints, &pchan->constraints);
1462                         copy_constraints(&pchanw.constraints, &pchanp->constraints);
1463                         addlisttolist(&pchanw.constraints, &proxylocal_constraints);
1464                         
1465                         /* constraints - set target ob pointer to own object */
1466                         for (con= pchanw.constraints.first; con; con= con->next) {
1467                                 bConstraintTypeInfo *cti= constraint_get_typeinfo(con);
1468                                 ListBase targets = {NULL, NULL};
1469                                 bConstraintTarget *ct;
1470                                 
1471                                 if (cti && cti->get_constraint_targets) {
1472                                         cti->get_constraint_targets(con, &targets);
1473                                         
1474                                         for (ct= targets.first; ct; ct= ct->next) {
1475                                                 if (ct->tar == from)
1476                                                         ct->tar = ob;
1477                                         }
1478                                         
1479                                         if (cti->flush_constraint_targets)
1480                                                 cti->flush_constraint_targets(con, &targets, 0);
1481                                 }
1482                         }
1483                         
1484                         /* free stuff from current channel */
1485                         if (pchan->path) MEM_freeN(pchan->path);
1486                         free_constraints(&pchan->constraints);
1487                         
1488                         /* the final copy */
1489                         *pchan= pchanw;
1490                 }
1491         }
1492 }
1493
1494 static int rebuild_pose_bone(bPose *pose, Bone *bone, bPoseChannel *parchan, int counter)
1495 {
1496         bPoseChannel *pchan = verify_pose_channel (pose, bone->name);   // verify checks and/or adds
1497
1498         pchan->bone= bone;
1499         pchan->parent= parchan;
1500         
1501         counter++;
1502         
1503         for(bone= bone->childbase.first; bone; bone= bone->next) {
1504                 counter= rebuild_pose_bone(pose, bone, pchan, counter);
1505                 /* for quick detecting of next bone in chain, only b-bone uses it now */
1506                 if(bone->flag & BONE_CONNECTED)
1507                         pchan->child= get_pose_channel(pose, bone->name);
1508         }
1509         
1510         return counter;
1511 }
1512
1513 /* only after leave editmode, duplicating, validating older files, library syncing */
1514 /* NOTE: pose->flag is set for it */
1515 void armature_rebuild_pose(Object *ob, bArmature *arm)
1516 {
1517         Bone *bone;
1518         bPose *pose;
1519         bPoseChannel *pchan, *next;
1520         int counter=0;
1521                 
1522         /* only done here */
1523         if(ob->pose==NULL) ob->pose= MEM_callocN(sizeof(bPose), "new pose");
1524         pose= ob->pose;
1525         
1526         /* clear */
1527         for(pchan= pose->chanbase.first; pchan; pchan= pchan->next) {
1528                 pchan->bone= NULL;
1529                 pchan->child= NULL;
1530         }
1531         
1532         /* first step, check if all channels are there */
1533         for(bone= arm->bonebase.first; bone; bone= bone->next) {
1534                 counter= rebuild_pose_bone(pose, bone, NULL, counter);
1535         }
1536
1537         /* and a check for garbage */
1538         for(pchan= pose->chanbase.first; pchan; pchan= next) {
1539                 next= pchan->next;
1540                 if(pchan->bone==NULL) {
1541                         if(pchan->path)
1542                                 MEM_freeN(pchan->path);
1543                         free_constraints(&pchan->constraints);
1544                         BLI_freelinkN(&pose->chanbase, pchan);
1545                 }
1546         }
1547         // printf("rebuild pose %s, %d bones\n", ob->id.name, counter);
1548         
1549         /* synchronize protected layers with proxy */
1550         if(ob->proxy)
1551                 pose_proxy_synchronize(ob, ob->proxy, arm->layer_protected);
1552         
1553         update_pose_constraint_flags(ob->pose); // for IK detection for example
1554         
1555         /* the sorting */
1556         if(counter>1)
1557                 DAG_pose_sort(ob);
1558         
1559         ob->pose->flag &= ~POSE_RECALC;
1560 }
1561
1562
1563 /* ********************** THE IK SOLVER ******************* */
1564
1565
1566
1567 /* allocates PoseTree, and links that to root bone/channel */
1568 /* Note: detecting the IK chain is duplicate code... in drawarmature.c and in transform_conversions.c */
1569 static void initialize_posetree(struct Object *ob, bPoseChannel *pchan_tip)
1570 {
1571         bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
1572         PoseTree *tree;
1573         PoseTarget *target;
1574         bConstraint *con;
1575         bKinematicConstraint *data;
1576         int a, segcount= 0, size, newsize, *oldparent, parent;
1577         
1578         /* find IK constraint, and validate it */
1579         for(con= pchan_tip->constraints.first; con; con= con->next) {
1580                 if(con->type==CONSTRAINT_TYPE_KINEMATIC) {
1581                         data=(bKinematicConstraint*)con->data;
1582                         if (data->flag & CONSTRAINT_IK_AUTO) break;
1583                         if (data->tar==NULL) continue;
1584                         if (data->tar->type==OB_ARMATURE && data->subtarget[0]==0) continue;
1585                         if ((con->flag & CONSTRAINT_DISABLE)==0 && (con->enforce!=0.0)) break;
1586                 }
1587         }
1588         if(con==NULL) return;
1589         
1590         /* exclude tip from chain? */
1591         if(!(data->flag & CONSTRAINT_IK_TIP))
1592                 pchan_tip= pchan_tip->parent;
1593         
1594         /* Find the chain's root & count the segments needed */
1595         for (curchan = pchan_tip; curchan; curchan=curchan->parent){
1596                 pchan_root = curchan;
1597                 
1598                 curchan->flag |= POSE_CHAIN;    // don't forget to clear this
1599                 chanlist[segcount]=curchan;
1600                 segcount++;
1601                 
1602                 if(segcount==data->rootbone || segcount>255) break; // 255 is weak
1603         }
1604         if (!segcount) return;
1605
1606         /* setup the chain data */
1607         
1608         /* we make tree-IK, unless all existing targets are in this chain */
1609         for(tree= pchan_root->iktree.first; tree; tree= tree->next) {
1610                 for(target= tree->targets.first; target; target= target->next) {
1611                         curchan= tree->pchan[target->tip];
1612                         if(curchan->flag & POSE_CHAIN)
1613                                 curchan->flag &= ~POSE_CHAIN;
1614                         else
1615                                 break;
1616                 }
1617                 if(target) break;
1618         }
1619
1620         /* create a target */
1621         target= MEM_callocN(sizeof(PoseTarget), "posetarget");
1622         target->con= con;
1623         pchan_tip->flag &= ~POSE_CHAIN;
1624
1625         if(tree==NULL) {
1626                 /* make new tree */
1627                 tree= MEM_callocN(sizeof(PoseTree), "posetree");
1628
1629                 tree->iterations= data->iterations;
1630                 tree->totchannel= segcount;
1631                 tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
1632                 
1633                 tree->pchan= MEM_callocN(segcount*sizeof(void*), "ik tree pchan");
1634                 tree->parent= MEM_callocN(segcount*sizeof(int), "ik tree parent");
1635                 for(a=0; a<segcount; a++) {
1636                         tree->pchan[a]= chanlist[segcount-a-1];
1637                         tree->parent[a]= a-1;
1638                 }
1639                 target->tip= segcount-1;
1640                 
1641                 /* AND! link the tree to the root */
1642                 BLI_addtail(&pchan_root->iktree, tree);
1643         }
1644         else {
1645                 tree->iterations= MAX2(data->iterations, tree->iterations);
1646                 tree->stretch= tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
1647
1648                 /* skip common pose channels and add remaining*/
1649                 size= MIN2(segcount, tree->totchannel);
1650                 for(a=0; a<size && tree->pchan[a]==chanlist[segcount-a-1]; a++);
1651                 parent= a-1;
1652
1653                 segcount= segcount-a;
1654                 target->tip= tree->totchannel + segcount - 1;
1655
1656                 if (segcount > 0) {
1657                         /* resize array */
1658                         newsize= tree->totchannel + segcount;
1659                         oldchan= tree->pchan;
1660                         oldparent= tree->parent;
1661
1662                         tree->pchan= MEM_callocN(newsize*sizeof(void*), "ik tree pchan");
1663                         tree->parent= MEM_callocN(newsize*sizeof(int), "ik tree parent");
1664                         memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel);
1665                         memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel);
1666                         MEM_freeN(oldchan);
1667                         MEM_freeN(oldparent);
1668
1669                         /* add new pose channels at the end, in reverse order */
1670                         for(a=0; a<segcount; a++) {
1671                                 tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1];
1672                                 tree->parent[tree->totchannel+a]= tree->totchannel+a-1;
1673                         }
1674                         tree->parent[tree->totchannel]= parent;
1675                         
1676                         tree->totchannel= newsize;
1677                 }
1678
1679                 /* move tree to end of list, for correct evaluation order */
1680                 BLI_remlink(&pchan_root->iktree, tree);
1681                 BLI_addtail(&pchan_root->iktree, tree);
1682         }
1683
1684         /* add target to the tree */
1685         BLI_addtail(&tree->targets, target);
1686 }
1687
1688 /* called from within the core where_is_pose loop, all animsystems and constraints
1689 were executed & assigned. Now as last we do an IK pass */
1690 static void execute_posetree(Object *ob, PoseTree *tree)
1691 {
1692         float R_parmat[3][3], identity[3][3];
1693         float iR_parmat[3][3];
1694         float R_bonemat[3][3];
1695         float goalrot[3][3], goalpos[3];
1696         float rootmat[4][4], imat[4][4];
1697         float goal[4][4], goalinv[4][4];
1698         float irest_basis[3][3], full_basis[3][3];
1699         float end_pose[4][4], world_pose[4][4];
1700         float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch=NULL;
1701         float resultinf=0.0f;
1702         int a, flag, hasstretch=0, resultblend=0;
1703         bPoseChannel *pchan;
1704         IK_Segment *seg, *parent, **iktree, *iktarget;
1705         IK_Solver *solver;
1706         PoseTarget *target;
1707         bKinematicConstraint *data, *poleangledata=NULL;
1708         Bone *bone;
1709
1710         if (tree->totchannel == 0)
1711                 return;
1712         
1713         iktree= MEM_mallocN(sizeof(void*)*tree->totchannel, "ik tree");
1714
1715         for(a=0; a<tree->totchannel; a++) {
1716                 pchan= tree->pchan[a];
1717                 bone= pchan->bone;
1718                 
1719                 /* set DoF flag */
1720                 flag= 0;
1721                 if(!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP))
1722                         flag |= IK_XDOF;
1723                 if(!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP))
1724                         flag |= IK_YDOF;
1725                 if(!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP))
1726                         flag |= IK_ZDOF;
1727                 
1728                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
1729                         flag |= IK_TRANS_YDOF;
1730                         hasstretch = 1;
1731                 }
1732                 
1733                 seg= iktree[a]= IK_CreateSegment(flag);
1734                 
1735                 /* find parent */
1736                 if(a == 0)
1737                         parent= NULL;
1738                 else
1739                         parent= iktree[tree->parent[a]];
1740                         
1741                 IK_SetParent(seg, parent);
1742                         
1743                 /* get the matrix that transforms from prevbone into this bone */
1744                 Mat3CpyMat4(R_bonemat, pchan->pose_mat);
1745                 
1746                 /* gather transformations for this IK segment */
1747                 
1748                 if (pchan->parent)
1749                         Mat3CpyMat4(R_parmat, pchan->parent->pose_mat);
1750                 else
1751                         Mat3One(R_parmat);
1752                 
1753                 /* bone offset */
1754                 if (pchan->parent && (a > 0))
1755                         VecSubf(start, pchan->pose_head, pchan->parent->pose_tail);
1756                 else
1757                         /* only root bone (a = 0) has no parent */
1758                         start[0]= start[1]= start[2]= 0.0f;
1759                 
1760                 /* change length based on bone size */
1761                 length= bone->length*VecLength(R_bonemat[1]);
1762                 
1763                 /* compute rest basis and its inverse */
1764                 Mat3CpyMat3(rest_basis, bone->bone_mat);
1765                 Mat3CpyMat3(irest_basis, bone->bone_mat);
1766                 Mat3Transp(irest_basis);
1767                 
1768                 /* compute basis with rest_basis removed */
1769                 Mat3Inv(iR_parmat, R_parmat);
1770                 Mat3MulMat3(full_basis, iR_parmat, R_bonemat);
1771                 Mat3MulMat3(basis, irest_basis, full_basis);
1772                 
1773                 /* basis must be pure rotation */
1774                 Mat3Ortho(basis);
1775                 
1776                 /* transform offset into local bone space */
1777                 Mat3Ortho(iR_parmat);
1778                 Mat3MulVecfl(iR_parmat, start);
1779                 
1780                 IK_SetTransform(seg, start, rest_basis, basis, length);
1781                 
1782                 if (pchan->ikflag & BONE_IK_XLIMIT)
1783                         IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]);
1784                 if (pchan->ikflag & BONE_IK_YLIMIT)
1785                         IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]);
1786                 if (pchan->ikflag & BONE_IK_ZLIMIT)
1787                         IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]);
1788                 
1789                 IK_SetStiffness(seg, IK_X, pchan->stiffness[0]);
1790                 IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]);
1791                 IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]);
1792                 
1793                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
1794                         float ikstretch = pchan->ikstretch*pchan->ikstretch;
1795                         IK_SetStiffness(seg, IK_TRANS_Y, MIN2(1.0-ikstretch, 0.99));
1796                         IK_SetLimit(seg, IK_TRANS_Y, 0.001, 1e10);
1797                 }
1798         }
1799
1800         solver= IK_CreateSolver(iktree[0]);
1801
1802         /* set solver goals */
1803
1804         /* first set the goal inverse transform, assuming the root of tree was done ok! */
1805         pchan= tree->pchan[0];
1806         if (pchan->parent)
1807                 /* transform goal by parent mat, so this rotation is not part of the
1808                    segment's basis. otherwise rotation limits do not work on the
1809                    local transform of the segment itself. */
1810                 Mat4CpyMat4(rootmat, pchan->parent->pose_mat);
1811         else
1812                 Mat4One(rootmat);
1813         VECCOPY(rootmat[3], pchan->pose_head);
1814         
1815         Mat4MulMat4 (imat, rootmat, ob->obmat);
1816         Mat4Invert (goalinv, imat);
1817         
1818         for (target=tree->targets.first; target; target=target->next) {
1819                 float polepos[3];
1820                 int poleconstrain= 0;
1821                 
1822                 data= (bKinematicConstraint*)target->con->data;
1823                 
1824                 /* 1.0=ctime, we pass on object for auto-ik (owner-type here is object, even though
1825                  * strictly speaking, it is a posechannel)
1826                  */
1827                 get_constraint_target_matrix(target->con, 0, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
1828                 
1829                 /* and set and transform goal */
1830                 Mat4MulMat4(goal, rootmat, goalinv);
1831                 
1832                 VECCOPY(goalpos, goal[3]);
1833                 Mat3CpyMat4(goalrot, goal);
1834                 
1835                 /* same for pole vector target */
1836                 if(data->poletar) {
1837                         get_constraint_target_matrix(target->con, 1, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
1838                         
1839                         if(data->flag & CONSTRAINT_IK_SETANGLE) {
1840                                 /* don't solve IK when we are setting the pole angle */
1841                                 break;
1842                         }
1843                         else {
1844                                 Mat4MulMat4(goal, rootmat, goalinv);
1845                                 VECCOPY(polepos, goal[3]);
1846                                 poleconstrain= 1;
1847
1848                                 /* for pole targets, we blend the result of the ik solver
1849                                  * instead of the target position, otherwise we can't get
1850                                  * a smooth transition */
1851                                 resultblend= 1;
1852                                 resultinf= target->con->enforce;
1853                                 
1854                                 if(data->flag & CONSTRAINT_IK_GETANGLE) {
1855                                         poleangledata= data;
1856                                         data->flag &= ~CONSTRAINT_IK_GETANGLE;
1857                                 }
1858                         }
1859                 }
1860
1861                 /* do we need blending? */
1862                 if (!resultblend && target->con->enforce!=1.0) {
1863                         float q1[4], q2[4], q[4];
1864                         float fac= target->con->enforce;
1865                         float mfac= 1.0-fac;
1866                         
1867                         pchan= tree->pchan[target->tip];
1868                         
1869                         /* end effector in world space */
1870                         Mat4CpyMat4(end_pose, pchan->pose_mat);
1871                         VECCOPY(end_pose[3], pchan->pose_tail);
1872                         Mat4MulSerie(world_pose, goalinv, ob->obmat, end_pose, 0, 0, 0, 0, 0);
1873                         
1874                         /* blend position */
1875                         goalpos[0]= fac*goalpos[0] + mfac*world_pose[3][0];
1876                         goalpos[1]= fac*goalpos[1] + mfac*world_pose[3][1];
1877                         goalpos[2]= fac*goalpos[2] + mfac*world_pose[3][2];
1878                         
1879                         /* blend rotation */
1880                         Mat3ToQuat(goalrot, q1);
1881                         Mat4ToQuat(world_pose, q2);
1882                         QuatInterpol(q, q1, q2, mfac);
1883                         QuatToMat3(q, goalrot);
1884                 }
1885                 
1886                 iktarget= iktree[target->tip];
1887                 
1888                 if(data->weight != 0.0) {
1889                         if(poleconstrain)
1890                                 IK_SolverSetPoleVectorConstraint(solver, iktarget, goalpos,
1891                                         polepos, data->poleangle*M_PI/180, (poleangledata == data));
1892                         IK_SolverAddGoal(solver, iktarget, goalpos, data->weight);
1893                 }
1894                 if((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0))
1895                         if((data->flag & CONSTRAINT_IK_AUTO)==0)
1896                                 IK_SolverAddGoalOrientation(solver, iktarget, goalrot,
1897                                         data->orientweight);
1898         }
1899
1900         /* solve */
1901         IK_Solve(solver, 0.0f, tree->iterations);
1902
1903         if(poleangledata)
1904                 poleangledata->poleangle= IK_SolverGetPoleAngle(solver)*180/M_PI;
1905
1906         IK_FreeSolver(solver);
1907
1908         /* gather basis changes */
1909         tree->basis_change= MEM_mallocN(sizeof(float[3][3])*tree->totchannel, "ik basis change");
1910         if(hasstretch)
1911                 ikstretch= MEM_mallocN(sizeof(float)*tree->totchannel, "ik stretch");
1912         
1913         for(a=0; a<tree->totchannel; a++) {
1914                 IK_GetBasisChange(iktree[a], tree->basis_change[a]);
1915                 
1916                 if(hasstretch) {
1917                         /* have to compensate for scaling received from parent */
1918                         float parentstretch, stretch;
1919                         
1920                         pchan= tree->pchan[a];
1921                         parentstretch= (tree->parent[a] >= 0)? ikstretch[tree->parent[a]]: 1.0;
1922                         
1923                         if(tree->stretch && (pchan->ikstretch > 0.0)) {
1924                                 float trans[3], length;
1925                                 
1926                                 IK_GetTranslationChange(iktree[a], trans);
1927                                 length= pchan->bone->length*VecLength(pchan->pose_mat[1]);
1928                                 
1929                                 ikstretch[a]= (length == 0.0)? 1.0: (trans[1]+length)/length;
1930                         }
1931                         else
1932                                 ikstretch[a] = 1.0;
1933                         
1934                         stretch= (parentstretch == 0.0)? 1.0: ikstretch[a]/parentstretch;
1935                         
1936                         VecMulf(tree->basis_change[a][0], stretch);
1937                         VecMulf(tree->basis_change[a][1], stretch);
1938                         VecMulf(tree->basis_change[a][2], stretch);
1939                 }
1940
1941                 if(resultblend && resultinf!=1.0f) {
1942                         Mat3One(identity);
1943                         Mat3BlendMat3(tree->basis_change[a], identity,
1944                                 tree->basis_change[a], resultinf);
1945                 }
1946                 
1947                 IK_FreeSegment(iktree[a]);
1948         }
1949         
1950         MEM_freeN(iktree);
1951         if(ikstretch) MEM_freeN(ikstretch);
1952 }
1953
1954 void free_posetree(PoseTree *tree)
1955 {
1956         BLI_freelistN(&tree->targets);
1957         if(tree->pchan) MEM_freeN(tree->pchan);
1958         if(tree->parent) MEM_freeN(tree->parent);
1959         if(tree->basis_change) MEM_freeN(tree->basis_change);
1960         MEM_freeN(tree);
1961 }
1962
1963 /* ********************** THE POSE SOLVER ******************* */
1964
1965
1966 /* loc/rot/size to mat4 */
1967 /* used in constraint.c too */
1968 void chan_calc_mat(bPoseChannel *chan)
1969 {
1970         float smat[3][3];
1971         float rmat[3][3];
1972         float tmat[3][3];
1973         
1974         /* get scaling matrix */
1975         SizeToMat3(chan->size, smat);
1976         
1977         /* rotations may either be quats or eulers (no rotation modes for now...) */
1978         if (chan->rotmode) {
1979                 /* euler rotations (will cause gimble lock... no rotation order to solve that yet) */
1980                 EulToMat3(chan->eul, rmat);
1981         }
1982         else {
1983                 /* quats are normalised before use to eliminate scaling issues */
1984                 NormalQuat(chan->quat);
1985                 QuatToMat3(chan->quat, rmat);
1986         }
1987         
1988         /* calculate matrix of bone (as 3x3 matrix, but then copy the 4x4) */
1989         Mat3MulMat3(tmat, rmat, smat);
1990         Mat4CpyMat3(chan->chan_mat, tmat);
1991         
1992         /* prevent action channels breaking chains */
1993         /* need to check for bone here, CONSTRAINT_TYPE_ACTION uses this call */
1994         if ((chan->bone==NULL) || !(chan->bone->flag & BONE_CONNECTED)) {
1995                 VECCOPY(chan->chan_mat[3], chan->loc);
1996         }
1997 }
1998
1999 /* transform from bone(b) to bone(b+1), store in chan_mat */
2000 static void make_dmats(bPoseChannel *pchan)
2001 {
2002         if (pchan->parent) {
2003                 float iR_parmat[4][4];
2004                 Mat4Invert(iR_parmat, pchan->parent->pose_mat);
2005                 Mat4MulMat4(pchan->chan_mat,  pchan->pose_mat, iR_parmat);      // delta mat
2006         }
2007         else Mat4CpyMat4(pchan->chan_mat, pchan->pose_mat);
2008 }
2009
2010 /* applies IK matrix to pchan, IK is done separated */
2011 /* formula: pose_mat(b) = pose_mat(b-1) * diffmat(b-1, b) * ik_mat(b) */
2012 /* to make this work, the diffmats have to be precalculated! Stored in chan_mat */
2013 static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[][3])   // nr = to detect if this is first bone
2014 {
2015         float vec[3], ikmat[4][4];
2016         
2017         Mat4CpyMat3(ikmat, ik_mat);
2018         
2019         if (pchan->parent)
2020                 Mat4MulSerie(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat, ikmat, NULL, NULL, NULL, NULL, NULL);
2021         else 
2022                 Mat4MulMat4(pchan->pose_mat, ikmat, pchan->chan_mat);
2023
2024         /* calculate head */
2025         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
2026         /* calculate tail */
2027         VECCOPY(vec, pchan->pose_mat[1]);
2028         VecMulf(vec, pchan->bone->length);
2029         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
2030
2031         pchan->flag |= POSE_DONE;
2032 }
2033
2034 /* NLA strip modifiers */
2035 static void do_strip_modifiers(Scene *scene, Object *armob, Bone *bone, bPoseChannel *pchan)
2036 {
2037         bActionModifier *amod;
2038         bActionStrip *strip, *strip2;
2039         float scene_cfra= scene->r.cfra;
2040         int do_modif;
2041
2042         for (strip=armob->nlastrips.first; strip; strip=strip->next) {
2043                 do_modif=0;
2044                 
2045                 if (scene_cfra>=strip->start && scene_cfra<=strip->end)
2046                         do_modif=1;
2047                 
2048                 if ((scene_cfra > strip->end) && (strip->flag & ACTSTRIP_HOLDLASTFRAME)) {
2049                         do_modif=1;
2050                         
2051                         /* if there are any other strips active, ignore modifiers for this strip - 
2052                          * 'hold' option should only hold action modifiers if there are 
2053                          * no other active strips */
2054                         for (strip2=strip->next; strip2; strip2=strip2->next) {
2055                                 if (strip2 == strip) continue;
2056                                 
2057                                 if (scene_cfra>=strip2->start && scene_cfra<=strip2->end) {
2058                                         if (!(strip2->flag & ACTSTRIP_MUTE))
2059                                                 do_modif=0;
2060                                 }
2061                         }
2062                         
2063                         /* if there are any later, activated, strips with 'hold' set, they take precedence, 
2064                          * so ignore modifiers for this strip */
2065                         for (strip2=strip->next; strip2; strip2=strip2->next) {
2066                                 if (scene_cfra < strip2->start) continue;
2067                                 if ((strip2->flag & ACTSTRIP_HOLDLASTFRAME) && !(strip2->flag & ACTSTRIP_MUTE)) {
2068                                         do_modif=0;
2069                                 }
2070                         }
2071                 }
2072                 
2073                 if (do_modif) {
2074                         /* temporal solution to prevent 2 strips accumulating */
2075                         if(scene_cfra==strip->end && strip->next && strip->next->start==scene_cfra)
2076                                 continue;
2077                         
2078                         for(amod= strip->modifiers.first; amod; amod= amod->next) {
2079                                 switch (amod->type) {
2080                                 case ACTSTRIP_MOD_DEFORM:
2081                                 {
2082                                         /* validate first */
2083                                         if(amod->ob && amod->ob->type==OB_CURVE && amod->channel[0]) {
2084                                                 
2085                                                 if( strcmp(pchan->name, amod->channel)==0 ) {
2086                                                         float mat4[4][4], mat3[3][3];
2087                                                         
2088                                                         curve_deform_vector(scene, amod->ob, armob, bone->arm_mat[3], pchan->pose_mat[3], mat3, amod->no_rot_axis);
2089                                                         Mat4CpyMat4(mat4, pchan->pose_mat);
2090                                                         Mat4MulMat34(pchan->pose_mat, mat3, mat4);
2091                                                         
2092                                                 }
2093                                         }
2094                                 }
2095                                         break;
2096                                 case ACTSTRIP_MOD_NOISE:        
2097                                 {
2098                                         if( strcmp(pchan->name, amod->channel)==0 ) {
2099                                                 float nor[3], loc[3], ofs;
2100                                                 float eul[3], size[3], eulo[3], sizeo[3];
2101                                                 
2102                                                 /* calculate turbulance */
2103                                                 ofs = amod->turbul / 200.0f;
2104                                                 
2105                                                 /* make a copy of starting conditions */
2106                                                 VECCOPY(loc, pchan->pose_mat[3]);
2107                                                 Mat4ToEul(pchan->pose_mat, eul);
2108                                                 Mat4ToSize(pchan->pose_mat, size);
2109                                                 VECCOPY(eulo, eul);
2110                                                 VECCOPY(sizeo, size);
2111                                                 
2112                                                 /* apply noise to each set of channels */
2113                                                 if (amod->channels & 4) {
2114                                                         /* for scaling */
2115                                                         nor[0] = BLI_gNoise(amod->noisesize, size[0]+ofs, size[1], size[2], 0, 0) - ofs;
2116                                                         nor[1] = BLI_gNoise(amod->noisesize, size[0], size[1]+ofs, size[2], 0, 0) - ofs;        
2117                                                         nor[2] = BLI_gNoise(amod->noisesize, size[0], size[1], size[2]+ofs, 0, 0) - ofs;
2118                                                         VecAddf(size, size, nor);
2119                                                         
2120                                                         if (sizeo[0] != 0)
2121                                                                 VecMulf(pchan->pose_mat[0], size[0] / sizeo[0]);
2122                                                         if (sizeo[1] != 0)
2123                                                                 VecMulf(pchan->pose_mat[1], size[1] / sizeo[1]);
2124                                                         if (sizeo[2] != 0)
2125                                                                 VecMulf(pchan->pose_mat[2], size[2] / sizeo[2]);
2126                                                 }
2127                                                 if (amod->channels & 2) {
2128                                                         /* for rotation */
2129                                                         nor[0] = BLI_gNoise(amod->noisesize, eul[0]+ofs, eul[1], eul[2], 0, 0) - ofs;
2130                                                         nor[1] = BLI_gNoise(amod->noisesize, eul[0], eul[1]+ofs, eul[2], 0, 0) - ofs;   
2131                                                         nor[2] = BLI_gNoise(amod->noisesize, eul[0], eul[1], eul[2]+ofs, 0, 0) - ofs;
2132                                                         
2133                                                         compatible_eul(nor, eulo);
2134                                                         VecAddf(eul, eul, nor);
2135                                                         compatible_eul(eul, eulo);
2136                                                         
2137                                                         LocEulSizeToMat4(pchan->pose_mat, loc, eul, size);
2138                                                 }
2139                                                 if (amod->channels & 1) {
2140                                                         /* for location */
2141                                                         nor[0] = BLI_gNoise(amod->noisesize, loc[0]+ofs, loc[1], loc[2], 0, 0) - ofs;
2142                                                         nor[1] = BLI_gNoise(amod->noisesize, loc[0], loc[1]+ofs, loc[2], 0, 0) - ofs;   
2143                                                         nor[2] = BLI_gNoise(amod->noisesize, loc[0], loc[1], loc[2]+ofs, 0, 0) - ofs;
2144                                                         
2145                                                         VecAddf(pchan->pose_mat[3], loc, nor);
2146                                                 }
2147                                         }
2148                                 }
2149                                         break;
2150                                 }
2151                         }
2152                 }
2153         }
2154 }
2155
2156
2157 /* The main armature solver, does all constraints excluding IK */
2158 /* pchan is validated, as having bone and parent pointer */
2159 static void where_is_pose_bone(Scene *scene, Object *ob, bPoseChannel *pchan, float ctime)
2160 {
2161         Bone *bone, *parbone;
2162         bPoseChannel *parchan;
2163         float vec[3];
2164         
2165         /* set up variables for quicker access below */
2166         bone= pchan->bone;
2167         parbone= bone->parent;
2168         parchan= pchan->parent;
2169         
2170         /* this gives a chan_mat with actions (ipos) results */
2171         chan_calc_mat(pchan);
2172         
2173         /* construct the posemat based on PoseChannels, that we do before applying constraints */
2174         /* pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b) */
2175         
2176         if(parchan) {
2177                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
2178                 
2179                 /* bone transform itself */
2180                 Mat4CpyMat3(offs_bone, bone->bone_mat);
2181                 
2182                 /* The bone's root offset (is in the parent's coordinate system) */
2183                 VECCOPY(offs_bone[3], bone->head);
2184                 
2185                 /* Get the length translation of parent (length along y axis) */
2186                 offs_bone[3][1]+= parbone->length;
2187                 
2188                 /* Compose the matrix for this bone  */
2189                 if(bone->flag & BONE_HINGE) {   // uses restposition rotation, but actual position
2190                         float tmat[4][4];
2191                         
2192                         /* the rotation of the parent restposition */
2193                         Mat4CpyMat4(tmat, parbone->arm_mat);
2194                         
2195                         /* the location of actual parent transform */
2196                         VECCOPY(tmat[3], offs_bone[3]);
2197                         offs_bone[3][0]= offs_bone[3][1]= offs_bone[3][2]= 0.0f;
2198                         Mat4MulVecfl(parchan->pose_mat, tmat[3]);
2199                         
2200                         Mat4MulSerie(pchan->pose_mat, tmat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
2201                 }
2202                 else if(bone->flag & BONE_NO_SCALE) {
2203                         float orthmat[4][4], vec[3];
2204                         
2205                         /* get the official transform, but we only use the vector from it (optimize...) */
2206                         Mat4MulSerie(pchan->pose_mat, parchan->pose_mat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
2207                         VECCOPY(vec, pchan->pose_mat[3]);
2208                         
2209                         /* do this again, but with an ortho-parent matrix */
2210                         Mat4CpyMat4(orthmat, parchan->pose_mat);
2211                         Mat4Ortho(orthmat);
2212                         Mat4MulSerie(pchan->pose_mat, orthmat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
2213                         
2214                         /* copy correct transform */
2215                         VECCOPY(pchan->pose_mat[3], vec);
2216                 }
2217                 else 
2218                         Mat4MulSerie(pchan->pose_mat, parchan->pose_mat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
2219         }
2220         else {
2221                 Mat4MulMat4(pchan->pose_mat, pchan->chan_mat, bone->arm_mat);
2222                 
2223                 /* only rootbones get the cyclic offset (unless user doesn't want that) */
2224                 if ((bone->flag & BONE_NO_CYCLICOFFSET) == 0)
2225                         VecAddf(pchan->pose_mat[3], pchan->pose_mat[3], ob->pose->cyclic_offset);
2226         }
2227         
2228         /* do NLA strip modifiers - i.e. curve follow */
2229         do_strip_modifiers(scene, ob, bone, pchan);
2230         
2231         /* Do constraints */
2232         if (pchan->constraints.first) {
2233                 bConstraintOb *cob;
2234                 
2235                 /* local constraints */
2236                 do_constraint_channels(&pchan->constraints, NULL, ctime, 0);
2237                 
2238                 /* make a copy of location of PoseChannel for later */
2239                 VECCOPY(vec, pchan->pose_mat[3]);
2240                 
2241                 /* prepare PoseChannel for Constraint solving 
2242                  * - makes a copy of matrix, and creates temporary struct to use 
2243                  */
2244                 cob= constraints_make_evalob(scene, ob, pchan, CONSTRAINT_OBTYPE_BONE);
2245                 
2246                 /* Solve PoseChannel's Constraints */
2247                 solve_constraints(&pchan->constraints, cob, ctime);     // ctime doesnt alter objects
2248                 
2249                 /* cleanup after Constraint Solving 
2250                  * - applies matrix back to pchan, and frees temporary struct used
2251                  */
2252                 constraints_clear_evalob(cob);
2253                 
2254                 /* prevent constraints breaking a chain */
2255                 if(pchan->bone->flag & BONE_CONNECTED) {
2256                         VECCOPY(pchan->pose_mat[3], vec);
2257                 }
2258         }
2259         
2260         /* calculate head */
2261         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
2262         /* calculate tail */
2263         VECCOPY(vec, pchan->pose_mat[1]);
2264         VecMulf(vec, bone->length);
2265         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
2266 }
2267
2268 /* This only reads anim data from channels, and writes to channels */
2269 /* This is the only function adding poses */
2270 void where_is_pose (Scene *scene, Object *ob)
2271 {
2272         bArmature *arm;
2273         Bone *bone;
2274         bPoseChannel *pchan;
2275         float imat[4][4];
2276         float ctime;
2277         
2278         arm = get_armature(ob);
2279         
2280         if(ELEM(NULL, arm, scene)) return;
2281         if((ob->pose==NULL) || (ob->pose->flag & POSE_RECALC)) 
2282            armature_rebuild_pose(ob, arm);
2283            
2284         ctime= bsystem_time(scene, ob, (float)scene->r.cfra, 0.0);      /* not accurate... */
2285         
2286         /* In editmode or restposition we read the data from the bones */
2287         if(arm->edbo || (arm->flag & ARM_RESTPOS)) {
2288                 
2289                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2290                         bone= pchan->bone;
2291                         if(bone) {
2292                                 Mat4CpyMat4(pchan->pose_mat, bone->arm_mat);
2293                                 VECCOPY(pchan->pose_head, bone->arm_head);
2294                                 VECCOPY(pchan->pose_tail, bone->arm_tail);
2295                         }
2296                 }
2297         }
2298         else {
2299                 Mat4Invert(ob->imat, ob->obmat);        // imat is needed 
2300
2301                 /* 1. construct the PoseTrees, clear flags */
2302                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2303                         pchan->flag &= ~(POSE_DONE|POSE_CHAIN);
2304                         if(pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints
2305                                 initialize_posetree(ob, pchan); // will attach it to root!
2306                 }
2307                 
2308                 /* 2. the main loop, channels are already hierarchical sorted from root to children */
2309                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2310                         
2311                         /* 3. if we find an IK root, we handle it separated */
2312                         if(pchan->iktree.first) {
2313                                 while(pchan->iktree.first) {
2314                                         PoseTree *tree= pchan->iktree.first;
2315                                         int a;
2316                                         
2317                                         /* 4. walk over the tree for regular solving */
2318                                         for(a=0; a<tree->totchannel; a++) {
2319                                                 if(!(tree->pchan[a]->flag & POSE_DONE)) // successive trees can set the flag
2320                                                         where_is_pose_bone(scene, ob, tree->pchan[a], ctime);
2321                                         }
2322                                         /* 5. execute the IK solver */
2323                                         execute_posetree(ob, tree);
2324                                         
2325                                         /* 6. apply the differences to the channels, 
2326                                                   we need to calculate the original differences first */
2327                                         for(a=0; a<tree->totchannel; a++)
2328                                                 make_dmats(tree->pchan[a]);
2329                                         
2330                                         for(a=0; a<tree->totchannel; a++)
2331                                                 /* sets POSE_DONE */
2332                                                 where_is_ik_bone(tree->pchan[a], tree->basis_change[a]);
2333                                         
2334                                         /* 7. and free */
2335                                         BLI_remlink(&pchan->iktree, tree);
2336                                         free_posetree(tree);
2337                                 }
2338                         }
2339                         else if(!(pchan->flag & POSE_DONE)) {
2340                                 where_is_pose_bone(scene, ob, pchan, ctime);
2341                         }
2342                 }
2343         }
2344                 
2345         /* calculating deform matrices */
2346         for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
2347                 if(pchan->bone) {
2348                         Mat4Invert(imat, pchan->bone->arm_mat);
2349                         Mat4MulMat4(pchan->chan_mat, imat, pchan->pose_mat);
2350                 }
2351         }
2352 }