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