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