style cleanup
[blender.git] / source / blender / ikplugin / intern / iksolver_plugin.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  * The Original Code is: all of this file.
22  *
23  * Original author: Benoit Bolsee
24  * Contributor(s): 
25  *
26  * ***** END GPL LICENSE BLOCK *****
27  */
28
29 /** \file blender/ikplugin/intern/iksolver_plugin.c
30  *  \ingroup ikplugin
31  */
32
33
34 #include "MEM_guardedalloc.h"
35
36 #include "BIK_api.h"
37 #include "BLI_blenlib.h"
38 #include "BLI_math.h"
39 #include "BLI_utildefines.h"
40
41 #include "BKE_armature.h"
42 #include "BKE_constraint.h"
43
44 #include "DNA_object_types.h"
45 #include "DNA_action_types.h"
46 #include "DNA_constraint_types.h"
47 #include "DNA_armature_types.h"
48
49 #include "IK_solver.h"
50 #include "iksolver_plugin.h"
51
52 #include <string.h> /* memcpy */
53
54 /* ********************** THE IK SOLVER ******************* */
55
56 /* allocates PoseTree, and links that to root bone/channel */
57 /* Note: detecting the IK chain is duplicate code... in drawarmature.c and in transform_conversions.c */
58 static void initialize_posetree(struct Object *UNUSED(ob), bPoseChannel *pchan_tip)
59 {
60         bPoseChannel *curchan, *pchan_root = NULL, *chanlist[256], **oldchan;
61         PoseTree *tree;
62         PoseTarget *target;
63         bConstraint *con;
64         bKinematicConstraint *data;
65         int a, t, segcount = 0, size, newsize, *oldparent, parent;
66
67         /* find IK constraint, and validate it */
68         for (con = pchan_tip->constraints.first; con; con = con->next) {
69                 if (con->type == CONSTRAINT_TYPE_KINEMATIC) {
70                         data = (bKinematicConstraint *)con->data;
71                         if (data->flag & CONSTRAINT_IK_AUTO) break;
72                         if (data->tar == NULL) continue;
73                         if (data->tar->type == OB_ARMATURE && data->subtarget[0] == 0) continue;
74                         if ((con->flag & (CONSTRAINT_DISABLE | CONSTRAINT_OFF)) == 0 && (con->enforce != 0.0f)) break;
75                 }
76         }
77         if (con == NULL) return;
78         
79         /* exclude tip from chain? */
80         if (!(data->flag & CONSTRAINT_IK_TIP))
81                 pchan_tip = pchan_tip->parent;
82         
83         /* Find the chain's root & count the segments needed */
84         for (curchan = pchan_tip; curchan; curchan = curchan->parent) {
85                 pchan_root = curchan;
86                 
87                 curchan->flag |= POSE_CHAIN;    // don't forget to clear this
88                 chanlist[segcount] = curchan;
89                 segcount++;
90                 
91                 if (segcount == data->rootbone || segcount > 255) break;  // 255 is weak
92         }
93         if (!segcount) return;
94
95         /* setup the chain data */
96         
97         /* we make tree-IK, unless all existing targets are in this chain */
98         for (tree = pchan_root->iktree.first; tree; tree = tree->next) {
99                 for (target = tree->targets.first; target; target = target->next) {
100                         curchan = tree->pchan[target->tip];
101                         if (curchan->flag & POSE_CHAIN)
102                                 curchan->flag &= ~POSE_CHAIN;
103                         else
104                                 break;
105                 }
106                 if (target) break;
107         }
108
109         /* create a target */
110         target = MEM_callocN(sizeof(PoseTarget), "posetarget");
111         target->con = con;
112         pchan_tip->flag &= ~POSE_CHAIN;
113
114         if (tree == NULL) {
115                 /* make new tree */
116                 tree = MEM_callocN(sizeof(PoseTree), "posetree");
117
118                 tree->type = CONSTRAINT_TYPE_KINEMATIC;
119                 
120                 tree->iterations = data->iterations;
121                 tree->totchannel = segcount;
122                 tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
123                 
124                 tree->pchan = MEM_callocN(segcount * sizeof(void *), "ik tree pchan");
125                 tree->parent = MEM_callocN(segcount * sizeof(int), "ik tree parent");
126                 for (a = 0; a < segcount; a++) {
127                         tree->pchan[a] = chanlist[segcount - a - 1];
128                         tree->parent[a] = a - 1;
129                 }
130                 target->tip = segcount - 1;
131                 
132                 /* AND! link the tree to the root */
133                 BLI_addtail(&pchan_root->iktree, tree);
134         }
135         else {
136                 tree->iterations = MAX2(data->iterations, tree->iterations);
137                 tree->stretch = tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
138
139                 /* skip common pose channels and add remaining*/
140                 size = MIN2(segcount, tree->totchannel);
141                 a = t = 0;
142                 while (a < size && t < tree->totchannel) {
143                         // locate first matching channel
144                         for (; t < tree->totchannel && tree->pchan[t] != chanlist[segcount - a - 1]; t++) ;
145                         if (t >= tree->totchannel)
146                                 break;
147                         for (; a < size && t < tree->totchannel && tree->pchan[t] == chanlist[segcount - a - 1]; a++, t++) ;
148                 }
149
150                 segcount = segcount - a;
151                 target->tip = tree->totchannel + segcount - 1;
152
153                 if (segcount > 0) {
154                         for (parent = a - 1; parent < tree->totchannel; parent++)
155                                 if (tree->pchan[parent] == chanlist[segcount - 1]->parent)
156                                         break;
157                         
158                         /* shouldn't happen, but could with dependency cycles */
159                         if (parent == tree->totchannel)
160                                 parent = a - 1;
161
162                         /* resize array */
163                         newsize = tree->totchannel + segcount;
164                         oldchan = tree->pchan;
165                         oldparent = tree->parent;
166
167                         tree->pchan = MEM_callocN(newsize * sizeof(void *), "ik tree pchan");
168                         tree->parent = MEM_callocN(newsize * sizeof(int), "ik tree parent");
169                         memcpy(tree->pchan, oldchan, sizeof(void *) * tree->totchannel);
170                         memcpy(tree->parent, oldparent, sizeof(int) * tree->totchannel);
171                         MEM_freeN(oldchan);
172                         MEM_freeN(oldparent);
173
174                         /* add new pose channels at the end, in reverse order */
175                         for (a = 0; a < segcount; a++) {
176                                 tree->pchan[tree->totchannel + a] = chanlist[segcount - a - 1];
177                                 tree->parent[tree->totchannel + a] = tree->totchannel + a - 1;
178                         }
179                         tree->parent[tree->totchannel] = parent;
180                         
181                         tree->totchannel = newsize;
182                 }
183
184                 /* move tree to end of list, for correct evaluation order */
185                 BLI_remlink(&pchan_root->iktree, tree);
186                 BLI_addtail(&pchan_root->iktree, tree);
187         }
188
189         /* add target to the tree */
190         BLI_addtail(&tree->targets, target);
191         /* mark root channel having an IK tree */
192         pchan_root->flag |= POSE_IKTREE;
193 }
194
195
196 /* transform from bone(b) to bone(b+1), store in chan_mat */
197 static void make_dmats(bPoseChannel *pchan)
198 {
199         if (pchan->parent) {
200                 float iR_parmat[4][4];
201                 invert_m4_m4(iR_parmat, pchan->parent->pose_mat);
202                 mult_m4_m4m4(pchan->chan_mat, iR_parmat,  pchan->pose_mat); // delta mat
203         }
204         else copy_m4_m4(pchan->chan_mat, pchan->pose_mat);
205 }
206
207 /* applies IK matrix to pchan, IK is done separated */
208 /* formula: pose_mat(b) = pose_mat(b-1) * diffmat(b-1, b) * ik_mat(b) */
209 /* to make this work, the diffmats have to be precalculated! Stored in chan_mat */
210 static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[][3])   // nr = to detect if this is first bone
211 {
212         float vec[3], ikmat[4][4];
213         
214         copy_m4_m3(ikmat, ik_mat);
215         
216         if (pchan->parent)
217                 mul_serie_m4(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat, ikmat, NULL, NULL, NULL, NULL, NULL);
218         else 
219                 mult_m4_m4m4(pchan->pose_mat, pchan->chan_mat, ikmat);
220
221         /* calculate head */
222         copy_v3_v3(pchan->pose_head, pchan->pose_mat[3]);
223         /* calculate tail */
224         copy_v3_v3(vec, pchan->pose_mat[1]);
225         mul_v3_fl(vec, pchan->bone->length);
226         add_v3_v3v3(pchan->pose_tail, pchan->pose_head, vec);
227
228         pchan->flag |= POSE_DONE;
229 }
230
231
232 /* called from within the core BKE_pose_where_is loop, all animsystems and constraints
233  * were executed & assigned. Now as last we do an IK pass */
234 static void execute_posetree(struct Scene *scene, Object *ob, PoseTree *tree)
235 {
236         float R_parmat[3][3], identity[3][3];
237         float iR_parmat[3][3];
238         float R_bonemat[3][3];
239         float goalrot[3][3], goalpos[3];
240         float rootmat[4][4], imat[4][4];
241         float goal[4][4], goalinv[4][4];
242         float irest_basis[3][3], full_basis[3][3];
243         float end_pose[4][4], world_pose[4][4];
244         float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch = NULL;
245         float resultinf = 0.0f;
246         int a, flag, hasstretch = 0, resultblend = 0;
247         bPoseChannel *pchan;
248         IK_Segment *seg, *parent, **iktree, *iktarget;
249         IK_Solver *solver;
250         PoseTarget *target;
251         bKinematicConstraint *data, *poleangledata = NULL;
252         Bone *bone;
253
254         if (tree->totchannel == 0)
255                 return;
256         
257         iktree = MEM_mallocN(sizeof(void *) * tree->totchannel, "ik tree");
258
259         for (a = 0; a < tree->totchannel; a++) {
260                 pchan = tree->pchan[a];
261                 bone = pchan->bone;
262                 
263                 /* set DoF flag */
264                 flag = 0;
265                 if (!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP))
266                         flag |= IK_XDOF;
267                 if (!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP))
268                         flag |= IK_YDOF;
269                 if (!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP))
270                         flag |= IK_ZDOF;
271                 
272                 if (tree->stretch && (pchan->ikstretch > 0.0f)) {
273                         flag |= IK_TRANS_YDOF;
274                         hasstretch = 1;
275                 }
276                 
277                 seg = iktree[a] = IK_CreateSegment(flag);
278                 
279                 /* find parent */
280                 if (a == 0)
281                         parent = NULL;
282                 else
283                         parent = iktree[tree->parent[a]];
284                         
285                 IK_SetParent(seg, parent);
286                         
287                 /* get the matrix that transforms from prevbone into this bone */
288                 copy_m3_m4(R_bonemat, pchan->pose_mat);
289                 
290                 /* gather transformations for this IK segment */
291                 
292                 if (pchan->parent)
293                         copy_m3_m4(R_parmat, pchan->parent->pose_mat);
294                 else
295                         unit_m3(R_parmat);
296                 
297                 /* bone offset */
298                 if (pchan->parent && (a > 0))
299                         sub_v3_v3v3(start, pchan->pose_head, pchan->parent->pose_tail);
300                 else
301                         /* only root bone (a = 0) has no parent */
302                         start[0] = start[1] = start[2] = 0.0f;
303                 
304                 /* change length based on bone size */
305                 length = bone->length * len_v3(R_bonemat[1]);
306                 
307                 /* compute rest basis and its inverse */
308                 copy_m3_m3(rest_basis, bone->bone_mat);
309                 copy_m3_m3(irest_basis, bone->bone_mat);
310                 transpose_m3(irest_basis);
311                 
312                 /* compute basis with rest_basis removed */
313                 invert_m3_m3(iR_parmat, R_parmat);
314                 mul_m3_m3m3(full_basis, iR_parmat, R_bonemat);
315                 mul_m3_m3m3(basis, irest_basis, full_basis);
316                 
317                 /* basis must be pure rotation */
318                 normalize_m3(basis);
319                 
320                 /* transform offset into local bone space */
321                 normalize_m3(iR_parmat);
322                 mul_m3_v3(iR_parmat, start);
323                 
324                 IK_SetTransform(seg, start, rest_basis, basis, length);
325                 
326                 if (pchan->ikflag & BONE_IK_XLIMIT)
327                         IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]);
328                 if (pchan->ikflag & BONE_IK_YLIMIT)
329                         IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]);
330                 if (pchan->ikflag & BONE_IK_ZLIMIT)
331                         IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]);
332                 
333                 IK_SetStiffness(seg, IK_X, pchan->stiffness[0]);
334                 IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]);
335                 IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]);
336                 
337                 if (tree->stretch && (pchan->ikstretch > 0.0f)) {
338                         float ikstretch = pchan->ikstretch * pchan->ikstretch;
339                         IK_SetStiffness(seg, IK_TRANS_Y, MIN2(1.0f - ikstretch, 0.99f));
340                         IK_SetLimit(seg, IK_TRANS_Y, 0.001, 1e10);
341                 }
342         }
343
344         solver = IK_CreateSolver(iktree[0]);
345
346         /* set solver goals */
347
348         /* first set the goal inverse transform, assuming the root of tree was done ok! */
349         pchan = tree->pchan[0];
350         if (pchan->parent) {
351                 /* transform goal by parent mat, so this rotation is not part of the
352                  * segment's basis. otherwise rotation limits do not work on the
353                  * local transform of the segment itself. */
354                 copy_m4_m4(rootmat, pchan->parent->pose_mat);
355                 /* However, we do not want to get (i.e. reverse) parent's scale, as it generates [#31008]
356                  * kind of nasty bugs... */
357                 normalize_m4(rootmat);
358         }
359         else
360                 unit_m4(rootmat);
361         copy_v3_v3(rootmat[3], pchan->pose_head);
362         
363         mult_m4_m4m4(imat, ob->obmat, rootmat);
364         invert_m4_m4(goalinv, imat);
365         
366         for (target = tree->targets.first; target; target = target->next) {
367                 float polepos[3];
368                 int poleconstrain = 0;
369                 
370                 data = (bKinematicConstraint *)target->con->data;
371                 
372                 /* 1.0=ctime, we pass on object for auto-ik (owner-type here is object, even though
373                  * strictly speaking, it is a posechannel)
374                  */
375                 get_constraint_target_matrix(scene, target->con, 0, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
376                 
377                 /* and set and transform goal */
378                 mult_m4_m4m4(goal, goalinv, rootmat);
379                 
380                 copy_v3_v3(goalpos, goal[3]);
381                 copy_m3_m4(goalrot, goal);
382                 
383                 /* same for pole vector target */
384                 if (data->poletar) {
385                         get_constraint_target_matrix(scene, target->con, 1, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
386                         
387                         if (data->flag & CONSTRAINT_IK_SETANGLE) {
388                                 /* don't solve IK when we are setting the pole angle */
389                                 break;
390                         }
391                         else {
392                                 mult_m4_m4m4(goal, goalinv, rootmat);
393                                 copy_v3_v3(polepos, goal[3]);
394                                 poleconstrain = 1;
395
396                                 /* for pole targets, we blend the result of the ik solver
397                                  * instead of the target position, otherwise we can't get
398                                  * a smooth transition */
399                                 resultblend = 1;
400                                 resultinf = target->con->enforce;
401                                 
402                                 if (data->flag & CONSTRAINT_IK_GETANGLE) {
403                                         poleangledata = data;
404                                         data->flag &= ~CONSTRAINT_IK_GETANGLE;
405                                 }
406                         }
407                 }
408
409                 /* do we need blending? */
410                 if (!resultblend && target->con->enforce != 1.0f) {
411                         float q1[4], q2[4], q[4];
412                         float fac = target->con->enforce;
413                         float mfac = 1.0f - fac;
414                         
415                         pchan = tree->pchan[target->tip];
416                         
417                         /* end effector in world space */
418                         copy_m4_m4(end_pose, pchan->pose_mat);
419                         copy_v3_v3(end_pose[3], pchan->pose_tail);
420                         mul_serie_m4(world_pose, goalinv, ob->obmat, end_pose, NULL, NULL, NULL, NULL, NULL);
421                         
422                         /* blend position */
423                         goalpos[0] = fac * goalpos[0] + mfac * world_pose[3][0];
424                         goalpos[1] = fac * goalpos[1] + mfac * world_pose[3][1];
425                         goalpos[2] = fac * goalpos[2] + mfac * world_pose[3][2];
426                         
427                         /* blend rotation */
428                         mat3_to_quat(q1, goalrot);
429                         mat4_to_quat(q2, world_pose);
430                         interp_qt_qtqt(q, q1, q2, mfac);
431                         quat_to_mat3(goalrot, q);
432                 }
433                 
434                 iktarget = iktree[target->tip];
435                 
436                 if (data->weight != 0.0f) {
437                         if (poleconstrain)
438                                 IK_SolverSetPoleVectorConstraint(solver, iktarget, goalpos,
439                                                                  polepos, data->poleangle, (poleangledata == data));
440                         IK_SolverAddGoal(solver, iktarget, goalpos, data->weight);
441                 }
442                 if ((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0f))
443                         if ((data->flag & CONSTRAINT_IK_AUTO) == 0)
444                                 IK_SolverAddGoalOrientation(solver, iktarget, goalrot,
445                                                             data->orientweight);
446         }
447
448         /* solve */
449         IK_Solve(solver, 0.0f, tree->iterations);
450
451         if (poleangledata)
452                 poleangledata->poleangle = IK_SolverGetPoleAngle(solver);
453
454         IK_FreeSolver(solver);
455
456         /* gather basis changes */
457         tree->basis_change = MEM_mallocN(sizeof(float[3][3]) * tree->totchannel, "ik basis change");
458         if (hasstretch)
459                 ikstretch = MEM_mallocN(sizeof(float) * tree->totchannel, "ik stretch");
460         
461         for (a = 0; a < tree->totchannel; a++) {
462                 IK_GetBasisChange(iktree[a], tree->basis_change[a]);
463                 
464                 if (hasstretch) {
465                         /* have to compensate for scaling received from parent */
466                         float parentstretch, stretch;
467                         
468                         pchan = tree->pchan[a];
469                         parentstretch = (tree->parent[a] >= 0) ? ikstretch[tree->parent[a]] : 1.0f;
470                         
471                         if (tree->stretch && (pchan->ikstretch > 0.0f)) {
472                                 float trans[3], length;
473                                 
474                                 IK_GetTranslationChange(iktree[a], trans);
475                                 length = pchan->bone->length * len_v3(pchan->pose_mat[1]);
476                                 
477                                 ikstretch[a] = (length == 0.0f) ? 1.0f : (trans[1] + length) / length;
478                         }
479                         else
480                                 ikstretch[a] = 1.0;
481                         
482                         stretch = (parentstretch == 0.0f) ? 1.0f : ikstretch[a] / parentstretch;
483                         
484                         mul_v3_fl(tree->basis_change[a][0], stretch);
485                         mul_v3_fl(tree->basis_change[a][1], stretch);
486                         mul_v3_fl(tree->basis_change[a][2], stretch);
487                 }
488
489                 if (resultblend && resultinf != 1.0f) {
490                         unit_m3(identity);
491                         blend_m3_m3m3(tree->basis_change[a], identity,
492                                       tree->basis_change[a], resultinf);
493                 }
494                 
495                 IK_FreeSegment(iktree[a]);
496         }
497         
498         MEM_freeN(iktree);
499         if (ikstretch) MEM_freeN(ikstretch);
500 }
501
502 static void free_posetree(PoseTree *tree)
503 {
504         BLI_freelistN(&tree->targets);
505         if (tree->pchan) MEM_freeN(tree->pchan);
506         if (tree->parent) MEM_freeN(tree->parent);
507         if (tree->basis_change) MEM_freeN(tree->basis_change);
508         MEM_freeN(tree);
509 }
510
511 ///----------------------------------------
512 /// Plugin API for legacy iksolver
513
514 void iksolver_initialize_tree(struct Scene *UNUSED(scene), struct Object *ob, float UNUSED(ctime))
515 {
516         bPoseChannel *pchan;
517         
518         for (pchan = ob->pose->chanbase.first; pchan; pchan = pchan->next) {
519                 if (pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints
520                         initialize_posetree(ob, pchan);  // will attach it to root!
521         }
522         ob->pose->flag &= ~POSE_WAS_REBUILT;
523 }
524
525 void iksolver_execute_tree(struct Scene *scene, struct Object *ob,  struct bPoseChannel *pchan, float ctime)
526 {
527         while (pchan->iktree.first) {
528                 PoseTree *tree = pchan->iktree.first;
529                 int a;
530                 
531                 /* stop on the first tree that isn't a standard IK chain */
532                 if (tree->type != CONSTRAINT_TYPE_KINEMATIC)
533                         return;
534                 
535                 /* 4. walk over the tree for regular solving */
536                 for (a = 0; a < tree->totchannel; a++) {
537                         if (!(tree->pchan[a]->flag & POSE_DONE))    // successive trees can set the flag
538                                 BKE_pose_where_is_bone(scene, ob, tree->pchan[a], ctime, 1);
539                         // tell blender that this channel was controlled by IK, it's cleared on each BKE_pose_where_is()
540                         tree->pchan[a]->flag |= POSE_CHAIN;
541                 }
542                 /* 5. execute the IK solver */
543                 execute_posetree(scene, ob, tree);
544                 
545                 /* 6. apply the differences to the channels, 
546                  *    we need to calculate the original differences first */
547                 for (a = 0; a < tree->totchannel; a++) {
548                         make_dmats(tree->pchan[a]);
549                 }
550                 
551                 for (a = 0; a < tree->totchannel; a++) {
552                         /* sets POSE_DONE */
553                         where_is_ik_bone(tree->pchan[a], tree->basis_change[a]);
554                 }
555                 
556                 /* 7. and free */
557                 BLI_remlink(&pchan->iktree, tree);
558                 free_posetree(tree);
559         }
560 }
561