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