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