Quaternion Deform Interpolation
[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, int use_quaternion)
503 {
504         Bone *bone= pchan->bone;
505         Mat4 *b_bone= b_bone_spline_setup(pchan);
506         Mat4 *b_bone_mats;
507         DualQuat *b_bone_dual_quats= NULL;
508         float tmat[4][4];
509         int a;
510         
511         /* allocate b_bone matrices and dual quats */
512         b_bone_mats= MEM_mallocN((1+bone->segments)*sizeof(Mat4), "BBone defmats");
513         pchan->b_bone_mats= b_bone_mats;
514
515         if(use_quaternion) {
516                 b_bone_dual_quats= MEM_mallocN((bone->segments)*sizeof(DualQuat), "BBone dqs");
517                 pchan->b_bone_dual_quats= b_bone_dual_quats;
518         }
519         
520         /* first matrix is the inverse arm_mat, to bring points in local bone space
521            for finding out which segment it belongs to */
522         Mat4Invert(b_bone_mats[0].mat, bone->arm_mat);
523
524         /* then we make the b_bone_mats:
525             - first transform to local bone space
526                 - translate over the curve to the bbone mat space
527                 - transform with b_bone matrix
528                 - transform back into global space */
529         Mat4One(tmat);
530
531         for(a=0; a<bone->segments; a++) {
532                 tmat[3][1] = -a*(bone->length/(float)bone->segments);
533
534                 Mat4MulSerie(b_bone_mats[a+1].mat, pchan->chan_mat, bone->arm_mat,
535                         b_bone[a].mat, tmat, b_bone_mats[0].mat, NULL, NULL, NULL);
536
537                 if(use_quaternion)
538                         Mat4ToDQuat(bone->arm_mat, b_bone_mats[a+1].mat, &b_bone_dual_quats[a]);
539         }
540 }
541
542 static void b_bone_deform(bPoseChannel *pchan, Bone *bone, float *co, DualQuat *dq, float defmat[][3])
543 {
544         Mat4 *b_bone= pchan->b_bone_mats;
545         float (*mat)[4]= b_bone[0].mat;
546         float segment, y;
547         int a;
548         
549         /* need to transform co back to bonespace, only need y */
550         y= mat[0][1]*co[0] + mat[1][1]*co[1] + mat[2][1]*co[2] + mat[3][1];
551         
552         /* now calculate which of the b_bones are deforming this */
553         segment= bone->length/((float)bone->segments);
554         a= (int)(y/segment);
555         
556         /* note; by clamping it extends deform at endpoints, goes best with
557            straight joints in restpos. */
558         CLAMP(a, 0, bone->segments-1);
559
560         if(dq) {
561                 DQuatCpyDQuat(dq, &((DualQuat*)pchan->b_bone_dual_quats)[a]);
562         }
563         else {
564                 Mat4MulVecfl(b_bone[a+1].mat, co);
565
566                 if(defmat)
567                         Mat3CpyMat4(defmat, b_bone[a+1].mat);
568         }
569 }
570
571 /* using vec with dist to bone b1 - b2 */
572 float distfactor_to_bone (float vec[3], float b1[3], float b2[3], float rad1, float rad2, float rdist)
573 {
574         float dist=0.0f; 
575         float bdelta[3];
576         float pdelta[3];
577         float hsqr, a, l, rad;
578         
579         VecSubf (bdelta, b2, b1);
580         l = Normalize (bdelta);
581         
582         VecSubf (pdelta, vec, b1);
583         
584         a = bdelta[0]*pdelta[0] + bdelta[1]*pdelta[1] + bdelta[2]*pdelta[2];
585         hsqr = ((pdelta[0]*pdelta[0]) + (pdelta[1]*pdelta[1]) + (pdelta[2]*pdelta[2]));
586         
587         if (a < 0.0F){
588                 /* If we're past the end of the bone, do a spherical field attenuation thing */
589                 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])) ;
590                 rad= rad1;
591         }
592         else if (a > l){
593                 /* If we're past the end of the bone, do a spherical field attenuation thing */
594                 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])) ;
595                 rad= rad2;
596         }
597         else {
598                 dist= (hsqr - (a*a));
599                 
600                 if(l!=0.0f) {
601                         rad= a/l;
602                         rad= rad*rad2 + (1.0-rad)*rad1;
603                 }
604                 else rad= rad1;
605         }
606         
607         a= rad*rad;
608         if(dist < a) 
609                 return 1.0f;
610         else {
611                 l= rad+rdist;
612                 l*= l;
613                 if(rdist==0.0f || dist >= l) 
614                         return 0.0f;
615                 else {
616                         a= sqrt(dist)-rad;
617                         return 1.0-( a*a )/( rdist*rdist );
618                 }
619         }
620 }
621
622 static void pchan_deform_mat_add(bPoseChannel *pchan, float weight, float bbonemat[][3], float mat[][3])
623 {
624         float wmat[3][3];
625
626         if(pchan->bone->segments>1)
627                 Mat3CpyMat3(wmat, bbonemat);
628         else
629                 Mat3CpyMat4(wmat, pchan->chan_mat);
630
631         Mat3MulFloat((float*)wmat, weight);
632         Mat3AddMat3(mat, mat, wmat);
633 }
634
635 static float dist_bone_deform(bPoseChannel *pchan, float *vec, DualQuat *dq, float mat[][3], float *co)
636 {
637         Bone *bone= pchan->bone;
638         float fac, contrib=0.0;
639         float cop[3], bbonemat[3][3];
640         DualQuat bbonedq;
641
642         if(bone==NULL) return 0.0f;
643         
644         VECCOPY (cop, co);
645
646         fac= distfactor_to_bone(cop, bone->arm_head, bone->arm_tail, bone->rad_head, bone->rad_tail, bone->dist);
647         
648         if (fac>0.0) {
649                 
650                 fac*=bone->weight;
651                 contrib= fac;
652                 if(contrib>0.0) {
653                         if(vec) {
654                                 if(bone->segments>1)
655                                         // applies on cop and bbonemat
656                                         b_bone_deform(pchan, bone, cop, NULL, (mat)?bbonemat:NULL);
657                                 else
658                                         Mat4MulVecfl(pchan->chan_mat, cop);
659
660                                 //      Make this a delta from the base position
661                                 VecSubf (cop, cop, co);
662                                 cop[0]*=fac; cop[1]*=fac; cop[2]*=fac;
663                                 VecAddf (vec, vec, cop);
664
665                                 if(mat)
666                                         pchan_deform_mat_add(pchan, fac, bbonemat, mat);
667                         }
668                         else {
669                                 if(bone->segments>1) {
670                                         b_bone_deform(pchan, bone, cop, &bbonedq, NULL);
671                                         DQuatAddWeighted(dq, &bbonedq, fac);
672                                 }
673                                 else
674                                         DQuatAddWeighted(dq, pchan->dual_quat, fac);
675                         }
676                 }
677         }
678         
679         return contrib;
680 }
681
682 static void pchan_bone_deform(bPoseChannel *pchan, float weight, float *vec, DualQuat *dq, float mat[][3], float *co, float *contrib)
683 {
684         float cop[3], bbonemat[3][3];
685         DualQuat bbonedq;
686
687         if (!weight)
688                 return;
689
690         VECCOPY(cop, co);
691
692         if(vec) {
693                 if(pchan->bone->segments>1)
694                         // applies on cop and bbonemat
695                         b_bone_deform(pchan, pchan->bone, cop, NULL, (mat)?bbonemat:NULL);
696                 else
697                         Mat4MulVecfl(pchan->chan_mat, cop);
698                 
699                 vec[0]+=(cop[0]-co[0])*weight;
700                 vec[1]+=(cop[1]-co[1])*weight;
701                 vec[2]+=(cop[2]-co[2])*weight;
702
703                 if(mat)
704                         pchan_deform_mat_add(pchan, weight, bbonemat, mat);
705         }
706         else {
707                 if(pchan->bone->segments>1) {
708                         b_bone_deform(pchan, pchan->bone, cop, &bbonedq, NULL);
709                         DQuatAddWeighted(dq, &bbonedq, weight);
710                 }
711                 else
712                         DQuatAddWeighted(dq, pchan->dual_quat, weight);
713         }
714
715         (*contrib)+=weight;
716 }
717
718 void armature_deform_verts(Object *armOb, Object *target, DerivedMesh *dm,
719                            float (*vertexCos)[3], float (*defMats)[3][3],
720                                                    int numVerts, int deformflag, const char *defgrp_name)
721 {
722         bPoseChannel *pchan, **defnrToPC = NULL;
723         MDeformVert *dverts = NULL;
724         bDeformGroup *dg;
725         DualQuat *dualquats= NULL;
726         float obinv[4][4], premat[4][4], postmat[4][4];
727         int use_envelope = deformflag & ARM_DEF_ENVELOPE;
728         int use_quaternion = deformflag & ARM_DEF_QUATERNION;
729         int numGroups = 0;              /* safety for vertexgroup index overflow */
730         int i, target_totvert = 0;      /* safety for vertexgroup overflow */
731         int use_dverts = 0;
732         int armature_def_nr = -1;
733         int totchan;
734
735         if(armOb == G.obedit) return;
736         
737         Mat4Invert(obinv, target->obmat);
738         Mat4CpyMat4(premat, target->obmat);
739         Mat4MulMat4(postmat, armOb->obmat, obinv);
740         Mat4Invert(premat, postmat);
741
742         /* bone defmats are already in the channels, chan_mat */
743         
744         /* initialize B_bone matrices and dual quaternions */
745         if(use_quaternion) {
746                 totchan= BLI_countlist(&armOb->pose->chanbase);
747                 dualquats= MEM_callocN(sizeof(DualQuat)*totchan, "dualquats");
748         }
749
750         totchan= 0;
751         for(pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next) {
752                 if(!(pchan->bone->flag & BONE_NO_DEFORM)) {
753                         if(pchan->bone->segments > 1)
754                                 pchan_b_bone_defmats(pchan, use_quaternion);
755
756                         if(use_quaternion) {
757                                 pchan->dual_quat= &dualquats[totchan++];
758                                 Mat4ToDQuat(pchan->bone->arm_mat, pchan->chan_mat, pchan->dual_quat);
759                         }
760                 }
761         }
762
763         /* get the def_nr for the overall armature vertex group if present */
764         for(i = 0, dg = target->defbase.first; dg; i++, dg = dg->next)
765                 if(defgrp_name && strcmp(defgrp_name, dg->name) == 0)
766                         armature_def_nr = i;
767
768         /* get a vertex-deform-index to posechannel array */
769         if(deformflag & ARM_DEF_VGROUP) {
770                 if(ELEM(target->type, OB_MESH, OB_LATTICE)) {
771                         numGroups = BLI_countlist(&target->defbase);
772                         
773                         if(target->type==OB_MESH) {
774                                 Mesh *me= target->data;
775                                 dverts = me->dvert;
776                                 target_totvert = me->totvert;
777                         }
778                         else {
779                                 Lattice *lt= target->data;
780                                 dverts = lt->dvert;
781                                 if(dverts)
782                                         target_totvert = lt->pntsu*lt->pntsv*lt->pntsw;
783                         }
784                         /* if we have a DerivedMesh, only use dverts if it has them */
785                         if(dm)
786                                 if(dm->getVertData(dm, 0, CD_MDEFORMVERT))
787                                         use_dverts = 1;
788                                 else use_dverts = 0;
789                         else if(dverts) use_dverts = 1;
790
791                         if(use_dverts) {
792                                 defnrToPC = MEM_callocN(sizeof(*defnrToPC) * numGroups,
793                                                         "defnrToBone");
794                                 for(i = 0, dg = target->defbase.first; dg;
795                                     i++, dg = dg->next) {
796                                         defnrToPC[i] = get_pose_channel(armOb->pose, dg->name);
797                                         /* exclude non-deforming bones */
798                                         if(defnrToPC[i]) {
799                                                 if(defnrToPC[i]->bone->flag & BONE_NO_DEFORM)
800                                                         defnrToPC[i]= NULL;
801                                         }
802                                 }
803                         }
804                 }
805         }
806
807         for(i = 0; i < numVerts; i++) {
808                 MDeformVert *dvert;
809                 DualQuat sumdq, *dq = NULL;
810                 float *co = vertexCos[i];
811                 float sumvec[3], summat[3][3];
812                 float *vec = NULL, (*smat)[3] = NULL;
813                 float contrib = 0.0f;
814                 float armature_weight = 1.0f; /* default to 1 if no overall def group */
815                 int       j;
816
817                 if(use_quaternion) {
818                         memset(&sumdq, 0, sizeof(DualQuat));
819                         dq= &sumdq;
820                 }
821                 else {
822                         sumvec[0] = sumvec[1] = sumvec[2] = 0.0f;
823                         vec= sumvec;
824
825                         if(defMats) {
826                                 Mat3Clr((float*)summat);
827                                 smat = summat;
828                         }
829                 }
830
831                 if(use_dverts || armature_def_nr >= 0) {
832                         if(dm) dvert = dm->getVertData(dm, i, CD_MDEFORMVERT);
833                         else if(dverts && i < target_totvert) dvert = dverts + i;
834                         else dvert = NULL;
835                 } else
836                         dvert = NULL;
837
838                 if(armature_def_nr >= 0 && dvert) {
839                         armature_weight = 0.0f; /* a def group was given, so default to 0 */
840                         for(j = 0; j < dvert->totweight; j++) {
841                                 if(dvert->dw[j].def_nr == armature_def_nr) {
842                                         armature_weight = dvert->dw[j].weight;
843                                         break;
844                                 }
845                         }
846                 }
847
848                 /* check if there's any  point in calculating for this vert */
849                 if(armature_weight == 0.0f) continue;
850                 
851                 /* Apply the object's matrix */
852                 Mat4MulVecfl(premat, co);
853                 
854                 if(use_dverts && dvert && dvert->totweight) { // use weight groups ?
855                         int deformed = 0;
856                         
857                         for(j = 0; j < dvert->totweight; j++){
858                                 int index = dvert->dw[j].def_nr;
859                                 pchan = index < numGroups?defnrToPC[index]:NULL;
860                                 if(pchan) {
861                                         float weight = dvert->dw[j].weight;
862                                         Bone *bone = pchan->bone;
863
864                                         deformed = 1;
865                                         
866                                         if(bone && bone->flag & BONE_MULT_VG_ENV) {
867                                                 weight *= distfactor_to_bone(co, bone->arm_head,
868                                                                              bone->arm_tail,
869                                                                              bone->rad_head,
870                                                                              bone->rad_tail,
871                                                                              bone->dist);
872                                         }
873                                         pchan_bone_deform(pchan, weight, vec, dq, smat, co, &contrib);
874                                 }
875                         }
876                         /* if there are vertexgroups but not groups with bones
877                          * (like for softbody groups)
878                          */
879                         if(deformed == 0 && use_envelope) {
880                                 for(pchan = armOb->pose->chanbase.first; pchan;
881                                     pchan = pchan->next) {
882                                         if(!(pchan->bone->flag & BONE_NO_DEFORM))
883                                                 contrib += dist_bone_deform(pchan, vec, dq, smat, co);
884                                 }
885                         }
886                 }
887                 else if(use_envelope) {
888                         for(pchan = armOb->pose->chanbase.first; pchan;
889                             pchan = pchan->next) {
890                                 if(!(pchan->bone->flag & BONE_NO_DEFORM))
891                                         contrib += dist_bone_deform(pchan, vec, dq, smat, co);
892                         }
893                 }
894
895                 /* actually should be EPSILON? weight values and contrib can be like 10e-39 small */
896                 if(contrib > 0.0001f) {
897                         if(use_quaternion) {
898                                 DQuatNormalize(dq, contrib, armature_weight);
899                                 DQuatMulVecfl(dq, co, (defMats)? summat: NULL);
900                                 smat = summat;
901                         }
902                         else {
903                                 VecMulf(vec, armature_weight/contrib);
904                                 VecAddf(co, vec, co);
905                         }
906
907                         if(defMats) {
908                                 float pre[3][3], post[3][3], tmpmat[3][3];
909
910                                 Mat3CpyMat4(pre, premat);
911                                 Mat3CpyMat4(post, postmat);
912                                 Mat3CpyMat3(tmpmat, defMats[i]);
913
914                                 if(!use_quaternion) /* quaternion already is scale corrected */
915                                         Mat3MulFloat((float*)smat, armature_weight/contrib);
916
917                                 Mat3MulSerie(defMats[i], tmpmat, pre, smat, post,
918                                         NULL, NULL, NULL, NULL);
919                         }
920                 }
921                 
922                 /* always, check above code */
923                 Mat4MulVecfl(postmat, co);
924         }
925
926         if(dualquats) MEM_freeN(dualquats);
927         if(defnrToPC) MEM_freeN(defnrToPC);
928         
929         /* free B_bone matrices */
930         for(pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next) {
931                 if(pchan->b_bone_mats) {
932                         MEM_freeN(pchan->b_bone_mats);
933                         pchan->b_bone_mats = NULL;
934                 }
935                 if(pchan->b_bone_dual_quats) {
936                         MEM_freeN(pchan->b_bone_dual_quats);
937                         pchan->b_bone_dual_quats = NULL;
938                 }
939
940                 pchan->dual_quat = NULL;
941         }
942 }
943
944 /* ************ END Armature Deform ******************* */
945
946 void get_objectspace_bone_matrix (struct Bone* bone, float M_accumulatedMatrix[][4], int root, int posed)
947 {
948         Mat4CpyMat4(M_accumulatedMatrix, bone->arm_mat);
949 }
950
951 /* **************** Space to Space API ****************** */
952
953 /* Convert World-Space Matrix to Pose-Space Matrix */
954 void armature_mat_world_to_pose(Object *ob, float inmat[][4], float outmat[][4]) 
955 {
956         float obmat[4][4];
957         
958         /* prevent crashes */
959         if (ob==NULL) return;
960         
961         /* get inverse of (armature) object's matrix  */
962         Mat4Invert(obmat, ob->obmat);
963         
964         /* multiply given matrix by object's-inverse to find pose-space matrix */
965         Mat4MulMat4(outmat, obmat, inmat);
966 }
967
968 /* Convert Wolrd-Space Location to Pose-Space Location
969  * NOTE: this cannot be used to convert to pose-space location of the supplied
970  *              pose-channel into its local space (i.e. 'visual'-keyframing) 
971  */
972 void armature_loc_world_to_pose(Object *ob, float *inloc, float *outloc) 
973 {
974         float xLocMat[4][4];
975         float nLocMat[4][4];
976         
977         /* build matrix for location */
978         Mat4One(xLocMat);
979         VECCOPY(xLocMat[3], inloc);
980
981         /* get bone-space cursor matrix and extract location */
982         armature_mat_world_to_pose(ob, xLocMat, nLocMat);
983         VECCOPY(outloc, nLocMat[3]);
984 }
985
986 /* Convert Pose-Space Matrix to Bone-Space Matrix 
987  * NOTE: this cannot be used to convert to pose-space transforms of the supplied
988  *              pose-channel into its local space (i.e. 'visual'-keyframing)
989  */
990 void armature_mat_pose_to_bone(bPoseChannel *pchan, float inmat[][4], float outmat[][4])
991 {
992         float pc_trans[4][4], inv_trans[4][4];
993         float pc_posemat[4][4], inv_posemat[4][4];
994         
995         /* paranoia: prevent crashes with no pose-channel supplied */
996         if (pchan==NULL) return;
997         
998         /* get the inverse matrix of the pchan's transforms */
999         LocQuatSizeToMat4(pc_trans, pchan->loc, pchan->quat, pchan->size);
1000         Mat4Invert(inv_trans, pc_trans);
1001         
1002         /* Remove the pchan's transforms from it's pose_mat.
1003          * This should leave behind the effects of restpose + 
1004          * parenting + constraints
1005          */
1006         Mat4MulMat4(pc_posemat, inv_trans, pchan->pose_mat);
1007         
1008         /* get the inverse of the leftovers so that we can remove 
1009          * that component from the supplied matrix
1010          */
1011         Mat4Invert(inv_posemat, pc_posemat);
1012         
1013         /* get the new matrix */
1014         Mat4MulMat4(outmat, inmat, inv_posemat);
1015 }
1016
1017 /* Convert Pose-Space Location to Bone-Space Location
1018  * NOTE: this cannot be used to convert to pose-space location of the supplied
1019  *              pose-channel into its local space (i.e. 'visual'-keyframing) 
1020  */
1021 void armature_loc_pose_to_bone(bPoseChannel *pchan, float *inloc, float *outloc) 
1022 {
1023         float xLocMat[4][4];
1024         float nLocMat[4][4];
1025         
1026         /* build matrix for location */
1027         Mat4One(xLocMat);
1028         VECCOPY(xLocMat[3], inloc);
1029
1030         /* get bone-space cursor matrix and extract location */
1031         armature_mat_pose_to_bone(pchan, xLocMat, nLocMat);
1032         VECCOPY(outloc, nLocMat[3]);
1033 }
1034
1035 /* Remove rest-position effects from pose-transform for obtaining
1036  * 'visual' transformation of pose-channel. 
1037  * (used by the Visual-Keyframing stuff)
1038  */
1039 void armature_mat_pose_to_delta(float delta_mat[][4], float pose_mat[][4], float arm_mat[][4])
1040 {
1041         float imat[4][4];
1042  
1043         Mat4Invert(imat, arm_mat);
1044         Mat4MulMat4(delta_mat, pose_mat, imat);
1045 }
1046
1047
1048 /* **************** The new & simple (but OK!) armature evaluation ********* */ 
1049
1050 /*  ****************** And how it works! ****************************************
1051
1052   This is the bone transformation trick; they're hierarchical so each bone(b)
1053   is in the coord system of bone(b-1):
1054
1055   arm_mat(b)= arm_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) 
1056   
1057   -> yoffs is just the y axis translation in parent's coord system
1058   -> d_root is the translation of the bone root, also in parent's coord system
1059
1060   pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b)
1061
1062   we then - in init deform - store the deform in chan_mat, such that:
1063
1064   pose_mat(b)= arm_mat(b) * chan_mat(b)
1065   
1066   *************************************************************************** */
1067 /*  Computes vector and roll based on a rotation. "mat" must
1068      contain only a rotation, and no scaling. */ 
1069 void mat3_to_vec_roll(float mat[][3], float *vec, float *roll) 
1070 {
1071     if (vec)
1072         VecCopyf(vec, mat[1]);
1073
1074     if (roll) {
1075         float vecmat[3][3], vecmatinv[3][3], rollmat[3][3];
1076
1077         vec_roll_to_mat3(mat[1], 0.0f, vecmat);
1078         Mat3Inv(vecmatinv, vecmat);
1079         Mat3MulMat3(rollmat, vecmatinv, mat);
1080
1081         *roll= atan2(rollmat[2][0], rollmat[2][2]);
1082     }
1083 }
1084
1085 /*      Calculates the rest matrix of a bone based
1086         On its vector and a roll around that vector */
1087 void vec_roll_to_mat3(float *vec, float roll, float mat[][3])
1088 {
1089         float   nor[3], axis[3], target[3]={0,1,0};
1090         float   theta;
1091         float   rMatrix[3][3], bMatrix[3][3];
1092         
1093         VECCOPY (nor, vec);
1094         Normalize (nor);
1095         
1096         /*      Find Axis & Amount for bone matrix*/
1097         Crossf (axis,target,nor);
1098
1099         if (Inpf(axis,axis) > 0.0000000000001) {
1100                 /* if nor is *not* a multiple of target ... */
1101                 Normalize (axis);
1102                 
1103                 theta= NormalizedVecAngle2(target, nor);
1104                 
1105                 /*      Make Bone matrix*/
1106                 VecRotToMat3(axis, theta, bMatrix);
1107         }
1108         else {
1109                 /* if nor is a multiple of target ... */
1110                 float updown;
1111                 
1112                 /* point same direction, or opposite? */
1113                 updown = ( Inpf (target,nor) > 0 ) ? 1.0 : -1.0;
1114                 
1115                 /* I think this should work ... */
1116                 bMatrix[0][0]=updown; bMatrix[0][1]=0.0;    bMatrix[0][2]=0.0;
1117                 bMatrix[1][0]=0.0;    bMatrix[1][1]=updown; bMatrix[1][2]=0.0;
1118                 bMatrix[2][0]=0.0;    bMatrix[2][1]=0.0;    bMatrix[2][2]=1.0;
1119         }
1120         
1121         /*      Make Roll matrix*/
1122         VecRotToMat3(nor, roll, rMatrix);
1123         
1124         /*      Combine and output result*/
1125         Mat3MulMat3 (mat, rMatrix, bMatrix);
1126 }
1127
1128
1129 /* recursive part, calculates restposition of entire tree of children */
1130 /* used by exiting editmode too */
1131 void where_is_armature_bone(Bone *bone, Bone *prevbone)
1132 {
1133         float vec[3];
1134         
1135         /* Bone Space */
1136         VecSubf (vec, bone->tail, bone->head);
1137         vec_roll_to_mat3(vec, bone->roll, bone->bone_mat);
1138
1139         bone->length= VecLenf(bone->head, bone->tail);
1140         
1141         /* this is called on old file reading too... */
1142         if(bone->xwidth==0.0) {
1143                 bone->xwidth= 0.1f;
1144                 bone->zwidth= 0.1f;
1145                 bone->segments= 1;
1146         }
1147         
1148         if(prevbone) {
1149                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
1150                 
1151                 /* bone transform itself */
1152                 Mat4CpyMat3(offs_bone, bone->bone_mat);
1153                                 
1154                 /* The bone's root offset (is in the parent's coordinate system) */
1155                 VECCOPY(offs_bone[3], bone->head);
1156
1157                 /* Get the length translation of parent (length along y axis) */
1158                 offs_bone[3][1]+= prevbone->length;
1159                 
1160                 /* Compose the matrix for this bone  */
1161                 Mat4MulMat4(bone->arm_mat, offs_bone, prevbone->arm_mat);
1162         }
1163         else {
1164                 Mat4CpyMat3(bone->arm_mat, bone->bone_mat);
1165                 VECCOPY(bone->arm_mat[3], bone->head);
1166         }
1167         
1168         /* head */
1169         VECCOPY(bone->arm_head, bone->arm_mat[3]);
1170         /* tail is in current local coord system */
1171         VECCOPY(vec, bone->arm_mat[1]);
1172         VecMulf(vec, bone->length);
1173         VecAddf(bone->arm_tail, bone->arm_head, vec);
1174         
1175         /* and the kiddies */
1176         prevbone= bone;
1177         for(bone= bone->childbase.first; bone; bone= bone->next) {
1178                 where_is_armature_bone(bone, prevbone);
1179         }
1180 }
1181
1182 /* updates vectors and matrices on rest-position level, only needed 
1183    after editing armature itself, now only on reading file */
1184 void where_is_armature (bArmature *arm)
1185 {
1186         Bone *bone;
1187         
1188         /* hierarchical from root to children */
1189         for(bone= arm->bonebase.first; bone; bone= bone->next) {
1190                 where_is_armature_bone(bone, NULL);
1191         }
1192 }
1193
1194 /* if bone layer is protected, copy the data from from->pose */
1195 static void pose_proxy_synchronize(Object *ob, Object *from, int layer_protected)
1196 {
1197         bPose *pose= ob->pose, *frompose= from->pose;
1198         bPoseChannel *pchan, *pchanp, pchanw;
1199         bConstraint *con;
1200         char *str;
1201         
1202         if(frompose==NULL) return;
1203         
1204         /* exception, armature local layer should be proxied too */
1205         if(pose->proxy_layer)
1206                 ((bArmature *)ob->data)->layer= pose->proxy_layer;
1207         
1208         /* clear all transformation values from library */
1209         rest_pose(frompose);
1210         
1211         pchan= pose->chanbase.first;
1212         for(; pchan; pchan= pchan->next) {
1213                 if(pchan->bone->layer & layer_protected) {
1214                         pchanp= get_pose_channel(frompose, pchan->name);
1215                         
1216                         /* copy posechannel to temp, but restore important pointers */
1217                         pchanw= *pchanp;
1218                         pchanw.prev= pchan->prev;
1219                         pchanw.next= pchan->next;
1220                         pchanw.parent= pchan->parent;
1221                         pchanw.child= pchan->child;
1222                         pchanw.path= NULL;
1223                         
1224                         /* constraints, set target ob pointer to own object */
1225                         copy_constraints(&pchanw.constraints, &pchanp->constraints);
1226
1227                         for(con= pchanw.constraints.first; con; con= con->next) {
1228                                 if(from==get_constraint_target(con, &str))
1229                                         set_constraint_target(con, ob, NULL);
1230                         }
1231                         
1232                         /* free stuff from current channel */
1233                         if(pchan->path) MEM_freeN(pchan->path);
1234                         free_constraints(&pchan->constraints);
1235                         
1236                         /* the final copy */
1237                         *pchan= pchanw;
1238                 }
1239         }
1240 }
1241
1242 static int rebuild_pose_bone(bPose *pose, Bone *bone, bPoseChannel *parchan, int counter)
1243 {
1244         bPoseChannel *pchan = verify_pose_channel (pose, bone->name);   // verify checks and/or adds
1245
1246         pchan->bone= bone;
1247         pchan->parent= parchan;
1248         
1249         counter++;
1250         
1251         for(bone= bone->childbase.first; bone; bone= bone->next) {
1252                 counter= rebuild_pose_bone(pose, bone, pchan, counter);
1253                 /* for quick detecting of next bone in chain, only b-bone uses it now */
1254                 if(bone->flag & BONE_CONNECTED)
1255                         pchan->child= get_pose_channel(pose, bone->name);
1256         }
1257         
1258         return counter;
1259 }
1260
1261 /* only after leave editmode, duplicating, validating older files, library syncing */
1262 /* NOTE: pose->flag is set for it */
1263 void armature_rebuild_pose(Object *ob, bArmature *arm)
1264 {
1265         Bone *bone;
1266         bPose *pose;
1267         bPoseChannel *pchan, *next;
1268         int counter=0;
1269                 
1270         /* only done here */
1271         if(ob->pose==NULL) ob->pose= MEM_callocN(sizeof(bPose), "new pose");
1272         pose= ob->pose;
1273         
1274         /* clear */
1275         for(pchan= pose->chanbase.first; pchan; pchan= pchan->next) {
1276                 pchan->bone= NULL;
1277                 pchan->child= NULL;
1278         }
1279         
1280         /* first step, check if all channels are there */
1281         for(bone= arm->bonebase.first; bone; bone= bone->next) {
1282                 counter= rebuild_pose_bone(pose, bone, NULL, counter);
1283         }
1284
1285         /* and a check for garbage */
1286         for(pchan= pose->chanbase.first; pchan; pchan= next) {
1287                 next= pchan->next;
1288                 if(pchan->bone==NULL) {
1289                         if(pchan->path)
1290                                 MEM_freeN(pchan->path);
1291                         free_constraints(&pchan->constraints);
1292                         BLI_freelinkN(&pose->chanbase, pchan);
1293                 }
1294         }
1295         // printf("rebuild pose %s, %d bones\n", ob->id.name, counter);
1296         
1297         /* synchronize protected layers with proxy */
1298         if(ob->proxy)
1299                 pose_proxy_synchronize(ob, ob->proxy, arm->layer_protected);
1300         
1301         update_pose_constraint_flags(ob->pose); // for IK detection for example
1302         
1303         /* the sorting */
1304         if(counter>1)
1305                 DAG_pose_sort(ob);
1306         
1307         ob->pose->flag &= ~POSE_RECALC;
1308 }
1309
1310
1311 /* ********************** THE IK SOLVER ******************* */
1312
1313
1314
1315 /* allocates PoseTree, and links that to root bone/channel */
1316 /* Note: detecting the IK chain is duplicate code... in drawarmature.c and in transform_conversions.c */
1317 static void initialize_posetree(struct Object *ob, bPoseChannel *pchan_tip)
1318 {
1319         bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
1320         PoseTree *tree;
1321         PoseTarget *target;
1322         bConstraint *con;
1323         bKinematicConstraint *data;
1324         int a, segcount= 0, size, newsize, *oldparent, parent;
1325         
1326         /* find IK constraint, and validate it */
1327         for(con= pchan_tip->constraints.first; con; con= con->next) {
1328                 if(con->type==CONSTRAINT_TYPE_KINEMATIC) break;
1329         }
1330         if(con==NULL) return;
1331         
1332         data=(bKinematicConstraint*)con->data;
1333         
1334         /* two types of targets */
1335         if(data->flag & CONSTRAINT_IK_AUTO);
1336         else {
1337                 if(con->flag & CONSTRAINT_DISABLE) return;      /* checked in editconstraint.c */
1338                 if(con->enforce == 0.0f) return;
1339                 if(data->tar==NULL) return;
1340                 if(data->tar->type==OB_ARMATURE && data->subtarget[0]==0) return;
1341         }
1342         
1343         /* exclude tip from chain? */
1344         if(!(data->flag & CONSTRAINT_IK_TIP))
1345                 pchan_tip= pchan_tip->parent;
1346         
1347         /* Find the chain's root & count the segments needed */
1348         for (curchan = pchan_tip; curchan; curchan=curchan->parent){
1349                 pchan_root = curchan;
1350                 
1351                 curchan->flag |= POSE_CHAIN;    // don't forget to clear this
1352                 chanlist[segcount]=curchan;
1353                 segcount++;
1354                 
1355                 if(segcount==data->rootbone || segcount>255) break; // 255 is weak
1356         }
1357         if (!segcount) return;
1358
1359         /* setup the chain data */
1360         
1361         /* we make tree-IK, unless all existing targets are in this chain */
1362         for(tree= pchan_root->iktree.first; tree; tree= tree->next) {
1363                 for(target= tree->targets.first; target; target= target->next) {
1364                         curchan= tree->pchan[target->tip];
1365                         if(curchan->flag & POSE_CHAIN)
1366                                 curchan->flag &= ~POSE_CHAIN;
1367                         else
1368                                 break;
1369                 }
1370                 if(target) break;
1371         }
1372
1373         /* create a target */
1374         target= MEM_callocN(sizeof(PoseTarget), "posetarget");
1375         target->con= con;
1376         pchan_tip->flag &= ~POSE_CHAIN;
1377
1378         if(tree==NULL) {
1379                 /* make new tree */
1380                 tree= MEM_callocN(sizeof(PoseTree), "posetree");
1381
1382                 tree->iterations= data->iterations;
1383                 tree->totchannel= segcount;
1384                 tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
1385                 
1386                 tree->pchan= MEM_callocN(segcount*sizeof(void*), "ik tree pchan");
1387                 tree->parent= MEM_callocN(segcount*sizeof(int), "ik tree parent");
1388                 for(a=0; a<segcount; a++) {
1389                         tree->pchan[a]= chanlist[segcount-a-1];
1390                         tree->parent[a]= a-1;
1391                 }
1392                 target->tip= segcount-1;
1393                 
1394                 /* AND! link the tree to the root */
1395                 BLI_addtail(&pchan_root->iktree, tree);
1396         }
1397         else {
1398                 tree->iterations= MAX2(data->iterations, tree->iterations);
1399                 tree->stretch= tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
1400
1401                 /* skip common pose channels and add remaining*/
1402                 size= MIN2(segcount, tree->totchannel);
1403                 for(a=0; a<size && tree->pchan[a]==chanlist[segcount-a-1]; a++);
1404                 parent= a-1;
1405
1406                 segcount= segcount-a;
1407                 target->tip= tree->totchannel + segcount - 1;
1408
1409                 if (segcount > 0) {
1410                         /* resize array */
1411                         newsize= tree->totchannel + segcount;
1412                         oldchan= tree->pchan;
1413                         oldparent= tree->parent;
1414
1415                         tree->pchan= MEM_callocN(newsize*sizeof(void*), "ik tree pchan");
1416                         tree->parent= MEM_callocN(newsize*sizeof(int), "ik tree parent");
1417                         memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel);
1418                         memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel);
1419                         MEM_freeN(oldchan);
1420                         MEM_freeN(oldparent);
1421
1422                         /* add new pose channels at the end, in reverse order */
1423                         for(a=0; a<segcount; a++) {
1424                                 tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1];
1425                                 tree->parent[tree->totchannel+a]= tree->totchannel+a-1;
1426                         }
1427                         tree->parent[tree->totchannel]= parent;
1428                         
1429                         tree->totchannel= newsize;
1430                 }
1431
1432                 /* move tree to end of list, for correct evaluation order */
1433                 BLI_remlink(&pchan_root->iktree, tree);
1434                 BLI_addtail(&pchan_root->iktree, tree);
1435         }
1436
1437         /* add target to the tree */
1438         BLI_addtail(&tree->targets, target);
1439 }
1440
1441 /* called from within the core where_is_pose loop, all animsystems and constraints
1442 were executed & assigned. Now as last we do an IK pass */
1443 static void execute_posetree(Object *ob, PoseTree *tree)
1444 {
1445         float R_parmat[3][3];
1446         float iR_parmat[3][3];
1447         float R_bonemat[3][3];
1448         float goalrot[3][3], goalpos[3];
1449         float rootmat[4][4], imat[4][4];
1450         float goal[4][4], goalinv[4][4];
1451         float irest_basis[3][3], full_basis[3][3];
1452         float end_pose[4][4], world_pose[4][4];
1453         float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch=NULL;
1454         int a, flag, hasstretch=0;
1455         bPoseChannel *pchan;
1456         IK_Segment *seg, *parent, **iktree, *iktarget;
1457         IK_Solver *solver;
1458         PoseTarget *target;
1459         bKinematicConstraint *data;
1460         Bone *bone;
1461
1462         if (tree->totchannel == 0)
1463                 return;
1464
1465         iktree= MEM_mallocN(sizeof(void*)*tree->totchannel, "ik tree");
1466
1467         for(a=0; a<tree->totchannel; a++) {
1468                 pchan= tree->pchan[a];
1469                 bone= pchan->bone;
1470
1471                 /* set DoF flag */
1472                 flag= 0;
1473                 if((pchan->ikflag & BONE_IK_NO_XDOF) == 0)
1474                         flag |= IK_XDOF;
1475                 if((pchan->ikflag & BONE_IK_NO_YDOF) == 0)
1476                         flag |= IK_YDOF;
1477                 if((pchan->ikflag & BONE_IK_NO_ZDOF) == 0)
1478                         flag |= IK_ZDOF;
1479
1480                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
1481                         flag |= IK_TRANS_YDOF;
1482                         hasstretch = 1;
1483                 }
1484
1485                 seg= iktree[a]= IK_CreateSegment(flag);
1486
1487                 /* find parent */
1488                 if(a == 0)
1489                         parent= NULL;
1490                 else
1491                         parent= iktree[tree->parent[a]];
1492
1493                 IK_SetParent(seg, parent);
1494         
1495                 /* get the matrix that transforms from prevbone into this bone */
1496                 Mat3CpyMat4(R_bonemat, pchan->pose_mat);
1497
1498                 /* gather transformations for this IK segment */
1499
1500                 if (pchan->parent)
1501                         Mat3CpyMat4(R_parmat, pchan->parent->pose_mat);
1502                 else
1503                         Mat3One(R_parmat);
1504
1505                 /* bone offset */
1506                 if (pchan->parent && (a > 0))
1507                         VecSubf(start, pchan->pose_head, pchan->parent->pose_tail);
1508                 else
1509                         /* only root bone (a = 0) has no parent */
1510                         start[0]= start[1]= start[2]= 0.0f;
1511                 
1512                 /* change length based on bone size */
1513                 length= bone->length*VecLength(R_bonemat[1]);
1514
1515                 /* compute rest basis and its inverse */
1516                 Mat3CpyMat3(rest_basis, bone->bone_mat);
1517                 Mat3CpyMat3(irest_basis, bone->bone_mat);
1518                 Mat3Transp(irest_basis);
1519
1520                 /* compute basis with rest_basis removed */
1521                 Mat3Inv(iR_parmat, R_parmat);
1522                 Mat3MulMat3(full_basis, iR_parmat, R_bonemat);
1523                 Mat3MulMat3(basis, irest_basis, full_basis);
1524
1525                 /* basis must be pure rotation */
1526                 Mat3Ortho(basis);
1527
1528                 /* transform offset into local bone space */
1529                 Mat3Ortho(iR_parmat);
1530                 Mat3MulVecfl(iR_parmat, start);
1531
1532                 IK_SetTransform(seg, start, rest_basis, basis, length);
1533
1534                 if (pchan->ikflag & BONE_IK_XLIMIT)
1535                         IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]);
1536                 if (pchan->ikflag & BONE_IK_YLIMIT)
1537                         IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]);
1538                 if (pchan->ikflag & BONE_IK_ZLIMIT)
1539                         IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]);
1540
1541                 IK_SetStiffness(seg, IK_X, pchan->stiffness[0]);
1542                 IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]);
1543                 IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]);
1544
1545                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
1546                         float ikstretch = pchan->ikstretch*pchan->ikstretch;
1547                         IK_SetStiffness(seg, IK_TRANS_Y, MIN2(1.0-ikstretch, 0.99));
1548                         IK_SetLimit(seg, IK_TRANS_Y, 0.001, 1e10);
1549                 }
1550         }
1551
1552         solver= IK_CreateSolver(iktree[0]);
1553
1554         /* set solver goals */
1555
1556         /* first set the goal inverse transform, assuming the root of tree was done ok! */
1557         pchan= tree->pchan[0];
1558         if (pchan->parent)
1559                 /* transform goal by parent mat, so this rotation is not part of the
1560                    segment's basis. otherwise rotation limits do not work on the
1561                    local transform of the segment itself. */
1562                 Mat4CpyMat4(rootmat, pchan->parent->pose_mat);
1563         else
1564                 Mat4One(rootmat);
1565         VECCOPY(rootmat[3], pchan->pose_head);
1566         
1567         Mat4MulMat4 (imat, rootmat, ob->obmat);
1568         Mat4Invert (goalinv, imat);
1569         
1570         for(target=tree->targets.first; target; target=target->next) {
1571                 data= (bKinematicConstraint*)target->con->data;
1572
1573                 /* 1.0=ctime, we pass on object for auto-ik */
1574                 get_constraint_target_matrix(target->con, TARGET_BONE, ob, rootmat, 1.0);
1575
1576                 /* and set and transform goal */
1577                 Mat4MulMat4(goal, rootmat, goalinv);
1578
1579                 VECCOPY(goalpos, goal[3]);
1580                 Mat3CpyMat4(goalrot, goal);
1581
1582                 /* do we need blending? */
1583                 if(target->con->enforce!=1.0) {
1584                         float q1[4], q2[4], q[4];
1585                         float fac= target->con->enforce;
1586                         float mfac= 1.0-fac;
1587                         
1588                         pchan= tree->pchan[target->tip];
1589
1590                         /* end effector in world space */
1591                         Mat4CpyMat4(end_pose, pchan->pose_mat);
1592                         VECCOPY(end_pose[3], pchan->pose_tail);
1593                         Mat4MulSerie(world_pose, goalinv, ob->obmat, end_pose, 0, 0, 0, 0, 0);
1594
1595                         /* blend position */
1596                         goalpos[0]= fac*goalpos[0] + mfac*world_pose[3][0];
1597                         goalpos[1]= fac*goalpos[1] + mfac*world_pose[3][1];
1598                         goalpos[2]= fac*goalpos[2] + mfac*world_pose[3][2];
1599
1600                         /* blend rotation */
1601                         Mat3ToQuat(goalrot, q1);
1602                         Mat4ToQuat(world_pose, q2);
1603                         QuatInterpol(q, q1, q2, mfac);
1604                         QuatToMat3(q, goalrot);
1605                 }
1606
1607                 iktarget= iktree[target->tip];
1608
1609                 if(data->weight != 0.0)
1610                         IK_SolverAddGoal(solver, iktarget, goalpos, data->weight);
1611                 if((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0) && (data->flag & CONSTRAINT_IK_AUTO)==0)
1612                         IK_SolverAddGoalOrientation(solver, iktarget, goalrot, data->orientweight);
1613         }
1614
1615         /* solve */
1616         IK_Solve(solver, 0.0f, tree->iterations);
1617         IK_FreeSolver(solver);
1618
1619         /* gather basis changes */
1620         tree->basis_change= MEM_mallocN(sizeof(float[3][3])*tree->totchannel, "ik basis change");
1621         if(hasstretch)
1622                 ikstretch= MEM_mallocN(sizeof(float)*tree->totchannel, "ik stretch");
1623
1624         for(a=0; a<tree->totchannel; a++) {
1625                 IK_GetBasisChange(iktree[a], tree->basis_change[a]);
1626
1627                 if(hasstretch) {
1628                         /* have to compensate for scaling received from parent */
1629                         float parentstretch, stretch;
1630
1631                         pchan= tree->pchan[a];
1632                         parentstretch= (tree->parent[a] >= 0)? ikstretch[tree->parent[a]]: 1.0;
1633
1634                         if(tree->stretch && (pchan->ikstretch > 0.0)) {
1635                                 float trans[3], length;
1636
1637                                 IK_GetTranslationChange(iktree[a], trans);
1638                                 length= pchan->bone->length*VecLength(pchan->pose_mat[1]);
1639
1640                                 ikstretch[a]= (length == 0.0)? 1.0: (trans[1]+length)/length;
1641                         }
1642                         else
1643                                 ikstretch[a] = 1.0;
1644
1645                         stretch= (parentstretch == 0.0)? 1.0: ikstretch[a]/parentstretch;
1646
1647                         VecMulf(tree->basis_change[a][0], stretch);
1648                         VecMulf(tree->basis_change[a][1], stretch);
1649                         VecMulf(tree->basis_change[a][2], stretch);
1650
1651                 }
1652
1653                 IK_FreeSegment(iktree[a]);
1654         }
1655         
1656         MEM_freeN(iktree);
1657         if(ikstretch) MEM_freeN(ikstretch);
1658 }
1659
1660 void free_posetree(PoseTree *tree)
1661 {
1662         BLI_freelistN(&tree->targets);
1663         if(tree->pchan) MEM_freeN(tree->pchan);
1664         if(tree->parent) MEM_freeN(tree->parent);
1665         if(tree->basis_change) MEM_freeN(tree->basis_change);
1666         MEM_freeN(tree);
1667 }
1668
1669 /* ********************** THE POSE SOLVER ******************* */
1670
1671
1672 /* loc/rot/size to mat4 */
1673 /* used in constraint.c too */
1674 void chan_calc_mat(bPoseChannel *chan)
1675 {
1676         float smat[3][3];
1677         float rmat[3][3];
1678         float tmat[3][3];
1679         
1680         SizeToMat3(chan->size, smat);
1681         
1682         NormalQuat(chan->quat);
1683
1684         QuatToMat3(chan->quat, rmat);
1685         
1686         Mat3MulMat3(tmat, rmat, smat);
1687         
1688         Mat4CpyMat3(chan->chan_mat, tmat);
1689         
1690         /* prevent action channels breaking chains */
1691         /* need to check for bone here, CONSTRAINT_TYPE_ACTION uses this call */
1692         if (chan->bone==NULL || !(chan->bone->flag & BONE_CONNECTED)) {
1693                 VECCOPY(chan->chan_mat[3], chan->loc);
1694         }
1695
1696 }
1697
1698 /* transform from bone(b) to bone(b+1), store in chan_mat */
1699 static void make_dmats(bPoseChannel *pchan)
1700 {
1701         if (pchan->parent) {
1702                 float iR_parmat[4][4];
1703                 Mat4Invert(iR_parmat, pchan->parent->pose_mat);
1704                 Mat4MulMat4(pchan->chan_mat,  pchan->pose_mat, iR_parmat);      // delta mat
1705         }
1706         else Mat4CpyMat4(pchan->chan_mat, pchan->pose_mat);
1707 }
1708
1709 /* applies IK matrix to pchan, IK is done separated */
1710 /* formula: pose_mat(b) = pose_mat(b-1) * diffmat(b-1, b) * ik_mat(b) */
1711 /* to make this work, the diffmats have to be precalculated! Stored in chan_mat */
1712 static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[][3])   // nr = to detect if this is first bone
1713 {
1714         float vec[3], ikmat[4][4];
1715         
1716         Mat4CpyMat3(ikmat, ik_mat);
1717         
1718         if (pchan->parent)
1719                 Mat4MulSerie(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat, ikmat, NULL, NULL, NULL, NULL, NULL);
1720         else 
1721                 Mat4MulMat4(pchan->pose_mat, ikmat, pchan->chan_mat);
1722
1723         /* calculate head */
1724         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
1725         /* calculate tail */
1726         VECCOPY(vec, pchan->pose_mat[1]);
1727         VecMulf(vec, pchan->bone->length);
1728         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
1729
1730         pchan->flag |= POSE_DONE;
1731 }
1732
1733 /* NLA strip modifiers */
1734 static void do_strip_modifiers(Object *armob, Bone *bone, bPoseChannel *pchan)
1735 {
1736         bActionModifier *amod;
1737         bActionStrip *strip;
1738         float scene_cfra= G.scene->r.cfra;
1739
1740         for (strip=armob->nlastrips.first; strip; strip=strip->next) {
1741                 if(scene_cfra>=strip->start && scene_cfra<=strip->end) {
1742                         
1743                         /* temporal solution to prevent 2 strips accumulating */
1744                         if(scene_cfra==strip->end && strip->next && strip->next->start==scene_cfra)
1745                                 continue;
1746                         
1747                         for(amod= strip->modifiers.first; amod; amod= amod->next) {
1748                                 switch (amod->type) {
1749                                 case ACTSTRIP_MOD_DEFORM:
1750                                 {
1751                                         /* validate first */
1752                                         if(amod->ob && amod->ob->type==OB_CURVE && amod->channel[0]) {
1753                                                 
1754                                                 if( strcmp(pchan->name, amod->channel)==0 ) {
1755                                                         float mat4[4][4], mat3[3][3];
1756                                                         
1757                                                         curve_deform_vector(amod->ob, armob, bone->arm_mat[3], pchan->pose_mat[3], mat3, amod->no_rot_axis);
1758                                                         Mat4CpyMat4(mat4, pchan->pose_mat);
1759                                                         Mat4MulMat34(pchan->pose_mat, mat3, mat4);
1760                                                         
1761                                                 }
1762                                         }
1763                                 }
1764                                         break;
1765                                 case ACTSTRIP_MOD_NOISE:        
1766                                 {
1767                                         if( strcmp(pchan->name, amod->channel)==0 ) {
1768                                                 float nor[3], loc[3], ofs;
1769                                                 float eul[3], size[3], eulo[3], sizeo[3];
1770                                                 
1771                                                 /* calculate turbulance */
1772                                                 ofs = amod->turbul / 200.0f;
1773                                                 
1774                                                 /* make a copy of starting conditions */
1775                                                 VECCOPY(loc, pchan->pose_mat[3]);
1776                                                 Mat4ToEul(pchan->pose_mat, eul);
1777                                                 Mat4ToSize(pchan->pose_mat, size);
1778                                                 VECCOPY(eulo, eul);
1779                                                 VECCOPY(sizeo, size);
1780                                                 
1781                                                 /* apply noise to each set of channels */
1782                                                 if (amod->channels & 4) {
1783                                                         /* for scaling */
1784                                                         nor[0] = BLI_gNoise(amod->noisesize, size[0]+ofs, size[1], size[2], 0, 0) - ofs;
1785                                                         nor[1] = BLI_gNoise(amod->noisesize, size[0], size[1]+ofs, size[2], 0, 0) - ofs;        
1786                                                         nor[2] = BLI_gNoise(amod->noisesize, size[0], size[1], size[2]+ofs, 0, 0) - ofs;
1787                                                         VecAddf(size, size, nor);
1788                                                         
1789                                                         if (sizeo[0] != 0)
1790                                                                 VecMulf(pchan->pose_mat[0], size[0] / sizeo[0]);
1791                                                         if (sizeo[1] != 0)
1792                                                                 VecMulf(pchan->pose_mat[1], size[1] / sizeo[1]);
1793                                                         if (sizeo[2] != 0)
1794                                                                 VecMulf(pchan->pose_mat[2], size[2] / sizeo[2]);
1795                                                 }
1796                                                 if (amod->channels & 2) {
1797                                                         /* for rotation */
1798                                                         nor[0] = BLI_gNoise(amod->noisesize, eul[0]+ofs, eul[1], eul[2], 0, 0) - ofs;
1799                                                         nor[1] = BLI_gNoise(amod->noisesize, eul[0], eul[1]+ofs, eul[2], 0, 0) - ofs;   
1800                                                         nor[2] = BLI_gNoise(amod->noisesize, eul[0], eul[1], eul[2]+ofs, 0, 0) - ofs;
1801                                                         
1802                                                         compatible_eul(nor, eulo);
1803                                                         VecAddf(eul, eul, nor);
1804                                                         compatible_eul(eul, eulo);
1805                                                         
1806                                                         LocEulSizeToMat4(pchan->pose_mat, loc, eul, size);
1807                                                 }
1808                                                 if (amod->channels & 1) {
1809                                                         /* for location */
1810                                                         nor[0] = BLI_gNoise(amod->noisesize, loc[0]+ofs, loc[1], loc[2], 0, 0) - ofs;
1811                                                         nor[1] = BLI_gNoise(amod->noisesize, loc[0], loc[1]+ofs, loc[2], 0, 0) - ofs;   
1812                                                         nor[2] = BLI_gNoise(amod->noisesize, loc[0], loc[1], loc[2]+ofs, 0, 0) - ofs;
1813                                                         
1814                                                         VecAddf(pchan->pose_mat[3], loc, nor);
1815                                                 }
1816                                         }
1817                                 }
1818                                         break;
1819                                 }
1820                         }
1821                 }
1822         }
1823 }
1824
1825
1826 /* The main armature solver, does all constraints excluding IK */
1827 /* pchan is validated, as having bone and parent pointer */
1828 static void where_is_pose_bone(Object *ob, bPoseChannel *pchan, float ctime)
1829 {
1830         Bone *bone, *parbone;
1831         bPoseChannel *parchan;
1832         float vec[3];
1833         
1834         /* set up variables for quicker access below */
1835         bone= pchan->bone;
1836         parbone= bone->parent;
1837         parchan= pchan->parent;
1838         
1839         /* this gives a chan_mat with actions (ipos) results */
1840         chan_calc_mat(pchan);
1841         
1842         /* construct the posemat based on PoseChannels, that we do before applying constraints */
1843         /* pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b) */
1844         
1845         if(parchan) {
1846                 float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
1847                 
1848                 /* bone transform itself */
1849                 Mat4CpyMat3(offs_bone, bone->bone_mat);
1850                 
1851                 /* The bone's root offset (is in the parent's coordinate system) */
1852                 VECCOPY(offs_bone[3], bone->head);
1853                 
1854                 /* Get the length translation of parent (length along y axis) */
1855                 offs_bone[3][1]+= parbone->length;
1856                 
1857                 /* Compose the matrix for this bone  */
1858                 if(bone->flag & BONE_HINGE) {   // uses restposition rotation, but actual position
1859                         float tmat[4][4];
1860                         
1861                         /* the rotation of the parent restposition */
1862                         Mat4CpyMat4(tmat, parbone->arm_mat);
1863                         
1864                         /* the location of actual parent transform */
1865                         VECCOPY(tmat[3], offs_bone[3]);
1866                         offs_bone[3][0]= offs_bone[3][1]= offs_bone[3][2]= 0.0f;
1867                         Mat4MulVecfl(parchan->pose_mat, tmat[3]);
1868                         
1869                         Mat4MulSerie(pchan->pose_mat, tmat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
1870                 }
1871                 else 
1872                         Mat4MulSerie(pchan->pose_mat, parchan->pose_mat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
1873         }
1874         else {
1875                 Mat4MulMat4(pchan->pose_mat, pchan->chan_mat, bone->arm_mat);
1876                 /* only rootbones get the cyclic offset */
1877                 VecAddf(pchan->pose_mat[3], pchan->pose_mat[3], ob->pose->cyclic_offset);
1878         }
1879         
1880         /* do NLA strip modifiers - i.e. curve follow */
1881         do_strip_modifiers(ob, bone, pchan);
1882         
1883         /* Do constraints */
1884         if (pchan->constraints.first) {
1885                 bConstraintOb *cob;
1886                 
1887                 /* make a copy of location of PoseChannel for later */
1888                 VECCOPY(vec, pchan->pose_mat[3]);
1889                 
1890                 /* prepare PoseChannel for Constraint solving 
1891                  * - makes a copy of matrix, and creates temporary struct to use 
1892                  */
1893                 cob= constraints_make_evalob(ob, pchan, TARGET_BONE);
1894                 
1895                 /* Solve PoseChannel's Constraints */
1896                 solve_constraints(&pchan->constraints, cob, ctime);     // ctime doesnt alter objects
1897                 
1898                 /* cleanup after Constraint Solving 
1899                  * - applies matrix back to pchan, and frees temporary struct used
1900                  */
1901                 constraints_clear_evalob(cob);
1902                 
1903                 /* prevent constraints breaking a chain */
1904                 if(pchan->bone->flag & BONE_CONNECTED) {
1905                         VECCOPY(pchan->pose_mat[3], vec);
1906                 }
1907         }
1908         
1909         /* calculate head */
1910         VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
1911         /* calculate tail */
1912         VECCOPY(vec, pchan->pose_mat[1]);
1913         VecMulf(vec, bone->length);
1914         VecAddf(pchan->pose_tail, pchan->pose_head, vec);
1915 }
1916
1917 /* This only reads anim data from channels, and writes to channels */
1918 /* This is the only function adding poses */
1919 void where_is_pose (Object *ob)
1920 {
1921         bArmature *arm;
1922         Bone *bone;
1923         bPoseChannel *pchan;
1924         float imat[4][4];
1925         float ctime= bsystem_time(ob, NULL, (float)G.scene->r.cfra, 0.0);       /* not accurate... */
1926         
1927         arm = get_armature(ob);
1928         
1929         if(arm==NULL) return;
1930         if(ob->pose==NULL || (ob->pose->flag & POSE_RECALC)) 
1931            armature_rebuild_pose(ob, arm);
1932         
1933         /* In restposition we read the data from the bones */
1934         if(ob==G.obedit || (arm->flag & ARM_RESTPOS)) {
1935                 
1936                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1937                         bone= pchan->bone;
1938                         if(bone) {
1939                                 Mat4CpyMat4(pchan->pose_mat, bone->arm_mat);
1940                                 VECCOPY(pchan->pose_head, bone->arm_head);
1941                                 VECCOPY(pchan->pose_tail, bone->arm_tail);
1942                         }
1943                 }
1944         }
1945         else {
1946                 Mat4Invert(ob->imat, ob->obmat);        // imat is needed 
1947
1948                 /* 1. construct the PoseTrees, clear flags */
1949                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1950                         pchan->flag &= ~(POSE_DONE|POSE_CHAIN);
1951                         if(pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints
1952                                 initialize_posetree(ob, pchan); // will attach it to root!
1953                 }
1954                 
1955                 /* 2. the main loop, channels are already hierarchical sorted from root to children */
1956                 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1957                         
1958                         /* 3. if we find an IK root, we handle it separated */
1959                         if(pchan->iktree.first) {
1960                                 while(pchan->iktree.first) {
1961                                         PoseTree *tree= pchan->iktree.first;
1962                                         int a;
1963                                         
1964                                         /* 4. walk over the tree for regular solving */
1965                                         for(a=0; a<tree->totchannel; a++) {
1966                                                 if(!(tree->pchan[a]->flag & POSE_DONE)) // successive trees can set the flag
1967                                                         where_is_pose_bone(ob, tree->pchan[a], ctime);
1968                                         }
1969                                         /* 5. execute the IK solver */
1970                                         execute_posetree(ob, tree);
1971                                         
1972                                         /* 6. apply the differences to the channels, 
1973                                                   we need to calculate the original differences first */
1974                                         for(a=0; a<tree->totchannel; a++)
1975                                                 make_dmats(tree->pchan[a]);
1976                                         
1977                                         for(a=0; a<tree->totchannel; a++)
1978                                                 /* sets POSE_DONE */
1979                                                 where_is_ik_bone(tree->pchan[a], tree->basis_change[a]);
1980                                         
1981                                         /* 7. and free */
1982                                         BLI_remlink(&pchan->iktree, tree);
1983                                         free_posetree(tree);
1984                                 }
1985                         }
1986                         else if(!(pchan->flag & POSE_DONE)) {
1987                                 where_is_pose_bone(ob, pchan, ctime);
1988                         }
1989                 }
1990         }
1991                 
1992         /* calculating deform matrices */
1993         for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
1994                 if(pchan->bone) {
1995                         Mat4Invert(imat, pchan->bone->arm_mat);
1996                         Mat4MulMat4(pchan->chan_mat, imat, pchan->pose_mat);
1997                 }
1998         }
1999 }