Armature drawing: custom shape scale options
[blender-staging.git] / source / blender / blenkernel / intern / armature.c
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
19  * All rights reserved.
20  *
21  * Contributor(s): Full recode, Ton Roosendaal, Crete 2005
22  *
23  * ***** END GPL LICENSE BLOCK *****
24  */
25
26 /** \file blender/blenkernel/intern/armature.c
27  *  \ingroup bke
28  */
29
30
31 #include <ctype.h>
32 #include <stdlib.h>
33 #include <math.h>
34 #include <string.h>
35 #include <stdio.h>
36 #include <float.h>
37
38 #include "MEM_guardedalloc.h"
39
40 #include "BLI_math.h"
41 #include "BLI_blenlib.h"
42 #include "BLI_utildefines.h"
43
44 #include "DNA_anim_types.h"
45 #include "DNA_armature_types.h"
46 #include "DNA_constraint_types.h"
47 #include "DNA_mesh_types.h"
48 #include "DNA_lattice_types.h"
49 #include "DNA_meshdata_types.h"
50 #include "DNA_scene_types.h"
51 #include "DNA_object_types.h"
52
53 #include "BKE_animsys.h"
54 #include "BKE_armature.h"
55 #include "BKE_action.h"
56 #include "BKE_anim.h"
57 #include "BKE_constraint.h"
58 #include "BKE_curve.h"
59 #include "BKE_depsgraph.h"
60 #include "BKE_DerivedMesh.h"
61 #include "BKE_deform.h"
62 #include "BKE_displist.h"
63 #include "BKE_global.h"
64 #include "BKE_idprop.h"
65 #include "BKE_library.h"
66 #include "BKE_lattice.h"
67 #include "BKE_main.h"
68 #include "BKE_object.h"
69 #include "BKE_scene.h"
70
71 #include "BIK_api.h"
72 #include "BKE_sketch.h"
73
74 /* **************** Generic Functions, data level *************** */
75
76 bArmature *BKE_armature_add(Main *bmain, const char *name)
77 {
78         bArmature *arm;
79
80         arm = BKE_libblock_alloc(bmain, ID_AR, name);
81         arm->deformflag = ARM_DEF_VGROUP | ARM_DEF_ENVELOPE;
82         arm->flag = ARM_COL_CUSTOM; /* custom bone-group colors */
83         arm->layer = 1;
84         return arm;
85 }
86
87 bArmature *BKE_armature_from_object(Object *ob)
88 {
89         if (ob->type == OB_ARMATURE)
90                 return (bArmature *)ob->data;
91         return NULL;
92 }
93
94 void BKE_armature_bonelist_free(ListBase *lb)
95 {
96         Bone *bone;
97
98         for (bone = lb->first; bone; bone = bone->next) {
99                 if (bone->prop) {
100                         IDP_FreeProperty(bone->prop);
101                         MEM_freeN(bone->prop);
102                 }
103                 BKE_armature_bonelist_free(&bone->childbase);
104         }
105
106         BLI_freelistN(lb);
107 }
108
109 void BKE_armature_free(bArmature *arm)
110 {
111         if (arm) {
112                 BKE_armature_bonelist_free(&arm->bonebase);
113
114                 /* free editmode data */
115                 if (arm->edbo) {
116                         BLI_freelistN(arm->edbo);
117
118                         MEM_freeN(arm->edbo);
119                         arm->edbo = NULL;
120                 }
121
122                 /* free sketch */
123                 if (arm->sketch) {
124                         freeSketch(arm->sketch);
125                         arm->sketch = NULL;
126                 }
127
128                 /* free animation data */
129                 if (arm->adt) {
130                         BKE_animdata_free(&arm->id);
131                         arm->adt = NULL;
132                 }
133         }
134 }
135
136 void BKE_armature_make_local(bArmature *arm)
137 {
138         Main *bmain = G.main;
139         bool is_local = false, is_lib = false;
140         Object *ob;
141
142         if (arm->id.lib == NULL)
143                 return;
144         if (arm->id.us == 1) {
145                 id_clear_lib_data(bmain, &arm->id);
146                 return;
147         }
148
149         for (ob = bmain->object.first; ob && ELEM(0, is_lib, is_local); ob = ob->id.next) {
150                 if (ob->data == arm) {
151                         if (ob->id.lib)
152                                 is_lib = true;
153                         else
154                                 is_local = true;
155                 }
156         }
157
158         if (is_local && is_lib == false) {
159                 id_clear_lib_data(bmain, &arm->id);
160         }
161         else if (is_local && is_lib) {
162                 bArmature *arm_new = BKE_armature_copy(arm);
163                 arm_new->id.us = 0;
164
165                 /* Remap paths of new ID using old library as base. */
166                 BKE_id_lib_local_paths(bmain, arm->id.lib, &arm_new->id);
167
168                 for (ob = bmain->object.first; ob; ob = ob->id.next) {
169                         if (ob->data == arm) {
170                                 if (ob->id.lib == NULL) {
171                                         ob->data = arm_new;
172                                         arm_new->id.us++;
173                                         arm->id.us--;
174                                 }
175                         }
176                 }
177         }
178 }
179
180 static void copy_bonechildren(Bone *newBone, Bone *oldBone, Bone *actBone, Bone **newActBone)
181 {
182         Bone *curBone, *newChildBone;
183
184         if (oldBone == actBone)
185                 *newActBone = newBone;
186
187         if (oldBone->prop)
188                 newBone->prop = IDP_CopyProperty(oldBone->prop);
189
190         /* Copy this bone's list */
191         BLI_duplicatelist(&newBone->childbase, &oldBone->childbase);
192
193         /* For each child in the list, update it's children */
194         newChildBone = newBone->childbase.first;
195         for (curBone = oldBone->childbase.first; curBone; curBone = curBone->next) {
196                 newChildBone->parent = newBone;
197                 copy_bonechildren(newChildBone, curBone, actBone, newActBone);
198                 newChildBone = newChildBone->next;
199         }
200 }
201
202 bArmature *BKE_armature_copy(bArmature *arm)
203 {
204         bArmature *newArm;
205         Bone *oldBone, *newBone;
206         Bone *newActBone = NULL;
207
208         newArm = BKE_libblock_copy(&arm->id);
209         BLI_duplicatelist(&newArm->bonebase, &arm->bonebase);
210
211         /* Duplicate the childrens' lists */
212         newBone = newArm->bonebase.first;
213         for (oldBone = arm->bonebase.first; oldBone; oldBone = oldBone->next) {
214                 newBone->parent = NULL;
215                 copy_bonechildren(newBone, oldBone, arm->act_bone, &newActBone);
216                 newBone = newBone->next;
217         }
218
219         newArm->act_bone = newActBone;
220
221         newArm->edbo = NULL;
222         newArm->act_edbone = NULL;
223         newArm->sketch = NULL;
224
225         if (arm->id.lib) {
226                 BKE_id_lib_local_paths(G.main, arm->id.lib, &newArm->id);
227         }
228
229         return newArm;
230 }
231
232 static Bone *get_named_bone_bonechildren(Bone *bone, const char *name)
233 {
234         Bone *curBone, *rbone;
235
236         if (STREQ(bone->name, name))
237                 return bone;
238
239         for (curBone = bone->childbase.first; curBone; curBone = curBone->next) {
240                 rbone = get_named_bone_bonechildren(curBone, name);
241                 if (rbone)
242                         return rbone;
243         }
244
245         return NULL;
246 }
247
248
249 /* Walk the list until the bone is found */
250 Bone *BKE_armature_find_bone_name(bArmature *arm, const char *name)
251 {
252         Bone *bone = NULL, *curBone;
253
254         if (!arm)
255                 return NULL;
256
257         for (curBone = arm->bonebase.first; curBone; curBone = curBone->next) {
258                 bone = get_named_bone_bonechildren(curBone, name);
259                 if (bone)
260                         return bone;
261         }
262
263         return bone;
264 }
265
266 bool BKE_armature_bone_flag_test_recursive(const Bone *bone, int flag)
267 {
268         if (bone->flag & flag) {
269                 return true;
270         }
271         else if (bone->parent) {
272                 return BKE_armature_bone_flag_test_recursive(bone->parent, flag);
273         }
274         else {
275                 return false;
276         }
277 }
278
279 /* Finds the best possible extension to the name on a particular axis. (For renaming, check for
280  * unique names afterwards) strip_number: removes number extensions  (TODO: not used)
281  * axis: the axis to name on
282  * head/tail: the head/tail co-ordinate of the bone on the specified axis */
283 int bone_autoside_name(char name[MAXBONENAME], int UNUSED(strip_number), short axis, float head, float tail)
284 {
285         unsigned int len;
286         char basename[MAXBONENAME] = "";
287         char extension[5] = "";
288
289         len = strlen(name);
290         if (len == 0)
291                 return 0;
292         BLI_strncpy(basename, name, sizeof(basename));
293
294         /* Figure out extension to append:
295          *      - The extension to append is based upon the axis that we are working on.
296          *      - If head happens to be on 0, then we must consider the tail position as well to decide
297          *        which side the bone is on
298          *              -> If tail is 0, then it's bone is considered to be on axis, so no extension should be added
299          *              -> Otherwise, extension is added from perspective of object based on which side tail goes to
300          *      - If head is non-zero, extension is added from perspective of object based on side head is on
301          */
302         if (axis == 2) {
303                 /* z-axis - vertical (top/bottom) */
304                 if (IS_EQF(head, 0.0f)) {
305                         if (tail < 0)
306                                 strcpy(extension, "Bot");
307                         else if (tail > 0)
308                                 strcpy(extension, "Top");
309                 }
310                 else {
311                         if (head < 0)
312                                 strcpy(extension, "Bot");
313                         else
314                                 strcpy(extension, "Top");
315                 }
316         }
317         else if (axis == 1) {
318                 /* y-axis - depth (front/back) */
319                 if (IS_EQF(head, 0.0f)) {
320                         if (tail < 0)
321                                 strcpy(extension, "Fr");
322                         else if (tail > 0)
323                                 strcpy(extension, "Bk");
324                 }
325                 else {
326                         if (head < 0)
327                                 strcpy(extension, "Fr");
328                         else
329                                 strcpy(extension, "Bk");
330                 }
331         }
332         else {
333                 /* x-axis - horizontal (left/right) */
334                 if (IS_EQF(head, 0.0f)) {
335                         if (tail < 0)
336                                 strcpy(extension, "R");
337                         else if (tail > 0)
338                                 strcpy(extension, "L");
339                 }
340                 else {
341                         if (head < 0)
342                                 strcpy(extension, "R");
343                         /* XXX Shouldn't this be simple else, as for z and y axes? */
344                         else if (head > 0)
345                                 strcpy(extension, "L");
346                 }
347         }
348
349         /* Simple name truncation
350          *      - truncate if there is an extension and it wouldn't be able to fit
351          *      - otherwise, just append to end
352          */
353         if (extension[0]) {
354                 bool changed = true;
355
356                 while (changed) { /* remove extensions */
357                         changed = false;
358                         if (len > 2 && basename[len - 2] == '.') {
359                                 if (basename[len - 1] == 'L' || basename[len - 1] == 'R') { /* L R */
360                                         basename[len - 2] = '\0';
361                                         len -= 2;
362                                         changed = true;
363                                 }
364                         }
365                         else if (len > 3 && basename[len - 3] == '.') {
366                                 if ((basename[len - 2] == 'F' && basename[len - 1] == 'r') || /* Fr */
367                                     (basename[len - 2] == 'B' && basename[len - 1] == 'k')) /* Bk */
368                                 {
369                                         basename[len - 3] = '\0';
370                                         len -= 3;
371                                         changed = true;
372                                 }
373                         }
374                         else if (len > 4 && basename[len - 4] == '.') {
375                                 if ((basename[len - 3] == 'T' && basename[len - 2] == 'o' && basename[len - 1] == 'p') || /* Top */
376                                     (basename[len - 3] == 'B' && basename[len - 2] == 'o' && basename[len - 1] == 't')) /* Bot */
377                                 {
378                                         basename[len - 4] = '\0';
379                                         len -= 4;
380                                         changed = true;
381                                 }
382                         }
383                 }
384
385                 if ((MAXBONENAME - len) < strlen(extension) + 1) { /* add 1 for the '.' */
386                         strncpy(name, basename, len - strlen(extension));
387                 }
388
389                 BLI_snprintf(name, MAXBONENAME, "%s.%s", basename, extension);
390
391                 return 1;
392         }
393
394         else
395                 return 0;
396 }
397
398 /* ************* B-Bone support ******************* */
399
400 /* data has MAX_BBONE_SUBDIV+1 interpolated points, will become desired amount with equal distances */
401 static void equalize_bezier(float *data, int desired)
402 {
403         float *fp, totdist, ddist, dist, fac1, fac2;
404         float pdist[MAX_BBONE_SUBDIV + 1];
405         float temp[MAX_BBONE_SUBDIV + 1][4];
406         int a, nr;
407
408         pdist[0] = 0.0f;
409         for (a = 0, fp = data; a < MAX_BBONE_SUBDIV; a++, fp += 4) {
410                 copy_qt_qt(temp[a], fp);
411                 pdist[a + 1] = pdist[a] + len_v3v3(fp, fp + 4);
412         }
413         /* do last point */
414         copy_qt_qt(temp[a], fp);
415         totdist = pdist[a];
416
417         /* go over distances and calculate new points */
418         ddist = totdist / ((float)desired);
419         nr = 1;
420         for (a = 1, fp = data + 4; a < desired; a++, fp += 4) {
421                 dist = ((float)a) * ddist;
422
423                 /* we're looking for location (distance) 'dist' in the array */
424                 while ((nr < MAX_BBONE_SUBDIV) && (dist >= pdist[nr]))
425                         nr++;
426
427                 fac1 = pdist[nr] - pdist[nr - 1];
428                 fac2 = pdist[nr] - dist;
429                 fac1 = fac2 / fac1;
430                 fac2 = 1.0f - fac1;
431
432                 fp[0] = fac1 * temp[nr - 1][0] + fac2 * temp[nr][0];
433                 fp[1] = fac1 * temp[nr - 1][1] + fac2 * temp[nr][1];
434                 fp[2] = fac1 * temp[nr - 1][2] + fac2 * temp[nr][2];
435                 fp[3] = fac1 * temp[nr - 1][3] + fac2 * temp[nr][3];
436         }
437         /* set last point, needed for orientation calculus */
438         copy_qt_qt(fp, temp[MAX_BBONE_SUBDIV]);
439 }
440
441 /* returns pointer to static array, filled with desired amount of bone->segments elements */
442 /* this calculation is done  within unit bone space */
443 void b_bone_spline_setup(bPoseChannel *pchan, int rest, Mat4 result_array[MAX_BBONE_SUBDIV])
444 {
445         bPoseChannel *next, *prev;
446         Bone *bone = pchan->bone;
447         float h1[3], h2[3], scale[3], length, hlength1, hlength2, roll1 = 0.0f, roll2;
448         float mat3[3][3], imat[4][4], posemat[4][4], scalemat[4][4], iscalemat[4][4];
449         float data[MAX_BBONE_SUBDIV + 1][4], *fp;
450         int a;
451         bool do_scale = false;
452
453         length = bone->length;
454
455         if (!rest) {
456                 /* check if we need to take non-uniform bone scaling into account */
457                 mat4_to_size(scale, pchan->pose_mat);
458
459                 if (fabsf(scale[0] - scale[1]) > 1e-6f || fabsf(scale[1] - scale[2]) > 1e-6f) {
460                         size_to_mat4(scalemat, scale);
461                         invert_m4_m4(iscalemat, scalemat);
462
463                         length *= scale[1];
464                         do_scale = 1;
465                 }
466         }
467
468         hlength1 = bone->ease1 * length * 0.390464f; /* 0.5f * sqrt(2) * kappa, the handle length for near-perfect circles */
469         hlength2 = bone->ease2 * length * 0.390464f;
470
471         /* evaluate next and prev bones */
472         if (bone->flag & BONE_CONNECTED)
473                 prev = pchan->parent;
474         else
475                 prev = NULL;
476
477         next = pchan->child;
478
479         /* find the handle points, since this is inside bone space, the
480          * first point = (0, 0, 0)
481          * last point =  (0, length, 0) */
482         if (rest) {
483                 invert_m4_m4(imat, pchan->bone->arm_mat);
484         }
485         else if (do_scale) {
486                 copy_m4_m4(posemat, pchan->pose_mat);
487                 normalize_m4(posemat);
488                 invert_m4_m4(imat, posemat);
489         }
490         else
491                 invert_m4_m4(imat, pchan->pose_mat);
492
493         if (prev) {
494                 float difmat[4][4], result[3][3], imat3[3][3];
495
496                 /* transform previous point inside this bone space */
497                 if (rest)
498                         copy_v3_v3(h1, prev->bone->arm_head);
499                 else
500                         copy_v3_v3(h1, prev->pose_head);
501                 mul_m4_v3(imat, h1);
502
503                 if (prev->bone->segments > 1) {
504                         /* if previous bone is B-bone too, use average handle direction */
505                         h1[1] -= length;
506                         roll1 = 0.0f;
507                 }
508
509                 normalize_v3(h1);
510                 mul_v3_fl(h1, -hlength1);
511
512                 if (prev->bone->segments == 1) {
513                         /* find the previous roll to interpolate */
514                         if (rest)
515                                 mul_m4_m4m4(difmat, imat, prev->bone->arm_mat);
516                         else
517                                 mul_m4_m4m4(difmat, imat, prev->pose_mat);
518                         copy_m3_m4(result, difmat); /* the desired rotation at beginning of next bone */
519
520                         vec_roll_to_mat3(h1, 0.0f, mat3); /* the result of vec_roll without roll */
521
522                         invert_m3_m3(imat3, mat3);
523                         mul_m3_m3m3(mat3, result, imat3); /* the matrix transforming vec_roll to desired roll */
524
525                         roll1 = atan2f(mat3[2][0], mat3[2][2]);
526                 }
527         }
528         else {
529                 h1[0] = 0.0f; h1[1] = hlength1; h1[2] = 0.0f;
530                 roll1 = 0.0f;
531         }
532         if (next) {
533                 float difmat[4][4], result[3][3], imat3[3][3];
534
535                 /* transform next point inside this bone space */
536                 if (rest)
537                         copy_v3_v3(h2, next->bone->arm_tail);
538                 else
539                         copy_v3_v3(h2, next->pose_tail);
540                 mul_m4_v3(imat, h2);
541
542                 /* if next bone is B-bone too, use average handle direction */
543                 if (next->bone->segments > 1) {
544                         /* pass */
545                 }
546                 else {
547                         h2[1] -= length;
548                 }
549                 normalize_v3(h2);
550
551                 /* find the next roll to interpolate as well */
552                 if (rest)
553                         mul_m4_m4m4(difmat, imat, next->bone->arm_mat);
554                 else
555                         mul_m4_m4m4(difmat, imat, next->pose_mat);
556                 copy_m3_m4(result, difmat); /* the desired rotation at beginning of next bone */
557
558                 vec_roll_to_mat3(h2, 0.0f, mat3); /* the result of vec_roll without roll */
559
560                 invert_m3_m3(imat3, mat3);
561                 mul_m3_m3m3(mat3, imat3, result); /* the matrix transforming vec_roll to desired roll */
562
563                 roll2 = atan2f(mat3[2][0], mat3[2][2]);
564
565                 /* and only now negate handle */
566                 mul_v3_fl(h2, -hlength2);
567         }
568         else {
569                 h2[0] = 0.0f; h2[1] = -hlength2; h2[2] = 0.0f;
570                 roll2 = 0.0;
571         }
572
573         /* make curve */
574         if (bone->segments > MAX_BBONE_SUBDIV)
575                 bone->segments = MAX_BBONE_SUBDIV;
576
577         BKE_curve_forward_diff_bezier(0.0f,  h1[0],                               h2[0],                               0.0f,   data[0],     MAX_BBONE_SUBDIV, 4 * sizeof(float));
578         BKE_curve_forward_diff_bezier(0.0f,  h1[1],                               length + h2[1],                      length, data[0] + 1, MAX_BBONE_SUBDIV, 4 * sizeof(float));
579         BKE_curve_forward_diff_bezier(0.0f,  h1[2],                               h2[2],                               0.0f,   data[0] + 2, MAX_BBONE_SUBDIV, 4 * sizeof(float));
580         BKE_curve_forward_diff_bezier(roll1, roll1 + 0.390464f * (roll2 - roll1), roll2 - 0.390464f * (roll2 - roll1), roll2,  data[0] + 3, MAX_BBONE_SUBDIV, 4 * sizeof(float));
581
582         equalize_bezier(data[0], bone->segments); /* note: does stride 4! */
583
584         /* make transformation matrices for the segments for drawing */
585         for (a = 0, fp = data[0]; a < bone->segments; a++, fp += 4) {
586                 sub_v3_v3v3(h1, fp + 4, fp);
587                 vec_roll_to_mat3(h1, fp[3], mat3); /* fp[3] is roll */
588
589                 copy_m4_m3(result_array[a].mat, mat3);
590                 copy_v3_v3(result_array[a].mat[3], fp);
591
592                 if (do_scale) {
593                         /* correct for scaling when this matrix is used in scaled space */
594                         mul_m4_series(result_array[a].mat, iscalemat, result_array[a].mat, scalemat);
595                 }
596         }
597 }
598
599 /* ************ Armature Deform ******************* */
600
601 typedef struct bPoseChanDeform {
602         Mat4     *b_bone_mats;
603         DualQuat *dual_quat;
604         DualQuat *b_bone_dual_quats;
605 } bPoseChanDeform;
606
607 static void pchan_b_bone_defmats(bPoseChannel *pchan, bPoseChanDeform *pdef_info, int use_quaternion)
608 {
609         Bone *bone = pchan->bone;
610         Mat4 b_bone[MAX_BBONE_SUBDIV], b_bone_rest[MAX_BBONE_SUBDIV];
611         Mat4 *b_bone_mats;
612         DualQuat *b_bone_dual_quats = NULL;
613         int a;
614
615         b_bone_spline_setup(pchan, 0, b_bone);
616         b_bone_spline_setup(pchan, 1, b_bone_rest);
617
618         /* allocate b_bone matrices and dual quats */
619         b_bone_mats = MEM_mallocN((1 + bone->segments) * sizeof(Mat4), "BBone defmats");
620         pdef_info->b_bone_mats = b_bone_mats;
621
622         if (use_quaternion) {
623                 b_bone_dual_quats = MEM_mallocN((bone->segments) * sizeof(DualQuat), "BBone dqs");
624                 pdef_info->b_bone_dual_quats = b_bone_dual_quats;
625         }
626
627         /* first matrix is the inverse arm_mat, to bring points in local bone space
628          * for finding out which segment it belongs to */
629         invert_m4_m4(b_bone_mats[0].mat, bone->arm_mat);
630
631         /* then we make the b_bone_mats:
632          * - first transform to local bone space
633          * - translate over the curve to the bbone mat space
634          * - transform with b_bone matrix
635          * - transform back into global space */
636
637         for (a = 0; a < bone->segments; a++) {
638                 float tmat[4][4];
639
640                 invert_m4_m4(tmat, b_bone_rest[a].mat);
641
642                 mul_m4_series(b_bone_mats[a + 1].mat, pchan->chan_mat, bone->arm_mat, b_bone[a].mat, tmat, b_bone_mats[0].mat);
643
644                 if (use_quaternion)
645                         mat4_to_dquat(&b_bone_dual_quats[a], bone->arm_mat, b_bone_mats[a + 1].mat);
646         }
647 }
648
649 static void b_bone_deform(bPoseChanDeform *pdef_info, Bone *bone, float co[3], DualQuat *dq, float defmat[3][3])
650 {
651         Mat4 *b_bone = pdef_info->b_bone_mats;
652         float (*mat)[4] = b_bone[0].mat;
653         float segment, y;
654         int a;
655
656         /* need to transform co back to bonespace, only need y */
657         y = mat[0][1] * co[0] + mat[1][1] * co[1] + mat[2][1] * co[2] + mat[3][1];
658
659         /* now calculate which of the b_bones are deforming this */
660         segment = bone->length / ((float)bone->segments);
661         a = (int)(y / segment);
662
663         /* note; by clamping it extends deform at endpoints, goes best with
664          * straight joints in restpos. */
665         CLAMP(a, 0, bone->segments - 1);
666
667         if (dq) {
668                 copy_dq_dq(dq, &(pdef_info->b_bone_dual_quats)[a]);
669         }
670         else {
671                 mul_m4_v3(b_bone[a + 1].mat, co);
672
673                 if (defmat) {
674                         copy_m3_m4(defmat, b_bone[a + 1].mat);
675                 }
676         }
677 }
678
679 /* using vec with dist to bone b1 - b2 */
680 float distfactor_to_bone(const float vec[3], const float b1[3], const float b2[3], float rad1, float rad2, float rdist)
681 {
682         float dist_sq;
683         float bdelta[3];
684         float pdelta[3];
685         float hsqr, a, l, rad;
686
687         sub_v3_v3v3(bdelta, b2, b1);
688         l = normalize_v3(bdelta);
689
690         sub_v3_v3v3(pdelta, vec, b1);
691
692         a = dot_v3v3(bdelta, pdelta);
693         hsqr = len_squared_v3(pdelta);
694
695         if (a < 0.0f) {
696                 /* If we're past the end of the bone, do a spherical field attenuation thing */
697                 dist_sq = len_squared_v3v3(b1, vec);
698                 rad = rad1;
699         }
700         else if (a > l) {
701                 /* If we're past the end of the bone, do a spherical field attenuation thing */
702                 dist_sq = len_squared_v3v3(b2, vec);
703                 rad = rad2;
704         }
705         else {
706                 dist_sq = (hsqr - (a * a));
707
708                 if (l != 0.0f) {
709                         rad = a / l;
710                         rad = rad * rad2 + (1.0f - rad) * rad1;
711                 }
712                 else
713                         rad = rad1;
714         }
715
716         a = rad * rad;
717         if (dist_sq < a)
718                 return 1.0f;
719         else {
720                 l = rad + rdist;
721                 l *= l;
722                 if (rdist == 0.0f || dist_sq >= l)
723                         return 0.0f;
724                 else {
725                         a = sqrtf(dist_sq) - rad;
726                         return 1.0f - (a * a) / (rdist * rdist);
727                 }
728         }
729 }
730
731 static void pchan_deform_mat_add(bPoseChannel *pchan, float weight, float bbonemat[3][3], float mat[3][3])
732 {
733         float wmat[3][3];
734
735         if (pchan->bone->segments > 1)
736                 copy_m3_m3(wmat, bbonemat);
737         else
738                 copy_m3_m4(wmat, pchan->chan_mat);
739
740         mul_m3_fl(wmat, weight);
741         add_m3_m3m3(mat, mat, wmat);
742 }
743
744 static float dist_bone_deform(bPoseChannel *pchan, bPoseChanDeform *pdef_info, float vec[3], DualQuat *dq,
745                               float mat[3][3], const float co[3])
746 {
747         Bone *bone = pchan->bone;
748         float fac, contrib = 0.0;
749         float cop[3], bbonemat[3][3];
750         DualQuat bbonedq;
751
752         if (bone == NULL)
753                 return 0.0f;
754
755         copy_v3_v3(cop, co);
756
757         fac = distfactor_to_bone(cop, bone->arm_head, bone->arm_tail, bone->rad_head, bone->rad_tail, bone->dist);
758
759         if (fac > 0.0f) {
760                 fac *= bone->weight;
761                 contrib = fac;
762                 if (contrib > 0.0f) {
763                         if (vec) {
764                                 if (bone->segments > 1)
765                                         /* applies on cop and bbonemat */
766                                         b_bone_deform(pdef_info, bone, cop, NULL, (mat) ? bbonemat : NULL);
767                                 else
768                                         mul_m4_v3(pchan->chan_mat, cop);
769
770                                 /* Make this a delta from the base position */
771                                 sub_v3_v3(cop, co);
772                                 madd_v3_v3fl(vec, cop, fac);
773
774                                 if (mat)
775                                         pchan_deform_mat_add(pchan, fac, bbonemat, mat);
776                         }
777                         else {
778                                 if (bone->segments > 1) {
779                                         b_bone_deform(pdef_info, bone, cop, &bbonedq, NULL);
780                                         add_weighted_dq_dq(dq, &bbonedq, fac);
781                                 }
782                                 else
783                                         add_weighted_dq_dq(dq, pdef_info->dual_quat, fac);
784                         }
785                 }
786         }
787
788         return contrib;
789 }
790
791 static void pchan_bone_deform(bPoseChannel *pchan, bPoseChanDeform *pdef_info, float weight, float vec[3], DualQuat *dq,
792                               float mat[3][3], const float co[3], float *contrib)
793 {
794         float cop[3], bbonemat[3][3];
795         DualQuat bbonedq;
796
797         if (!weight)
798                 return;
799
800         copy_v3_v3(cop, co);
801
802         if (vec) {
803                 if (pchan->bone->segments > 1)
804                         /* applies on cop and bbonemat */
805                         b_bone_deform(pdef_info, pchan->bone, cop, NULL, (mat) ? bbonemat : NULL);
806                 else
807                         mul_m4_v3(pchan->chan_mat, cop);
808
809                 vec[0] += (cop[0] - co[0]) * weight;
810                 vec[1] += (cop[1] - co[1]) * weight;
811                 vec[2] += (cop[2] - co[2]) * weight;
812
813                 if (mat)
814                         pchan_deform_mat_add(pchan, weight, bbonemat, mat);
815         }
816         else {
817                 if (pchan->bone->segments > 1) {
818                         b_bone_deform(pdef_info, pchan->bone, cop, &bbonedq, NULL);
819                         add_weighted_dq_dq(dq, &bbonedq, weight);
820                 }
821                 else
822                         add_weighted_dq_dq(dq, pdef_info->dual_quat, weight);
823         }
824
825         (*contrib) += weight;
826 }
827
828 void armature_deform_verts(Object *armOb, Object *target, DerivedMesh *dm, float (*vertexCos)[3],
829                            float (*defMats)[3][3], int numVerts, int deformflag,
830                            float (*prevCos)[3], const char *defgrp_name)
831 {
832         bPoseChanDeform *pdef_info_array;
833         bPoseChanDeform *pdef_info = NULL;
834         bArmature *arm = armOb->data;
835         bPoseChannel *pchan, **defnrToPC = NULL;
836         int *defnrToPCIndex = NULL;
837         MDeformVert *dverts = NULL;
838         bDeformGroup *dg;
839         DualQuat *dualquats = NULL;
840         float obinv[4][4], premat[4][4], postmat[4][4];
841         const short use_envelope = deformflag & ARM_DEF_ENVELOPE;
842         const short use_quaternion = deformflag & ARM_DEF_QUATERNION;
843         const short invert_vgroup = deformflag & ARM_DEF_INVERT_VGROUP;
844         int defbase_tot = 0;       /* safety for vertexgroup index overflow */
845         int i, target_totvert = 0; /* safety for vertexgroup overflow */
846         bool use_dverts = false;
847         int armature_def_nr;
848         int totchan;
849
850         if (arm->edbo) return;
851
852         invert_m4_m4(obinv, target->obmat);
853         copy_m4_m4(premat, target->obmat);
854         mul_m4_m4m4(postmat, obinv, armOb->obmat);
855         invert_m4_m4(premat, postmat);
856
857         /* bone defmats are already in the channels, chan_mat */
858
859         /* initialize B_bone matrices and dual quaternions */
860         totchan = BLI_listbase_count(&armOb->pose->chanbase);
861
862         if (use_quaternion) {
863                 dualquats = MEM_callocN(sizeof(DualQuat) * totchan, "dualquats");
864         }
865
866         pdef_info_array = MEM_callocN(sizeof(bPoseChanDeform) * totchan, "bPoseChanDeform");
867
868         totchan = 0;
869         pdef_info = pdef_info_array;
870         for (pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next, pdef_info++) {
871                 if (!(pchan->bone->flag & BONE_NO_DEFORM)) {
872                         if (pchan->bone->segments > 1)
873                                 pchan_b_bone_defmats(pchan, pdef_info, use_quaternion);
874
875                         if (use_quaternion) {
876                                 pdef_info->dual_quat = &dualquats[totchan++];
877                                 mat4_to_dquat(pdef_info->dual_quat, pchan->bone->arm_mat, pchan->chan_mat);
878                         }
879                 }
880         }
881
882         /* get the def_nr for the overall armature vertex group if present */
883         armature_def_nr = defgroup_name_index(target, defgrp_name);
884
885         if (ELEM(target->type, OB_MESH, OB_LATTICE)) {
886                 defbase_tot = BLI_listbase_count(&target->defbase);
887
888                 if (target->type == OB_MESH) {
889                         Mesh *me = target->data;
890                         dverts = me->dvert;
891                         if (dverts)
892                                 target_totvert = me->totvert;
893                 }
894                 else {
895                         Lattice *lt = target->data;
896                         dverts = lt->dvert;
897                         if (dverts)
898                                 target_totvert = lt->pntsu * lt->pntsv * lt->pntsw;
899                 }
900         }
901
902         /* get a vertex-deform-index to posechannel array */
903         if (deformflag & ARM_DEF_VGROUP) {
904                 if (ELEM(target->type, OB_MESH, OB_LATTICE)) {
905                         /* if we have a DerivedMesh, only use dverts if it has them */
906                         if (dm) {
907                                 use_dverts = (dm->getVertDataArray(dm, CD_MDEFORMVERT) != NULL);
908                         }
909                         else if (dverts) {
910                                 use_dverts = true;
911                         }
912
913                         if (use_dverts) {
914                                 defnrToPC = MEM_callocN(sizeof(*defnrToPC) * defbase_tot, "defnrToBone");
915                                 defnrToPCIndex = MEM_callocN(sizeof(*defnrToPCIndex) * defbase_tot, "defnrToIndex");
916                                 for (i = 0, dg = target->defbase.first; dg; i++, dg = dg->next) {
917                                         defnrToPC[i] = BKE_pose_channel_find_name(armOb->pose, dg->name);
918                                         /* exclude non-deforming bones */
919                                         if (defnrToPC[i]) {
920                                                 if (defnrToPC[i]->bone->flag & BONE_NO_DEFORM) {
921                                                         defnrToPC[i] = NULL;
922                                                 }
923                                                 else {
924                                                         defnrToPCIndex[i] = BLI_findindex(&armOb->pose->chanbase, defnrToPC[i]);
925                                                 }
926                                         }
927                                 }
928                         }
929                 }
930         }
931
932         for (i = 0; i < numVerts; i++) {
933                 MDeformVert *dvert;
934                 DualQuat sumdq, *dq = NULL;
935                 float *co, dco[3];
936                 float sumvec[3], summat[3][3];
937                 float *vec = NULL, (*smat)[3] = NULL;
938                 float contrib = 0.0f;
939                 float armature_weight = 1.0f; /* default to 1 if no overall def group */
940                 float prevco_weight = 1.0f;   /* weight for optional cached vertexcos */
941
942                 if (use_quaternion) {
943                         memset(&sumdq, 0, sizeof(DualQuat));
944                         dq = &sumdq;
945                 }
946                 else {
947                         sumvec[0] = sumvec[1] = sumvec[2] = 0.0f;
948                         vec = sumvec;
949
950                         if (defMats) {
951                                 zero_m3(summat);
952                                 smat = summat;
953                         }
954                 }
955
956                 if (use_dverts || armature_def_nr != -1) {
957                         if (dm)
958                                 dvert = dm->getVertData(dm, i, CD_MDEFORMVERT);
959                         else if (dverts && i < target_totvert)
960                                 dvert = dverts + i;
961                         else
962                                 dvert = NULL;
963                 }
964                 else
965                         dvert = NULL;
966
967                 if (armature_def_nr != -1 && dvert) {
968                         armature_weight = defvert_find_weight(dvert, armature_def_nr);
969
970                         if (invert_vgroup)
971                                 armature_weight = 1.0f - armature_weight;
972
973                         /* hackish: the blending factor can be used for blending with prevCos too */
974                         if (prevCos) {
975                                 prevco_weight = armature_weight;
976                                 armature_weight = 1.0f;
977                         }
978                 }
979
980                 /* check if there's any  point in calculating for this vert */
981                 if (armature_weight == 0.0f)
982                         continue;
983
984                 /* get the coord we work on */
985                 co = prevCos ? prevCos[i] : vertexCos[i];
986
987                 /* Apply the object's matrix */
988                 mul_m4_v3(premat, co);
989
990                 if (use_dverts && dvert && dvert->totweight) { /* use weight groups ? */
991                         MDeformWeight *dw = dvert->dw;
992                         int deformed = 0;
993                         unsigned int j;
994
995                         for (j = dvert->totweight; j != 0; j--, dw++) {
996                                 const int index = dw->def_nr;
997                                 if (index >= 0 && index < defbase_tot && (pchan = defnrToPC[index])) {
998                                         float weight = dw->weight;
999                                         Bone *bone = pchan->bone;
1000                                         pdef_info = pdef_info_array + defnrToPCIndex[index];
1001
1002                                         deformed = 1;
1003
1004                                         if (bone && bone->flag & BONE_MULT_VG_ENV) {
1005                                                 weight *= distfactor_to_bone(co, bone->arm_head, bone->arm_tail,
1006                                                                              bone->rad_head, bone->rad_tail, bone->dist);
1007                                         }
1008                                         pchan_bone_deform(pchan, pdef_info, weight, vec, dq, smat, co, &contrib);
1009                                 }
1010                         }
1011                         /* if there are vertexgroups but not groups with bones
1012                          * (like for softbody groups) */
1013                         if (deformed == 0 && use_envelope) {
1014                                 pdef_info = pdef_info_array;
1015                                 for (pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next, pdef_info++) {
1016                                         if (!(pchan->bone->flag & BONE_NO_DEFORM))
1017                                                 contrib += dist_bone_deform(pchan, pdef_info, vec, dq, smat, co);
1018                                 }
1019                         }
1020                 }
1021                 else if (use_envelope) {
1022                         pdef_info = pdef_info_array;
1023                         for (pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next, pdef_info++) {
1024                                 if (!(pchan->bone->flag & BONE_NO_DEFORM))
1025                                         contrib += dist_bone_deform(pchan, pdef_info, vec, dq, smat, co);
1026                         }
1027                 }
1028
1029                 /* actually should be EPSILON? weight values and contrib can be like 10e-39 small */
1030                 if (contrib > 0.0001f) {
1031                         if (use_quaternion) {
1032                                 normalize_dq(dq, contrib);
1033
1034                                 if (armature_weight != 1.0f) {
1035                                         copy_v3_v3(dco, co);
1036                                         mul_v3m3_dq(dco, (defMats) ? summat : NULL, dq);
1037                                         sub_v3_v3(dco, co);
1038                                         mul_v3_fl(dco, armature_weight);
1039                                         add_v3_v3(co, dco);
1040                                 }
1041                                 else
1042                                         mul_v3m3_dq(co, (defMats) ? summat : NULL, dq);
1043
1044                                 smat = summat;
1045                         }
1046                         else {
1047                                 mul_v3_fl(vec, armature_weight / contrib);
1048                                 add_v3_v3v3(co, vec, co);
1049                         }
1050
1051                         if (defMats) {
1052                                 float pre[3][3], post[3][3], tmpmat[3][3];
1053
1054                                 copy_m3_m4(pre, premat);
1055                                 copy_m3_m4(post, postmat);
1056                                 copy_m3_m3(tmpmat, defMats[i]);
1057
1058                                 if (!use_quaternion) /* quaternion already is scale corrected */
1059                                         mul_m3_fl(smat, armature_weight / contrib);
1060
1061                                 mul_m3_series(defMats[i], post, smat, pre, tmpmat);
1062                         }
1063                 }
1064
1065                 /* always, check above code */
1066                 mul_m4_v3(postmat, co);
1067
1068                 /* interpolate with previous modifier position using weight group */
1069                 if (prevCos) {
1070                         float mw = 1.0f - prevco_weight;
1071                         vertexCos[i][0] = prevco_weight * vertexCos[i][0] + mw * co[0];
1072                         vertexCos[i][1] = prevco_weight * vertexCos[i][1] + mw * co[1];
1073                         vertexCos[i][2] = prevco_weight * vertexCos[i][2] + mw * co[2];
1074                 }
1075         }
1076
1077         if (dualquats)
1078                 MEM_freeN(dualquats);
1079         if (defnrToPC)
1080                 MEM_freeN(defnrToPC);
1081         if (defnrToPCIndex)
1082                 MEM_freeN(defnrToPCIndex);
1083
1084         /* free B_bone matrices */
1085         pdef_info = pdef_info_array;
1086         for (pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next, pdef_info++) {
1087                 if (pdef_info->b_bone_mats)
1088                         MEM_freeN(pdef_info->b_bone_mats);
1089                 if (pdef_info->b_bone_dual_quats)
1090                         MEM_freeN(pdef_info->b_bone_dual_quats);
1091         }
1092
1093         MEM_freeN(pdef_info_array);
1094 }
1095
1096 /* ************ END Armature Deform ******************* */
1097
1098 void get_objectspace_bone_matrix(struct Bone *bone, float M_accumulatedMatrix[4][4], int UNUSED(root),
1099                                  int UNUSED(posed))
1100 {
1101         copy_m4_m4(M_accumulatedMatrix, bone->arm_mat);
1102 }
1103
1104 /* **************** Space to Space API ****************** */
1105
1106 /* Convert World-Space Matrix to Pose-Space Matrix */
1107 void BKE_armature_mat_world_to_pose(Object *ob, float inmat[4][4], float outmat[4][4])
1108 {
1109         float obmat[4][4];
1110
1111         /* prevent crashes */
1112         if (ob == NULL)
1113                 return;
1114
1115         /* get inverse of (armature) object's matrix  */
1116         invert_m4_m4(obmat, ob->obmat);
1117
1118         /* multiply given matrix by object's-inverse to find pose-space matrix */
1119         mul_m4_m4m4(outmat, inmat, obmat);
1120 }
1121
1122 /* Convert World-Space Location to Pose-Space Location
1123  * NOTE: this cannot be used to convert to pose-space location of the supplied
1124  *       pose-channel into its local space (i.e. 'visual'-keyframing) */
1125 void BKE_armature_loc_world_to_pose(Object *ob, const float inloc[3], float outloc[3])
1126 {
1127         float xLocMat[4][4];
1128         float nLocMat[4][4];
1129
1130         /* build matrix for location */
1131         unit_m4(xLocMat);
1132         copy_v3_v3(xLocMat[3], inloc);
1133
1134         /* get bone-space cursor matrix and extract location */
1135         BKE_armature_mat_world_to_pose(ob, xLocMat, nLocMat);
1136         copy_v3_v3(outloc, nLocMat[3]);
1137 }
1138
1139 /* Simple helper, computes the offset bone matrix.
1140  *     offs_bone = yoffs(b-1) + root(b) + bonemat(b).
1141  * Not exported, as it is only used in this file currently... */
1142 static void get_offset_bone_mat(Bone *bone, float offs_bone[4][4])
1143 {
1144         BLI_assert(bone->parent != NULL);
1145
1146         /* Bone transform itself. */
1147         copy_m4_m3(offs_bone, bone->bone_mat);
1148
1149         /* The bone's root offset (is in the parent's coordinate system). */
1150         copy_v3_v3(offs_bone[3], bone->head);
1151
1152         /* Get the length translation of parent (length along y axis). */
1153         offs_bone[3][1] += bone->parent->length;
1154 }
1155
1156 /* Construct the matrices (rot/scale and loc) to apply the PoseChannels into the armature (object) space.
1157  * I.e. (roughly) the "pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b)" in the
1158  *     pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b)
1159  * ...function.
1160  *
1161  * This allows to get the transformations of a bone in its object space, *before* constraints (and IK)
1162  * get applied (used by pose evaluation code).
1163  * And reverse: to find pchan transformations needed to place a bone at a given loc/rot/scale
1164  * in object space (used by interactive transform, and snapping code).
1165  *
1166  * Note that, with the HINGE/NO_SCALE/NO_LOCAL_LOCATION options, the location matrix
1167  * will differ from the rotation/scale matrix...
1168  *
1169  * NOTE: This cannot be used to convert to pose-space transforms of the supplied
1170  *       pose-channel into its local space (i.e. 'visual'-keyframing).
1171  *       (note: I don't understand that, so I keep it :p --mont29).
1172  */
1173 void BKE_pchan_to_pose_mat(bPoseChannel *pchan, float rotscale_mat[4][4], float loc_mat[4][4])
1174 {
1175         Bone *bone, *parbone;
1176         bPoseChannel *parchan;
1177
1178         /* set up variables for quicker access below */
1179         bone = pchan->bone;
1180         parbone = bone->parent;
1181         parchan = pchan->parent;
1182
1183         if (parchan) {
1184                 float offs_bone[4][4];
1185                 /* yoffs(b-1) + root(b) + bonemat(b). */
1186                 get_offset_bone_mat(bone, offs_bone);
1187
1188                 /* Compose the rotscale matrix for this bone. */
1189                 if ((bone->flag & BONE_HINGE) && (bone->flag & BONE_NO_SCALE)) {
1190                         /* Parent rest rotation and scale. */
1191                         mul_m4_m4m4(rotscale_mat, parbone->arm_mat, offs_bone);
1192                 }
1193                 else if (bone->flag & BONE_HINGE) {
1194                         /* Parent rest rotation and pose scale. */
1195                         float tmat[4][4], tscale[3];
1196
1197                         /* Extract the scale of the parent pose matrix. */
1198                         mat4_to_size(tscale, parchan->pose_mat);
1199                         size_to_mat4(tmat, tscale);
1200
1201                         /* Applies the parent pose scale to the rest matrix. */
1202                         mul_m4_m4m4(tmat, tmat, parbone->arm_mat);
1203
1204                         mul_m4_m4m4(rotscale_mat, tmat, offs_bone);
1205                 }
1206                 else if (bone->flag & BONE_NO_SCALE) {
1207                         /* Parent pose rotation and rest scale (i.e. no scaling). */
1208                         float tmat[4][4];
1209                         copy_m4_m4(tmat, parchan->pose_mat);
1210                         normalize_m4(tmat);
1211                         mul_m4_m4m4(rotscale_mat, tmat, offs_bone);
1212                 }
1213                 else
1214                         mul_m4_m4m4(rotscale_mat, parchan->pose_mat, offs_bone);
1215
1216                 /* Compose the loc matrix for this bone. */
1217                 /* NOTE: That version does not modify bone's loc when HINGE/NO_SCALE options are set. */
1218
1219                 /* In this case, use the object's space *orientation*. */
1220                 if (bone->flag & BONE_NO_LOCAL_LOCATION) {
1221                         /* XXX I'm sure that code can be simplified! */
1222                         float bone_loc[4][4], bone_rotscale[3][3], tmat4[4][4], tmat3[3][3];
1223                         unit_m4(bone_loc);
1224                         unit_m4(loc_mat);
1225                         unit_m4(tmat4);
1226
1227                         mul_v3_m4v3(bone_loc[3], parchan->pose_mat, offs_bone[3]);
1228
1229                         unit_m3(bone_rotscale);
1230                         copy_m3_m4(tmat3, parchan->pose_mat);
1231                         mul_m3_m3m3(bone_rotscale, tmat3, bone_rotscale);
1232
1233                         copy_m4_m3(tmat4, bone_rotscale);
1234                         mul_m4_m4m4(loc_mat, bone_loc, tmat4);
1235                 }
1236                 /* Those flags do not affect position, use plain parent transform space! */
1237                 else if (bone->flag & (BONE_HINGE | BONE_NO_SCALE)) {
1238                         mul_m4_m4m4(loc_mat, parchan->pose_mat, offs_bone);
1239                 }
1240                 /* Else (i.e. default, usual case), just use the same matrix for rotation/scaling, and location. */
1241                 else
1242                         copy_m4_m4(loc_mat, rotscale_mat);
1243         }
1244         /* Root bones. */
1245         else {
1246                 /* Rotation/scaling. */
1247                 copy_m4_m4(rotscale_mat, pchan->bone->arm_mat);
1248                 /* Translation. */
1249                 if (pchan->bone->flag & BONE_NO_LOCAL_LOCATION) {
1250                         /* Translation of arm_mat, without the rotation. */
1251                         unit_m4(loc_mat);
1252                         copy_v3_v3(loc_mat[3], pchan->bone->arm_mat[3]);
1253                 }
1254                 else
1255                         copy_m4_m4(loc_mat, rotscale_mat);
1256         }
1257 }
1258
1259 /* Convert Pose-Space Matrix to Bone-Space Matrix.
1260  * NOTE: this cannot be used to convert to pose-space transforms of the supplied
1261  *       pose-channel into its local space (i.e. 'visual'-keyframing) */
1262 void BKE_armature_mat_pose_to_bone(bPoseChannel *pchan, float inmat[4][4], float outmat[4][4])
1263 {
1264         float rotscale_mat[4][4], loc_mat[4][4], inmat_[4][4];
1265
1266         /* Security, this allows to call with inmat == outmat! */
1267         copy_m4_m4(inmat_, inmat);
1268
1269         BKE_pchan_to_pose_mat(pchan, rotscale_mat, loc_mat);
1270         invert_m4(rotscale_mat);
1271         invert_m4(loc_mat);
1272
1273         mul_m4_m4m4(outmat, rotscale_mat, inmat_);
1274         mul_v3_m4v3(outmat[3], loc_mat, inmat_[3]);
1275 }
1276
1277 /* Convert Bone-Space Matrix to Pose-Space Matrix. */
1278 void BKE_armature_mat_bone_to_pose(bPoseChannel *pchan, float inmat[4][4], float outmat[4][4])
1279 {
1280         float rotscale_mat[4][4], loc_mat[4][4], inmat_[4][4];
1281
1282         /* Security, this allows to call with inmat == outmat! */
1283         copy_m4_m4(inmat_, inmat);
1284
1285         BKE_pchan_to_pose_mat(pchan, rotscale_mat, loc_mat);
1286
1287         mul_m4_m4m4(outmat, rotscale_mat, inmat_);
1288         mul_v3_m4v3(outmat[3], loc_mat, inmat_[3]);
1289 }
1290
1291 /* Convert Pose-Space Location to Bone-Space Location
1292  * NOTE: this cannot be used to convert to pose-space location of the supplied
1293  *       pose-channel into its local space (i.e. 'visual'-keyframing) */
1294 void BKE_armature_loc_pose_to_bone(bPoseChannel *pchan, const float inloc[3], float outloc[3])
1295 {
1296         float xLocMat[4][4];
1297         float nLocMat[4][4];
1298
1299         /* build matrix for location */
1300         unit_m4(xLocMat);
1301         copy_v3_v3(xLocMat[3], inloc);
1302
1303         /* get bone-space cursor matrix and extract location */
1304         BKE_armature_mat_pose_to_bone(pchan, xLocMat, nLocMat);
1305         copy_v3_v3(outloc, nLocMat[3]);
1306 }
1307
1308 void BKE_armature_mat_pose_to_bone_ex(Object *ob, bPoseChannel *pchan, float inmat[4][4], float outmat[4][4])
1309 {
1310         bPoseChannel work_pchan = *pchan;
1311
1312         /* recalculate pose matrix with only parent transformations,
1313          * bone loc/sca/rot is ignored, scene and frame are not used. */
1314         BKE_pose_where_is_bone(NULL, ob, &work_pchan, 0.0f, false);
1315
1316         /* find the matrix, need to remove the bone transforms first so this is
1317          * calculated as a matrix to set rather then a difference ontop of whats
1318          * already there. */
1319         unit_m4(outmat);
1320         BKE_pchan_apply_mat4(&work_pchan, outmat, false);
1321
1322         BKE_armature_mat_pose_to_bone(&work_pchan, inmat, outmat);
1323 }
1324
1325 /* same as BKE_object_mat3_to_rot() */
1326 void BKE_pchan_mat3_to_rot(bPoseChannel *pchan, float mat[3][3], bool use_compat)
1327 {
1328         switch (pchan->rotmode) {
1329                 case ROT_MODE_QUAT:
1330                         mat3_to_quat(pchan->quat, mat);
1331                         break;
1332                 case ROT_MODE_AXISANGLE:
1333                         mat3_to_axis_angle(pchan->rotAxis, &pchan->rotAngle, mat);
1334                         break;
1335                 default: /* euler */
1336                         if (use_compat)
1337                                 mat3_to_compatible_eulO(pchan->eul, pchan->eul, pchan->rotmode, mat);
1338                         else
1339                                 mat3_to_eulO(pchan->eul, pchan->rotmode, mat);
1340                         break;
1341         }
1342 }
1343
1344 /* Apply a 4x4 matrix to the pose bone,
1345  * similar to BKE_object_apply_mat4() */
1346 void BKE_pchan_apply_mat4(bPoseChannel *pchan, float mat[4][4], bool use_compat)
1347 {
1348         float rot[3][3];
1349         mat4_to_loc_rot_size(pchan->loc, rot, pchan->size, mat);
1350         BKE_pchan_mat3_to_rot(pchan, rot, use_compat);
1351 }
1352
1353 /* Remove rest-position effects from pose-transform for obtaining
1354  * 'visual' transformation of pose-channel.
1355  * (used by the Visual-Keyframing stuff) */
1356 void BKE_armature_mat_pose_to_delta(float delta_mat[4][4], float pose_mat[4][4], float arm_mat[4][4])
1357 {
1358         float imat[4][4];
1359
1360         invert_m4_m4(imat, arm_mat);
1361         mul_m4_m4m4(delta_mat, imat, pose_mat);
1362 }
1363
1364 /* **************** Rotation Mode Conversions ****************************** */
1365 /* Used for Objects and Pose Channels, since both can have multiple rotation representations */
1366
1367 /* Called from RNA when rotation mode changes
1368  * - the result should be that the rotations given in the provided pointers have had conversions
1369  *   applied (as appropriate), such that the rotation of the element hasn't 'visually' changed  */
1370 void BKE_rotMode_change_values(float quat[4], float eul[3], float axis[3], float *angle, short oldMode, short newMode)
1371 {
1372         /* check if any change - if so, need to convert data */
1373         if (newMode > 0) { /* to euler */
1374                 if (oldMode == ROT_MODE_AXISANGLE) {
1375                         /* axis-angle to euler */
1376                         axis_angle_to_eulO(eul, newMode, axis, *angle);
1377                 }
1378                 else if (oldMode == ROT_MODE_QUAT) {
1379                         /* quat to euler */
1380                         normalize_qt(quat);
1381                         quat_to_eulO(eul, newMode, quat);
1382                 }
1383                 /* else { no conversion needed } */
1384         }
1385         else if (newMode == ROT_MODE_QUAT) { /* to quat */
1386                 if (oldMode == ROT_MODE_AXISANGLE) {
1387                         /* axis angle to quat */
1388                         axis_angle_to_quat(quat, axis, *angle);
1389                 }
1390                 else if (oldMode > 0) {
1391                         /* euler to quat */
1392                         eulO_to_quat(quat, eul, oldMode);
1393                 }
1394                 /* else { no conversion needed } */
1395         }
1396         else if (newMode == ROT_MODE_AXISANGLE) { /* to axis-angle */
1397                 if (oldMode > 0) {
1398                         /* euler to axis angle */
1399                         eulO_to_axis_angle(axis, angle, eul, oldMode);
1400                 }
1401                 else if (oldMode == ROT_MODE_QUAT) {
1402                         /* quat to axis angle */
1403                         normalize_qt(quat);
1404                         quat_to_axis_angle(axis, angle, quat);
1405                 }
1406
1407                 /* when converting to axis-angle, we need a special exception for the case when there is no axis */
1408                 if (IS_EQF(axis[0], axis[1]) && IS_EQF(axis[1], axis[2])) {
1409                         /* for now, rotate around y-axis then (so that it simply becomes the roll) */
1410                         axis[1] = 1.0f;
1411                 }
1412         }
1413 }
1414
1415 /* **************** The new & simple (but OK!) armature evaluation ********* */
1416
1417 /* ****************** And how it works! ****************************************
1418  *
1419  * This is the bone transformation trick; they're hierarchical so each bone(b)
1420  * is in the coord system of bone(b-1):
1421  *
1422  * arm_mat(b)= arm_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b)
1423  *
1424  * -> yoffs is just the y axis translation in parent's coord system
1425  * -> d_root is the translation of the bone root, also in parent's coord system
1426  *
1427  * pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b)
1428  *
1429  * we then - in init deform - store the deform in chan_mat, such that:
1430  *
1431  * pose_mat(b)= arm_mat(b) * chan_mat(b)
1432  *
1433  * *************************************************************************** */
1434
1435 /* Computes vector and roll based on a rotation.
1436  * "mat" must contain only a rotation, and no scaling. */
1437 void mat3_to_vec_roll(float mat[3][3], float r_vec[3], float *r_roll)
1438 {
1439         if (r_vec) {
1440                 copy_v3_v3(r_vec, mat[1]);
1441         }
1442
1443         if (r_roll) {
1444                 float vecmat[3][3], vecmatinv[3][3], rollmat[3][3];
1445
1446                 vec_roll_to_mat3(mat[1], 0.0f, vecmat);
1447                 invert_m3_m3(vecmatinv, vecmat);
1448                 mul_m3_m3m3(rollmat, vecmatinv, mat);
1449
1450                 *r_roll = atan2f(rollmat[2][0], rollmat[2][2]);
1451         }
1452 }
1453
1454 /* Calculates the rest matrix of a bone based on its vector and a roll around that vector. */
1455 /* Given v = (v.x, v.y, v.z) our (normalized) bone vector, we want the rotation matrix M
1456  * from the Y axis (so that M * (0, 1, 0) = v).
1457  *   -> The rotation axis a lays on XZ plane, and it is orthonormal to v, hence to the projection of v onto XZ plane.
1458  *   -> a = (v.z, 0, -v.x)
1459  * We know a is eigenvector of M (so M * a = a).
1460  * Finally, we have w, such that M * w = (0, 1, 0) (i.e. the vector that will be aligned with Y axis once transformed).
1461  * We know w is symmetric to v by the Y axis.
1462  *   -> w = (-v.x, v.y, -v.z)
1463  *
1464  * Solving this, we get (x, y and z being the components of v):
1465  *     ┌ (x^2 * y + z^2) / (x^2 + z^2),   x,   x * z * (y - 1) / (x^2 + z^2) ┐
1466  * M = │  x * (y^2 - 1)  / (x^2 + z^2),   y,    z * (y^2 - 1)  / (x^2 + z^2) │
1467  *     └ x * z * (y - 1) / (x^2 + z^2),   z,   (x^2 + z^2 * y) / (x^2 + z^2) ┘
1468  *
1469  * This is stable as long as v (the bone) is not too much aligned with +/-Y (i.e. x and z components
1470  * are not too close to 0).
1471  *
1472  * Since v is normalized, we have x^2 + y^2 + z^2 = 1, hence x^2 + z^2 = 1 - y^2 = (1 - y)(1 + y).
1473  * This allows to simplifies M like this:
1474  *     ┌ 1 - x^2 / (1 + y),   x,     -x * z / (1 + y) ┐
1475  * M = │                -x,   y,                   -z │
1476  *     └  -x * z / (1 + y),   z,    1 - z^2 / (1 + y) ┘
1477  *
1478  * Written this way, we see the case v = +Y is no more a singularity. The only one remaining is the bone being
1479  * aligned with -Y.
1480  *
1481  * Let's handle the asymptotic behavior when bone vector is reaching the limit of y = -1. Each of the four corner
1482  * elements can vary from -1 to 1, depending on the axis a chosen for doing the rotation. And the "rotation" here
1483  * is in fact established by mirroring XZ plane by that given axis, then inversing the Y-axis.
1484  * For sufficiently small x and z, and with y approaching -1, all elements but the four corner ones of M
1485  * will degenerate. So let's now focus on these corner elements.
1486  *
1487  * We rewrite M so that it only contains its four corner elements, and combine the 1 / (1 + y) factor:
1488  *                    ┌ 1 + y - x^2,        -x * z ┐
1489  * M* = 1 / (1 + y) * │                            │
1490  *                    └      -x * z,   1 + y - z^2 ┘
1491  *
1492  * When y is close to -1, computing 1 / (1 + y) will cause severe numerical instability, so we ignore it and
1493  * normalize M instead. We know y^2 = 1 - (x^2 + z^2), and y < 0, hence y = -sqrt(1 - (x^2 + z^2)).
1494  * Since x and z are both close to 0, we apply the binomial expansion to the first order:
1495  * y = -sqrt(1 - (x^2 + z^2)) = -1 + (x^2 + z^2) / 2. Which gives:
1496  *                        ┌  z^2 - x^2,  -2 * x * z ┐
1497  * M* = 1 / (x^2 + z^2) * │                         │
1498  *                        └ -2 * x * z,   x^2 - z^2 ┘
1499  */
1500 void vec_roll_to_mat3_normalized(const float nor[3], const float roll, float mat[3][3])
1501 {
1502 #define THETA_THRESHOLD_NEGY 1.0e-9f
1503 #define THETA_THRESHOLD_NEGY_CLOSE 1.0e-5f
1504
1505         float theta;
1506         float rMatrix[3][3], bMatrix[3][3];
1507
1508         BLI_ASSERT_UNIT_V3(nor);
1509
1510         theta = 1.0f + nor[1];
1511
1512         /* With old algo, 1.0e-13f caused T23954 and T31333, 1.0e-6f caused T27675 and T30438,
1513          * so using 1.0e-9f as best compromise.
1514          *
1515          * New algo is supposed much more precise, since less complex computations are performed,
1516          * but it uses two different threshold values...
1517          *
1518          * Note: When theta is close to zero, we have to check we do have non-null X/Z components as well
1519          *       (due to float precision errors, we can have nor = (0.0, 0.99999994, 0.0)...).
1520          */
1521         if (theta > THETA_THRESHOLD_NEGY_CLOSE || ((nor[0] || nor[2]) && theta > THETA_THRESHOLD_NEGY)) {
1522                 /* nor is *not* -Y.
1523                  * We got these values for free... so be happy with it... ;)
1524                  */
1525                 bMatrix[0][1] = -nor[0];
1526                 bMatrix[1][0] = nor[0];
1527                 bMatrix[1][1] = nor[1];
1528                 bMatrix[1][2] = nor[2];
1529                 bMatrix[2][1] = -nor[2];
1530                 if (theta > THETA_THRESHOLD_NEGY_CLOSE) {
1531                         /* If nor is far enough from -Y, apply the general case. */
1532                         bMatrix[0][0] = 1 - nor[0] * nor[0] / theta;
1533                         bMatrix[2][2] = 1 - nor[2] * nor[2] / theta;
1534                         bMatrix[2][0] = bMatrix[0][2] = -nor[0] * nor[2] / theta;
1535                 }
1536                 else {
1537                         /* If nor is too close to -Y, apply the special case. */
1538                         theta = nor[0] * nor[0] + nor[2] * nor[2];
1539                         bMatrix[0][0] = (nor[0] + nor[2]) * (nor[0] - nor[2]) / -theta;
1540                         bMatrix[2][2] = -bMatrix[0][0];
1541                         bMatrix[2][0] = bMatrix[0][2] = 2.0f * nor[0] * nor[2] / theta;
1542                 }
1543         }
1544         else {
1545                 /* If nor is -Y, simple symmetry by Z axis. */
1546                 unit_m3(bMatrix);
1547                 bMatrix[0][0] = bMatrix[1][1] = -1.0;
1548         }
1549
1550         /* Make Roll matrix */
1551         axis_angle_normalized_to_mat3(rMatrix, nor, roll);
1552
1553         /* Combine and output result */
1554         mul_m3_m3m3(mat, rMatrix, bMatrix);
1555
1556 #undef THETA_THRESHOLD_NEGY
1557 #undef THETA_THRESHOLD_NEGY_CLOSE
1558 }
1559
1560 void vec_roll_to_mat3(const float vec[3], const float roll, float mat[3][3])
1561 {
1562         float nor[3];
1563
1564         normalize_v3_v3(nor, vec);
1565         vec_roll_to_mat3_normalized(nor, roll, mat);
1566 }
1567
1568 /* recursive part, calculates restposition of entire tree of children */
1569 /* used by exiting editmode too */
1570 void BKE_armature_where_is_bone(Bone *bone, Bone *prevbone, const bool use_recursion)
1571 {
1572         float vec[3];
1573
1574         /* Bone Space */
1575         sub_v3_v3v3(vec, bone->tail, bone->head);
1576         bone->length = len_v3(vec);
1577         vec_roll_to_mat3(vec, bone->roll, bone->bone_mat);
1578
1579         /* this is called on old file reading too... */
1580         if (bone->xwidth == 0.0f) {
1581                 bone->xwidth = 0.1f;
1582                 bone->zwidth = 0.1f;
1583                 bone->segments = 1;
1584         }
1585
1586         if (prevbone) {
1587                 float offs_bone[4][4];
1588                 /* yoffs(b-1) + root(b) + bonemat(b) */
1589                 get_offset_bone_mat(bone, offs_bone);
1590
1591                 /* Compose the matrix for this bone  */
1592                 mul_m4_m4m4(bone->arm_mat, prevbone->arm_mat, offs_bone);
1593         }
1594         else {
1595                 copy_m4_m3(bone->arm_mat, bone->bone_mat);
1596                 copy_v3_v3(bone->arm_mat[3], bone->head);
1597         }
1598
1599         /* and the kiddies */
1600         if (use_recursion) {
1601                 prevbone = bone;
1602                 for (bone = bone->childbase.first; bone; bone = bone->next) {
1603                         BKE_armature_where_is_bone(bone, prevbone, use_recursion);
1604                 }
1605         }
1606 }
1607
1608 /* updates vectors and matrices on rest-position level, only needed
1609  * after editing armature itself, now only on reading file */
1610 void BKE_armature_where_is(bArmature *arm)
1611 {
1612         Bone *bone;
1613
1614         /* hierarchical from root to children */
1615         for (bone = arm->bonebase.first; bone; bone = bone->next) {
1616                 BKE_armature_where_is_bone(bone, NULL, true);
1617         }
1618 }
1619
1620 /* if bone layer is protected, copy the data from from->pose
1621  * when used with linked libraries this copies from the linked pose into the local pose */
1622 static void pose_proxy_synchronize(Object *ob, Object *from, int layer_protected)
1623 {
1624         bPose *pose = ob->pose, *frompose = from->pose;
1625         bPoseChannel *pchan, *pchanp;
1626         bConstraint *con;
1627         int error = 0;
1628
1629         if (frompose == NULL)
1630                 return;
1631
1632         /* in some cases when rigs change, we cant synchronize
1633          * to avoid crashing check for possible errors here */
1634         for (pchan = pose->chanbase.first; pchan; pchan = pchan->next) {
1635                 if (pchan->bone->layer & layer_protected) {
1636                         if (BKE_pose_channel_find_name(frompose, pchan->name) == NULL) {
1637                                 printf("failed to sync proxy armature because '%s' is missing pose channel '%s'\n",
1638                                        from->id.name, pchan->name);
1639                                 error = 1;
1640                         }
1641                 }
1642         }
1643
1644         if (error)
1645                 return;
1646
1647         /* clear all transformation values from library */
1648         BKE_pose_rest(frompose);
1649
1650         /* copy over all of the proxy's bone groups */
1651         /* TODO for later
1652          * - implement 'local' bone groups as for constraints
1653          * Note: this isn't trivial, as bones reference groups by index not by pointer,
1654          *       so syncing things correctly needs careful attention */
1655         BLI_freelistN(&pose->agroups);
1656         BLI_duplicatelist(&pose->agroups, &frompose->agroups);
1657         pose->active_group = frompose->active_group;
1658
1659         for (pchan = pose->chanbase.first; pchan; pchan = pchan->next) {
1660                 pchanp = BKE_pose_channel_find_name(frompose, pchan->name);
1661                 
1662                 if (UNLIKELY(pchanp == NULL)) {
1663                         /* happens for proxies that become invalid because of a missing link
1664                          * for regular cases it shouldn't happen at all */
1665                 }
1666                 else if (pchan->bone->layer & layer_protected) {
1667                         ListBase proxylocal_constraints = {NULL, NULL};
1668                         bPoseChannel pchanw;
1669                         
1670                         /* copy posechannel to temp, but restore important pointers */
1671                         pchanw = *pchanp;
1672                         pchanw.prev = pchan->prev;
1673                         pchanw.next = pchan->next;
1674                         pchanw.parent = pchan->parent;
1675                         pchanw.child = pchan->child;
1676                         pchanw.custom_tx = pchan->custom_tx;
1677
1678                         pchanw.mpath = pchan->mpath;
1679                         pchan->mpath = NULL;
1680
1681                         /* this is freed so copy a copy, else undo crashes */
1682                         if (pchanw.prop) {
1683                                 pchanw.prop = IDP_CopyProperty(pchanw.prop);
1684                                 
1685                                 /* use the values from the existing props */
1686                                 if (pchan->prop) {
1687                                         IDP_SyncGroupValues(pchanw.prop, pchan->prop);
1688                                 }
1689                         }
1690                         
1691                         /* constraints - proxy constraints are flushed... local ones are added after
1692                          *     1. extract constraints not from proxy (CONSTRAINT_PROXY_LOCAL) from pchan's constraints
1693                          *     2. copy proxy-pchan's constraints on-to new
1694                          *     3. add extracted local constraints back on top
1695                          *
1696                          * Note for BKE_constraints_copy: when copying constraints, disable 'do_extern' otherwise
1697                          *                                we get the libs direct linked in this blend.
1698                          */
1699                         BKE_constraints_proxylocal_extract(&proxylocal_constraints, &pchan->constraints);
1700                         BKE_constraints_copy(&pchanw.constraints, &pchanp->constraints, false);
1701                         BLI_movelisttolist(&pchanw.constraints, &proxylocal_constraints);
1702                         
1703                         /* constraints - set target ob pointer to own object */
1704                         for (con = pchanw.constraints.first; con; con = con->next) {
1705                                 const bConstraintTypeInfo *cti = BKE_constraint_typeinfo_get(con);
1706                                 ListBase targets = {NULL, NULL};
1707                                 bConstraintTarget *ct;
1708                                 
1709                                 if (cti && cti->get_constraint_targets) {
1710                                         cti->get_constraint_targets(con, &targets);
1711                                         
1712                                         for (ct = targets.first; ct; ct = ct->next) {
1713                                                 if (ct->tar == from)
1714                                                         ct->tar = ob;
1715                                         }
1716                                         
1717                                         if (cti->flush_constraint_targets)
1718                                                 cti->flush_constraint_targets(con, &targets, 0);
1719                                 }
1720                         }
1721                         
1722                         /* free stuff from current channel */
1723                         BKE_pose_channel_free(pchan);
1724                         
1725                         /* copy data in temp back over to the cleaned-out (but still allocated) original channel */
1726                         *pchan = pchanw;
1727                         if (pchan->custom) {
1728                                 id_us_plus(&pchan->custom->id);
1729                         }
1730                 }
1731                 else {
1732                         /* always copy custom shape */
1733                         pchan->custom = pchanp->custom;
1734                         if (pchan->custom) {
1735                                 id_us_plus(&pchan->custom->id);
1736                         }
1737                         if (pchanp->custom_tx)
1738                                 pchan->custom_tx = BKE_pose_channel_find_name(pose, pchanp->custom_tx->name);
1739
1740                         /* ID-Property Syncing */
1741                         {
1742                                 IDProperty *prop_orig = pchan->prop;
1743                                 if (pchanp->prop) {
1744                                         pchan->prop = IDP_CopyProperty(pchanp->prop);
1745                                         if (prop_orig) {
1746                                                 /* copy existing values across when types match */
1747                                                 IDP_SyncGroupValues(pchan->prop, prop_orig);
1748                                         }
1749                                 }
1750                                 else {
1751                                         pchan->prop = NULL;
1752                                 }
1753                                 if (prop_orig) {
1754                                         IDP_FreeProperty(prop_orig);
1755                                         MEM_freeN(prop_orig);
1756                                 }
1757                         }
1758                 }
1759         }
1760 }
1761
1762 static int rebuild_pose_bone(bPose *pose, Bone *bone, bPoseChannel *parchan, int counter)
1763 {
1764         bPoseChannel *pchan = BKE_pose_channel_verify(pose, bone->name); /* verify checks and/or adds */
1765
1766         pchan->bone = bone;
1767         pchan->parent = parchan;
1768
1769         counter++;
1770
1771         for (bone = bone->childbase.first; bone; bone = bone->next) {
1772                 counter = rebuild_pose_bone(pose, bone, pchan, counter);
1773                 /* for quick detecting of next bone in chain, only b-bone uses it now */
1774                 if (bone->flag & BONE_CONNECTED)
1775                         pchan->child = BKE_pose_channel_find_name(pose, bone->name);
1776         }
1777
1778         return counter;
1779 }
1780
1781 /* only after leave editmode, duplicating, validating older files, library syncing */
1782 /* NOTE: pose->flag is set for it */
1783 void BKE_pose_rebuild(Object *ob, bArmature *arm)
1784 {
1785         Bone *bone;
1786         bPose *pose;
1787         bPoseChannel *pchan, *next;
1788         int counter = 0;
1789
1790         /* only done here */
1791         if (ob->pose == NULL) {
1792                 /* create new pose */
1793                 ob->pose = MEM_callocN(sizeof(bPose), "new pose");
1794
1795                 /* set default settings for animviz */
1796                 animviz_settings_init(&ob->pose->avs);
1797         }
1798         pose = ob->pose;
1799
1800         /* clear */
1801         for (pchan = pose->chanbase.first; pchan; pchan = pchan->next) {
1802                 pchan->bone = NULL;
1803                 pchan->child = NULL;
1804         }
1805
1806         /* first step, check if all channels are there */
1807         for (bone = arm->bonebase.first; bone; bone = bone->next) {
1808                 counter = rebuild_pose_bone(pose, bone, NULL, counter);
1809         }
1810
1811         /* and a check for garbage */
1812         for (pchan = pose->chanbase.first; pchan; pchan = next) {
1813                 next = pchan->next;
1814                 if (pchan->bone == NULL) {
1815                         BKE_pose_channel_free(pchan);
1816                         BKE_pose_channels_hash_free(pose);
1817                         BLI_freelinkN(&pose->chanbase, pchan);
1818                 }
1819         }
1820         /* printf("rebuild pose %s, %d bones\n", ob->id.name, counter); */
1821
1822         /* synchronize protected layers with proxy */
1823         if (ob->proxy) {
1824                 BKE_object_copy_proxy_drivers(ob, ob->proxy);
1825                 pose_proxy_synchronize(ob, ob->proxy, arm->layer_protected);
1826         }
1827
1828         BKE_pose_update_constraint_flags(ob->pose); /* for IK detection for example */
1829
1830 #ifdef WITH_LEGACY_DEPSGRAPH
1831         /* the sorting */
1832         /* Sorting for new dependnecy graph is done on the scene graph level. */
1833         if (counter > 1)
1834                 DAG_pose_sort(ob);
1835 #endif
1836
1837         ob->pose->flag &= ~POSE_RECALC;
1838         ob->pose->flag |= POSE_WAS_REBUILT;
1839
1840         BKE_pose_channels_hash_make(ob->pose);
1841 }
1842
1843 /* ********************** THE POSE SOLVER ******************* */
1844
1845 /* loc/rot/size to given mat4 */
1846 void BKE_pchan_to_mat4(bPoseChannel *pchan, float chan_mat[4][4])
1847 {
1848         float smat[3][3];
1849         float rmat[3][3];
1850         float tmat[3][3];
1851
1852         /* get scaling matrix */
1853         size_to_mat3(smat, pchan->size);
1854
1855         /* rotations may either be quats, eulers (with various rotation orders), or axis-angle */
1856         if (pchan->rotmode > 0) {
1857                 /* euler rotations (will cause gimble lock, but this can be alleviated a bit with rotation orders) */
1858                 eulO_to_mat3(rmat, pchan->eul, pchan->rotmode);
1859         }
1860         else if (pchan->rotmode == ROT_MODE_AXISANGLE) {
1861                 /* axis-angle - not really that great for 3D-changing orientations */
1862                 axis_angle_to_mat3(rmat, pchan->rotAxis, pchan->rotAngle);
1863         }
1864         else {
1865                 /* quats are normalized before use to eliminate scaling issues */
1866                 float quat[4];
1867
1868                 /* NOTE: we now don't normalize the stored values anymore, since this was kindof evil in some cases
1869                  * but if this proves to be too problematic, switch back to the old system of operating directly on
1870                  * the stored copy
1871                  */
1872                 normalize_qt_qt(quat, pchan->quat);
1873                 quat_to_mat3(rmat, quat);
1874         }
1875
1876         /* calculate matrix of bone (as 3x3 matrix, but then copy the 4x4) */
1877         mul_m3_m3m3(tmat, rmat, smat);
1878         copy_m4_m3(chan_mat, tmat);
1879
1880         /* prevent action channels breaking chains */
1881         /* need to check for bone here, CONSTRAINT_TYPE_ACTION uses this call */
1882         if ((pchan->bone == NULL) || !(pchan->bone->flag & BONE_CONNECTED)) {
1883                 copy_v3_v3(chan_mat[3], pchan->loc);
1884         }
1885 }
1886
1887 /* loc/rot/size to mat4 */
1888 /* used in constraint.c too */
1889 void BKE_pchan_calc_mat(bPoseChannel *pchan)
1890 {
1891         /* this is just a wrapper around the copy of this function which calculates the matrix
1892          * and stores the result in any given channel
1893          */
1894         BKE_pchan_to_mat4(pchan, pchan->chan_mat);
1895 }
1896
1897 #if 0 /* XXX OLD ANIMSYS, NLASTRIPS ARE NO LONGER USED */
1898
1899 /* NLA strip modifiers */
1900 static void do_strip_modifiers(Scene *scene, Object *armob, Bone *bone, bPoseChannel *pchan)
1901 {
1902         bActionModifier *amod;
1903         bActionStrip *strip, *strip2;
1904         float scene_cfra = BKE_scene_frame_get(scene);
1905         int do_modif;
1906
1907         for (strip = armob->nlastrips.first; strip; strip = strip->next) {
1908                 do_modif = false;
1909
1910                 if (scene_cfra >= strip->start && scene_cfra <= strip->end)
1911                         do_modif = true;
1912
1913                 if ((scene_cfra > strip->end) && (strip->flag & ACTSTRIP_HOLDLASTFRAME)) {
1914                         do_modif = true;
1915
1916                         /* if there are any other strips active, ignore modifiers for this strip -
1917                          * 'hold' option should only hold action modifiers if there are
1918                          * no other active strips */
1919                         for (strip2 = strip->next; strip2; strip2 = strip2->next) {
1920                                 if (strip2 == strip) continue;
1921
1922                                 if (scene_cfra >= strip2->start && scene_cfra <= strip2->end) {
1923                                         if (!(strip2->flag & ACTSTRIP_MUTE))
1924                                                 do_modif = false;
1925                                 }
1926                         }
1927
1928                         /* if there are any later, activated, strips with 'hold' set, they take precedence,
1929                          * so ignore modifiers for this strip */
1930                         for (strip2 = strip->next; strip2; strip2 = strip2->next) {
1931                                 if (scene_cfra < strip2->start) continue;
1932                                 if ((strip2->flag & ACTSTRIP_HOLDLASTFRAME) && !(strip2->flag & ACTSTRIP_MUTE)) {
1933                                         do_modif = false;
1934                                 }
1935                         }
1936                 }
1937
1938                 if (do_modif) {
1939                         /* temporal solution to prevent 2 strips accumulating */
1940                         if (scene_cfra == strip->end && strip->next && strip->next->start == scene_cfra)
1941                                 continue;
1942
1943                         for (amod = strip->modifiers.first; amod; amod = amod->next) {
1944                                 switch (amod->type) {
1945                                         case ACTSTRIP_MOD_DEFORM:
1946                                         {
1947                                                 /* validate first */
1948                                                 if (amod->ob && amod->ob->type == OB_CURVE && amod->channel[0]) {
1949
1950                                                         if (STREQ(pchan->name, amod->channel)) {
1951                                                                 float mat4[4][4], mat3[3][3];
1952
1953                                                                 curve_deform_vector(scene, amod->ob, armob, bone->arm_mat[3], pchan->pose_mat[3], mat3, amod->no_rot_axis);
1954                                                                 copy_m4_m4(mat4, pchan->pose_mat);
1955                                                                 mul_m4_m3m4(pchan->pose_mat, mat3, mat4);
1956
1957                                                         }
1958                                                 }
1959                                         }
1960                                         break;
1961                                         case ACTSTRIP_MOD_NOISE:
1962                                         {
1963                                                 if (STREQ(pchan->name, amod->channel)) {
1964                                                         float nor[3], loc[3], ofs;
1965                                                         float eul[3], size[3], eulo[3], sizeo[3];
1966
1967                                                         /* calculate turbulance */
1968                                                         ofs = amod->turbul / 200.0f;
1969
1970                                                         /* make a copy of starting conditions */
1971                                                         copy_v3_v3(loc, pchan->pose_mat[3]);
1972                                                         mat4_to_eul(eul, pchan->pose_mat);
1973                                                         mat4_to_size(size, pchan->pose_mat);
1974                                                         copy_v3_v3(eulo, eul);
1975                                                         copy_v3_v3(sizeo, size);
1976
1977                                                         /* apply noise to each set of channels */
1978                                                         if (amod->channels & 4) {
1979                                                                 /* for scaling */
1980                                                                 nor[0] = BLI_gNoise(amod->noisesize, size[0] + ofs, size[1], size[2], 0, 0) - ofs;
1981                                                                 nor[1] = BLI_gNoise(amod->noisesize, size[0], size[1] + ofs, size[2], 0, 0) - ofs;
1982                                                                 nor[2] = BLI_gNoise(amod->noisesize, size[0], size[1], size[2] + ofs, 0, 0) - ofs;
1983                                                                 add_v3_v3(size, nor);
1984
1985                                                                 if (sizeo[0] != 0)
1986                                                                         mul_v3_fl(pchan->pose_mat[0], size[0] / sizeo[0]);
1987                                                                 if (sizeo[1] != 0)
1988                                                                         mul_v3_fl(pchan->pose_mat[1], size[1] / sizeo[1]);
1989                                                                 if (sizeo[2] != 0)
1990                                                                         mul_v3_fl(pchan->pose_mat[2], size[2] / sizeo[2]);
1991                                                         }
1992                                                         if (amod->channels & 2) {
1993                                                                 /* for rotation */
1994                                                                 nor[0] = BLI_gNoise(amod->noisesize, eul[0] + ofs, eul[1], eul[2], 0, 0) - ofs;
1995                                                                 nor[1] = BLI_gNoise(amod->noisesize, eul[0], eul[1] + ofs, eul[2], 0, 0) - ofs;
1996                                                                 nor[2] = BLI_gNoise(amod->noisesize, eul[0], eul[1], eul[2] + ofs, 0, 0) - ofs;
1997
1998                                                                 compatible_eul(nor, eulo);
1999                                                                 add_v3_v3(eul, nor);
2000                                                                 compatible_eul(eul, eulo);
2001
2002                                                                 loc_eul_size_to_mat4(pchan->pose_mat, loc, eul, size);
2003                                                         }
2004                                                         if (amod->channels & 1) {
2005                                                                 /* for location */
2006                                                                 nor[0] = BLI_gNoise(amod->noisesize, loc[0] + ofs, loc[1], loc[2], 0, 0) - ofs;
2007                                                                 nor[1] = BLI_gNoise(amod->noisesize, loc[0], loc[1] + ofs, loc[2], 0, 0) - ofs;
2008                                                                 nor[2] = BLI_gNoise(amod->noisesize, loc[0], loc[1], loc[2] + ofs, 0, 0) - ofs;
2009
2010                                                                 add_v3_v3v3(pchan->pose_mat[3], loc, nor);
2011                                                         }
2012                                                 }
2013                                         }
2014                                         break;
2015                                 }
2016                         }
2017                 }
2018         }
2019 }
2020
2021 #endif
2022
2023 /* calculate tail of posechannel */
2024 void BKE_pose_where_is_bone_tail(bPoseChannel *pchan)
2025 {
2026         float vec[3];
2027
2028         copy_v3_v3(vec, pchan->pose_mat[1]);
2029         mul_v3_fl(vec, pchan->bone->length);
2030         add_v3_v3v3(pchan->pose_tail, pchan->pose_head, vec);
2031 }
2032
2033 /* The main armature solver, does all constraints excluding IK */
2034 /* pchan is validated, as having bone and parent pointer
2035  * 'do_extra': when zero skips loc/size/rot, constraints and strip modifiers.
2036  */
2037 void BKE_pose_where_is_bone(Scene *scene, Object *ob, bPoseChannel *pchan, float ctime, bool do_extra)
2038 {
2039         /* This gives a chan_mat with actions (ipos) results. */
2040         if (do_extra)
2041                 BKE_pchan_calc_mat(pchan);
2042         else
2043                 unit_m4(pchan->chan_mat);
2044
2045         /* Construct the posemat based on PoseChannels, that we do before applying constraints. */
2046         /* pose_mat(b) = pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b) */
2047         BKE_armature_mat_bone_to_pose(pchan, pchan->chan_mat, pchan->pose_mat);
2048
2049         /* Only rootbones get the cyclic offset (unless user doesn't want that). */
2050         /* XXX That could be a problem for snapping and other "reverse transform" features... */
2051         if (!pchan->parent) {
2052                 if ((pchan->bone->flag & BONE_NO_CYCLICOFFSET) == 0)
2053                         add_v3_v3(pchan->pose_mat[3], ob->pose->cyclic_offset);
2054         }
2055
2056         if (do_extra) {
2057 #if 0   /* XXX OLD ANIMSYS, NLASTRIPS ARE NO LONGER USED */
2058                 /* do NLA strip modifiers - i.e. curve follow */
2059                 do_strip_modifiers(scene, ob, bone, pchan);
2060 #endif
2061
2062                 /* Do constraints */
2063                 if (pchan->constraints.first) {
2064                         bConstraintOb *cob;
2065                         float vec[3];
2066
2067                         /* make a copy of location of PoseChannel for later */
2068                         copy_v3_v3(vec, pchan->pose_mat[3]);
2069
2070                         /* prepare PoseChannel for Constraint solving
2071                          * - makes a copy of matrix, and creates temporary struct to use
2072                          */
2073                         cob = BKE_constraints_make_evalob(scene, ob, pchan, CONSTRAINT_OBTYPE_BONE);
2074
2075                         /* Solve PoseChannel's Constraints */
2076                         BKE_constraints_solve(&pchan->constraints, cob, ctime); /* ctime doesnt alter objects */
2077
2078                         /* cleanup after Constraint Solving
2079                          * - applies matrix back to pchan, and frees temporary struct used
2080                          */
2081                         BKE_constraints_clear_evalob(cob);
2082
2083                         /* prevent constraints breaking a chain */
2084                         if (pchan->bone->flag & BONE_CONNECTED) {
2085                                 copy_v3_v3(pchan->pose_mat[3], vec);
2086                         }
2087                 }
2088         }
2089
2090         /* calculate head */
2091         copy_v3_v3(pchan->pose_head, pchan->pose_mat[3]);
2092         /* calculate tail */
2093         BKE_pose_where_is_bone_tail(pchan);
2094 }
2095
2096 /* This only reads anim data from channels, and writes to channels */
2097 /* This is the only function adding poses */
2098 void BKE_pose_where_is(Scene *scene, Object *ob)
2099 {
2100         bArmature *arm;
2101         Bone *bone;
2102         bPoseChannel *pchan;
2103         float imat[4][4];
2104         float ctime;
2105
2106         if (ob->type != OB_ARMATURE)
2107                 return;
2108         arm = ob->data;
2109
2110         if (ELEM(NULL, arm, scene))
2111                 return;
2112         if ((ob->pose == NULL) || (ob->pose->flag & POSE_RECALC))
2113                 BKE_pose_rebuild(ob, arm);
2114
2115         ctime = BKE_scene_frame_get(scene); /* not accurate... */
2116
2117         /* In editmode or restposition we read the data from the bones */
2118         if (arm->edbo || (arm->flag & ARM_RESTPOS)) {
2119                 for (pchan = ob->pose->chanbase.first; pchan; pchan = pchan->next) {
2120                         bone = pchan->bone;
2121                         if (bone) {
2122                                 copy_m4_m4(pchan->pose_mat, bone->arm_mat);
2123                                 copy_v3_v3(pchan->pose_head, bone->arm_head);
2124                                 copy_v3_v3(pchan->pose_tail, bone->arm_tail);
2125                         }
2126                 }
2127         }
2128         else {
2129                 invert_m4_m4(ob->imat, ob->obmat); /* imat is needed */
2130
2131                 /* 1. clear flags */
2132                 for (pchan = ob->pose->chanbase.first; pchan; pchan = pchan->next) {
2133                         pchan->flag &= ~(POSE_DONE | POSE_CHAIN | POSE_IKTREE | POSE_IKSPLINE);
2134                 }
2135
2136                 /* 2a. construct the IK tree (standard IK) */
2137                 BIK_initialize_tree(scene, ob, ctime);
2138
2139                 /* 2b. construct the Spline IK trees
2140                  *  - this is not integrated as an IK plugin, since it should be able
2141                  *        to function in conjunction with standard IK
2142                  */
2143                 BKE_pose_splineik_init_tree(scene, ob, ctime);
2144
2145                 /* 3. the main loop, channels are already hierarchical sorted from root to children */
2146                 for (pchan = ob->pose->chanbase.first; pchan; pchan = pchan->next) {
2147                         /* 4a. if we find an IK root, we handle it separated */
2148                         if (pchan->flag & POSE_IKTREE) {
2149                                 BIK_execute_tree(scene, ob, pchan, ctime);
2150                         }
2151                         /* 4b. if we find a Spline IK root, we handle it separated too */
2152                         else if (pchan->flag & POSE_IKSPLINE) {
2153                                 BKE_splineik_execute_tree(scene, ob, pchan, ctime);
2154                         }
2155                         /* 5. otherwise just call the normal solver */
2156                         else if (!(pchan->flag & POSE_DONE)) {
2157                                 BKE_pose_where_is_bone(scene, ob, pchan, ctime, 1);
2158                         }
2159                 }
2160                 /* 6. release the IK tree */
2161                 BIK_release_tree(scene, ob, ctime);
2162         }
2163
2164         /* calculating deform matrices */
2165         for (pchan = ob->pose->chanbase.first; pchan; pchan = pchan->next) {
2166                 if (pchan->bone) {
2167                         invert_m4_m4(imat, pchan->bone->arm_mat);
2168                         mul_m4_m4m4(pchan->chan_mat, pchan->pose_mat, imat);
2169                 }
2170         }
2171 }
2172
2173 /************** Bounding box ********************/
2174 static int minmax_armature(Object *ob, float r_min[3], float r_max[3])
2175 {
2176         bPoseChannel *pchan;
2177
2178         /* For now, we assume BKE_pose_where_is has already been called (hence we have valid data in pachan). */
2179         for (pchan = ob->pose->chanbase.first; pchan; pchan = pchan->next) {
2180                 minmax_v3v3_v3(r_min, r_max, pchan->pose_head);
2181                 minmax_v3v3_v3(r_min, r_max, pchan->pose_tail);
2182         }
2183
2184         return (BLI_listbase_is_empty(&ob->pose->chanbase) == false);
2185 }
2186
2187 static void boundbox_armature(Object *ob)
2188 {
2189         BoundBox *bb;
2190         float min[3], max[3];
2191
2192         if (ob->bb == NULL)
2193                 ob->bb = MEM_mallocN(sizeof(BoundBox), "Armature boundbox");
2194         bb = ob->bb;
2195
2196         INIT_MINMAX(min, max);
2197         if (!minmax_armature(ob, min, max)) {
2198                 min[0] = min[1] = min[2] = -1.0f;
2199                 max[0] = max[1] = max[2] = 1.0f;
2200         }
2201
2202         BKE_boundbox_init_from_minmax(bb, min, max);
2203 }
2204
2205 BoundBox *BKE_armature_boundbox_get(Object *ob)
2206 {
2207         boundbox_armature(ob);
2208
2209         return ob->bb;
2210 }
2211
2212 bool BKE_pose_minmax(Object *ob, float r_min[3], float r_max[3], bool use_hidden, bool use_select)
2213 {
2214         bool changed = false;
2215
2216         if (ob->pose) {
2217                 bArmature *arm = ob->data;
2218                 bPoseChannel *pchan;
2219
2220                 for (pchan = ob->pose->chanbase.first; pchan; pchan = pchan->next) {
2221                         /* XXX pchan->bone may be NULL for duplicated bones, see duplicateEditBoneObjects() comment
2222                          *     (editarmature.c:2592)... Skip in this case too! */
2223                         if (pchan->bone &&
2224                             (!((use_hidden == false) && (PBONE_VISIBLE(arm, pchan->bone) == false)) &&
2225                              !((use_select == true)  && ((pchan->bone->flag & BONE_SELECTED) == 0))))
2226                         {
2227                                 bPoseChannel *pchan_tx = (pchan->custom && pchan->custom_tx) ? pchan->custom_tx : pchan;
2228                                 BoundBox *bb_custom = ((pchan->custom) && !(arm->flag & ARM_NO_CUSTOM)) ?
2229                                                       BKE_object_boundbox_get(pchan->custom) : NULL;
2230                                 if (bb_custom) {
2231                                         float mat[4][4], smat[4][4];
2232                                         scale_m4_fl(smat, PCHAN_CUSTOM_DRAW_SIZE(pchan));
2233                                         mul_m4_series(mat, ob->obmat, pchan_tx->pose_mat, smat);
2234                                         BKE_boundbox_minmax(bb_custom, mat, r_min, r_max);
2235                                 }
2236                                 else {
2237                                         float vec[3];
2238                                         mul_v3_m4v3(vec, ob->obmat, pchan_tx->pose_head);
2239                                         minmax_v3v3_v3(r_min, r_max, vec);
2240                                         mul_v3_m4v3(vec, ob->obmat, pchan_tx->pose_tail);
2241                                         minmax_v3v3_v3(r_min, r_max, vec);
2242                                 }
2243
2244                                 changed = true;
2245                         }
2246                 }
2247         }
2248
2249         return changed;
2250 }
2251
2252 /************** Graph evaluation ********************/
2253
2254 bPoseChannel *BKE_armature_ik_solver_find_root(
2255         bPoseChannel *pchan,
2256         bKinematicConstraint *data)
2257 {
2258         bPoseChannel *rootchan = pchan;
2259         if (!(data->flag & CONSTRAINT_IK_TIP)) {
2260                 /* Exclude tip from chain. */
2261                 rootchan = rootchan->parent;
2262         }
2263         if (rootchan != NULL) {
2264                 int segcount = 0;
2265                 while (rootchan->parent) {
2266                         /* Continue up chain, until we reach target number of items. */
2267                         segcount++;
2268                         if (segcount == data->rootbone) {
2269                                 break;
2270                         }
2271                         rootchan = rootchan->parent;
2272                 }
2273         }
2274         return rootchan;
2275 }
2276
2277 bPoseChannel *BKE_armature_splineik_solver_find_root(
2278         bPoseChannel *pchan,
2279         bSplineIKConstraint *data)
2280 {
2281         bPoseChannel *rootchan = pchan;
2282         int segcount = 0;
2283         BLI_assert(rootchan != NULL);
2284         while (rootchan->parent) {
2285                 /* Continue up chain, until we reach target number of items. */
2286                 segcount++;
2287                 if (segcount == data->chainlen) {
2288                         break;
2289                 }
2290                 rootchan = rootchan->parent;
2291         }
2292         return rootchan;
2293 }