Integration of the Google Summer of Code Modifier Stack Upgrade project. The
[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_mesh_types.h"
41 #include "DNA_meshdata_types.h"
42 #include "DNA_armature_types.h"
43 #include "DNA_action_types.h"
44 #include "DNA_object_types.h"
45 #include "DNA_scene_types.h"
46 #include "DNA_view3d_types.h"
47 #include "DNA_constraint_types.h"
48
49 #include "BKE_curve.h"
50 #include "BKE_depsgraph.h"
51 #include "BKE_DerivedMesh.h"
52 #include "BKE_displist.h"
53 #include "BKE_global.h"
54 #include "BKE_main.h"
55 #include "BKE_library.h"
56 #include "BKE_blender.h"
57 #include "BKE_armature.h"
58 #include "BKE_action.h"
59 #include "BKE_constraint.h"
60 #include "BKE_object.h"
61 #include "BKE_object.h"
62 #include "BKE_deform.h"
63 #include "BKE_utildefines.h"
64
65 #include "BIF_editdeform.h"
66
67 #include "IK_solver.h"
68
69 #ifdef HAVE_CONFIG_H
70 #include <config.h>
71 #endif
72
73 /*      **************** Generic Functions, data level *************** */
74
75 bArmature *get_armature(Object *ob)
76 {
77         if(ob==NULL) return NULL;
78         if(ob->type==OB_ARMATURE) return ob->data;
79         else return NULL;
80 }
81
82 bArmature *add_armature()
83 {
84         bArmature *arm;
85         
86         arm= alloc_libblock (&G.main->armature, ID_AR, "Armature");
87         arm->deformflag = ARM_DEF_VGROUP|ARM_DEF_ENVELOPE;
88         arm->layer= 1;
89         return arm;
90 }
91
92
93 void free_boneChildren(Bone *bone)
94
95         Bone *child;
96         
97         if (bone) {
98                 
99                 child=bone->childbase.first;
100                 if (child){
101                         while (child){
102                                 free_boneChildren (child);
103                                 child=child->next;
104                         }
105                         BLI_freelistN (&bone->childbase);
106                 }
107         }
108 }
109
110 void free_bones (bArmature *arm)
111 {
112         Bone *bone;
113         /*      Free children (if any)  */
114         bone= arm->bonebase.first;
115         if (bone) {
116                 while (bone){
117                         free_boneChildren (bone);
118                         bone=bone->next;
119                 }
120         }
121         
122         
123         BLI_freelistN(&arm->bonebase);
124 }
125
126 void free_armature(bArmature *arm)
127 {
128         if (arm) {
129                 /*              unlink_armature(arm);*/
130                 free_bones(arm);
131         }
132 }
133
134 void make_local_armature(bArmature *arm)
135 {
136         int local=0, lib=0;
137         Object *ob;
138         bArmature *newArm;
139         
140         if (arm->id.lib==0)
141                 return;
142         if (arm->id.us==1) {
143                 arm->id.lib= 0;
144                 arm->id.flag= LIB_LOCAL;
145                 new_id(0, (ID*)arm, 0);
146                 return;
147         }
148         
149         if(local && lib==0) {
150                 arm->id.lib= 0;
151                 arm->id.flag= LIB_LOCAL;
152                 new_id(0, (ID *)arm, 0);
153         }
154         else if(local && lib) {
155                 newArm= copy_armature(arm);
156                 newArm->id.us= 0;
157                 
158                 ob= G.main->object.first;
159                 while(ob) {
160                         if(ob->data==arm) {
161                                 
162                                 if(ob->id.lib==0) {
163                                         ob->data= newArm;
164                                         newArm->id.us++;
165                                         arm->id.us--;
166                                 }
167                         }
168                         ob= ob->id.next;
169                 }
170         }
171 }
172
173 static void     copy_bonechildren (Bone* newBone, Bone* oldBone)
174 {
175         Bone    *curBone, *newChildBone;
176         
177         /*      Copy this bone's list*/
178         duplicatelist (&newBone->childbase, &oldBone->childbase);
179         
180         /*      For each child in the list, update it's children*/
181         newChildBone=newBone->childbase.first;
182         for (curBone=oldBone->childbase.first;curBone;curBone=curBone->next){
183                 newChildBone->parent=newBone;
184                 copy_bonechildren(newChildBone,curBone);
185                 newChildBone=newChildBone->next;
186         }
187 }
188
189 bArmature *copy_armature(bArmature *arm)
190 {
191         bArmature *newArm;
192         Bone            *oldBone, *newBone;
193         
194         newArm= copy_libblock (arm);
195         duplicatelist(&newArm->bonebase, &arm->bonebase);
196         
197         /*      Duplicate the childrens' lists*/
198         newBone=newArm->bonebase.first;
199         for (oldBone=arm->bonebase.first;oldBone;oldBone=oldBone->next){
200                 newBone->parent=NULL;
201                 copy_bonechildren (newBone, oldBone);
202                 newBone=newBone->next;
203         };
204         
205         return newArm;
206 }
207
208 static Bone *get_named_bone_bonechildren (Bone *bone, const char *name)
209 {
210         Bone *curBone, *rbone;
211         
212         if (!strcmp (bone->name, name))
213                 return bone;
214         
215         for (curBone=bone->childbase.first; curBone; curBone=curBone->next){
216                 rbone=get_named_bone_bonechildren (curBone, name);
217                 if (rbone)
218                         return rbone;
219         }
220         
221         return NULL;
222 }
223
224
225 Bone *get_named_bone (bArmature *arm, const char *name)
226 /*
227         Walk the list until the bone is found
228  */
229 {
230         Bone *bone=NULL, *curBone;
231         
232         if (!arm) return NULL;
233         
234         for (curBone=arm->bonebase.first; curBone; curBone=curBone->next){
235                 bone = get_named_bone_bonechildren (curBone, name);
236                 if (bone)
237                         return bone;
238         }
239         
240         return bone;
241 }
242
243 static char *strcasestr_(register char *s, register char *find)
244 {
245     register char c, sc;
246     register size_t len;
247         
248     if ((c = *find++) != 0) {
249                 c= tolower(c);
250                 len = strlen(find);
251                 do {
252                         do {
253                                 if ((sc = *s++) == 0)
254                                         return (NULL);
255                                 sc= tolower(sc);
256                         } while (sc != c);
257                 } while (BLI_strncasecmp(s, find, len) != 0);
258                 s--;
259     }
260     return ((char *) s);
261 }
262
263 #define IS_SEPARATOR(a) (a=='.' || a==' ' || a=='-' || a=='_')
264
265 /* finds the best possible flipped name. For renaming; check for unique names afterwards */
266 /* if strip_number: removes number extensions */
267 void bone_flip_name (char *name, int strip_number)
268 {
269         int             len;
270         char    prefix[128]={""};       /* The part before the facing */
271         char    suffix[128]={""};       /* The part after the facing */
272         char    replace[128]={""};      /* The replacement string */
273         char    number[128]={""};       /* The number extension string */
274         char    *index=NULL;
275
276         len= strlen(name);
277         if(len<3) return;       // we don't do names like .R or .L
278
279         /* We first check the case with a .### extension, let's find the last period */
280         if(isdigit(name[len-1])) {
281                 index= strrchr(name, '.');      // last occurrance
282                 if (index && isdigit(index[1]) ) {              // doesnt handle case bone.1abc2 correct..., whatever!
283                         if(strip_number==0) 
284                                 strcpy(number, index);
285                         *index= 0;
286                         len= strlen(name);
287                 }
288         }
289
290         strcpy (prefix, name);
291
292         /* first case; separator . - _ with extensions r R l L  */
293         if( IS_SEPARATOR(name[len-2]) ) {
294                 switch(name[len-1]) {
295                         case 'l':
296                                 prefix[len-1]= 0;
297                                 strcpy(replace, "r");
298                                 break;
299                         case 'r':
300                                 prefix[len-1]= 0;
301                                 strcpy(replace, "l");
302                                 break;
303                         case 'L':
304                                 prefix[len-1]= 0;
305                                 strcpy(replace, "R");
306                                 break;
307                         case 'R':
308                                 prefix[len-1]= 0;
309                                 strcpy(replace, "L");
310                                 break;
311                 }
312         }
313         /* case; beginning with r R l L , with separator after it */
314         else if( IS_SEPARATOR(name[1]) ) {
315                 switch(name[0]) {
316                         case 'l':
317                                 strcpy(replace, "r");
318                                 strcpy(suffix, name+1);
319                                 prefix[0]= 0;
320                                 break;
321                         case 'r':
322                                 strcpy(replace, "l");
323                                 strcpy(suffix, name+1);
324                                 prefix[0]= 0;
325                                 break;
326                         case 'L':
327                                 strcpy(replace, "R");
328                                 strcpy(suffix, name+1);
329                                 prefix[0]= 0;
330                                 break;
331                         case 'R':
332                                 strcpy(replace, "L");
333                                 strcpy(suffix, name+1);
334                                 prefix[0]= 0;
335                                 break;
336                 }
337         }
338         else if(len > 5) {
339                 /* hrms, why test for a separator? lets do the rule 'ultimate left or right' */
340                 index = strcasestr_(prefix, "right");
341                 if (index==prefix || index==prefix+len-5) {
342                         if(index[0]=='r') 
343                                 strcpy (replace, "left");
344                         else {
345                                 if(index[1]=='I') 
346                                         strcpy (replace, "LEFT");
347                                 else
348                                         strcpy (replace, "Left");
349                         }
350                         *index= 0;
351                         strcpy (suffix, index+5);
352                 }
353                 else {
354                         index = strcasestr_(prefix, "left");
355                         if (index==prefix || index==prefix+len-4) {
356                                 if(index[0]=='l') 
357                                         strcpy (replace, "right");
358                                 else {
359                                         if(index[1]=='E') 
360                                                 strcpy (replace, "RIGHT");
361                                         else
362                                                 strcpy (replace, "Right");
363                                 }
364                                 *index= 0;
365                                 strcpy (suffix, index+4);
366                         }
367                 }               
368         }
369
370         sprintf (name, "%s%s%s%s", prefix, replace, suffix, number);
371 }
372
373
374 /* ************* B-Bone support ******************* */
375
376 #define MAX_BBONE_SUBDIV        32
377
378 /* data has MAX_BBONE_SUBDIV+1 interpolated points, will become desired amount with equal distances */
379 static void equalize_bezier(float *data, int desired)
380 {
381         float *fp, totdist, ddist, dist, fac1, fac2;
382         float pdist[MAX_BBONE_SUBDIV+1];
383         float temp[MAX_BBONE_SUBDIV+1][4];
384         int a, nr;
385         
386         pdist[0]= 0.0f;
387         for(a=0, fp= data; a<MAX_BBONE_SUBDIV; a++, fp+=4) {
388                 QUATCOPY(temp[a], fp);
389                 pdist[a+1]= pdist[a]+VecLenf(fp, fp+4);
390         }
391         /* do last point */
392         QUATCOPY(temp[a], fp);
393         totdist= pdist[a];
394         
395         /* go over distances and calculate new points */
396         ddist= totdist/((float)desired);
397         nr= 1;
398         for(a=1, fp= data+4; a<desired; a++, fp+=4) {
399                 
400                 dist= ((float)a)*ddist;
401                 
402                 /* we're looking for location (distance) 'dist' in the array */
403                 while((dist>= pdist[nr]) && nr<MAX_BBONE_SUBDIV) {
404                         nr++;
405                 }
406                 
407                 fac1= pdist[nr]- pdist[nr-1];
408                 fac2= pdist[nr]-dist;
409                 fac1= fac2/fac1;
410                 fac2= 1.0f-fac1;
411                 
412                 fp[0]= fac1*temp[nr-1][0]+ fac2*temp[nr][0];
413                 fp[1]= fac1*temp[nr-1][1]+ fac2*temp[nr][1];
414                 fp[2]= fac1*temp[nr-1][2]+ fac2*temp[nr][2];
415                 fp[3]= fac1*temp[nr-1][3]+ fac2*temp[nr][3];
416         }
417         /* set last point, needed for orientation calculus */
418         QUATCOPY(fp, temp[MAX_BBONE_SUBDIV]);
419 }
420
421 /* returns pointer to static array, filled with desired amount of bone->segments elements */
422 /* this calculation is done  within unit bone space */
423 Mat4 *b_bone_spline_setup(bPoseChannel *pchan)
424 {
425         static Mat4 bbone_array[MAX_BBONE_SUBDIV];
426         bPoseChannel *next, *prev;
427         Bone *bone= pchan->bone;
428         float h1[3], h2[3], length, hlength1, hlength2, roll;
429         float mat3[3][3], imat[4][4];
430         float data[MAX_BBONE_SUBDIV+1][4], *fp;
431         int a;
432         
433         length= bone->length;
434         hlength1= bone->ease1*length*0.390464f;         // 0.5*sqrt(2)*kappa, the handle length for near-perfect circles
435         hlength2= bone->ease2*length*0.390464f;
436         
437         /* evaluate next and prev bones */
438         if(bone->flag & BONE_CONNECTED)
439                 prev= pchan->parent;
440         else
441                 prev= NULL;
442         
443         next= pchan->child;
444         
445         /* find the handle points, since this is inside bone space, the 
446                 first point = (0,0,0)
447                 last point =  (0, length, 0) */
448         
449         Mat4Invert(imat, pchan->pose_mat);
450         
451         if(prev) {
452                 /* transform previous point inside this bone space */
453                 VECCOPY(h1, prev->pose_head);
454                 Mat4MulVecfl(imat, h1);
455                 /* if previous bone is B-bone too, use average handle direction */
456                 if(prev->bone->segments>1) h1[1]-= length;
457                 Normalise(h1);
458                 VecMulf(h1, -hlength1);
459         }
460         else {
461                 h1[0]= 0.0f; h1[1]= hlength1; h1[2]= 0.0f;
462         }
463         if(next) {
464                 float difmat[4][4], result[3][3], imat3[3][3];
465                 
466                 /* transform next point inside this bone space */
467                 VECCOPY(h2, next->pose_tail);
468                 Mat4MulVecfl(imat, h2);
469                 /* if next bone is B-bone too, use average handle direction */
470                 if(next->bone->segments>1);
471                 else h2[1]-= length;
472                 
473                 /* find the next roll to interpolate as well */
474                 Mat4MulMat4(difmat, next->pose_mat, imat);
475                 Mat3CpyMat4(result, difmat);                            // the desired rotation at beginning of next bone
476                 
477                 vec_roll_to_mat3(h2, 0.0f, mat3);                       // the result of vec_roll without roll
478                 
479                 Mat3Inv(imat3, mat3);
480                 Mat3MulMat3(mat3, imat3, result);                       // the matrix transforming vec_roll to desired roll
481                 
482                 roll= atan2(mat3[2][0], mat3[2][2]);
483                 
484                 /* and only now negate handle */
485                 Normalise(h2);
486                 VecMulf(h2, -hlength2);
487                 
488         }
489         else {
490                 h2[0]= 0.0f; h2[1]= -hlength2; h2[2]= 0.0f;
491                 roll= 0.0;
492         }
493         
494         /* make curve */
495         if(bone->segments > MAX_BBONE_SUBDIV)
496                 bone->segments= MAX_BBONE_SUBDIV;
497         
498         forward_diff_bezier(0.0, h1[0],         h2[0],                  0.0,            data[0],        MAX_BBONE_SUBDIV, 4);
499         forward_diff_bezier(0.0, h1[1],         length + h2[1], length,         data[0]+1,      MAX_BBONE_SUBDIV, 4);
500         forward_diff_bezier(0.0, h1[2],         h2[2],                  0.0,            data[0]+2,      MAX_BBONE_SUBDIV, 4);
501         forward_diff_bezier(0.0, 0.390464f*roll, (1.0f-0.390464f)*roll, roll,   data[0]+3,      MAX_BBONE_SUBDIV, 4);
502         
503         equalize_bezier(data[0], bone->segments);       // note: does stride 4!
504         
505         /* make transformation matrices for the segments for drawing */
506         for(a=0, fp= data[0]; a<bone->segments; a++, fp+=4) {
507                 VecSubf(h1, fp+4, fp);
508                 vec_roll_to_mat3(h1, fp[3], mat3);              // fp[3] is roll
509                 Mat4CpyMat3(bbone_array[a].mat, mat3);
510                 VECCOPY(bbone_array[a].mat[3], fp);
511         }
512         
513         return bbone_array;
514 }
515
516 /* ************ Armature Deform ******************* */
517
518 static void pchan_b_bone_defmats(bPoseChannel *pchan)
519 {
520         Bone *bone= pchan->bone;
521         Mat4 *b_bone= b_bone_spline_setup(pchan);
522         Mat4 *b_bone_mats;
523         int a;
524         
525         pchan->b_bone_mats=b_bone_mats= MEM_mallocN((1+bone->segments)*sizeof(Mat4), "BBone defmats");
526         
527         /* first matrix is the inverse arm_mat, to bring points in local bone space */
528         Mat4Invert(b_bone_mats[0].mat, bone->arm_mat);
529         
530         /* then we multiply the bbone_mats with arm_mat */
531         for(a=0; a<bone->segments; a++) {
532                 Mat4MulMat4(b_bone_mats[a+1].mat, b_bone[a].mat, bone->arm_mat);
533         }
534 }
535
536 static void b_bone_deform(bPoseChannel *pchan, Bone *bone, float *defpos)
537 {
538         Mat4 *b_bone= pchan->b_bone_mats;
539         float segment;
540         int a;
541         
542         /* need to transform defpos back to bonespace */
543         Mat4MulVecfl(b_bone[0].mat, defpos);
544         
545         /* now calculate which of the b_bones are deforming this */
546         segment= bone->length/((float)bone->segments);
547         a= (int) (defpos[1]/segment);
548         
549         /* note; by clamping it extends deform at endpoints, goes best with straight joints in restpos. */
550         CLAMP(a, 0, bone->segments-1);
551
552         /* since the bbone mats translate from (0.0.0) on the curve, we subtract */
553         defpos[1] -= ((float)a)*segment;
554         
555         Mat4MulVecfl(b_bone[a+1].mat, defpos);
556 }
557
558 /* using vec with dist to bone b1 - b2 */
559 float distfactor_to_bone (float vec[3], float b1[3], float b2[3], float rad1, float rad2, float rdist)
560 {
561         float dist=0.0f; 
562         float bdelta[3];
563         float pdelta[3];
564         float hsqr, a, l, rad;
565         
566         VecSubf (bdelta, b2, b1);
567         l = Normalise (bdelta);
568         
569         VecSubf (pdelta, vec, b1);
570         
571         a = bdelta[0]*pdelta[0] + bdelta[1]*pdelta[1] + bdelta[2]*pdelta[2];
572         hsqr = ((pdelta[0]*pdelta[0]) + (pdelta[1]*pdelta[1]) + (pdelta[2]*pdelta[2]));
573         
574         if (a < 0.0F){
575                 /* If we're past the end of the bone, do a spherical field attenuation thing */
576                 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])) ;
577                 rad= rad1;
578         }
579         else if (a > l){
580                 /* If we're past the end of the bone, do a spherical field attenuation thing */
581                 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])) ;
582                 rad= rad2;
583         }
584         else {
585                 dist= (hsqr - (a*a));
586                 
587                 if(l!=0.0f) {
588                         rad= a/l;
589                         rad= rad*rad2 + (1.0-rad)*rad1;
590                 }
591                 else rad= rad1;
592         }
593         
594         a= rad*rad;
595         if(dist < a) 
596                 return 1.0f;
597         else {
598                 l= rad+rdist;
599                 l*= l;
600                 if(rdist==0.0f || dist >= l) 
601                         return 0.0f;
602                 else {
603                         a= sqrt(dist)-rad;
604                         return 1.0-( a*a )/( rdist*rdist );
605                 }
606         }
607 }
608
609 static float dist_bone_deform(bPoseChannel *pchan, float *vec, float *co)
610 {
611         Bone *bone= pchan->bone;
612         float   fac;
613         float   cop[3];
614         float   contrib=0.0;
615
616         if(bone==NULL) return 0.0f;
617         
618         VECCOPY (cop, co);
619
620         fac= distfactor_to_bone(cop, bone->arm_head, bone->arm_tail, bone->rad_head, bone->rad_tail, bone->dist);
621         
622         if (fac>0.0){
623                 
624                 fac*=bone->weight;
625                 contrib= fac;
626                 if(contrib>0.0) {
627
628                         VECCOPY (cop, co);
629                         
630                         if(bone->segments>1)
631                                 b_bone_deform(pchan, bone, cop);        // applies on cop
632                         
633                         Mat4MulVecfl(pchan->chan_mat, cop);
634                         
635                         VecSubf (cop, cop, co); //      Make this a delta from the base position
636                         cop[0]*=fac; cop[1]*=fac; cop[2]*=fac;
637                         VecAddf (vec, vec, cop);
638                 }
639         }
640         
641         return contrib;
642 }
643
644 static void pchan_bone_deform(bPoseChannel *pchan, float weight, float *vec, float *co, float *contrib)
645 {
646         float   cop[3];
647
648         if (!weight)
649                 return;
650
651         VECCOPY (cop, co);
652         
653         if(pchan->bone->segments>1)
654                 b_bone_deform(pchan, pchan->bone, cop); // applies on cop
655         
656         Mat4MulVecfl(pchan->chan_mat, cop);
657         
658         vec[0]+=(cop[0]-co[0])*weight;
659         vec[1]+=(cop[1]-co[1])*weight;
660         vec[2]+=(cop[2]-co[2])*weight;
661
662         (*contrib)+=weight;
663 }
664
665 void armature_deform_verts(Object *armOb, Object *target, DerivedMesh *dm,
666                            float (*vertexCos)[3], int numVerts, int deformflag) 
667 {
668         bPoseChannel *pchan, **defnrToPC = NULL;
669         MDeformVert *dverts = NULL;
670         float obinv[4][4], premat[4][4], postmat[4][4];
671         int use_envelope = deformflag & ARM_DEF_ENVELOPE;
672         int numGroups = 0;   /* safety for vertexgroup index overflow too */
673         int i;
674         int use_dverts = 0;
675
676         if(armOb == G.obedit) return;
677         
678         Mat4Invert(obinv, target->obmat);
679         Mat4CpyMat4(premat, target->obmat);
680         Mat4MulMat4(postmat, armOb->obmat, obinv);
681
682         Mat4Invert(premat, postmat);
683
684         /* bone defmats are already in the channels, chan_mat */
685         
686         /* initialize B_bone matrices */
687         for(pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next) {
688                 if(!(pchan->bone->flag & BONE_NO_DEFORM))
689                         if(pchan->bone->segments > 1)
690                                 pchan_b_bone_defmats(pchan);
691         }
692
693         /* get a vertex-deform-index to posechannel array */
694         if(deformflag & ARM_DEF_VGROUP) {
695                 if(target->type == OB_MESH){
696                         bDeformGroup *dg;
697                 
698                         numGroups = BLI_countlist(&target->defbase);
699                         dverts = ((Mesh*)target->data)->dvert;
700                         
701                         /* if we have a DerivedMesh, only use dverts if it has them */
702                         if(dm)
703                                 if(dm->getVertData(dm, 0, LAYERTYPE_MDEFORMVERT))
704                                         use_dverts = 1;
705                                 else use_dverts = 0;
706                         else if(dverts) use_dverts = 1;
707
708                         if(use_dverts) {
709                                 defnrToPC = MEM_callocN(sizeof(*defnrToPC) * numGroups,
710                                                         "defnrToBone");
711                                 for(i = 0, dg = target->defbase.first; dg;
712                                     i++, dg = dg->next) {
713                                         defnrToPC[i] = get_pose_channel(armOb->pose, dg->name);
714                                         /* exclude non-deforming bones */
715                                         if(defnrToPC[i]) {
716                                                 if(defnrToPC[i]->bone->flag & BONE_NO_DEFORM)
717                                                         defnrToPC[i]= NULL;
718                                         }
719                                 }
720                         }
721                 }
722         }
723
724         for(i = 0; i < numVerts; i++) {
725                 MDeformVert *dvert;
726                 float *co = vertexCos[i];
727                 float   vec[3];
728                 float   contrib = 0.0;
729                 int             j;
730
731                 vec[0] = vec[1] = vec[2] = 0;
732
733                 /* Apply the object's matrix */
734                 Mat4MulVecfl(premat, co);
735                 
736                 if(use_dverts) {
737                         if(dm) dvert = dm->getVertData(dm, i, LAYERTYPE_MDEFORMVERT);
738                         else if(i < ((Mesh*)target->data)->totvert) dvert = dverts + i;
739                 } else
740                         dvert = NULL;
741                 
742                 if(dvert && dvert->totweight) { // use weight groups ?
743                         int deformed = 0;
744                         
745                         for(j = 0; j < dvert->totweight; j++){
746                                 int index = dvert->dw[j].def_nr;
747                                 pchan = index < numGroups?defnrToPC[index]:NULL;
748                                 if(pchan) {
749                                         float weight = dvert->dw[j].weight;
750                                         Bone *bone = pchan->bone;
751                                         
752                                         deformed = 1;
753                                         
754                                         if(bone && bone->flag & BONE_MULT_VG_ENV) {
755                                                 weight *= distfactor_to_bone(co, bone->arm_head,
756                                                                              bone->arm_tail,
757                                                                              bone->rad_head,
758                                                                              bone->rad_tail,
759                                                                              bone->dist);
760                                         }
761                                         pchan_bone_deform(pchan, weight, vec, co, &contrib);
762                                 }
763                         }
764                         /* if there are vertexgroups but not groups with bones
765                          * (like for softbody groups)
766                          */
767                         if(deformed == 0 && use_envelope) {
768                                 for(pchan = armOb->pose->chanbase.first; pchan;
769                                     pchan = pchan->next) {
770                                         if(!(pchan->bone->flag & BONE_NO_DEFORM))
771                                                 contrib += dist_bone_deform(pchan, vec, co);
772                                 }
773                         }
774                 }
775                 else if(use_envelope) {
776                         for(pchan = armOb->pose->chanbase.first; pchan;
777                             pchan = pchan->next) {
778                                 if(!(pchan->bone->flag & BONE_NO_DEFORM))
779                                         contrib += dist_bone_deform(pchan, vec, co);
780                         }
781                 }
782
783                 if(contrib > 0.0){
784                         vec[0] /= contrib;
785                         vec[1] /= contrib;
786                         vec[2] /= contrib;
787                 }
788
789                 VecAddf(co, vec, co);
790                 Mat4MulVecfl(postmat, co);
791         }
792
793         if(defnrToPC) MEM_freeN(defnrToPC);
794         
795         /* free B_bone matrices */
796         for(pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next) {
797                 if(pchan->b_bone_mats) {
798                         MEM_freeN(pchan->b_bone_mats);
799                         pchan->b_bone_mats = NULL;
800                 }
801         }
802 }
803
804 /* ************ END Armature Deform ******************* */
805
806 void get_objectspace_bone_matrix (struct Bone* bone, float M_accumulatedMatrix[][4], int root, int posed)
807 {
808         Mat4CpyMat4(M_accumulatedMatrix, bone->arm_mat);
809 }
810
811
812 /* **************** The new & simple (but OK!) armature evaluation ********* */ 
813
814 /*  ****************** And how it works! ****************************************
815
816   This is the bone transformation trick; they're hierarchical so each bone(b)
817   is in the coord system of bone(b-1):
818
819   arm_mat(b)= arm_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) 
820   
821   -> yoffs is just the y axis translation in parent's coord system
822   -> d_root is the translation of the bone root, also in parent's coord system
823
824   pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b)
825
826   we then - in init deform - store the deform in chan_mat, such that:
827
828   pose_mat(b)= arm_mat(b) * chan_mat(b)
829   
830   *************************************************************************** */
831 /*  Computes vector and roll based on a rotation. "mat" must
832      contain only a rotation, and no scaling. */ 
833 void mat3_to_vec_roll(float mat[][3], float *vec, float *roll) {
834      if (vec)
835          VecCopyf(vec, mat[1]);
836
837      if (roll) {
838          float vecmat[3][3], vecmatinv[3][3], rollmat[3][3];
839
840          vec_roll_to_mat3(mat[1], 0.0f, vecmat);
841          Mat3Inv(vecmatinv, vecmat);
842          Mat3MulMat3(rollmat, vecmatinv, mat);
843
844          *roll= atan2(rollmat[2][0], rollmat[2][2]);
845      }
846 }
847
848 /*      Calculates the rest matrix of a bone based
849         On its vector and a roll around that vector */
850 void vec_roll_to_mat3(float *vec, float roll, float mat[][3])
851 {
852         float   nor[3], axis[3], target[3]={0,1,0};
853         float   theta;
854         float   rMatrix[3][3], bMatrix[3][3];
855         
856         VECCOPY (nor, vec);
857         Normalise (nor);
858         
859         /*      Find Axis & Amount for bone matrix*/
860         Crossf (axis,target,nor);
861         
862         if (Inpf(axis,axis) > 0.0000000000001) {
863                 /* if nor is *not* a multiple of target ... */
864                 Normalise (axis);
865                 theta=(float) acos (Inpf (target,nor));
866                 
867                 /*      Make Bone matrix*/
868                 VecRotToMat3(axis, theta, bMatrix);
869         }
870         else {
871                 /* if nor is a multiple of target ... */
872                 float updown;
873                 
874                 /* point same direction, or opposite? */
875                 updown = ( Inpf (target,nor) > 0 ) ? 1.0 : -1.0;
876                 
877                 /* I think this should work ... */
878                 bMatrix[0][0]=updown; bMatrix[0][1]=0.0;    bMatrix[0][2]=0.0;
879                 bMatrix[1][0]=0.0;    bMatrix[1][1]=updown; bMatrix[1][2]=0.0;
880                 bMatrix[2][0]=0.0;    bMatrix[2][1]=0.0;    bMatrix[2][2]=1.0;
881         }
882         
883         /*      Make Roll matrix*/
884         VecRotToMat3(nor, roll, rMatrix);
885         
886         /*      Combine and output result*/
887         Mat3MulMat3 (mat, rMatrix, bMatrix);
888 }
889
890
891 /* recursive part, calculates restposition of entire tree of children */
892 /* used by exiting editmode too */
893 void where_is_armature_bone(Bone *bone, Bone *prevbone)
894 {
895         float vec[3];
896         
897         /* Bone Space */
898         VecSubf (vec, bone->tail, bone->head);
899         vec_roll_to_mat3(vec, bone->roll, bone->bone_mat);
900
901         bone->length= VecLenf(bone->head, bone->tail);
902         
903         /* this is called on old file reading too... */
904         if(bone->xwidth==0.0) {
905                 bone->xwidth= 0.1f;
906                 bone->zwidth= 0.1f;
907                 bone->segments= 1;
908         }
909         
910         if(prevbone) {
911                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
912                 
913                 /* bone transform itself */
914                 Mat4CpyMat3(offs_bone, bone->bone_mat);
915                                 
916                 /* The bone's root offset (is in the parent's coordinate system) */
917                 VECCOPY(offs_bone[3], bone->head);
918
919                 /* Get the length translation of parent (length along y axis) */
920                 offs_bone[3][1]+= prevbone->length;
921                 
922                 /* Compose the matrix for this bone  */
923                 Mat4MulMat4(bone->arm_mat, offs_bone, prevbone->arm_mat);
924         }
925         else {
926                 Mat4CpyMat3(bone->arm_mat, bone->bone_mat);
927                 VECCOPY(bone->arm_mat[3], bone->head);
928         }
929         
930         /* head */
931         VECCOPY(bone->arm_head, bone->arm_mat[3]);
932         /* tail is in current local coord system */
933         VECCOPY(vec, bone->arm_mat[1]);
934         VecMulf(vec, bone->length);
935         VecAddf(bone->arm_tail, bone->arm_head, vec);
936         
937         /* and the kiddies */
938         prevbone= bone;
939         for(bone= bone->childbase.first; bone; bone= bone->next) {
940                 where_is_armature_bone(bone, prevbone);
941         }
942 }
943
944 /* updates vectors and matrices on rest-position level, only needed 
945    after editing armature itself, now only on reading file */
946 void where_is_armature (bArmature *arm)
947 {
948         Bone *bone;
949         
950         /* hierarchical from root to children */
951         for(bone= arm->bonebase.first; bone; bone= bone->next) {
952                 where_is_armature_bone(bone, NULL);
953         }
954 }
955
956 static int rebuild_pose_bone(bPose *pose, Bone *bone, bPoseChannel *parchan, int counter)
957 {
958         bPoseChannel *pchan = verify_pose_channel (pose, bone->name);   // verify checks and/or adds
959
960         pchan->bone= bone;
961         pchan->parent= parchan;
962         
963         counter++;
964         
965         for(bone= bone->childbase.first; bone; bone= bone->next) {
966                 counter= rebuild_pose_bone(pose, bone, pchan, counter);
967                 /* for quick detecting of next bone in chain, only b-bone uses it now */
968                 if(bone->flag & BONE_CONNECTED)
969                         pchan->child= get_pose_channel(pose, bone->name);
970         }
971         
972         return counter;
973 }
974
975 /* only after leave editmode, duplicating, but also for validating older files */
976 /* NOTE: pose->flag is set for it */
977 void armature_rebuild_pose(Object *ob, bArmature *arm)
978 {
979         Bone *bone;
980         bPose *pose;
981         bPoseChannel *pchan, *next;
982         int counter=0;
983                 
984         /* only done here */
985         if(ob->pose==NULL) ob->pose= MEM_callocN(sizeof(bPose), "new pose");
986         pose= ob->pose;
987         
988         /* clear */
989         for(pchan= pose->chanbase.first; pchan; pchan= pchan->next) {
990                 pchan->bone= NULL;
991                 pchan->child= NULL;
992         }
993         
994         /* first step, check if all channels are there */
995         for(bone= arm->bonebase.first; bone; bone= bone->next) {
996                 counter= rebuild_pose_bone(pose, bone, NULL, counter);
997         }
998
999         /* and a check for garbage */
1000         for(pchan= pose->chanbase.first; pchan; pchan= next) {
1001                 next= pchan->next;
1002                 if(pchan->bone==NULL) {
1003                         if(pchan->path)
1004                                 MEM_freeN(pchan->path);
1005                         free_constraints(&pchan->constraints);
1006                         BLI_freelinkN(&pose->chanbase, pchan);
1007                 }
1008         }
1009 //      printf("rebuild pose %s, %d bones\n", ob->id.name, counter);
1010         
1011         update_pose_constraint_flags(ob->pose); // for IK detection for example
1012         
1013         /* the sorting */
1014         if(counter>1)
1015                 DAG_pose_sort(ob);
1016         
1017         ob->pose->flag &= ~POSE_RECALC;
1018 }
1019
1020
1021 /* ********************** THE IK SOLVER ******************* */
1022
1023
1024
1025 /* allocates PoseTree, and links that to root bone/channel */
1026 /* Note: detecting the IK chain is duplicate code... in drawarmature.c and in transform_conversions.c */
1027 static void initialize_posetree(struct Object *ob, bPoseChannel *pchan_tip)
1028 {
1029         bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
1030         PoseTree *tree;
1031         PoseTarget *target;
1032         bConstraint *con;
1033         bKinematicConstraint *data;
1034         int a, segcount= 0, size, newsize, *oldparent, parent;
1035         
1036         /* find IK constraint, and validate it */
1037         for(con= pchan_tip->constraints.first; con; con= con->next) {
1038                 if(con->type==CONSTRAINT_TYPE_KINEMATIC) break;
1039         }
1040         if(con==NULL) return;
1041         
1042         data=(bKinematicConstraint*)con->data;
1043         
1044         /* two types of targets */
1045         if(data->flag & CONSTRAINT_IK_AUTO);
1046         else {
1047                 if(con->flag & CONSTRAINT_DISABLE) return;      /* checked in editconstraint.c */
1048                 if(data->tar==NULL) return;
1049                 if(data->tar->type==OB_ARMATURE && data->subtarget[0]==0) return;
1050         }
1051         
1052         /* exclude tip from chain? */
1053         if(!(data->flag & CONSTRAINT_IK_TIP))
1054                 pchan_tip= pchan_tip->parent;
1055         
1056         /* Find the chain's root & count the segments needed */
1057         for (curchan = pchan_tip; curchan; curchan=curchan->parent){
1058                 pchan_root = curchan;
1059                 
1060                 curchan->flag |= POSE_CHAIN;    // don't forget to clear this
1061                 chanlist[segcount]=curchan;
1062                 segcount++;
1063                 
1064                 if(segcount==data->rootbone || segcount>255) break; // 255 is weak
1065         }
1066         if (!segcount) return;
1067
1068         /* setup the chain data */
1069         
1070         /* we make tree-IK, unless all existing targets are in this chain */
1071         for(tree= pchan_root->iktree.first; tree; tree= tree->next) {
1072                 for(target= tree->targets.first; target; target= target->next) {
1073                         curchan= tree->pchan[target->tip];
1074                         if(curchan->flag & POSE_CHAIN)
1075                                 curchan->flag &= ~POSE_CHAIN;
1076                         else
1077                                 break;
1078                 }
1079                 if(target) break;
1080         }
1081
1082         /* create a target */
1083         target= MEM_callocN(sizeof(PoseTarget), "posetarget");
1084         target->con= con;
1085         pchan_tip->flag &= ~POSE_CHAIN;
1086
1087         if(tree==NULL) {
1088                 /* make new tree */
1089                 tree= MEM_callocN(sizeof(PoseTree), "posetree");
1090
1091                 tree->tolerance= data->tolerance;
1092                 tree->iterations= data->iterations;
1093                 tree->totchannel= segcount;
1094                 
1095                 tree->pchan= MEM_callocN(segcount*sizeof(void*), "ik tree pchan");
1096                 tree->parent= MEM_callocN(segcount*sizeof(int), "ik tree parent");
1097                 for(a=0; a<segcount; a++) {
1098                         tree->pchan[a]= chanlist[segcount-a-1];
1099                         tree->parent[a]= a-1;
1100                 }
1101                 target->tip= segcount-1;
1102                 
1103                 /* AND! link the tree to the root */
1104                 BLI_addtail(&pchan_root->iktree, tree);
1105         }
1106         else {
1107                 tree->tolerance= MIN2(tree->tolerance, data->tolerance);
1108                 tree->iterations= MAX2(data->iterations, tree->iterations);
1109
1110                 /* skip common pose channels and add remaining*/
1111                 size= MIN2(segcount, tree->totchannel);
1112                 for(a=0; a<size && tree->pchan[a]==chanlist[segcount-a-1]; a++);
1113                 parent= a-1;
1114
1115                 segcount= segcount-a;
1116                 target->tip= tree->totchannel + segcount - 1;
1117
1118                 if (segcount > 0) {
1119                         /* resize array */
1120                         newsize= tree->totchannel + segcount;
1121                         oldchan= tree->pchan;
1122                         oldparent= tree->parent;
1123
1124                         tree->pchan= MEM_callocN(newsize*sizeof(void*), "ik tree pchan");
1125                         tree->parent= MEM_callocN(newsize*sizeof(int), "ik tree parent");
1126                         memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel);
1127                         memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel);
1128                         MEM_freeN(oldchan);
1129                         MEM_freeN(oldparent);
1130
1131                         /* add new pose channels at the end, in reverse order */
1132                         for(a=0; a<segcount; a++) {
1133                                 tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1];
1134                                 tree->parent[tree->totchannel+a]= tree->totchannel+a-1;
1135                         }
1136                         tree->parent[tree->totchannel]= parent;
1137                         
1138                         tree->totchannel= newsize;
1139                 }
1140
1141                 /* move tree to end of list, for correct evaluation order */
1142                 BLI_remlink(&pchan_root->iktree, tree);
1143                 BLI_addtail(&pchan_root->iktree, tree);
1144         }
1145
1146         /* add target to the tree */
1147         BLI_addtail(&tree->targets, target);
1148 }
1149
1150 /* called from within the core where_is_pose loop, all animsystems and constraints
1151 were executed & assigned. Now as last we do an IK pass */
1152 static void execute_posetree(Object *ob, PoseTree *tree)
1153 {
1154         float R_parmat[3][3];
1155         float iR_parmat[3][3];
1156         float R_bonemat[3][3];
1157         float goalrot[3][3], goalpos[3];
1158         float rootmat[4][4], imat[4][4];
1159         float goal[4][4], goalinv[4][4];
1160         float size[3], irest_basis[3][3], full_basis[3][3];
1161         float end_pose[4][4], world_pose[4][4];
1162         float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch=NULL;
1163         int a, flag, hasstretch=0;
1164         bPoseChannel *pchan;
1165         IK_Segment *seg, *parent, **iktree, *iktarget;
1166         IK_Solver *solver;
1167         PoseTarget *target;
1168         bKinematicConstraint *data;
1169         Bone *bone;
1170
1171         if (tree->totchannel == 0)
1172                 return;
1173
1174         iktree= MEM_mallocN(sizeof(void*)*tree->totchannel, "ik tree");
1175
1176         for(a=0; a<tree->totchannel; a++) {
1177                 pchan= tree->pchan[a];
1178                 bone= pchan->bone;
1179
1180                 /* set DoF flag */
1181                 flag= 0;
1182                 if((pchan->ikflag & BONE_IK_NO_XDOF) == 0)
1183                         flag |= IK_XDOF;
1184                 if((pchan->ikflag & BONE_IK_NO_YDOF) == 0)
1185                         flag |= IK_YDOF;
1186                 if((pchan->ikflag & BONE_IK_NO_ZDOF) == 0)
1187                         flag |= IK_ZDOF;
1188
1189                 if(pchan->ikstretch > 0.0) {
1190                         flag |= IK_TRANS_YDOF;
1191                         hasstretch = 1;
1192                 }
1193
1194                 seg= iktree[a]= IK_CreateSegment(flag);
1195
1196                 /* find parent */
1197                 if(a == 0)
1198                         parent= NULL;
1199                 else
1200                         parent= iktree[tree->parent[a]];
1201
1202                 IK_SetParent(seg, parent);
1203         
1204                 /* get the matrix that transforms from prevbone into this bone */
1205                 Mat3CpyMat4(R_bonemat, pchan->pose_mat);
1206
1207                 /* gather transformations for this IK segment */
1208
1209                 if (pchan->parent)
1210                         Mat3CpyMat4(R_parmat, pchan->parent->pose_mat);
1211                 else
1212                         Mat3One(R_parmat);
1213
1214                 /* bone offset */
1215                 if (pchan->parent && (a > 0))
1216                         VecSubf(start, pchan->pose_head, pchan->parent->pose_tail);
1217                 else
1218                         /* only root bone (a = 0) has no parent */
1219                         start[0]= start[1]= start[2]= 0.0f;
1220                 
1221                 /* change length based on bone size */
1222                 length= bone->length*VecLength(R_bonemat[1]);
1223
1224                 /* compute rest basis and its inverse */
1225                 Mat3CpyMat3(rest_basis, bone->bone_mat);
1226                 Mat3CpyMat3(irest_basis, bone->bone_mat);
1227                 Mat3Transp(irest_basis);
1228
1229                 /* compute basis with rest_basis removed */
1230                 Mat3Inv(iR_parmat, R_parmat);
1231                 Mat3MulMat3(full_basis, iR_parmat, R_bonemat);
1232                 Mat3MulMat3(basis, irest_basis, full_basis);
1233
1234                 /* basis must be pure rotation */
1235                 Mat3Ortho(basis);
1236
1237                 /* transform offset into local bone space */
1238                 Mat3Ortho(iR_parmat);
1239                 Mat3MulVecfl(iR_parmat, start);
1240
1241                 IK_SetTransform(seg, start, rest_basis, basis, length);
1242
1243                 if (pchan->ikflag & BONE_IK_XLIMIT)
1244                         IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]);
1245                 if (pchan->ikflag & BONE_IK_YLIMIT)
1246                         IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]);
1247                 if (pchan->ikflag & BONE_IK_ZLIMIT)
1248                         IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]);
1249
1250                 IK_SetStiffness(seg, IK_X, pchan->stiffness[0]);
1251                 IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]);
1252                 IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]);
1253
1254                 if(pchan->ikstretch > 0.0) {
1255                         float ikstretch = pchan->ikstretch*pchan->ikstretch;
1256                         IK_SetStiffness(seg, IK_TRANS_Y, MIN2(1.0-ikstretch, 0.99));
1257                         IK_SetLimit(seg, IK_TRANS_Y, 0.001, 1e10);
1258                 }
1259         }
1260
1261         solver= IK_CreateSolver(iktree[0]);
1262
1263         /* set solver goals */
1264
1265         /* first set the goal inverse transform, assuming the root of tree was done ok! */
1266         pchan= tree->pchan[0];
1267         if (pchan->parent)
1268                 /* transform goal by parent mat, so this rotation is not part of the
1269                    segment's basis. otherwise rotation limits do not work on the
1270                    local transform of the segment itself. */
1271                 Mat4CpyMat4(rootmat, pchan->parent->pose_mat);
1272         else
1273                 Mat4One(rootmat);
1274         VECCOPY(rootmat[3], pchan->pose_head);
1275         
1276         Mat4MulMat4 (imat, rootmat, ob->obmat);
1277         Mat4Invert (goalinv, imat);
1278         
1279         for(target=tree->targets.first; target; target=target->next) {
1280                 data= (bKinematicConstraint*)target->con->data;
1281
1282                 /* 1.0=ctime, we pass on object for auto-ik */
1283                 get_constraint_target_matrix(target->con, TARGET_BONE, ob, rootmat, size, 1.0);
1284
1285                 /* and set and transform goal */
1286                 Mat4MulMat4(goal, rootmat, goalinv);
1287
1288                 VECCOPY(goalpos, goal[3]);
1289                 Mat3CpyMat4(goalrot, goal);
1290
1291                 /* do we need blending? */
1292                 if(target->con->enforce!=1.0) {
1293                         float q1[4], q2[4], q[4];
1294                         float fac= target->con->enforce;
1295                         float mfac= 1.0-fac;
1296                         
1297                         pchan= tree->pchan[target->tip];
1298
1299                         /* end effector in world space */
1300                         Mat4CpyMat4(end_pose, pchan->pose_mat);
1301                         VECCOPY(end_pose[3], pchan->pose_tail);
1302                         Mat4MulSerie(world_pose, goalinv, ob->obmat, end_pose, 0, 0, 0, 0, 0);
1303
1304                         /* blend position */
1305                         goalpos[0]= fac*goalpos[0] + mfac*world_pose[3][0];
1306                         goalpos[1]= fac*goalpos[1] + mfac*world_pose[3][1];
1307                         goalpos[2]= fac*goalpos[2] + mfac*world_pose[3][2];
1308
1309                         /* blend rotation */
1310                         Mat3ToQuat(goalrot, q1);
1311                         Mat4ToQuat(world_pose, q2);
1312                         QuatInterpol(q, q1, q2, mfac);
1313                         QuatToMat3(q, goalrot);
1314                 }
1315
1316                 iktarget= iktree[target->tip];
1317
1318                 if(data->weight != 0.0)
1319                         IK_SolverAddGoal(solver, iktarget, goalpos, data->weight);
1320                 if((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0) && (data->flag & CONSTRAINT_IK_AUTO)==0)
1321                         IK_SolverAddGoalOrientation(solver, iktarget, goalrot, data->orientweight);
1322         }
1323
1324         /* solve */
1325         IK_Solve(solver, tree->tolerance, tree->iterations);
1326         IK_FreeSolver(solver);
1327
1328         /* gather basis changes */
1329         tree->basis_change= MEM_mallocN(sizeof(float[3][3])*tree->totchannel, "ik basis change");
1330         if(hasstretch)
1331                 ikstretch= MEM_mallocN(sizeof(float)*tree->totchannel, "ik stretch");
1332
1333         for(a=0; a<tree->totchannel; a++) {
1334                 IK_GetBasisChange(iktree[a], tree->basis_change[a]);
1335
1336                 if(hasstretch) {
1337                         /* have to compensate for scaling received from parent */
1338                         float parentstretch, stretch;
1339
1340                         pchan= tree->pchan[a];
1341                         parentstretch= (tree->parent[a] >= 0)? ikstretch[tree->parent[a]]: 1.0;
1342
1343                         if(pchan->ikstretch > 0.0) {
1344                                 float trans[3], length;
1345
1346                                 IK_GetTranslationChange(iktree[a], trans);
1347                                 length= pchan->bone->length*VecLength(pchan->pose_mat[1]);
1348
1349                                 ikstretch[a]= (length == 0.0)? 1.0: (trans[1]+length)/length;
1350                         }
1351                         else
1352                                 ikstretch[a] = 1.0;
1353
1354                         stretch= (parentstretch == 0.0)? 1.0: ikstretch[a]/parentstretch;
1355
1356                         VecMulf(tree->basis_change[a][0], stretch);
1357                         VecMulf(tree->basis_change[a][1], stretch);
1358                         VecMulf(tree->basis_change[a][2], stretch);
1359
1360                 }
1361
1362                 IK_FreeSegment(iktree[a]);
1363         }
1364         
1365         MEM_freeN(iktree);
1366         if(ikstretch) MEM_freeN(ikstretch);
1367 }
1368
1369 void free_posetree(PoseTree *tree)
1370 {
1371         BLI_freelistN(&tree->targets);
1372         if(tree->pchan) MEM_freeN(tree->pchan);
1373         if(tree->parent) MEM_freeN(tree->parent);
1374         if(tree->basis_change) MEM_freeN(tree->basis_change);
1375         MEM_freeN(tree);
1376 }
1377
1378 /* ********************** THE POSE SOLVER ******************* */
1379
1380
1381 /* loc/rot/size to mat4 */
1382 /* used in constraint.c too */
1383 void chan_calc_mat(bPoseChannel *chan)
1384 {
1385         float smat[3][3];
1386         float rmat[3][3];
1387         float tmat[3][3];
1388         
1389         SizeToMat3(chan->size, smat);
1390         
1391         NormalQuat(chan->quat);
1392
1393         QuatToMat3(chan->quat, rmat);
1394         
1395         Mat3MulMat3(tmat, rmat, smat);
1396         
1397         Mat4CpyMat3(chan->chan_mat, tmat);
1398         
1399         /* prevent action channels breaking chains */
1400         /* need to check for bone here, CONSTRAINT_TYPE_ACTION uses this call */
1401         if (chan->bone==NULL || !(chan->bone->flag & BONE_CONNECTED)) {
1402                 VECCOPY(chan->chan_mat[3], chan->loc);
1403         }
1404
1405 }
1406
1407 /* transform from bone(b) to bone(b+1), store in chan_mat */
1408 static void make_dmats(bPoseChannel *pchan)
1409 {
1410         if (pchan->parent) {
1411                 float iR_parmat[4][4];
1412                 Mat4Invert(iR_parmat, pchan->parent->pose_mat);
1413                 Mat4MulMat4(pchan->chan_mat,  pchan->pose_mat, iR_parmat);      // delta mat
1414         }
1415         else Mat4CpyMat4(pchan->chan_mat, pchan->pose_mat);
1416 }
1417
1418 /* applies IK matrix to pchan, IK is done separated */
1419 /* formula: pose_mat(b) = pose_mat(b-1) * diffmat(b-1, b) * ik_mat(b) */
1420 /* to make this work, the diffmats have to be precalculated! Stored in chan_mat */
1421 static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[][3])   // nr = to detect if this is first bone
1422 {
1423         float vec[3], ikmat[4][4];
1424         
1425         Mat4CpyMat3(ikmat, ik_mat);
1426         
1427         if (pchan->parent)
1428                 Mat4MulSerie(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat, ikmat, NULL, NULL, NULL, NULL, NULL);
1429         else 
1430                 Mat4MulMat4(pchan->pose_mat, ikmat, pchan->chan_mat);
1431
1432         /* calculate head */
1433         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
1434         /* calculate tail */
1435         VECCOPY(vec, pchan->pose_mat[1]);
1436         VecMulf(vec, pchan->bone->length);
1437         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
1438
1439         pchan->flag |= POSE_DONE;
1440 }
1441
1442 static void do_local_constraint(bPoseChannel *pchan, bConstraint *con)
1443 {
1444         switch(con->type) {
1445                 case CONSTRAINT_TYPE_LOCLIKE:
1446                 {
1447                         bLocateLikeConstraint *data= con->data;
1448                         
1449                         if(data->tar && data->subtarget[0]) {
1450                                 bPoseChannel *pchant= get_pose_channel(data->tar->pose, data->subtarget);
1451                                 if(pchant) {
1452                                         if (data->flag & LOCLIKE_X)
1453                                                 pchan->loc[0]= pchant->loc[0];
1454                                         if (data->flag & LOCLIKE_Y)
1455                                                 pchan->loc[1]= pchant->loc[1];
1456                                         if (data->flag & LOCLIKE_Z)
1457                                                 pchan->loc[2]= pchant->loc[2];
1458                                 }
1459                         }
1460                 }
1461                         break;
1462                 case CONSTRAINT_TYPE_ROTLIKE:
1463                 {
1464                         bRotateLikeConstraint *data= con->data;
1465                         if(data->tar && data->subtarget[0]) {
1466                                 bPoseChannel *pchant= get_pose_channel(data->tar->pose, data->subtarget);
1467                                 if(pchant) {
1468                                         if(data->flag != (ROTLIKE_X|ROTLIKE_Y|ROTLIKE_Z)) {
1469                                                 float eul[3], eult[3], euln[3], fac= con->enforce;
1470                                                 
1471                                                 QuatToEul(pchan->quat, eul);
1472                                                 QuatToEul(pchant->quat, eult);
1473                                                 VECCOPY(euln, eul);
1474                                                 if(data->flag & ROTLIKE_X) euln[0]= fac*eult[0] + (1.0f-fac)*eul[0];
1475                                                 if(data->flag & ROTLIKE_Y) euln[1]= fac*eult[1] + (1.0f-fac)*eul[1];
1476                                                 if(data->flag & ROTLIKE_Z) euln[2]= fac*eult[2] + (1.0f-fac)*eul[2];
1477                                                 compatible_eul(eul, euln);
1478                                                 EulToQuat(euln, pchan->quat);
1479                                         }
1480                                         else {
1481                                                 QuatInterpol(pchan->quat, pchan->quat, pchant->quat, con->enforce);
1482                                         }
1483                                 }
1484                         }
1485                 }
1486                         break;
1487                 case CONSTRAINT_TYPE_LOCLIMIT:
1488                 {
1489                         bLocLimitConstraint *data= con->data;
1490                         
1491                         /* Aligorith: don't know whether this function really evaluates constraints, but here goes anyways */
1492                         if (data->flag & LIMIT_XMIN) {
1493                                 if(pchan->loc[0] < data->xmin)
1494                                         pchan->loc[0] = data->xmin;
1495                         }
1496                         if (data->flag & LIMIT_XMAX) {
1497                                 if (pchan->loc[0] > data->xmax)
1498                                         pchan->loc[0] = data->xmax;
1499                         }
1500                         if (data->flag & LIMIT_YMIN) {
1501                                 if(pchan->loc[1] < data->ymin)
1502                                         pchan->loc[1] = data->ymin;
1503                         }
1504                         if (data->flag & LIMIT_YMAX) {
1505                                 if (pchan->loc[1] > data->ymax)
1506                                         pchan->loc[1] = data->ymax;
1507                         }
1508                         if (data->flag & LIMIT_ZMIN) {
1509                                 if(pchan->loc[2] < data->zmin)
1510                                         pchan->loc[2] = data->zmin;
1511                         }
1512                         if (data->flag & LIMIT_ZMAX) {
1513                                 if (pchan->loc[2] > data->zmax)
1514                                         pchan->loc[2] = data->zmax;
1515                         }
1516                 }       
1517                         break;
1518                 case CONSTRAINT_TYPE_ROTLIMIT:
1519                 {
1520                         bRotLimitConstraint *data = con->data;
1521                         float eul[3];
1522                         
1523                         /*Aligorith:  don't know whether this function is really for evaluating constraints, but here goes anyways */
1524                         
1525                         QuatToEul(pchan->quat, eul);
1526                         
1527                         /* eulers: radians to degrees! */
1528                         eul[0] = (eul[0] / (2*M_PI) * 360);
1529                         eul[1] = (eul[1] / (2*M_PI) * 360);
1530                         eul[2] = (eul[2] / (2*M_PI) * 360);
1531                         
1532                         /* limiting of euler values... */
1533                         if (data->flag & LIMIT_XROT) {
1534                                 if (eul[0] < data->xmin) 
1535                                         eul[0] = data->xmin;
1536                                         
1537                                 if (eul[0] > data->xmax)
1538                                         eul[0] = data->xmax;
1539                         }
1540                         if (data->flag & LIMIT_YROT) {
1541                                 if (eul[1] < data->ymin)
1542                                         eul[1] = data->ymin;
1543                                         
1544                                 if (eul[1] > data->ymax)
1545                                         eul[1] = data->ymax;
1546                         }
1547                         if (data->flag & LIMIT_ZROT) {
1548                                 if (eul[2] < data->zmin)
1549                                         eul[2] = data->zmin;
1550                                         
1551                                 if (eul[2] > data->zmax)
1552                                         eul[2] = data->zmax;
1553                         }
1554                                 
1555                         /* eulers: degrees to radians ! */
1556                         eul[0] = (eul[0] / 360 * (2*M_PI)); 
1557                         eul[1] = (eul[1] / 360 * (2*M_PI));
1558                         eul[2] = (eul[2] / 360 * (2*M_PI));
1559                         
1560                         /* convert back */
1561                         EulToQuat(eul, pchan->quat);
1562                 }
1563                         break;
1564         }
1565 }
1566
1567
1568 /* The main armature solver, does all constraints excluding IK */
1569 /* pchan is validated, as having bone and parent pointer */
1570 static void where_is_pose_bone(Object *ob, bPoseChannel *pchan, float ctime)
1571 {
1572         Bone *bone, *parbone;
1573         bPoseChannel *parchan;
1574         float vec[3], quat[4];
1575         int did_local= 0;       /* copying quaternion should be limited, chan_calc_mat() normalizes quat */
1576         
1577         /* set up variables for quicker access below */
1578         bone= pchan->bone;
1579         parbone= bone->parent;
1580         parchan= pchan->parent;
1581                 
1582         /* Do local constraints, these only work on the channel data (loc rot size) */
1583         QUATCOPY(quat, pchan->quat);
1584         if(pchan->constraints.first) {
1585                 bConstraint *con;
1586                 for(con=pchan->constraints.first; con; con= con->next) {
1587                         if(con->flag & CONSTRAINT_LOCAL) {
1588                                 do_local_constraint(pchan, con);
1589                                 did_local= 1;
1590                         }
1591                 }
1592         }
1593         
1594         /* this gives a chan_mat with actions (ipos) results */
1595         chan_calc_mat(pchan);
1596         
1597         if(did_local) 
1598                 QUATCOPY(pchan->quat, quat);    /* local constraint hack. bad! */
1599         
1600         /* construct the posemat based on PoseChannels, that we do before applying constraints */
1601         /* pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b) */
1602         
1603         if(parchan) {
1604                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
1605                 
1606                 /* bone transform itself */
1607                 Mat4CpyMat3(offs_bone, bone->bone_mat);
1608                 
1609                 /* The bone's root offset (is in the parent's coordinate system) */
1610                 VECCOPY(offs_bone[3], bone->head);
1611                 
1612                 /* Get the length translation of parent (length along y axis) */
1613                 offs_bone[3][1]+= parbone->length;
1614                 
1615                 /* Compose the matrix for this bone  */
1616                 if(bone->flag & BONE_HINGE) {   // uses restposition rotation, but actual position
1617                         float tmat[4][4];
1618                         
1619                         /* the rotation of the parent restposition */
1620                         Mat4CpyMat4(tmat, parbone->arm_mat);
1621                         
1622                         /* the location of actual parent transform */
1623                         VECCOPY(tmat[3], offs_bone[3]);
1624                         offs_bone[3][0]= offs_bone[3][1]= offs_bone[3][2]= 0.0f;
1625                         Mat4MulVecfl(parchan->pose_mat, tmat[3]);
1626                         
1627                         Mat4MulSerie(pchan->pose_mat, tmat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
1628                 }
1629                 else 
1630                         Mat4MulSerie(pchan->pose_mat, parchan->pose_mat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
1631         }
1632         else 
1633                 Mat4MulMat4(pchan->pose_mat, pchan->chan_mat, bone->arm_mat);
1634         
1635         
1636         /* Do constraints */
1637         if(pchan->constraints.first) {
1638                 static Object conOb;
1639                 static int initialized= 0;
1640                 
1641                 VECCOPY(vec, pchan->pose_mat[3]);
1642                 
1643                 /* Build a workob to pass the bone to the constraint solver */
1644                 if(initialized==0) {
1645                         memset(&conOb, 0, sizeof(Object));
1646                         initialized= 1;
1647                 }
1648                 conOb.size[0]= conOb.size[1]= conOb.size[2]= 1.0;
1649                 conOb.data = ob->data;
1650                 conOb.type = ob->type;
1651                 conOb.parent = ob;      // ik solver retrieves the armature that way !?!?!?!
1652                 conOb.pose= ob->pose;                           // needed for retrieving pchan
1653                 conOb.trackflag = ob->trackflag;
1654                 conOb.upflag = ob->upflag;
1655                 
1656                 /* Collect the constraints from the pose (listbase copy) */
1657                 conOb.constraints = pchan->constraints;
1658                 
1659                 /* conOb.obmat takes bone to worldspace */
1660                 Mat4MulMat4 (conOb.obmat, pchan->pose_mat, ob->obmat);
1661                 
1662                 /* Solve */
1663                 solve_constraints (&conOb, TARGET_BONE, (void*)pchan, ctime);   // ctime doesnt alter objects
1664                 
1665                 /* Take out of worldspace */
1666                 Mat4MulMat4 (pchan->pose_mat, conOb.obmat, ob->imat);
1667                 
1668                 /* prevent constraints breaking a chain */
1669                 if(pchan->bone->flag & BONE_CONNECTED)
1670                         VECCOPY(pchan->pose_mat[3], vec);
1671
1672         }
1673         
1674         /* calculate head */
1675         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
1676         /* calculate tail */
1677         VECCOPY(vec, pchan->pose_mat[1]);
1678         VecMulf(vec, bone->length);
1679         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
1680         
1681 }
1682
1683 /* This only reads anim data from channels, and writes to channels */
1684 /* This is the only function adding poses */
1685 void where_is_pose (Object *ob)
1686 {
1687         bArmature *arm;
1688         Bone *bone;
1689         bPoseChannel *pchan;
1690         float imat[4][4];
1691         float ctime= bsystem_time(ob, NULL, (float)G.scene->r.cfra, 0.0);       /* not accurate... */
1692         
1693         arm = get_armature(ob);
1694         
1695         if(arm==NULL) return;
1696         if(ob->pose==NULL || (ob->pose->flag & POSE_RECALC)) 
1697            armature_rebuild_pose(ob, arm);
1698         
1699         /* In restposition we read the data from the bones */
1700         if(ob==G.obedit || (arm->flag & ARM_RESTPOS)) {
1701                 
1702                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1703                         bone= pchan->bone;
1704                         if(bone) {
1705                                 Mat4CpyMat4(pchan->pose_mat, bone->arm_mat);
1706                                 VECCOPY(pchan->pose_head, bone->arm_head);
1707                                 VECCOPY(pchan->pose_tail, bone->arm_tail);
1708                         }
1709                 }
1710         }
1711         else {
1712                 Mat4Invert(ob->imat, ob->obmat);        // imat is needed 
1713
1714                 /* 1. construct the PoseTrees, clear flags */
1715                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1716                         pchan->flag &= ~(POSE_DONE|POSE_CHAIN);
1717                         if(pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints
1718                                 initialize_posetree(ob, pchan); // will attach it to root!
1719                 }
1720                 
1721                 /* 2. the main loop, channels are already hierarchical sorted from root to children */
1722                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1723                         
1724                         /* 3. if we find an IK root, we handle it separated */
1725                         if(pchan->iktree.first) {
1726                                 while(pchan->iktree.first) {
1727                                         PoseTree *tree= pchan->iktree.first;
1728                                         int a;
1729                                         
1730                                         /* 4. walk over the tree for regular solving */
1731                                         for(a=0; a<tree->totchannel; a++) {
1732                                                 if(!(tree->pchan[a]->flag & POSE_DONE)) // successive trees can set the flag
1733                                                         where_is_pose_bone(ob, tree->pchan[a], ctime);
1734                                         }
1735                                         /* 5. execute the IK solver */
1736                                         execute_posetree(ob, tree);
1737                                         
1738                                         /* 6. apply the differences to the channels, 
1739                                                   we need to calculate the original differences first */
1740                                         for(a=0; a<tree->totchannel; a++)
1741                                                 make_dmats(tree->pchan[a]);
1742                                         
1743                                         for(a=0; a<tree->totchannel; a++)
1744                                                 /* sets POSE_DONE */
1745                                                 where_is_ik_bone(tree->pchan[a], tree->basis_change[a]);
1746                                         
1747                                         /* 7. and free */
1748                                         BLI_remlink(&pchan->iktree, tree);
1749                                         free_posetree(tree);
1750                                 }
1751                         }
1752                         else if(!(pchan->flag & POSE_DONE)) {
1753                                 where_is_pose_bone(ob, pchan, ctime);
1754                         }
1755                 }
1756         }
1757                 
1758         /* calculating deform matrices */
1759         for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1760                 if(pchan->bone) {
1761                         Mat4Invert(imat, pchan->bone->arm_mat);
1762                         Mat4MulMat4(pchan->chan_mat, imat, pchan->pose_mat);
1763                 }
1764         }
1765 }