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