Fix for weight painting errors, as reported by Bassam.
[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                                         Bone *bone= pchan->bone;
732                                         
733                                         if(bone && bone->flag & BONE_MULT_VG_ENV) {
734                                                 
735                                                 weight*= distfactor_to_bone(co, bone->arm_head, bone->arm_tail, bone->rad_head, bone->rad_tail, bone->dist);
736                                         }
737                                         pchan_bone_deform(pchan, weight, vec, co, &contrib);
738                                 }
739                         }
740                 }
741                 else if(use_envelope) {
742                         for(pchan= armOb->pose->chanbase.first; pchan; pchan= pchan->next) {
743                                 if(!(pchan->bone->flag & BONE_NO_DEFORM))
744                                         contrib+= dist_bone_deform(pchan, vec, co);
745                         }
746                 }
747
748                 if (contrib>0.0){
749                         vec[0]/=contrib;
750                         vec[1]/=contrib;
751                         vec[2]/=contrib;
752                 }
753
754                 VecAddf(co, vec, co);
755                 Mat4MulVecfl(postmat, co);
756         }
757
758         if (defnrToPC) MEM_freeN(defnrToPC);
759         
760         /* free B_bone matrices */
761         for(pchan= armOb->pose->chanbase.first; pchan; pchan= pchan->next) {
762                 if(pchan->b_bone_mats) {
763                         MEM_freeN(pchan->b_bone_mats);
764                         pchan->b_bone_mats= NULL;
765                 }
766         }
767         
768 }
769
770 /* ************ END Armature Deform ******************* */
771
772 void get_objectspace_bone_matrix (struct Bone* bone, float M_accumulatedMatrix[][4], int root, int posed)
773 {
774         Mat4CpyMat4(M_accumulatedMatrix, bone->arm_mat);
775 }
776
777
778 /* **************** The new & simple (but OK!) armature evaluation ********* */ 
779
780 /*  ****************** And how it works! ****************************************
781
782   This is the bone transformation trick; they're hierarchical so each bone(b)
783   is in the coord system of bone(b-1):
784
785   arm_mat(b)= arm_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) 
786   
787   -> yoffs is just the y axis translation in parent's coord system
788   -> d_root is the translation of the bone root, also in parent's coord system
789
790   pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b)
791
792   we then - in init deform - store the deform in chan_mat, such that:
793
794   pose_mat(b)= arm_mat(b) * chan_mat(b)
795   
796   *************************************************************************** */
797
798
799 /*      Calculates the rest matrix of a bone based
800         On its vector and a roll around that vector */
801 void vec_roll_to_mat3(float *vec, float roll, float mat[][3])
802 {
803         float   nor[3], axis[3], target[3]={0,1,0};
804         float   theta;
805         float   rMatrix[3][3], bMatrix[3][3];
806         
807         VECCOPY (nor, vec);
808         Normalise (nor);
809         
810         /*      Find Axis & Amount for bone matrix*/
811         Crossf (axis,target,nor);
812         
813         if (Inpf(axis,axis) > 0.0000000000001) {
814                 /* if nor is *not* a multiple of target ... */
815                 Normalise (axis);
816                 theta=(float) acos (Inpf (target,nor));
817                 
818                 /*      Make Bone matrix*/
819                 VecRotToMat3(axis, theta, bMatrix);
820         }
821         else {
822                 /* if nor is a multiple of target ... */
823                 float updown;
824                 
825                 /* point same direction, or opposite? */
826                 updown = ( Inpf (target,nor) > 0 ) ? 1.0 : -1.0;
827                 
828                 /* I think this should work ... */
829                 bMatrix[0][0]=updown; bMatrix[0][1]=0.0;    bMatrix[0][2]=0.0;
830                 bMatrix[1][0]=0.0;    bMatrix[1][1]=updown; bMatrix[1][2]=0.0;
831                 bMatrix[2][0]=0.0;    bMatrix[2][1]=0.0;    bMatrix[2][2]=1.0;
832         }
833         
834         /*      Make Roll matrix*/
835         VecRotToMat3(nor, roll, rMatrix);
836         
837         /*      Combine and output result*/
838         Mat3MulMat3 (mat, rMatrix, bMatrix);
839 }
840
841
842 /* recursive part, calculates restposition of entire tree of children */
843 /* used by exiting editmode too */
844 void where_is_armature_bone(Bone *bone, Bone *prevbone)
845 {
846         float vec[3];
847         
848         /* Bone Space */
849         VecSubf (vec, bone->tail, bone->head);
850         vec_roll_to_mat3(vec, bone->roll, bone->bone_mat);
851
852         bone->length= VecLenf(bone->head, bone->tail);
853         
854         /* this is called on old file reading too... */
855         if(bone->xwidth==0.0) {
856                 bone->xwidth= 0.1f;
857                 bone->zwidth= 0.1f;
858                 bone->segments= 1;
859         }
860         
861         if(prevbone) {
862                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
863                 
864                 /* bone transform itself */
865                 Mat4CpyMat3(offs_bone, bone->bone_mat);
866                                 
867                 /* The bone's root offset (is in the parent's coordinate system) */
868                 VECCOPY(offs_bone[3], bone->head);
869
870                 /* Get the length translation of parent (length along y axis) */
871                 offs_bone[3][1]+= prevbone->length;
872                 
873                 /* Compose the matrix for this bone  */
874                 Mat4MulMat4(bone->arm_mat, offs_bone, prevbone->arm_mat);
875         }
876         else {
877                 Mat4CpyMat3(bone->arm_mat, bone->bone_mat);
878                 VECCOPY(bone->arm_mat[3], bone->head);
879         }
880         
881         /* head */
882         VECCOPY(bone->arm_head, bone->arm_mat[3]);
883         /* tail is in current local coord system */
884         VECCOPY(vec, bone->arm_mat[1]);
885         VecMulf(vec, bone->length);
886         VecAddf(bone->arm_tail, bone->arm_head, vec);
887         
888         /* and the kiddies */
889         prevbone= bone;
890         for(bone= bone->childbase.first; bone; bone= bone->next) {
891                 where_is_armature_bone(bone, prevbone);
892         }
893 }
894
895 /* updates vectors and matrices on rest-position level, only needed 
896    after editing armature itself, now only on reading file */
897 void where_is_armature (bArmature *arm)
898 {
899         Bone *bone;
900         
901         /* hierarchical from root to children */
902         for(bone= arm->bonebase.first; bone; bone= bone->next) {
903                 where_is_armature_bone(bone, NULL);
904         }
905 }
906
907 static int rebuild_pose_bone(bPose *pose, Bone *bone, bPoseChannel *parchan, int counter)
908 {
909         bPoseChannel *pchan = verify_pose_channel (pose, bone->name);   // verify checks and/or adds
910
911         pchan->bone= bone;
912         pchan->parent= parchan;
913         
914         counter++;
915         
916         for(bone= bone->childbase.first; bone; bone= bone->next) {
917                 counter= rebuild_pose_bone(pose, bone, pchan, counter);
918                 /* for quick detecting of next bone in chain, only b-bone uses it now */
919                 if(bone->flag & BONE_CONNECTED)
920                         pchan->child= get_pose_channel(pose, bone->name);
921         }
922         
923         return counter;
924 }
925
926 /* only after leave editmode, but also for validating older files */
927 /* NOTE: pose->flag is set for it */
928 void armature_rebuild_pose(Object *ob, bArmature *arm)
929 {
930         Bone *bone;
931         bPose *pose;
932         bPoseChannel *pchan, *next;
933         int counter=0;
934                 
935         /* only done here */
936         if(ob->pose==NULL) ob->pose= MEM_callocN(sizeof(bPose), "new pose");
937         pose= ob->pose;
938         
939         /* clear */
940         for(pchan= pose->chanbase.first; pchan; pchan= pchan->next) {
941                 pchan->bone= NULL;
942                 pchan->child= NULL;
943         }
944         
945         /* first step, check if all channels are there */
946         for(bone= arm->bonebase.first; bone; bone= bone->next) {
947                 counter= rebuild_pose_bone(pose, bone, NULL, counter);
948         }
949         /* sort channels on dependency order, so we can walk the channel list */
950
951         /* and a check for garbage */
952         for(pchan= pose->chanbase.first; pchan; pchan= next) {
953                 next= pchan->next;
954                 if(pchan->bone==NULL) {
955                         BLI_freelinkN(&pose->chanbase, pchan);  // constraints?
956                 }
957         }
958         //printf("rebuild pose, %d bones\n", counter);
959         if(counter<2) return;
960         
961         update_pose_constraint_flags(ob->pose); // for IK detection for example
962         
963         /* the sorting */
964         DAG_pose_sort(ob);
965         
966         ob->pose->flag &= ~POSE_RECALC;
967 }
968
969
970 /* ********************** THE IK SOLVER ******************* */
971
972
973
974 /* allocates PoseTree, and links that to root bone/channel */
975 /* note; if we got this working, it can become static too? */
976 static void initialize_posetree(struct Object *ob, bPoseChannel *pchan_tip)
977 {
978         bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
979         PoseTree *tree;
980         PoseTarget *target;
981         bConstraint *con;
982         bKinematicConstraint *data;
983         int a, segcount= 0, size, newsize, *oldparent, parent;
984         
985         /* find IK constraint, and validate it */
986         for(con= pchan_tip->constraints.first; con; con= con->next) {
987                 if(con->type==CONSTRAINT_TYPE_KINEMATIC) break;
988         }
989         if(con==NULL) return;
990         if(con->flag & CONSTRAINT_DISABLE) return;  // not sure...
991         
992         data=(bKinematicConstraint*)con->data;
993         if(data->tar==NULL) return;
994         if(data->tar->type==OB_ARMATURE && data->subtarget[0]==0) return;
995         
996         /* exclude tip from chain? */
997         if(!(data->flag & CONSTRAINT_IK_TIP))
998                 pchan_tip= pchan_tip->parent;
999         
1000         /* Find the chain's root & count the segments needed */
1001         for (curchan = pchan_tip; curchan; curchan=curchan->parent){
1002                 pchan_root = curchan;
1003                 
1004                 curchan->flag |= POSE_CHAIN;    // don't forget to clear this
1005                 chanlist[segcount]=curchan;
1006                 segcount++;
1007                 
1008                 if(segcount==data->rootbone || segcount>255) break; // 255 is weak
1009         }
1010         if (!segcount) return;
1011
1012         /* setup the chain data */
1013         
1014         /* we make tree-IK, unless all existing targets are in this chain */
1015         for(tree= pchan_root->iktree.first; tree; tree= tree->next) {
1016                 for(target= tree->targets.first; target; target= target->next) {
1017                         curchan= tree->pchan[target->tip];
1018                         if(curchan->flag & POSE_CHAIN)
1019                                 curchan->flag &= ~POSE_CHAIN;
1020                         else
1021                                 break;
1022                 }
1023                 if(target) break;
1024         }
1025
1026         /* create a target */
1027         target= MEM_callocN(sizeof(PoseTarget), "posetarget");
1028         target->con= con;
1029         pchan_tip->flag &= ~POSE_CHAIN;
1030
1031         if(tree==NULL) {
1032                 /* make new tree */
1033                 tree= MEM_callocN(sizeof(PoseTree), "posetree");
1034
1035                 tree->tolerance= data->tolerance;
1036                 tree->iterations= data->iterations;
1037                 tree->totchannel= segcount;
1038                 
1039                 tree->pchan= MEM_callocN(segcount*sizeof(void*), "ik tree pchan");
1040                 tree->parent= MEM_callocN(segcount*sizeof(int), "ik tree parent");
1041                 for(a=0; a<segcount; a++) {
1042                         tree->pchan[a]= chanlist[segcount-a-1];
1043                         tree->parent[a]= a-1;
1044                 }
1045                 target->tip= segcount-1;
1046                 
1047                 /* AND! link the tree to the root */
1048                 BLI_addtail(&pchan_root->iktree, tree);
1049         }
1050         else {
1051                 tree->tolerance= MIN2(tree->tolerance, data->tolerance);
1052                 tree->iterations= MAX2(data->iterations, tree->iterations);
1053
1054                 /* skip common pose channels and add remaining*/
1055                 size= MIN2(segcount, tree->totchannel);
1056                 for(a=0; a<size && tree->pchan[a]==chanlist[segcount-a-1]; a++);
1057                 parent= a-1;
1058
1059                 segcount= segcount-a;
1060                 target->tip= tree->totchannel + segcount - 1;
1061
1062                 if (segcount > 0) {
1063                         /* resize array */
1064                         newsize= tree->totchannel + segcount;
1065                         oldchan= tree->pchan;
1066                         oldparent= tree->parent;
1067
1068                         tree->pchan= MEM_callocN(newsize*sizeof(void*), "ik tree pchan");
1069                         tree->parent= MEM_callocN(newsize*sizeof(int), "ik tree parent");
1070                         memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel);
1071                         memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel);
1072                         MEM_freeN(oldchan);
1073                         MEM_freeN(oldparent);
1074
1075                         /* add new pose channels at the end, in reverse order */
1076                         for(a=0; a<segcount; a++) {
1077                                 tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1];
1078                                 tree->parent[tree->totchannel+a]= tree->totchannel+a-1;
1079                         }
1080                         tree->parent[tree->totchannel]= parent;
1081                         
1082                         tree->totchannel= newsize;
1083                 }
1084
1085                 /* move tree to end of list, for correct evaluation order */
1086                 BLI_remlink(&pchan_root->iktree, tree);
1087                 BLI_addtail(&pchan_root->iktree, tree);
1088         }
1089
1090         /* add target to the tree */
1091         BLI_addtail(&tree->targets, target);
1092 }
1093
1094 /* called from within the core where_is_pose loop, all animsystems and constraints
1095 were executed & assigned. Now as last we do an IK pass */
1096 static void execute_posetree(Object *ob, PoseTree *tree)
1097 {
1098         float R_parmat[3][3];
1099         float iR_parmat[3][3];
1100         float R_bonemat[3][3];
1101         float goalrot[3][3], goalpos[3];
1102         float rootmat[4][4], imat[4][4];
1103         float goal[4][4], goalinv[4][4];
1104         float size[3], irest_basis[3][3], full_basis[3][3];
1105         float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch=NULL;
1106         int a, flag, hasstretch=0;
1107         bPoseChannel *pchan;
1108         IK_Segment *seg, *parent, **iktree, *iktarget;
1109         IK_Solver *solver;
1110         PoseTarget *target;
1111         bKinematicConstraint *data;
1112         Bone *bone;
1113
1114         if (tree->totchannel == 0)
1115                 return;
1116
1117         iktree= MEM_mallocN(sizeof(void*)*tree->totchannel, "ik tree");
1118
1119         for(a=0; a<tree->totchannel; a++) {
1120                 pchan= tree->pchan[a];
1121                 bone= pchan->bone;
1122
1123                 /* set DoF flag */
1124                 flag= 0;
1125                 if((pchan->ikflag & BONE_IK_NO_XDOF) == 0)
1126                         flag |= IK_XDOF;
1127                 if((pchan->ikflag & BONE_IK_NO_YDOF) == 0)
1128                         flag |= IK_YDOF;
1129                 if((pchan->ikflag & BONE_IK_NO_ZDOF) == 0)
1130                         flag |= IK_ZDOF;
1131
1132                 if(pchan->ikstretch > 0.0) {
1133                         flag |= IK_TRANS_YDOF;
1134                         hasstretch = 1;
1135                 }
1136
1137                 seg= iktree[a]= IK_CreateSegment(flag);
1138
1139                 /* find parent */
1140                 if(a == 0)
1141                         parent= NULL;
1142                 else
1143                         parent= iktree[tree->parent[a]];
1144
1145                 IK_SetParent(seg, parent);
1146         
1147                 /* get the matrix that transforms from prevbone into this bone */
1148                 Mat3CpyMat4(R_bonemat, pchan->pose_mat);
1149
1150                 /* gather transformations for this IK segment */
1151
1152                 if(a>0 && pchan->parent) {
1153                         Mat3CpyMat4(R_parmat, pchan->parent->pose_mat);
1154                         VECCOPY(start, bone->head); /* bone offset */
1155                 }
1156                 else {
1157                         Mat3One(R_parmat);
1158                         start[0]= start[1]= start[2]= 0.0f;
1159                 }
1160                 
1161                 /* change length based on bone size */
1162                 length= bone->length*VecLength(R_bonemat[1]);
1163
1164                 /* compute rest basis and its inverse */
1165                 Mat3CpyMat3(rest_basis, bone->bone_mat);
1166                 Mat3CpyMat3(irest_basis, bone->bone_mat);
1167                 Mat3Transp(irest_basis);
1168
1169                 /* compute basis with rest_basis removed */
1170                 Mat3Inv(iR_parmat, R_parmat);
1171                 Mat3MulMat3(full_basis, iR_parmat, R_bonemat);
1172                 Mat3MulMat3(basis, irest_basis, full_basis);
1173
1174                 /* basis must be pure rotation */
1175                 Mat3Ortho(basis);
1176
1177                 IK_SetTransform(seg, start, rest_basis, basis, length);
1178
1179                 if (pchan->ikflag & BONE_IK_XLIMIT)
1180                         IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]);
1181                 if (pchan->ikflag & BONE_IK_YLIMIT)
1182                         IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]);
1183                 if (pchan->ikflag & BONE_IK_ZLIMIT)
1184                         IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]);
1185
1186                 IK_SetStiffness(seg, IK_X, pchan->stiffness[0]);
1187                 IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]);
1188                 IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]);
1189
1190                 if(pchan->ikstretch > 0.0) {
1191                         float ikstretch = pchan->ikstretch*pchan->ikstretch;
1192                         IK_SetStiffness(seg, IK_TRANS_Y, MIN2(1.0-ikstretch, 0.99));
1193                         IK_SetLimit(seg, IK_TRANS_Y, 0.001, 1e10);
1194                 }
1195         }
1196
1197         solver= IK_CreateSolver(iktree[0]);
1198
1199         /* set solver goals */
1200
1201         /* first set the goal inverse transform, assuming the root of tree was done ok! */
1202         pchan= tree->pchan[0];
1203         Mat4One(rootmat);
1204         VECCOPY(rootmat[3], pchan->pose_head);
1205         
1206         Mat4MulMat4 (imat, rootmat, ob->obmat);
1207         Mat4Invert (goalinv, imat);
1208         
1209         for(target=tree->targets.first; target; target=target->next) {
1210                 data= (bKinematicConstraint*)target->con->data;
1211
1212                 /* 1.0=ctime */
1213                 get_constraint_target_matrix(target->con, TARGET_BONE, NULL, rootmat, size, 1.0);
1214
1215                 /* and set and transform goal */
1216                 Mat4MulMat4(goal, rootmat, goalinv);
1217
1218                 VECCOPY(goalpos, goal[3]);
1219                 Mat3CpyMat4(goalrot, goal);
1220
1221                 /* do we need blending? */
1222                 if(target->con->enforce!=1.0) {
1223                         float vec[3], q1[4], q2[4], q[4];
1224                         float fac= target->con->enforce;
1225                         float mfac= 1.0-fac;
1226                         
1227                         pchan= tree->pchan[target->tip];
1228
1229                         /* blend position */
1230                         VECCOPY(vec, pchan->pose_tail);
1231                         Mat4MulVecfl(ob->obmat, vec); // world space
1232                         Mat4MulVecfl(goalinv, vec);
1233                         goalpos[0]= fac*goalpos[0] + mfac*vec[0];
1234                         goalpos[1]= fac*goalpos[1] + mfac*vec[1];
1235                         goalpos[2]= fac*goalpos[2] + mfac*vec[2];
1236
1237                         /* blend rotation */
1238                         Mat3ToQuat(goalrot, q1);
1239                         Mat3CpyMat4(R_bonemat, pchan->pose_mat);
1240                         Mat3ToQuat(R_bonemat, q2);
1241                         QuatInterpol(q, q1, q2, mfac);
1242                         QuatToMat3(q, goalrot);
1243                 }
1244
1245                 iktarget= iktree[target->tip];
1246
1247                 if(data->weight != 0.0)
1248                         IK_SolverAddGoal(solver, iktarget, goalpos, data->weight);
1249                 if((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0))
1250                         IK_SolverAddGoalOrientation(solver, iktarget, goalrot, data->orientweight);
1251         }
1252
1253         /* solve */
1254         IK_Solve(solver, tree->tolerance, tree->iterations);
1255         IK_FreeSolver(solver);
1256
1257         /* gather basis changes */
1258         tree->basis_change= MEM_mallocN(sizeof(float[3][3])*tree->totchannel, "ik basis change");
1259         if(hasstretch)
1260                 ikstretch= MEM_mallocN(sizeof(float)*tree->totchannel, "ik stretch");
1261
1262         for(a=0; a<tree->totchannel; a++) {
1263                 IK_GetBasisChange(iktree[a], tree->basis_change[a]);
1264
1265                 if(hasstretch) {
1266                         /* have to compensate for scaling received from parent */
1267                         float parentstretch, stretch;
1268
1269                         pchan= tree->pchan[a];
1270                         parentstretch= (tree->parent[a] >= 0)? ikstretch[tree->parent[a]]: 1.0;
1271
1272                         if(pchan->ikstretch > 0.0) {
1273                                 float trans[3], length;
1274
1275                                 IK_GetTranslationChange(iktree[a], trans);
1276                                 length= pchan->bone->length*VecLength(pchan->pose_mat[1]);
1277
1278                                 ikstretch[a]= (length == 0.0)? 1.0: (trans[1]+length)/length;
1279                         }
1280                         else
1281                                 ikstretch[a] = 1.0;
1282
1283                         stretch= (parentstretch == 0.0)? 1.0: ikstretch[a]/parentstretch;
1284
1285                         VecMulf(tree->basis_change[a][0], stretch);
1286                         VecMulf(tree->basis_change[a][1], stretch);
1287                         VecMulf(tree->basis_change[a][2], stretch);
1288
1289                 }
1290
1291                 IK_FreeSegment(iktree[a]);
1292         }
1293         
1294         MEM_freeN(iktree);
1295         if(ikstretch) MEM_freeN(ikstretch);
1296 }
1297
1298 void free_posetree(PoseTree *tree)
1299 {
1300         BLI_freelistN(&tree->targets);
1301         if(tree->pchan) MEM_freeN(tree->pchan);
1302         if(tree->parent) MEM_freeN(tree->parent);
1303         if(tree->basis_change) MEM_freeN(tree->basis_change);
1304         MEM_freeN(tree);
1305 }
1306
1307 /* ********************** THE POSE SOLVER ******************* */
1308
1309
1310 /* loc/rot/size to mat4 */
1311 /* used in constraint.c too */
1312 void chan_calc_mat(bPoseChannel *chan)
1313 {
1314         float smat[3][3];
1315         float rmat[3][3];
1316         float tmat[3][3];
1317         
1318         SizeToMat3(chan->size, smat);
1319         
1320         NormalQuat(chan->quat);
1321         QuatToMat3(chan->quat, rmat);
1322         
1323         Mat3MulMat3(tmat, rmat, smat);
1324         
1325         Mat4CpyMat3(chan->chan_mat, tmat);
1326         
1327         /* prevent action channels breaking chains */
1328         /* need to check for bone here, CONSTRAINT_TYPE_ACTION uses this call */
1329         if (chan->bone==NULL || !(chan->bone->flag & BONE_CONNECTED)) {
1330                 VECCOPY(chan->chan_mat[3], chan->loc);
1331         }
1332
1333 }
1334
1335 /* transform from bone(b) to bone(b+1), store in chan_mat */
1336 static void make_dmats(bPoseChannel *pchan)
1337 {
1338         if (pchan->parent) {
1339                 float iR_parmat[4][4];
1340                 Mat4Invert(iR_parmat, pchan->parent->pose_mat);
1341                 Mat4MulMat4(pchan->chan_mat,  pchan->pose_mat, iR_parmat);      // delta mat
1342         }
1343         else Mat4CpyMat4(pchan->chan_mat, pchan->pose_mat);
1344 }
1345
1346 /* applies IK matrix to pchan, IK is done separated */
1347 /* formula: pose_mat(b) = pose_mat(b-1) * diffmat(b-1, b) * ik_mat(b) */
1348 /* to make this work, the diffmats have to be precalculated! Stored in chan_mat */
1349 static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[][3])   // nr = to detect if this is first bone
1350 {
1351         float vec[3], ikmat[4][4];
1352         
1353         Mat4CpyMat3(ikmat, ik_mat);
1354         
1355         if (pchan->parent)
1356                 Mat4MulSerie(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat, ikmat, NULL, NULL, NULL, NULL, NULL);
1357         else 
1358                 Mat4MulMat4(pchan->pose_mat, ikmat, pchan->chan_mat);
1359
1360         /* calculate head */
1361         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
1362         /* calculate tail */
1363         VECCOPY(vec, pchan->pose_mat[1]);
1364         VecMulf(vec, pchan->bone->length);
1365         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
1366
1367         pchan->flag |= POSE_DONE;
1368 }
1369
1370 /* The main armature solver, does all constraints excluding IK */
1371 /* pchan is validated, as having bone and parent pointer */
1372 static void where_is_pose_bone(Object *ob, bPoseChannel *pchan)
1373 {
1374         Bone *bone, *parbone;
1375         bPoseChannel *parchan;
1376         float vec[3], ctime= 1.0;   // ctime todo
1377
1378         /* set up variables for quicker access below */
1379         bone= pchan->bone;
1380         parbone= bone->parent;
1381         parchan= pchan->parent;
1382                 
1383         /* this gives a chan_mat with actions (ipos) results */
1384         chan_calc_mat(pchan);
1385         
1386         /* construct the posemat based on PoseChannels, that we do before applying constraints */
1387         /* pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b) */
1388         
1389         if(parchan) {
1390                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
1391                 
1392                 /* bone transform itself */
1393                 Mat4CpyMat3(offs_bone, bone->bone_mat);
1394                 
1395                 /* The bone's root offset (is in the parent's coordinate system) */
1396                 VECCOPY(offs_bone[3], bone->head);
1397                 
1398                 /* Get the length translation of parent (length along y axis) */
1399                 offs_bone[3][1]+= parbone->length;
1400                 
1401                 /* Compose the matrix for this bone  */
1402                 if(bone->flag & BONE_HINGE) {   // uses restposition rotation, but actual position
1403                         float tmat[4][4];
1404                         
1405                         /* the rotation of the parent restposition */
1406                         Mat4CpyMat4(tmat, parbone->arm_mat);
1407                         
1408                         /* the location of actual parent transform */
1409                         VECCOPY(tmat[3], offs_bone[3]);
1410                         offs_bone[3][0]= offs_bone[3][1]= offs_bone[3][2]= 0.0f;
1411                         Mat4MulVecfl(parchan->pose_mat, tmat[3]);
1412                         
1413                         Mat4MulSerie(pchan->pose_mat, tmat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
1414                 }
1415                 else 
1416                         Mat4MulSerie(pchan->pose_mat, parchan->pose_mat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
1417         }
1418         else 
1419                 Mat4MulMat4(pchan->pose_mat, pchan->chan_mat, bone->arm_mat);
1420         
1421         
1422         /* Do constraints */
1423         if(pchan->constraints.first) {
1424                 static Object conOb;
1425                 static int initialized= 0;
1426                 
1427                 VECCOPY(vec, pchan->pose_mat[3]);
1428                 
1429                 /* Build a workob to pass the bone to the constraint solver */
1430                 if(initialized==0) {
1431                         memset(&conOb, 0, sizeof(Object));
1432                         initialized= 1;
1433                 }
1434                 conOb.size[0]= conOb.size[1]= conOb.size[2]= 1.0;
1435                 conOb.data = ob->data;
1436                 conOb.type = ob->type;
1437                 conOb.parent = ob;      // ik solver retrieves the armature that way !?!?!?!
1438                 conOb.pose= ob->pose;                           // needed for retrieving pchan
1439                 conOb.trackflag = ob->trackflag;
1440                 conOb.upflag = ob->upflag;
1441                 
1442                 /* Collect the constraints from the pose (listbase copy) */
1443                 conOb.constraints = pchan->constraints;
1444                 
1445                 /* conOb.obmat takes bone to worldspace */
1446                 Mat4MulMat4 (conOb.obmat, pchan->pose_mat, ob->obmat);
1447                 
1448                 /* Solve */
1449                 solve_constraints (&conOb, TARGET_BONE, (void*)pchan, ctime);   // ctime doesnt alter objects
1450                 
1451                 /* Take out of worldspace */
1452                 Mat4MulMat4 (pchan->pose_mat, conOb.obmat, ob->imat);
1453                 
1454                 /* prevent constraints breaking a chain */
1455                 if(pchan->bone->flag & BONE_CONNECTED)
1456                         VECCOPY(pchan->pose_mat[3], vec);
1457
1458         }
1459         
1460         /* calculate head */
1461         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
1462         /* calculate tail */
1463         VECCOPY(vec, pchan->pose_mat[1]);
1464         VecMulf(vec, bone->length);
1465         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
1466         
1467 }
1468
1469 /* This only reads anim data from channels, and writes to channels */
1470 /* This is the only function adding poses */
1471 void where_is_pose (Object *ob)
1472 {
1473         bArmature *arm;
1474         Bone *bone;
1475         bPoseChannel *pchan;
1476         float imat[4][4];
1477
1478         arm = get_armature(ob);
1479         
1480         if(arm==NULL) return;
1481         if(ob->pose==NULL || (ob->pose->flag & POSE_RECALC)) 
1482            armature_rebuild_pose(ob, arm);
1483         
1484         /* In restposition we read the data from the bones */
1485         if(ob==G.obedit || (arm->flag & ARM_RESTPOS)) {
1486                 
1487                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1488                         bone= pchan->bone;
1489                         if(bone) {
1490                                 Mat4CpyMat4(pchan->pose_mat, bone->arm_mat);
1491                                 VECCOPY(pchan->pose_head, bone->arm_head);
1492                                 VECCOPY(pchan->pose_tail, bone->arm_tail);
1493                         }
1494                 }
1495         }
1496         else {
1497                 Mat4Invert(ob->imat, ob->obmat);        // imat is needed 
1498
1499                 /* 1. construct the PoseTrees, clear flags */
1500                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1501                         pchan->flag &= ~(POSE_DONE|POSE_CHAIN);
1502                         if(pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints
1503                                 initialize_posetree(ob, pchan); // will attach it to root!
1504                 }
1505                 
1506                 /* 2. the main loop, channels are already hierarchical sorted from root to children */
1507                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1508                         
1509                         /* 3. if we find an IK root, we handle it separated */
1510                         if(pchan->iktree.first) {
1511                                 while(pchan->iktree.first) {
1512                                         PoseTree *tree= pchan->iktree.first;
1513                                         int a;
1514                                         
1515                                         /* 4. walk over the tree for regular solving */
1516                                         for(a=0; a<tree->totchannel; a++) {
1517                                                 if(!(tree->pchan[a]->flag & POSE_DONE)) // successive trees can set the flag
1518                                                         where_is_pose_bone(ob, tree->pchan[a]);
1519                                         }
1520                                         /* 5. execute the IK solver */
1521                                         execute_posetree(ob, tree);
1522                                         
1523                                         /* 6. apply the differences to the channels, 
1524                                                   we need to calculate the original differences first */
1525                                         for(a=0; a<tree->totchannel; a++)
1526                                                 make_dmats(tree->pchan[a]);
1527                                         
1528                                         for(a=0; a<tree->totchannel; a++)
1529                                                 /* sets POSE_DONE */
1530                                                 where_is_ik_bone(tree->pchan[a], tree->basis_change[a]);
1531                                         
1532                                         /* 7. and free */
1533                                         BLI_remlink(&pchan->iktree, tree);
1534                                         free_posetree(tree);
1535                                 }
1536                         }
1537                         else if(!(pchan->flag & POSE_DONE)) {
1538                                 where_is_pose_bone(ob, pchan);
1539                         }
1540                 }
1541         }
1542                 
1543         /* calculating deform matrices */
1544         for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1545                 if(pchan->bone) {
1546                         Mat4Invert(imat, pchan->bone->arm_mat);
1547                         Mat4MulMat4(pchan->chan_mat, imat, pchan->pose_mat);
1548                 }
1549         }
1550 }
1551
1552 /* ****************** Game Blender functions, called by engine ************** */
1553
1554 /* NOTE: doesn't work at the moment!!! (ton) */
1555
1556 /* ugly Globals */
1557 static float g_premat[4][4];
1558 static float g_postmat[4][4];
1559
1560 void GB_build_mats (float parmat[][4], float obmat[][4], float premat[][4], float postmat[][4])
1561 {
1562         float obinv[4][4];
1563         
1564         Mat4Invert(obinv, obmat);
1565         Mat4CpyMat4(premat, obmat);
1566         Mat4MulMat4(postmat, parmat, obinv);
1567         
1568         Mat4Invert (premat, postmat);
1569 }
1570
1571 void GB_init_armature_deform(ListBase *defbase, float premat[][4], float postmat[][4])
1572 {
1573 //      g_defbase = defbase;
1574         Mat4CpyMat4 (g_premat, premat);
1575         Mat4CpyMat4 (g_postmat, postmat);
1576         
1577 }
1578
1579 void GB_validate_defgroups (Mesh *mesh, ListBase *defbase)
1580 {
1581         /* Should only be called when the mesh or armature changes */
1582 //      int j, i;
1583 //      MDeformVert *dvert;
1584         
1585 //      for (j=0; j<mesh->totvert; j++){
1586 //              dvert = mesh->dvert+j;
1587 //              for (i=0; i<dvert->totweight; i++)
1588 //                      dvert->dw[i].data = ((bDeformGroup*)BLI_findlink (defbase, dvert->dw[i].def_nr))->data;
1589 //      }
1590 }
1591
1592 void GB_calc_armature_deform (float *co, MDeformVert *dvert)
1593 {
1594         float vec[3]={0, 0, 0};
1595         float contrib = 0;
1596         int     i;
1597 //      bPoseChannel *pchan;
1598         
1599         Mat4MulVecfl(g_premat, co);
1600         
1601         for (i=0; i<dvert->totweight; i++){
1602 //              pchan = (bPoseChannel *)dvert->dw[i].data;
1603 //              if (pchan) pchan_bone_deform(pchan, dvert->dw[i].weight, vec, co, &contrib);
1604         }
1605         
1606         if (contrib){
1607                 vec[0]/=contrib;
1608                 vec[1]/=contrib;
1609                 vec[2]/=contrib;
1610         }
1611         
1612         VecAddf (co, vec, co);
1613         Mat4MulVecfl(g_postmat, co);
1614 }
1615
1616 /* ****************** END Game Blender functions, called by engine ************** */
1617