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