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