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