|
Blender
V2.59
|
00001 /* 00002 * $Id: iksolver_plugin.c 35336 2011-03-03 17:58:06Z campbellbarton $ 00003 * ***** BEGIN GPL LICENSE BLOCK ***** 00004 * 00005 * This program is free software; you can redistribute it and/or 00006 * modify it under the terms of the GNU General Public License 00007 * as published by the Free Software Foundation; either version 2 00008 * of the License, or (at your option) any later version. 00009 * 00010 * This program is distributed in the hope that it will be useful, 00011 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00012 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00013 * GNU General Public License for more details. 00014 * 00015 * You should have received a copy of the GNU General Public License 00016 * along with this program; if not, write to the Free Software Foundation, 00017 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 00018 * 00019 * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. 00020 * All rights reserved. 00021 * 00022 * The Original Code is: all of this file. 00023 * 00024 * Original author: Benoit Bolsee 00025 * Contributor(s): 00026 * 00027 * ***** END GPL LICENSE BLOCK ***** 00028 */ 00029 00035 #include "MEM_guardedalloc.h" 00036 00037 #include "BIK_api.h" 00038 #include "BLI_blenlib.h" 00039 #include "BLI_math.h" 00040 #include "BLI_utildefines.h" 00041 00042 #include "BKE_armature.h" 00043 #include "BKE_constraint.h" 00044 00045 #include "DNA_object_types.h" 00046 #include "DNA_action_types.h" 00047 #include "DNA_constraint_types.h" 00048 #include "DNA_armature_types.h" 00049 00050 #include "IK_solver.h" 00051 #include "iksolver_plugin.h" 00052 00053 #include <string.h> /* memcpy */ 00054 00055 /* ********************** THE IK SOLVER ******************* */ 00056 00057 /* allocates PoseTree, and links that to root bone/channel */ 00058 /* Note: detecting the IK chain is duplicate code... in drawarmature.c and in transform_conversions.c */ 00059 static void initialize_posetree(struct Object *UNUSED(ob), bPoseChannel *pchan_tip) 00060 { 00061 bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan; 00062 PoseTree *tree; 00063 PoseTarget *target; 00064 bConstraint *con; 00065 bKinematicConstraint *data; 00066 int a, segcount= 0, size, newsize, *oldparent, parent; 00067 00068 /* find IK constraint, and validate it */ 00069 for(con= pchan_tip->constraints.first; con; con= con->next) { 00070 if(con->type==CONSTRAINT_TYPE_KINEMATIC) { 00071 data=(bKinematicConstraint*)con->data; 00072 if (data->flag & CONSTRAINT_IK_AUTO) break; 00073 if (data->tar==NULL) continue; 00074 if (data->tar->type==OB_ARMATURE && data->subtarget[0]==0) continue; 00075 if ((con->flag & (CONSTRAINT_DISABLE|CONSTRAINT_OFF))==0 && (con->enforce!=0.0)) break; 00076 } 00077 } 00078 if(con==NULL) return; 00079 00080 /* exclude tip from chain? */ 00081 if(!(data->flag & CONSTRAINT_IK_TIP)) 00082 pchan_tip= pchan_tip->parent; 00083 00084 /* Find the chain's root & count the segments needed */ 00085 for (curchan = pchan_tip; curchan; curchan=curchan->parent){ 00086 pchan_root = curchan; 00087 00088 curchan->flag |= POSE_CHAIN; // don't forget to clear this 00089 chanlist[segcount]=curchan; 00090 segcount++; 00091 00092 if(segcount==data->rootbone || segcount>255) break; // 255 is weak 00093 } 00094 if (!segcount) return; 00095 00096 /* setup the chain data */ 00097 00098 /* we make tree-IK, unless all existing targets are in this chain */ 00099 for(tree= pchan_root->iktree.first; tree; tree= tree->next) { 00100 for(target= tree->targets.first; target; target= target->next) { 00101 curchan= tree->pchan[target->tip]; 00102 if(curchan->flag & POSE_CHAIN) 00103 curchan->flag &= ~POSE_CHAIN; 00104 else 00105 break; 00106 } 00107 if(target) break; 00108 } 00109 00110 /* create a target */ 00111 target= MEM_callocN(sizeof(PoseTarget), "posetarget"); 00112 target->con= con; 00113 pchan_tip->flag &= ~POSE_CHAIN; 00114 00115 if(tree==NULL) { 00116 /* make new tree */ 00117 tree= MEM_callocN(sizeof(PoseTree), "posetree"); 00118 00119 tree->type= CONSTRAINT_TYPE_KINEMATIC; 00120 00121 tree->iterations= data->iterations; 00122 tree->totchannel= segcount; 00123 tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH); 00124 00125 tree->pchan= MEM_callocN(segcount*sizeof(void*), "ik tree pchan"); 00126 tree->parent= MEM_callocN(segcount*sizeof(int), "ik tree parent"); 00127 for(a=0; a<segcount; a++) { 00128 tree->pchan[a]= chanlist[segcount-a-1]; 00129 tree->parent[a]= a-1; 00130 } 00131 target->tip= segcount-1; 00132 00133 /* AND! link the tree to the root */ 00134 BLI_addtail(&pchan_root->iktree, tree); 00135 } 00136 else { 00137 tree->iterations= MAX2(data->iterations, tree->iterations); 00138 tree->stretch= tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH); 00139 00140 /* skip common pose channels and add remaining*/ 00141 size= MIN2(segcount, tree->totchannel); 00142 for(a=0; a<size && tree->pchan[a]==chanlist[segcount-a-1]; a++); 00143 parent= a-1; 00144 00145 segcount= segcount-a; 00146 target->tip= tree->totchannel + segcount - 1; 00147 00148 if (segcount > 0) { 00149 /* resize array */ 00150 newsize= tree->totchannel + segcount; 00151 oldchan= tree->pchan; 00152 oldparent= tree->parent; 00153 00154 tree->pchan= MEM_callocN(newsize*sizeof(void*), "ik tree pchan"); 00155 tree->parent= MEM_callocN(newsize*sizeof(int), "ik tree parent"); 00156 memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel); 00157 memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel); 00158 MEM_freeN(oldchan); 00159 MEM_freeN(oldparent); 00160 00161 /* add new pose channels at the end, in reverse order */ 00162 for(a=0; a<segcount; a++) { 00163 tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1]; 00164 tree->parent[tree->totchannel+a]= tree->totchannel+a-1; 00165 } 00166 tree->parent[tree->totchannel]= parent; 00167 00168 tree->totchannel= newsize; 00169 } 00170 00171 /* move tree to end of list, for correct evaluation order */ 00172 BLI_remlink(&pchan_root->iktree, tree); 00173 BLI_addtail(&pchan_root->iktree, tree); 00174 } 00175 00176 /* add target to the tree */ 00177 BLI_addtail(&tree->targets, target); 00178 /* mark root channel having an IK tree */ 00179 pchan_root->flag |= POSE_IKTREE; 00180 } 00181 00182 00183 /* transform from bone(b) to bone(b+1), store in chan_mat */ 00184 static void make_dmats(bPoseChannel *pchan) 00185 { 00186 if (pchan->parent) { 00187 float iR_parmat[4][4]; 00188 invert_m4_m4(iR_parmat, pchan->parent->pose_mat); 00189 mul_m4_m4m4(pchan->chan_mat, pchan->pose_mat, iR_parmat); // delta mat 00190 } 00191 else copy_m4_m4(pchan->chan_mat, pchan->pose_mat); 00192 } 00193 00194 /* applies IK matrix to pchan, IK is done separated */ 00195 /* formula: pose_mat(b) = pose_mat(b-1) * diffmat(b-1, b) * ik_mat(b) */ 00196 /* to make this work, the diffmats have to be precalculated! Stored in chan_mat */ 00197 static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[][3]) // nr = to detect if this is first bone 00198 { 00199 float vec[3], ikmat[4][4]; 00200 00201 copy_m4_m3(ikmat, ik_mat); 00202 00203 if (pchan->parent) 00204 mul_serie_m4(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat, ikmat, NULL, NULL, NULL, NULL, NULL); 00205 else 00206 mul_m4_m4m4(pchan->pose_mat, ikmat, pchan->chan_mat); 00207 00208 /* calculate head */ 00209 VECCOPY(pchan->pose_head, pchan->pose_mat[3]); 00210 /* calculate tail */ 00211 VECCOPY(vec, pchan->pose_mat[1]); 00212 mul_v3_fl(vec, pchan->bone->length); 00213 add_v3_v3v3(pchan->pose_tail, pchan->pose_head, vec); 00214 00215 pchan->flag |= POSE_DONE; 00216 } 00217 00218 00219 /* called from within the core where_is_pose loop, all animsystems and constraints 00220 were executed & assigned. Now as last we do an IK pass */ 00221 static void execute_posetree(struct Scene *scene, Object *ob, PoseTree *tree) 00222 { 00223 float R_parmat[3][3], identity[3][3]; 00224 float iR_parmat[3][3]; 00225 float R_bonemat[3][3]; 00226 float goalrot[3][3], goalpos[3]; 00227 float rootmat[4][4], imat[4][4]; 00228 float goal[4][4], goalinv[4][4]; 00229 float irest_basis[3][3], full_basis[3][3]; 00230 float end_pose[4][4], world_pose[4][4]; 00231 float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch=NULL; 00232 float resultinf=0.0f; 00233 int a, flag, hasstretch=0, resultblend=0; 00234 bPoseChannel *pchan; 00235 IK_Segment *seg, *parent, **iktree, *iktarget; 00236 IK_Solver *solver; 00237 PoseTarget *target; 00238 bKinematicConstraint *data, *poleangledata=NULL; 00239 Bone *bone; 00240 00241 if (tree->totchannel == 0) 00242 return; 00243 00244 iktree= MEM_mallocN(sizeof(void*)*tree->totchannel, "ik tree"); 00245 00246 for(a=0; a<tree->totchannel; a++) { 00247 pchan= tree->pchan[a]; 00248 bone= pchan->bone; 00249 00250 /* set DoF flag */ 00251 flag= 0; 00252 if(!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP)) 00253 flag |= IK_XDOF; 00254 if(!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP)) 00255 flag |= IK_YDOF; 00256 if(!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP)) 00257 flag |= IK_ZDOF; 00258 00259 if(tree->stretch && (pchan->ikstretch > 0.0)) { 00260 flag |= IK_TRANS_YDOF; 00261 hasstretch = 1; 00262 } 00263 00264 seg= iktree[a]= IK_CreateSegment(flag); 00265 00266 /* find parent */ 00267 if(a == 0) 00268 parent= NULL; 00269 else 00270 parent= iktree[tree->parent[a]]; 00271 00272 IK_SetParent(seg, parent); 00273 00274 /* get the matrix that transforms from prevbone into this bone */ 00275 copy_m3_m4(R_bonemat, pchan->pose_mat); 00276 00277 /* gather transformations for this IK segment */ 00278 00279 if (pchan->parent) 00280 copy_m3_m4(R_parmat, pchan->parent->pose_mat); 00281 else 00282 unit_m3(R_parmat); 00283 00284 /* bone offset */ 00285 if (pchan->parent && (a > 0)) 00286 sub_v3_v3v3(start, pchan->pose_head, pchan->parent->pose_tail); 00287 else 00288 /* only root bone (a = 0) has no parent */ 00289 start[0]= start[1]= start[2]= 0.0f; 00290 00291 /* change length based on bone size */ 00292 length= bone->length*len_v3(R_bonemat[1]); 00293 00294 /* compute rest basis and its inverse */ 00295 copy_m3_m3(rest_basis, bone->bone_mat); 00296 copy_m3_m3(irest_basis, bone->bone_mat); 00297 transpose_m3(irest_basis); 00298 00299 /* compute basis with rest_basis removed */ 00300 invert_m3_m3(iR_parmat, R_parmat); 00301 mul_m3_m3m3(full_basis, iR_parmat, R_bonemat); 00302 mul_m3_m3m3(basis, irest_basis, full_basis); 00303 00304 /* basis must be pure rotation */ 00305 normalize_m3(basis); 00306 00307 /* transform offset into local bone space */ 00308 normalize_m3(iR_parmat); 00309 mul_m3_v3(iR_parmat, start); 00310 00311 IK_SetTransform(seg, start, rest_basis, basis, length); 00312 00313 if (pchan->ikflag & BONE_IK_XLIMIT) 00314 IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]); 00315 if (pchan->ikflag & BONE_IK_YLIMIT) 00316 IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]); 00317 if (pchan->ikflag & BONE_IK_ZLIMIT) 00318 IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]); 00319 00320 IK_SetStiffness(seg, IK_X, pchan->stiffness[0]); 00321 IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]); 00322 IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]); 00323 00324 if(tree->stretch && (pchan->ikstretch > 0.0)) { 00325 float ikstretch = pchan->ikstretch*pchan->ikstretch; 00326 IK_SetStiffness(seg, IK_TRANS_Y, MIN2(1.0-ikstretch, 0.99)); 00327 IK_SetLimit(seg, IK_TRANS_Y, 0.001, 1e10); 00328 } 00329 } 00330 00331 solver= IK_CreateSolver(iktree[0]); 00332 00333 /* set solver goals */ 00334 00335 /* first set the goal inverse transform, assuming the root of tree was done ok! */ 00336 pchan= tree->pchan[0]; 00337 if (pchan->parent) 00338 /* transform goal by parent mat, so this rotation is not part of the 00339 segment's basis. otherwise rotation limits do not work on the 00340 local transform of the segment itself. */ 00341 copy_m4_m4(rootmat, pchan->parent->pose_mat); 00342 else 00343 unit_m4(rootmat); 00344 VECCOPY(rootmat[3], pchan->pose_head); 00345 00346 mul_m4_m4m4(imat, rootmat, ob->obmat); 00347 invert_m4_m4(goalinv, imat); 00348 00349 for (target=tree->targets.first; target; target=target->next) { 00350 float polepos[3]; 00351 int poleconstrain= 0; 00352 00353 data= (bKinematicConstraint*)target->con->data; 00354 00355 /* 1.0=ctime, we pass on object for auto-ik (owner-type here is object, even though 00356 * strictly speaking, it is a posechannel) 00357 */ 00358 get_constraint_target_matrix(scene, target->con, 0, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0); 00359 00360 /* and set and transform goal */ 00361 mul_m4_m4m4(goal, rootmat, goalinv); 00362 00363 VECCOPY(goalpos, goal[3]); 00364 copy_m3_m4(goalrot, goal); 00365 00366 /* same for pole vector target */ 00367 if(data->poletar) { 00368 get_constraint_target_matrix(scene, target->con, 1, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0); 00369 00370 if(data->flag & CONSTRAINT_IK_SETANGLE) { 00371 /* don't solve IK when we are setting the pole angle */ 00372 break; 00373 } 00374 else { 00375 mul_m4_m4m4(goal, rootmat, goalinv); 00376 VECCOPY(polepos, goal[3]); 00377 poleconstrain= 1; 00378 00379 /* for pole targets, we blend the result of the ik solver 00380 * instead of the target position, otherwise we can't get 00381 * a smooth transition */ 00382 resultblend= 1; 00383 resultinf= target->con->enforce; 00384 00385 if(data->flag & CONSTRAINT_IK_GETANGLE) { 00386 poleangledata= data; 00387 data->flag &= ~CONSTRAINT_IK_GETANGLE; 00388 } 00389 } 00390 } 00391 00392 /* do we need blending? */ 00393 if (!resultblend && target->con->enforce!=1.0) { 00394 float q1[4], q2[4], q[4]; 00395 float fac= target->con->enforce; 00396 float mfac= 1.0-fac; 00397 00398 pchan= tree->pchan[target->tip]; 00399 00400 /* end effector in world space */ 00401 copy_m4_m4(end_pose, pchan->pose_mat); 00402 VECCOPY(end_pose[3], pchan->pose_tail); 00403 mul_serie_m4(world_pose, goalinv, ob->obmat, end_pose, NULL, NULL, NULL, NULL, NULL); 00404 00405 /* blend position */ 00406 goalpos[0]= fac*goalpos[0] + mfac*world_pose[3][0]; 00407 goalpos[1]= fac*goalpos[1] + mfac*world_pose[3][1]; 00408 goalpos[2]= fac*goalpos[2] + mfac*world_pose[3][2]; 00409 00410 /* blend rotation */ 00411 mat3_to_quat( q1,goalrot); 00412 mat4_to_quat( q2,world_pose); 00413 interp_qt_qtqt(q, q1, q2, mfac); 00414 quat_to_mat3( goalrot,q); 00415 } 00416 00417 iktarget= iktree[target->tip]; 00418 00419 if(data->weight != 0.0) { 00420 if(poleconstrain) 00421 IK_SolverSetPoleVectorConstraint(solver, iktarget, goalpos, 00422 polepos, data->poleangle, (poleangledata == data)); 00423 IK_SolverAddGoal(solver, iktarget, goalpos, data->weight); 00424 } 00425 if((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0)) 00426 if((data->flag & CONSTRAINT_IK_AUTO)==0) 00427 IK_SolverAddGoalOrientation(solver, iktarget, goalrot, 00428 data->orientweight); 00429 } 00430 00431 /* solve */ 00432 IK_Solve(solver, 0.0f, tree->iterations); 00433 00434 if(poleangledata) 00435 poleangledata->poleangle= IK_SolverGetPoleAngle(solver); 00436 00437 IK_FreeSolver(solver); 00438 00439 /* gather basis changes */ 00440 tree->basis_change= MEM_mallocN(sizeof(float[3][3])*tree->totchannel, "ik basis change"); 00441 if(hasstretch) 00442 ikstretch= MEM_mallocN(sizeof(float)*tree->totchannel, "ik stretch"); 00443 00444 for(a=0; a<tree->totchannel; a++) { 00445 IK_GetBasisChange(iktree[a], tree->basis_change[a]); 00446 00447 if(hasstretch) { 00448 /* have to compensate for scaling received from parent */ 00449 float parentstretch, stretch; 00450 00451 pchan= tree->pchan[a]; 00452 parentstretch= (tree->parent[a] >= 0)? ikstretch[tree->parent[a]]: 1.0; 00453 00454 if(tree->stretch && (pchan->ikstretch > 0.0)) { 00455 float trans[3], length; 00456 00457 IK_GetTranslationChange(iktree[a], trans); 00458 length= pchan->bone->length*len_v3(pchan->pose_mat[1]); 00459 00460 ikstretch[a]= (length == 0.0)? 1.0: (trans[1]+length)/length; 00461 } 00462 else 00463 ikstretch[a] = 1.0; 00464 00465 stretch= (parentstretch == 0.0)? 1.0: ikstretch[a]/parentstretch; 00466 00467 mul_v3_fl(tree->basis_change[a][0], stretch); 00468 mul_v3_fl(tree->basis_change[a][1], stretch); 00469 mul_v3_fl(tree->basis_change[a][2], stretch); 00470 } 00471 00472 if(resultblend && resultinf!=1.0f) { 00473 unit_m3(identity); 00474 blend_m3_m3m3(tree->basis_change[a], identity, 00475 tree->basis_change[a], resultinf); 00476 } 00477 00478 IK_FreeSegment(iktree[a]); 00479 } 00480 00481 MEM_freeN(iktree); 00482 if(ikstretch) MEM_freeN(ikstretch); 00483 } 00484 00485 static void free_posetree(PoseTree *tree) 00486 { 00487 BLI_freelistN(&tree->targets); 00488 if(tree->pchan) MEM_freeN(tree->pchan); 00489 if(tree->parent) MEM_freeN(tree->parent); 00490 if(tree->basis_change) MEM_freeN(tree->basis_change); 00491 MEM_freeN(tree); 00492 } 00493 00496 00497 void iksolver_initialize_tree(struct Scene *UNUSED(scene), struct Object *ob, float UNUSED(ctime)) 00498 { 00499 bPoseChannel *pchan; 00500 00501 for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) { 00502 if(pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints 00503 initialize_posetree(ob, pchan); // will attach it to root! 00504 } 00505 ob->pose->flag &= ~POSE_WAS_REBUILT; 00506 } 00507 00508 void iksolver_execute_tree(struct Scene *scene, struct Object *ob, struct bPoseChannel *pchan, float ctime) 00509 { 00510 while(pchan->iktree.first) { 00511 PoseTree *tree= pchan->iktree.first; 00512 int a; 00513 00514 /* stop on the first tree that isn't a standard IK chain */ 00515 if (tree->type != CONSTRAINT_TYPE_KINEMATIC) 00516 return; 00517 00518 /* 4. walk over the tree for regular solving */ 00519 for(a=0; a<tree->totchannel; a++) { 00520 if(!(tree->pchan[a]->flag & POSE_DONE)) // successive trees can set the flag 00521 where_is_pose_bone(scene, ob, tree->pchan[a], ctime, 1); 00522 // tell blender that this channel was controlled by IK, it's cleared on each where_is_pose() 00523 tree->pchan[a]->flag |= POSE_CHAIN; 00524 } 00525 /* 5. execute the IK solver */ 00526 execute_posetree(scene, ob, tree); 00527 00528 /* 6. apply the differences to the channels, 00529 we need to calculate the original differences first */ 00530 for(a=0; a<tree->totchannel; a++) 00531 make_dmats(tree->pchan[a]); 00532 00533 for(a=0; a<tree->totchannel; a++) 00534 /* sets POSE_DONE */ 00535 where_is_ik_bone(tree->pchan[a], tree->basis_change[a]); 00536 00537 /* 7. and free */ 00538 BLI_remlink(&pchan->iktree, tree); 00539 free_posetree(tree); 00540 } 00541 } 00542