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