|
Blender
V2.59
|
00001 /* 00002 * $Id: itasc_plugin.cpp 35814 2011-03-27 07:56:29Z 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 <stdlib.h> 00036 #include <string.h> 00037 #include <vector> 00038 00039 // iTaSC headers 00040 #ifdef WITH_IK_ITASC 00041 #include "Armature.hpp" 00042 #include "MovingFrame.hpp" 00043 #include "CopyPose.hpp" 00044 #include "WSDLSSolver.hpp" 00045 #include "WDLSSolver.hpp" 00046 #include "Scene.hpp" 00047 #include "Cache.hpp" 00048 #include "Distance.hpp" 00049 #endif 00050 00051 #include "MEM_guardedalloc.h" 00052 00053 extern "C" { 00054 #include "BIK_api.h" 00055 #include "BLI_blenlib.h" 00056 #include "BLI_math.h" 00057 #include "BLI_utildefines.h" 00058 00059 #include "BKE_global.h" 00060 #include "BKE_armature.h" 00061 #include "BKE_action.h" 00062 #include "BKE_utildefines.h" 00063 #include "BKE_constraint.h" 00064 #include "DNA_object_types.h" 00065 #include "DNA_action_types.h" 00066 #include "DNA_constraint_types.h" 00067 #include "DNA_armature_types.h" 00068 #include "DNA_scene_types.h" 00069 }; 00070 00071 #include "itasc_plugin.h" 00072 00073 // default parameters 00074 bItasc DefIKParam; 00075 00076 // in case of animation mode, feedback and timestep is fixed 00077 #define ANIM_TIMESTEP 1.0 00078 #define ANIM_FEEDBACK 0.8 00079 #define ANIM_QMAX 0.52 00080 00081 00082 // Structure pointed by bPose.ikdata 00083 // It contains everything needed to simulate the armatures 00084 // There can be several simulation islands independent to each other 00085 struct IK_Data 00086 { 00087 struct IK_Scene* first; 00088 }; 00089 00090 typedef float Vector3[3]; 00091 typedef float Vector4[4]; 00092 struct IK_Target; 00093 typedef void (*ErrorCallback)(const iTaSC::ConstraintValues* values, unsigned int nvalues, IK_Target* iktarget); 00094 // For some reason, gcc doesn't find the declaration of this function in linux 00095 void KDL::SetToZero(JntArray& array); 00096 00097 // one structure for each target in the scene 00098 struct IK_Target 00099 { 00100 struct Scene *blscene; 00101 iTaSC::MovingFrame* target; 00102 iTaSC::ConstraintSet* constraint; 00103 struct bConstraint* blenderConstraint; 00104 struct bPoseChannel* rootChannel; 00105 Object* owner; //for auto IK 00106 ErrorCallback errorCallback; 00107 std::string targetName; 00108 std::string constraintName; 00109 unsigned short controlType; 00110 short channel; //index in IK channel array of channel on which this target is defined 00111 short ee; //end effector number 00112 bool simulation; //true when simulation mode is used (update feedback) 00113 bool eeBlend; //end effector affected by enforce blending 00114 float eeRest[4][4]; //end effector initial pose relative to armature 00115 00116 IK_Target() { 00117 blscene = NULL; 00118 target = NULL; 00119 constraint = NULL; 00120 blenderConstraint = NULL; 00121 rootChannel = NULL; 00122 owner = NULL; 00123 controlType = 0; 00124 channel = 0; 00125 ee = 0; 00126 eeBlend = true; 00127 simulation = true; 00128 targetName.reserve(32); 00129 constraintName.reserve(32); 00130 } 00131 ~IK_Target() { 00132 if (constraint) 00133 delete constraint; 00134 if (target) 00135 delete target; 00136 } 00137 }; 00138 00139 struct IK_Channel { 00140 bPoseChannel* pchan; // channel where we must copy matrix back 00141 KDL::Frame frame; // frame of the bone relative to object base, not armature base 00142 std::string tail; // segment name of the joint from which we get the bone tail 00143 std::string head; // segment name of the joint from which we get the bone head 00144 int parent; // index in this array of the parent channel 00145 short jointType; // type of joint, combination of IK_SegmentFlag 00146 char ndof; // number of joint angles for this channel 00147 char jointValid; // set to 1 when jointValue has been computed 00148 // for joint constraint 00149 Object* owner; // for pose and IK param 00150 double jointValue[4]; // computed joint value 00151 00152 IK_Channel() { 00153 pchan = NULL; 00154 parent = -1; 00155 jointType = 0; 00156 ndof = 0; 00157 jointValid = 0; 00158 owner = NULL; 00159 jointValue[0] = 0.0; 00160 jointValue[1] = 0.0; 00161 jointValue[2] = 0.0; 00162 jointValue[3] = 0.0; 00163 } 00164 }; 00165 00166 struct IK_Scene 00167 { 00168 struct Scene *blscene; 00169 IK_Scene* next; 00170 int numchan; // number of channel in pchan 00171 int numjoint; // number of joint in jointArray 00172 // array of bone information, one per channel in the tree 00173 IK_Channel* channels; 00174 iTaSC::Armature* armature; 00175 iTaSC::Cache* cache; 00176 iTaSC::Scene* scene; 00177 iTaSC::MovingFrame* base; // armature base object 00178 KDL::Frame baseFrame; // frame of armature base relative to blArmature 00179 KDL::JntArray jointArray; // buffer for storing temporary joint array 00180 iTaSC::Solver* solver; 00181 Object* blArmature; 00182 struct bConstraint* polarConstraint; 00183 std::vector<IK_Target*> targets; 00184 00185 IK_Scene() { 00186 blscene = NULL; 00187 next = NULL; 00188 channels = NULL; 00189 armature = NULL; 00190 cache = NULL; 00191 scene = NULL; 00192 base = NULL; 00193 solver = NULL; 00194 blArmature = NULL; 00195 numchan = 0; 00196 numjoint = 0; 00197 polarConstraint = NULL; 00198 } 00199 00200 ~IK_Scene() { 00201 // delete scene first 00202 if (scene) 00203 delete scene; 00204 for(std::vector<IK_Target*>::iterator it = targets.begin(); it != targets.end(); ++it) 00205 delete (*it); 00206 targets.clear(); 00207 if (channels) 00208 delete [] channels; 00209 if (solver) 00210 delete solver; 00211 if (armature) 00212 delete armature; 00213 if (base) 00214 delete base; 00215 // delete cache last 00216 if (cache) 00217 delete cache; 00218 } 00219 }; 00220 00221 // type of IK joint, can be combined to list the joints corresponding to a bone 00222 enum IK_SegmentFlag { 00223 IK_XDOF = 1, 00224 IK_YDOF = 2, 00225 IK_ZDOF = 4, 00226 IK_SWING = 8, 00227 IK_REVOLUTE = 16, 00228 IK_TRANSY = 32, 00229 }; 00230 00231 enum IK_SegmentAxis { 00232 IK_X = 0, 00233 IK_Y = 1, 00234 IK_Z = 2, 00235 IK_TRANS_X = 3, 00236 IK_TRANS_Y = 4, 00237 IK_TRANS_Z = 5 00238 }; 00239 00240 static int initialize_chain(Object *ob, bPoseChannel *pchan_tip, bConstraint *con) 00241 { 00242 bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan; 00243 PoseTree *tree; 00244 PoseTarget *target; 00245 bKinematicConstraint *data; 00246 int a, t, segcount= 0, size, newsize, *oldparent, parent, rootbone, treecount; 00247 00248 data=(bKinematicConstraint*)con->data; 00249 00250 /* exclude tip from chain? */ 00251 if(!(data->flag & CONSTRAINT_IK_TIP)) 00252 pchan_tip= pchan_tip->parent; 00253 00254 rootbone = data->rootbone; 00255 /* Find the chain's root & count the segments needed */ 00256 for (curchan = pchan_tip; curchan; curchan=curchan->parent){ 00257 pchan_root = curchan; 00258 00259 if (++segcount > 255) // 255 is weak 00260 break; 00261 00262 if(segcount==rootbone){ 00263 // reached this end of the chain but if the chain is overlapping with a 00264 // previous one, we must go back up to the root of the other chain 00265 if ((curchan->flag & POSE_CHAIN) && curchan->iktree.first == NULL){ 00266 rootbone++; 00267 continue; 00268 } 00269 break; 00270 } 00271 00272 if (curchan->iktree.first != NULL) 00273 // Oh oh, there is already a chain starting from this channel and our chain is longer... 00274 // Should handle this by moving the previous chain up to the begining of our chain 00275 // For now we just stop here 00276 break; 00277 } 00278 if (!segcount) return 0; 00279 // we reached a limit and still not the end of a previous chain, quit 00280 if ((pchan_root->flag & POSE_CHAIN) && pchan_root->iktree.first == NULL) return 0; 00281 00282 // now that we know how many segment we have, set the flag 00283 for (rootbone = segcount, segcount = 0, curchan = pchan_tip; segcount < rootbone; segcount++, curchan=curchan->parent) { 00284 chanlist[segcount]=curchan; 00285 curchan->flag |= POSE_CHAIN; 00286 } 00287 00288 /* setup the chain data */ 00289 /* create a target */ 00290 target= (PoseTarget*)MEM_callocN(sizeof(PoseTarget), "posetarget"); 00291 target->con= con; 00292 // by contruction there can be only one tree per channel and each channel can be part of at most one tree. 00293 tree = (PoseTree*)pchan_root->iktree.first; 00294 00295 if(tree==NULL) { 00296 /* make new tree */ 00297 tree= (PoseTree*)MEM_callocN(sizeof(PoseTree), "posetree"); 00298 00299 tree->iterations= data->iterations; 00300 tree->totchannel= segcount; 00301 tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH); 00302 00303 tree->pchan= (bPoseChannel**)MEM_callocN(segcount*sizeof(void*), "ik tree pchan"); 00304 tree->parent= (int*)MEM_callocN(segcount*sizeof(int), "ik tree parent"); 00305 for(a=0; a<segcount; a++) { 00306 tree->pchan[a]= chanlist[segcount-a-1]; 00307 tree->parent[a]= a-1; 00308 } 00309 target->tip= segcount-1; 00310 00311 /* AND! link the tree to the root */ 00312 BLI_addtail(&pchan_root->iktree, tree); 00313 // new tree 00314 treecount = 1; 00315 } 00316 else { 00317 tree->iterations= MAX2(data->iterations, tree->iterations); 00318 tree->stretch= tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH); 00319 00320 /* skip common pose channels and add remaining*/ 00321 size= MIN2(segcount, tree->totchannel); 00322 a = t = 0; 00323 while (a<size && t<tree->totchannel) { 00324 // locate first matching channel 00325 for (;t<tree->totchannel && tree->pchan[t]!=chanlist[segcount-a-1];t++); 00326 if (t>=tree->totchannel) 00327 break; 00328 for(; a<size && t<tree->totchannel && tree->pchan[t]==chanlist[segcount-a-1]; a++, t++); 00329 } 00330 parent= a-1; 00331 segcount= segcount-a; 00332 target->tip= tree->totchannel + segcount - 1; 00333 00334 if (segcount > 0) { 00335 /* resize array */ 00336 newsize= tree->totchannel + segcount; 00337 oldchan= tree->pchan; 00338 oldparent= tree->parent; 00339 00340 tree->pchan= (bPoseChannel**)MEM_callocN(newsize*sizeof(void*), "ik tree pchan"); 00341 tree->parent= (int*)MEM_callocN(newsize*sizeof(int), "ik tree parent"); 00342 memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel); 00343 memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel); 00344 MEM_freeN(oldchan); 00345 MEM_freeN(oldparent); 00346 00347 /* add new pose channels at the end, in reverse order */ 00348 for(a=0; a<segcount; a++) { 00349 tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1]; 00350 tree->parent[tree->totchannel+a]= tree->totchannel+a-1; 00351 } 00352 tree->parent[tree->totchannel]= parent; 00353 00354 tree->totchannel= newsize; 00355 } 00356 // reusing tree 00357 treecount = 0; 00358 } 00359 00360 /* add target to the tree */ 00361 BLI_addtail(&tree->targets, target); 00362 /* mark root channel having an IK tree */ 00363 pchan_root->flag |= POSE_IKTREE; 00364 return treecount; 00365 } 00366 00367 static bool is_cartesian_constraint(bConstraint *con) 00368 { 00369 //bKinematicConstraint* data=(bKinematicConstraint*)con->data; 00370 00371 return true; 00372 } 00373 00374 static bool constraint_valid(bConstraint *con) 00375 { 00376 bKinematicConstraint* data=(bKinematicConstraint*)con->data; 00377 00378 if (data->flag & CONSTRAINT_IK_AUTO) 00379 return true; 00380 if (con->flag & CONSTRAINT_DISABLE) 00381 return false; 00382 if (is_cartesian_constraint(con)) { 00383 /* cartesian space constraint */ 00384 if (data->tar==NULL) 00385 return false; 00386 if (data->tar->type==OB_ARMATURE && data->subtarget[0]==0) 00387 return false; 00388 } 00389 return true; 00390 } 00391 00392 int initialize_scene(Object *ob, bPoseChannel *pchan_tip) 00393 { 00394 bConstraint *con; 00395 int treecount; 00396 00397 /* find all IK constraints and validate them */ 00398 treecount = 0; 00399 for(con= (bConstraint *)pchan_tip->constraints.first; con; con= (bConstraint *)con->next) { 00400 if(con->type==CONSTRAINT_TYPE_KINEMATIC) { 00401 if (constraint_valid(con)) 00402 treecount += initialize_chain(ob, pchan_tip, con); 00403 } 00404 } 00405 return treecount; 00406 } 00407 00408 static IK_Data* get_ikdata(bPose *pose) 00409 { 00410 if (pose->ikdata) 00411 return (IK_Data*)pose->ikdata; 00412 pose->ikdata = MEM_callocN(sizeof(IK_Data), "iTaSC ikdata"); 00413 // here init ikdata if needed 00414 // now that we have scene, make sure the default param are initialized 00415 if (!DefIKParam.iksolver) 00416 init_pose_itasc(&DefIKParam); 00417 00418 return (IK_Data*)pose->ikdata; 00419 } 00420 static double EulerAngleFromMatrix(const KDL::Rotation& R, int axis) 00421 { 00422 double t = KDL::sqrt(R(0,0)*R(0,0) + R(0,1)*R(0,1)); 00423 00424 if (t > 16.0*KDL::epsilon) { 00425 if (axis == 0) return -KDL::atan2(R(1,2), R(2,2)); 00426 else if(axis == 1) return KDL::atan2(-R(0,2), t); 00427 else return -KDL::atan2(R(0,1), R(0,0)); 00428 } else { 00429 if (axis == 0) return -KDL::atan2(-R(2,1), R(1,1)); 00430 else if(axis == 1) return KDL::atan2(-R(0,2), t); 00431 else return 0.0f; 00432 } 00433 } 00434 00435 static double ComputeTwist(const KDL::Rotation& R) 00436 { 00437 // qy and qw are the y and w components of the quaternion from R 00438 double qy = R(0,2) - R(2,0); 00439 double qw = R(0,0) + R(1,1) + R(2,2) + 1; 00440 00441 double tau = 2*KDL::atan2(qy, qw); 00442 00443 return tau; 00444 } 00445 00446 static void RemoveEulerAngleFromMatrix(KDL::Rotation& R, double angle, int axis) 00447 { 00448 // compute twist parameter 00449 KDL::Rotation T; 00450 switch (axis) { 00451 case 0: 00452 T = KDL::Rotation::RotX(-angle); 00453 break; 00454 case 1: 00455 T = KDL::Rotation::RotY(-angle); 00456 break; 00457 case 2: 00458 T = KDL::Rotation::RotZ(-angle); 00459 break; 00460 default: 00461 return; 00462 } 00463 // remove angle 00464 R = R*T; 00465 } 00466 00467 #if 0 00468 static void GetEulerXZY(const KDL::Rotation& R, double& X,double& Z,double& Y) 00469 { 00470 if (fabs(R(0,1)) > 1.0 - KDL::epsilon ) { 00471 X = -KDL::sign(R(0,1)) * KDL::atan2(R(1,2), R(1,0)); 00472 Z = -KDL::sign(R(0,1)) * KDL::PI / 2; 00473 Y = 0.0 ; 00474 } else { 00475 X = KDL::atan2(R(2,1), R(1,1)); 00476 Z = KDL::atan2(-R(0,1), KDL::sqrt( KDL::sqr(R(0,0)) + KDL::sqr(R(0,2)))); 00477 Y = KDL::atan2(R(0,2), R(0,0)); 00478 } 00479 } 00480 00481 static void GetEulerXYZ(const KDL::Rotation& R, double& X,double& Y,double& Z) 00482 { 00483 if (fabs(R(0,2)) > 1.0 - KDL::epsilon ) { 00484 X = KDL::sign(R(0,2)) * KDL::atan2(-R(1,0), R(1,1)); 00485 Y = KDL::sign(R(0,2)) * KDL::PI / 2; 00486 Z = 0.0 ; 00487 } else { 00488 X = KDL::atan2(-R(1,2), R(2,2)); 00489 Y = KDL::atan2(R(0,2), KDL::sqrt( KDL::sqr(R(0,0)) + KDL::sqr(R(0,1)))); 00490 Z = KDL::atan2(-R(0,1), R(0,0)); 00491 } 00492 } 00493 #endif 00494 00495 static void GetJointRotation(KDL::Rotation& boneRot, int type, double* rot) 00496 { 00497 switch (type & ~IK_TRANSY) 00498 { 00499 default: 00500 // fixed bone, no joint 00501 break; 00502 case IK_XDOF: 00503 // RX only, get the X rotation 00504 rot[0] = EulerAngleFromMatrix(boneRot, 0); 00505 break; 00506 case IK_YDOF: 00507 // RY only, get the Y rotation 00508 rot[0] = ComputeTwist(boneRot); 00509 break; 00510 case IK_ZDOF: 00511 // RZ only, get the Z rotation 00512 rot[0] = EulerAngleFromMatrix(boneRot, 2); 00513 break; 00514 case IK_XDOF|IK_YDOF: 00515 rot[1] = ComputeTwist(boneRot); 00516 RemoveEulerAngleFromMatrix(boneRot, rot[1], 1); 00517 rot[0] = EulerAngleFromMatrix(boneRot, 0); 00518 break; 00519 case IK_SWING: 00520 // RX+RZ 00521 boneRot.GetXZRot().GetValue(rot); 00522 break; 00523 case IK_YDOF|IK_ZDOF: 00524 // RZ+RY 00525 rot[1] = ComputeTwist(boneRot); 00526 RemoveEulerAngleFromMatrix(boneRot, rot[1], 1); 00527 rot[0] = EulerAngleFromMatrix(boneRot, 2); 00528 break; 00529 case IK_SWING|IK_YDOF: 00530 rot[2] = ComputeTwist(boneRot); 00531 RemoveEulerAngleFromMatrix(boneRot, rot[2], 1); 00532 boneRot.GetXZRot().GetValue(rot); 00533 break; 00534 case IK_REVOLUTE: 00535 boneRot.GetRot().GetValue(rot); 00536 break; 00537 } 00538 } 00539 00540 static bool target_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame& current, iTaSC::Frame& next, void *param) 00541 { 00542 IK_Target* target = (IK_Target*)param; 00543 // compute next target position 00544 // get target matrix from constraint. 00545 bConstraint* constraint = (bConstraint*)target->blenderConstraint; 00546 float tarmat[4][4]; 00547 00548 get_constraint_target_matrix(target->blscene, constraint, 0, CONSTRAINT_OBTYPE_OBJECT, target->owner, tarmat, 1.0); 00549 00550 // rootmat contains the target pose in world coordinate 00551 // if enforce is != 1.0, blend the target position with the end effector position 00552 // if the armature was in rest position. This information is available in eeRest 00553 if (constraint->enforce != 1.0f && target->eeBlend) { 00554 // eeRest is relative to the reference frame of the IK root 00555 // get this frame in world reference 00556 float restmat[4][4]; 00557 bPoseChannel* pchan = target->rootChannel; 00558 if (pchan->parent) { 00559 pchan = pchan->parent; 00560 float chanmat[4][4]; 00561 copy_m4_m4(chanmat, pchan->pose_mat); 00562 VECCOPY(chanmat[3], pchan->pose_tail); 00563 mul_serie_m4(restmat, target->owner->obmat, chanmat, target->eeRest, NULL, NULL, NULL, NULL, NULL); 00564 } 00565 else { 00566 mul_m4_m4m4(restmat, target->eeRest, target->owner->obmat); 00567 } 00568 // blend the target 00569 blend_m4_m4m4(tarmat, restmat, tarmat, constraint->enforce); 00570 } 00571 next.setValue(&tarmat[0][0]); 00572 return true; 00573 } 00574 00575 static bool base_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame& current, iTaSC::Frame& next, void *param) 00576 { 00577 IK_Scene* ikscene = (IK_Scene*)param; 00578 // compute next armature base pose 00579 // algorithm: 00580 // ikscene->pchan[0] is the root channel of the tree 00581 // if it has a parent, get the pose matrix from it and replace [3] by parent pchan->tail 00582 // then multiply by the armature matrix to get ikscene->armature base position 00583 bPoseChannel* pchan = ikscene->channels[0].pchan; 00584 float rootmat[4][4]; 00585 if (pchan->parent) { 00586 pchan = pchan->parent; 00587 float chanmat[4][4]; 00588 copy_m4_m4(chanmat, pchan->pose_mat); 00589 VECCOPY(chanmat[3], pchan->pose_tail); 00590 // save the base as a frame too so that we can compute deformation 00591 // after simulation 00592 ikscene->baseFrame.setValue(&chanmat[0][0]); 00593 mul_m4_m4m4(rootmat, chanmat, ikscene->blArmature->obmat); 00594 } 00595 else { 00596 copy_m4_m4(rootmat, ikscene->blArmature->obmat); 00597 ikscene->baseFrame = iTaSC::F_identity; 00598 } 00599 next.setValue(&rootmat[0][0]); 00600 // if there is a polar target (only during solving otherwise we don't have end efffector) 00601 if (ikscene->polarConstraint && timestamp.update) { 00602 // compute additional rotation of base frame so that armature follows the polar target 00603 float imat[4][4]; // IK tree base inverse matrix 00604 float polemat[4][4]; // polar target in IK tree base frame 00605 float goalmat[4][4]; // target in IK tree base frame 00606 float mat[4][4]; // temp matrix 00607 bKinematicConstraint* poledata = (bKinematicConstraint*)ikscene->polarConstraint->data; 00608 00609 invert_m4_m4(imat, rootmat); 00610 // polar constraint imply only one target 00611 IK_Target *iktarget = ikscene->targets[0]; 00612 // root channel from which we take the bone initial orientation 00613 IK_Channel &rootchan = ikscene->channels[0]; 00614 00615 // get polar target matrix in world space 00616 get_constraint_target_matrix(ikscene->blscene, ikscene->polarConstraint, 1, CONSTRAINT_OBTYPE_OBJECT, ikscene->blArmature, mat, 1.0); 00617 // convert to armature space 00618 mul_m4_m4m4(polemat, mat, imat); 00619 // get the target in world space (was computed before as target object are defined before base object) 00620 iktarget->target->getPose().getValue(mat[0]); 00621 // convert to armature space 00622 mul_m4_m4m4(goalmat, mat, imat); 00623 // take position of target, polar target, end effector, in armature space 00624 KDL::Vector goalpos(goalmat[3]); 00625 KDL::Vector polepos(polemat[3]); 00626 KDL::Vector endpos = ikscene->armature->getPose(iktarget->ee).p; 00627 // get root bone orientation 00628 KDL::Frame rootframe; 00629 ikscene->armature->getRelativeFrame(rootframe, rootchan.tail); 00630 KDL::Vector rootx = rootframe.M.UnitX(); 00631 KDL::Vector rootz = rootframe.M.UnitZ(); 00632 // and compute root bone head 00633 double q_rest[3], q[3], length; 00634 const KDL::Joint* joint; 00635 const KDL::Frame* tip; 00636 ikscene->armature->getSegment(rootchan.tail, 3, joint, q_rest[0], q[0], tip); 00637 length = (joint->getType() == KDL::Joint::TransY) ? q[0] : tip->p(1); 00638 KDL::Vector rootpos = rootframe.p - length*rootframe.M.UnitY(); 00639 00640 // compute main directions 00641 KDL::Vector dir = KDL::Normalize(endpos - rootpos); 00642 KDL::Vector poledir = KDL::Normalize(goalpos-rootpos); 00643 // compute up directions 00644 KDL::Vector poleup = KDL::Normalize(polepos-rootpos); 00645 KDL::Vector up = rootx*KDL::cos(poledata->poleangle) + rootz*KDL::sin(poledata->poleangle); 00646 // from which we build rotation matrix 00647 KDL::Rotation endrot, polerot; 00648 // for the armature, using the root bone orientation 00649 KDL::Vector x = KDL::Normalize(dir*up); 00650 endrot.UnitX(x); 00651 endrot.UnitY(KDL::Normalize(x*dir)); 00652 endrot.UnitZ(-dir); 00653 // for the polar target 00654 x = KDL::Normalize(poledir*poleup); 00655 polerot.UnitX(x); 00656 polerot.UnitY(KDL::Normalize(x*poledir)); 00657 polerot.UnitZ(-poledir); 00658 // the difference between the two is the rotation we want to apply 00659 KDL::Rotation result(polerot*endrot.Inverse()); 00660 // apply on base frame as this is an artificial additional rotation 00661 next.M = next.M*result; 00662 ikscene->baseFrame.M = ikscene->baseFrame.M*result; 00663 } 00664 return true; 00665 } 00666 00667 static bool copypose_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues* const _values, unsigned int _nvalues, void* _param) 00668 { 00669 IK_Target* iktarget =(IK_Target*)_param; 00670 bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data; 00671 iTaSC::ConstraintValues* values = _values; 00672 bItasc* ikparam = (bItasc*) iktarget->owner->pose->ikparam; 00673 00674 // we need default parameters 00675 if (!ikparam) 00676 ikparam = &DefIKParam; 00677 00678 if (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) { 00679 if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) { 00680 values->alpha = 0.0; 00681 values->action = iTaSC::ACT_ALPHA; 00682 values++; 00683 } 00684 if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) { 00685 values->alpha = 0.0; 00686 values->action = iTaSC::ACT_ALPHA; 00687 values++; 00688 } 00689 } else { 00690 if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) { 00691 // update error 00692 values->alpha = condata->weight; 00693 values->action = iTaSC::ACT_ALPHA|iTaSC::ACT_FEEDBACK; 00694 values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK; 00695 values++; 00696 } 00697 if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) { 00698 // update error 00699 values->alpha = condata->orientweight; 00700 values->action = iTaSC::ACT_ALPHA|iTaSC::ACT_FEEDBACK; 00701 values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK; 00702 values++; 00703 } 00704 } 00705 return true; 00706 } 00707 00708 static void copypose_error(const iTaSC::ConstraintValues* values, unsigned int nvalues, IK_Target* iktarget) 00709 { 00710 iTaSC::ConstraintSingleValue* value; 00711 double error; 00712 int i; 00713 00714 if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) { 00715 // update error 00716 for (i=0, error=0.0, value=values->values; i<values->number; ++i, ++value) 00717 error += KDL::sqr(value->y - value->yd); 00718 iktarget->blenderConstraint->lin_error = (float)KDL::sqrt(error); 00719 values++; 00720 } 00721 if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) { 00722 // update error 00723 for (i=0, error=0.0, value=values->values; i<values->number; ++i, ++value) 00724 error += KDL::sqr(value->y - value->yd); 00725 iktarget->blenderConstraint->rot_error = (float)KDL::sqrt(error); 00726 values++; 00727 } 00728 } 00729 00730 static bool distance_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues* const _values, unsigned int _nvalues, void* _param) 00731 { 00732 IK_Target* iktarget =(IK_Target*)_param; 00733 bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data; 00734 iTaSC::ConstraintValues* values = _values; 00735 bItasc* ikparam = (bItasc*) iktarget->owner->pose->ikparam; 00736 // we need default parameters 00737 if (!ikparam) 00738 ikparam = &DefIKParam; 00739 00740 // update weight according to mode 00741 if (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) { 00742 values->alpha = 0.0; 00743 } else { 00744 switch (condata->mode) { 00745 case LIMITDIST_INSIDE: 00746 values->alpha = (values->values[0].y > condata->dist) ? condata->weight : 0.0; 00747 break; 00748 case LIMITDIST_OUTSIDE: 00749 values->alpha = (values->values[0].y < condata->dist) ? condata->weight : 0.0; 00750 break; 00751 default: 00752 values->alpha = condata->weight; 00753 break; 00754 } 00755 if (!timestamp.substep) { 00756 // only update value on first timestep 00757 switch (condata->mode) { 00758 case LIMITDIST_INSIDE: 00759 values->values[0].yd = condata->dist*0.95; 00760 break; 00761 case LIMITDIST_OUTSIDE: 00762 values->values[0].yd = condata->dist*1.05; 00763 break; 00764 default: 00765 values->values[0].yd = condata->dist; 00766 break; 00767 } 00768 values->values[0].action = iTaSC::ACT_VALUE|iTaSC::ACT_FEEDBACK; 00769 values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK; 00770 } 00771 } 00772 values->action |= iTaSC::ACT_ALPHA; 00773 return true; 00774 } 00775 00776 static void distance_error(const iTaSC::ConstraintValues* values, unsigned int _nvalues, IK_Target* iktarget) 00777 { 00778 iktarget->blenderConstraint->lin_error = (float)(values->values[0].y - values->values[0].yd); 00779 } 00780 00781 static bool joint_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues* const _values, unsigned int _nvalues, void* _param) 00782 { 00783 IK_Channel* ikchan = (IK_Channel*)_param; 00784 bItasc* ikparam = (bItasc*)ikchan->owner->pose->ikparam; 00785 bPoseChannel* chan = ikchan->pchan; 00786 int dof; 00787 00788 // a channel can be splitted into multiple joints, so we get called multiple 00789 // times for one channel (this callback is only for 1 joint in the armature) 00790 // the IK_JointTarget structure is shared between multiple joint constraint 00791 // and the target joint values is computed only once, remember this in jointValid 00792 // Don't forget to reset it before each frame 00793 if (!ikchan->jointValid) { 00794 float rmat[3][3]; 00795 00796 if (chan->rotmode > 0) { 00797 /* euler rotations (will cause gimble lock, but this can be alleviated a bit with rotation orders) */ 00798 eulO_to_mat3( rmat,chan->eul, chan->rotmode); 00799 } 00800 else if (chan->rotmode == ROT_MODE_AXISANGLE) { 00801 /* axis-angle - stored in quaternion data, but not really that great for 3D-changing orientations */ 00802 axis_angle_to_mat3( rmat,&chan->quat[1], chan->quat[0]); 00803 } 00804 else { 00805 /* quats are normalised before use to eliminate scaling issues */ 00806 normalize_qt(chan->quat); 00807 quat_to_mat3( rmat,chan->quat); 00808 } 00809 KDL::Rotation jointRot( 00810 rmat[0][0], rmat[1][0], rmat[2][0], 00811 rmat[0][1], rmat[1][1], rmat[2][1], 00812 rmat[0][2], rmat[1][2], rmat[2][2]); 00813 GetJointRotation(jointRot, ikchan->jointType, ikchan->jointValue); 00814 ikchan->jointValid = 1; 00815 } 00816 // determine which part of jointValue is used for this joint 00817 // closely related to the way the joints are defined 00818 switch (ikchan->jointType & ~IK_TRANSY) 00819 { 00820 case IK_XDOF: 00821 case IK_YDOF: 00822 case IK_ZDOF: 00823 dof = 0; 00824 break; 00825 case IK_XDOF|IK_YDOF: 00826 // X + Y 00827 dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RX) ? 0 : 1; 00828 break; 00829 case IK_SWING: 00830 // XZ 00831 dof = 0; 00832 break; 00833 case IK_YDOF|IK_ZDOF: 00834 // Z + Y 00835 dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RZ) ? 0 : 1; 00836 break; 00837 case IK_SWING|IK_YDOF: 00838 // XZ + Y 00839 dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RY) ? 2 : 0; 00840 break; 00841 case IK_REVOLUTE: 00842 dof = 0; 00843 break; 00844 default: 00845 dof = -1; 00846 break; 00847 } 00848 if (dof >= 0) { 00849 for (unsigned int i=0; i<_nvalues; i++, dof++) { 00850 _values[i].values[0].yd = ikchan->jointValue[dof]; 00851 _values[i].alpha = chan->ikrotweight; 00852 _values[i].feedback = ikparam->feedback; 00853 } 00854 } 00855 return true; 00856 } 00857 00858 // build array of joint corresponding to IK chain 00859 static int convert_channels(IK_Scene *ikscene, PoseTree *tree) 00860 { 00861 IK_Channel *ikchan; 00862 bPoseChannel *pchan; 00863 int a, flag, njoint; 00864 00865 njoint = 0; 00866 for(a=0, ikchan = ikscene->channels; a<ikscene->numchan; ++a, ++ikchan) { 00867 pchan= tree->pchan[a]; 00868 ikchan->pchan = pchan; 00869 ikchan->parent = (a>0) ? tree->parent[a] : -1; 00870 ikchan->owner = ikscene->blArmature; 00871 00872 /* set DoF flag */ 00873 flag = 0; 00874 if(!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP) && 00875 (!(pchan->ikflag & BONE_IK_XLIMIT) || pchan->limitmin[0]<0.f || pchan->limitmax[0]>0.f)) 00876 flag |= IK_XDOF; 00877 if(!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP) && 00878 (!(pchan->ikflag & BONE_IK_YLIMIT) || pchan->limitmin[1]<0.f || pchan->limitmax[1]>0.f)) 00879 flag |= IK_YDOF; 00880 if(!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP) && 00881 (!(pchan->ikflag & BONE_IK_ZLIMIT) || pchan->limitmin[2]<0.f || pchan->limitmax[2]>0.f)) 00882 flag |= IK_ZDOF; 00883 00884 if(tree->stretch && (pchan->ikstretch > 0.0)) { 00885 flag |= IK_TRANSY; 00886 } 00887 /* 00888 Logic to create the segments: 00889 RX,RY,RZ = rotational joints with no length 00890 RY(tip) = rotational joints with a fixed length arm = (0,length,0) 00891 TY = translational joint on Y axis 00892 F(pos) = fixed joint with an arm at position pos 00893 Conversion rule of the above flags: 00894 - ==> F(tip) 00895 X ==> RX(tip) 00896 Y ==> RY(tip) 00897 Z ==> RZ(tip) 00898 XY ==> RX+RY(tip) 00899 XZ ==> RX+RZ(tip) 00900 YZ ==> RZ+RY(tip) 00901 XYZ ==> full spherical unless there are limits, in which case RX+RZ+RY(tip) 00902 In case of stretch, tip=(0,0,0) and there is an additional TY joint 00903 The frame at last of these joints represents the tail of the bone. 00904 The head is computed by a reverse translation on Y axis of the bone length 00905 or in case of TY joint, by the frame at previous joint. 00906 In case of separation of bones, there is an additional F(head) joint 00907 00908 Computing rest pose and length is complicated: the solver works in world space 00909 Here is the logic: 00910 rest position is computed only from bone->bone_mat. 00911 bone length is computed from bone->length multiplied by the scaling factor of 00912 the armature. Non-uniform scaling will give bad result! 00913 00914 */ 00915 switch (flag & (IK_XDOF|IK_YDOF|IK_ZDOF)) 00916 { 00917 default: 00918 ikchan->jointType = 0; 00919 ikchan->ndof = 0; 00920 break; 00921 case IK_XDOF: 00922 // RX only, get the X rotation 00923 ikchan->jointType = IK_XDOF; 00924 ikchan->ndof = 1; 00925 break; 00926 case IK_YDOF: 00927 // RY only, get the Y rotation 00928 ikchan->jointType = IK_YDOF; 00929 ikchan->ndof = 1; 00930 break; 00931 case IK_ZDOF: 00932 // RZ only, get the Zz rotation 00933 ikchan->jointType = IK_ZDOF; 00934 ikchan->ndof = 1; 00935 break; 00936 case IK_XDOF|IK_YDOF: 00937 ikchan->jointType = IK_XDOF|IK_YDOF; 00938 ikchan->ndof = 2; 00939 break; 00940 case IK_XDOF|IK_ZDOF: 00941 // RX+RZ 00942 ikchan->jointType = IK_SWING; 00943 ikchan->ndof = 2; 00944 break; 00945 case IK_YDOF|IK_ZDOF: 00946 // RZ+RY 00947 ikchan->jointType = IK_ZDOF|IK_YDOF; 00948 ikchan->ndof = 2; 00949 break; 00950 case IK_XDOF|IK_YDOF|IK_ZDOF: 00951 // spherical joint 00952 if (pchan->ikflag & (BONE_IK_XLIMIT|BONE_IK_YLIMIT|BONE_IK_ZLIMIT)) 00953 // decompose in a Swing+RotY joint 00954 ikchan->jointType = IK_SWING|IK_YDOF; 00955 else 00956 ikchan->jointType = IK_REVOLUTE; 00957 ikchan->ndof = 3; 00958 break; 00959 } 00960 if (flag & IK_TRANSY) { 00961 ikchan->jointType |= IK_TRANSY; 00962 ikchan->ndof++; 00963 } 00964 njoint += ikchan->ndof; 00965 } 00966 // njoint is the joint coordinate, create the Joint Array 00967 ikscene->jointArray.resize(njoint); 00968 ikscene->numjoint = njoint; 00969 return njoint; 00970 } 00971 00972 // compute array of joint value corresponding to current pose 00973 static void convert_pose(IK_Scene *ikscene) 00974 { 00975 KDL::Rotation boneRot; 00976 bPoseChannel *pchan; 00977 IK_Channel *ikchan; 00978 Bone *bone; 00979 float rmat[4][4]; // rest pose of bone with parent taken into account 00980 float bmat[4][4]; // difference 00981 float scale; 00982 double *rot; 00983 int a, joint; 00984 00985 // assume uniform scaling and take Y scale as general scale for the armature 00986 scale = len_v3(ikscene->blArmature->obmat[1]); 00987 rot = &ikscene->jointArray(0); 00988 for(joint=a=0, ikchan = ikscene->channels; a<ikscene->numchan && joint<ikscene->numjoint; ++a, ++ikchan) { 00989 pchan= ikchan->pchan; 00990 bone= pchan->bone; 00991 00992 if (pchan->parent) { 00993 unit_m4(bmat); 00994 mul_m4_m4m3(bmat, pchan->parent->pose_mat, bone->bone_mat); 00995 } else { 00996 copy_m4_m4(bmat, bone->arm_mat); 00997 } 00998 invert_m4_m4(rmat, bmat); 00999 mul_m4_m4m4(bmat, pchan->pose_mat, rmat); 01000 normalize_m4(bmat); 01001 boneRot.setValue(bmat[0]); 01002 GetJointRotation(boneRot, ikchan->jointType, rot); 01003 if (ikchan->jointType & IK_TRANSY) { 01004 // compute actual length 01005 rot[ikchan->ndof-1] = len_v3v3(pchan->pose_tail, pchan->pose_head) * scale; 01006 } 01007 rot += ikchan->ndof; 01008 joint += ikchan->ndof; 01009 } 01010 } 01011 01012 // compute array of joint value corresponding to current pose 01013 static void rest_pose(IK_Scene *ikscene) 01014 { 01015 bPoseChannel *pchan; 01016 IK_Channel *ikchan; 01017 Bone *bone; 01018 float scale; 01019 double *rot; 01020 int a, joint; 01021 01022 // assume uniform scaling and take Y scale as general scale for the armature 01023 scale = len_v3(ikscene->blArmature->obmat[1]); 01024 // rest pose is 0 01025 KDL::SetToZero(ikscene->jointArray); 01026 // except for transY joints 01027 rot = &ikscene->jointArray(0); 01028 for(joint=a=0, ikchan = ikscene->channels; a<ikscene->numchan && joint<ikscene->numjoint; ++a, ++ikchan) { 01029 pchan= ikchan->pchan; 01030 bone= pchan->bone; 01031 01032 if (ikchan->jointType & IK_TRANSY) 01033 rot[ikchan->ndof-1] = bone->length*scale; 01034 rot += ikchan->ndof; 01035 joint += ikchan->ndof; 01036 } 01037 } 01038 01039 static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) 01040 { 01041 PoseTree *tree = (PoseTree*)pchan->iktree.first; 01042 PoseTarget *target; 01043 bKinematicConstraint *condata; 01044 bConstraint *polarcon; 01045 bItasc *ikparam; 01046 iTaSC::Armature* arm; 01047 iTaSC::Scene* scene; 01048 IK_Scene* ikscene; 01049 IK_Channel* ikchan; 01050 KDL::Frame initPose; 01051 KDL::Rotation boneRot; 01052 Bone *bone; 01053 int a, numtarget; 01054 unsigned int t; 01055 float length; 01056 bool ret = true, ingame; 01057 double *rot; 01058 01059 if (tree->totchannel == 0) 01060 return NULL; 01061 01062 ikscene = new IK_Scene; 01063 ikscene->blscene = blscene; 01064 arm = new iTaSC::Armature(); 01065 scene = new iTaSC::Scene(); 01066 ikscene->channels = new IK_Channel[tree->totchannel]; 01067 ikscene->numchan = tree->totchannel; 01068 ikscene->armature = arm; 01069 ikscene->scene = scene; 01070 ikparam = (bItasc*)ob->pose->ikparam; 01071 ingame = (ob->pose->flag & POSE_GAME_ENGINE); 01072 if (!ikparam) { 01073 // you must have our own copy 01074 ikparam = &DefIKParam; 01075 } else if (ingame) { 01076 // tweak the param when in game to have efficient stepping 01077 // using fixed substep is not effecient since frames in the GE are often 01078 // shorter than in animation => move to auto step automatically and set 01079 // the target substep duration via min/max 01080 if (!(ikparam->flag & ITASC_AUTO_STEP)) { 01081 float timestep = blscene->r.frs_sec_base/blscene->r.frs_sec; 01082 if (ikparam->numstep > 0) 01083 timestep /= ikparam->numstep; 01084 // with equal min and max, the algorythm will take this step and the indicative substep most of the time 01085 ikparam->minstep = ikparam->maxstep = timestep; 01086 ikparam->flag |= ITASC_AUTO_STEP; 01087 } 01088 } 01089 if ((ikparam->flag & ITASC_SIMULATION) && !ingame) 01090 // no cache in animation mode 01091 ikscene->cache = new iTaSC::Cache(); 01092 01093 switch (ikparam->solver) { 01094 case ITASC_SOLVER_SDLS: 01095 ikscene->solver = new iTaSC::WSDLSSolver(); 01096 break; 01097 case ITASC_SOLVER_DLS: 01098 ikscene->solver = new iTaSC::WDLSSolver(); 01099 break; 01100 default: 01101 delete ikscene; 01102 return NULL; 01103 } 01104 ikscene->blArmature = ob; 01105 01106 std::string joint; 01107 std::string root("root"); 01108 std::string parent; 01109 std::vector<double> weights; 01110 double weight[3]; 01111 // assume uniform scaling and take Y scale as general scale for the armature 01112 float scale = len_v3(ob->obmat[1]); 01113 // build the array of joints corresponding to the IK chain 01114 convert_channels(ikscene, tree); 01115 if (ingame) { 01116 // in the GE, set the initial joint angle to match the current pose 01117 // this will update the jointArray in ikscene 01118 convert_pose(ikscene); 01119 } else { 01120 // in Blender, the rest pose is always 0 for joints 01121 rest_pose(ikscene); 01122 } 01123 rot = &ikscene->jointArray(0); 01124 for(a=0, ikchan = ikscene->channels; a<tree->totchannel; ++a, ++ikchan) { 01125 pchan= ikchan->pchan; 01126 bone= pchan->bone; 01127 01128 KDL::Frame tip(iTaSC::F_identity); 01129 Vector3 *fl = bone->bone_mat; 01130 KDL::Rotation brot( 01131 fl[0][0], fl[1][0], fl[2][0], 01132 fl[0][1], fl[1][1], fl[2][1], 01133 fl[0][2], fl[1][2], fl[2][2]); 01134 KDL::Vector bpos(bone->head[0], bone->head[1], bone->head[2]); 01135 bpos = bpos*scale; 01136 KDL::Frame head(brot, bpos); 01137 01138 // rest pose length of the bone taking scaling into account 01139 length= bone->length*scale; 01140 parent = (a > 0) ? ikscene->channels[tree->parent[a]].tail : root; 01141 // first the fixed segment to the bone head 01142 if (head.p.Norm() > KDL::epsilon || head.M.GetRot().Norm() > KDL::epsilon) { 01143 joint = bone->name; 01144 joint += ":H"; 01145 ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, head); 01146 parent = joint; 01147 } 01148 if (!(ikchan->jointType & IK_TRANSY)) { 01149 // fixed length, put it in tip 01150 tip.p[1] = length; 01151 } 01152 joint = bone->name; 01153 weight[0] = (1.0-pchan->stiffness[0]); 01154 weight[1] = (1.0-pchan->stiffness[1]); 01155 weight[2] = (1.0-pchan->stiffness[2]); 01156 switch (ikchan->jointType & ~IK_TRANSY) 01157 { 01158 case 0: 01159 // fixed bone 01160 if (!(ikchan->jointType & IK_TRANSY)) { 01161 joint += ":F"; 01162 ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, tip); 01163 } 01164 break; 01165 case IK_XDOF: 01166 // RX only, get the X rotation 01167 joint += ":RX"; 01168 ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0], tip); 01169 weights.push_back(weight[0]); 01170 break; 01171 case IK_YDOF: 01172 // RY only, get the Y rotation 01173 joint += ":RY"; 01174 ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[0], tip); 01175 weights.push_back(weight[1]); 01176 break; 01177 case IK_ZDOF: 01178 // RZ only, get the Zz rotation 01179 joint += ":RZ"; 01180 ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0], tip); 01181 weights.push_back(weight[2]); 01182 break; 01183 case IK_XDOF|IK_YDOF: 01184 joint += ":RX"; 01185 ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0]); 01186 weights.push_back(weight[0]); 01187 if (ret) { 01188 parent = joint; 01189 joint = bone->name; 01190 joint += ":RY"; 01191 ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip); 01192 weights.push_back(weight[1]); 01193 } 01194 break; 01195 case IK_SWING: 01196 joint += ":SW"; 01197 ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0], tip); 01198 weights.push_back(weight[0]); 01199 weights.push_back(weight[2]); 01200 break; 01201 case IK_YDOF|IK_ZDOF: 01202 // RZ+RY 01203 joint += ":RZ"; 01204 ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0]); 01205 weights.push_back(weight[2]); 01206 if (ret) { 01207 parent = joint; 01208 joint = bone->name; 01209 joint += ":RY"; 01210 ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip); 01211 weights.push_back(weight[1]); 01212 } 01213 break; 01214 case IK_SWING|IK_YDOF: 01215 // decompose in a Swing+RotY joint 01216 joint += ":SW"; 01217 ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0]); 01218 weights.push_back(weight[0]); 01219 weights.push_back(weight[2]); 01220 if (ret) { 01221 parent = joint; 01222 joint = bone->name; 01223 joint += ":RY"; 01224 ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[2], tip); 01225 weights.push_back(weight[1]); 01226 } 01227 break; 01228 case IK_REVOLUTE: 01229 joint += ":SJ"; 01230 ret = arm->addSegment(joint, parent, KDL::Joint::Sphere, rot[0], tip); 01231 weights.push_back(weight[0]); 01232 weights.push_back(weight[1]); 01233 weights.push_back(weight[2]); 01234 break; 01235 } 01236 if (ret && (ikchan->jointType & IK_TRANSY)) { 01237 parent = joint; 01238 joint = bone->name; 01239 joint += ":TY"; 01240 ret = arm->addSegment(joint, parent, KDL::Joint::TransY, rot[ikchan->ndof-1]); 01241 float ikstretch = pchan->ikstretch*pchan->ikstretch; 01242 weight[1] = (1.0-MIN2(1.0-ikstretch, 0.99)); 01243 weights.push_back(weight[1]); 01244 } 01245 if (!ret) 01246 // error making the armature?? 01247 break; 01248 // joint points to the segment that correspond to the bone per say 01249 ikchan->tail = joint; 01250 ikchan->head = parent; 01251 // in case of error 01252 ret = false; 01253 if ((ikchan->jointType & IK_XDOF) && (pchan->ikflag & (BONE_IK_XLIMIT|BONE_IK_ROTCTL))) { 01254 joint = bone->name; 01255 joint += ":RX"; 01256 if (pchan->ikflag & BONE_IK_XLIMIT) { 01257 if (arm->addLimitConstraint(joint, 0, pchan->limitmin[0], pchan->limitmax[0]) < 0) 01258 break; 01259 } 01260 if (pchan->ikflag & BONE_IK_ROTCTL) { 01261 if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) 01262 break; 01263 } 01264 } 01265 if ((ikchan->jointType & IK_YDOF) && (pchan->ikflag & (BONE_IK_YLIMIT|BONE_IK_ROTCTL))) { 01266 joint = bone->name; 01267 joint += ":RY"; 01268 if (pchan->ikflag & BONE_IK_YLIMIT) { 01269 if (arm->addLimitConstraint(joint, 0, pchan->limitmin[1], pchan->limitmax[1]) < 0) 01270 break; 01271 } 01272 if (pchan->ikflag & BONE_IK_ROTCTL) { 01273 if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) 01274 break; 01275 } 01276 } 01277 if ((ikchan->jointType & IK_ZDOF) && (pchan->ikflag & (BONE_IK_ZLIMIT|BONE_IK_ROTCTL))) { 01278 joint = bone->name; 01279 joint += ":RZ"; 01280 if (pchan->ikflag & BONE_IK_ZLIMIT) { 01281 if (arm->addLimitConstraint(joint, 0, pchan->limitmin[2], pchan->limitmax[2]) < 0) 01282 break; 01283 } 01284 if (pchan->ikflag & BONE_IK_ROTCTL) { 01285 if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) 01286 break; 01287 } 01288 } 01289 if ((ikchan->jointType & IK_SWING) && (pchan->ikflag & (BONE_IK_XLIMIT|BONE_IK_ZLIMIT|BONE_IK_ROTCTL))) { 01290 joint = bone->name; 01291 joint += ":SW"; 01292 if (pchan->ikflag & BONE_IK_XLIMIT) { 01293 if (arm->addLimitConstraint(joint, 0, pchan->limitmin[0], pchan->limitmax[0]) < 0) 01294 break; 01295 } 01296 if (pchan->ikflag & BONE_IK_ZLIMIT) { 01297 if (arm->addLimitConstraint(joint, 1, pchan->limitmin[2], pchan->limitmax[2]) < 0) 01298 break; 01299 } 01300 if (pchan->ikflag & BONE_IK_ROTCTL) { 01301 if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) 01302 break; 01303 } 01304 } 01305 if ((ikchan->jointType & IK_REVOLUTE) && (pchan->ikflag & BONE_IK_ROTCTL)) { 01306 joint = bone->name; 01307 joint += ":SJ"; 01308 if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) 01309 break; 01310 } 01311 // no error, so restore 01312 ret = true; 01313 rot += ikchan->ndof; 01314 } 01315 if (!ret) { 01316 delete ikscene; 01317 return NULL; 01318 } 01319 // for each target, we need to add an end effector in the armature 01320 for (numtarget=0, polarcon=NULL, ret = true, target=(PoseTarget*)tree->targets.first; target; target=(PoseTarget*)target->next) { 01321 condata= (bKinematicConstraint*)target->con->data; 01322 pchan = tree->pchan[target->tip]; 01323 01324 if (is_cartesian_constraint(target->con)) { 01325 // add the end effector 01326 IK_Target* iktarget = new IK_Target(); 01327 ikscene->targets.push_back(iktarget); 01328 iktarget->ee = arm->addEndEffector(ikscene->channels[target->tip].tail); 01329 if (iktarget->ee == -1) { 01330 ret = false; 01331 break; 01332 } 01333 // initialize all the fields that we can set at this time 01334 iktarget->blenderConstraint = target->con; 01335 iktarget->channel = target->tip; 01336 iktarget->simulation = (ikparam->flag & ITASC_SIMULATION); 01337 iktarget->rootChannel = ikscene->channels[0].pchan; 01338 iktarget->owner = ob; 01339 iktarget->targetName = pchan->bone->name; 01340 iktarget->targetName += ":T:"; 01341 iktarget->targetName += target->con->name; 01342 iktarget->constraintName = pchan->bone->name; 01343 iktarget->constraintName += ":C:"; 01344 iktarget->constraintName += target->con->name; 01345 numtarget++; 01346 if (condata->poletar) 01347 // this constraint has a polar target 01348 polarcon = target->con; 01349 } 01350 } 01351 // deal with polar target if any 01352 if (numtarget == 1 && polarcon) { 01353 ikscene->polarConstraint = polarcon; 01354 } 01355 // we can now add the armature 01356 // the armature is based on a moving frame. 01357 // initialize with the correct position in case there is no cache 01358 base_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, ikscene); 01359 ikscene->base = new iTaSC::MovingFrame(initPose); 01360 ikscene->base->setCallback(base_callback, ikscene); 01361 std::string armname; 01362 armname = ob->id.name; 01363 armname += ":B"; 01364 ret = scene->addObject(armname, ikscene->base); 01365 armname = ob->id.name; 01366 armname += ":AR"; 01367 if (ret) 01368 ret = scene->addObject(armname, ikscene->armature, ikscene->base); 01369 if (!ret) { 01370 delete ikscene; 01371 return NULL; 01372 } 01373 // set the weight 01374 e_matrix& Wq = arm->getWq(); 01375 assert(Wq.cols() == (int)weights.size()); 01376 for (int q=0; q<Wq.cols(); q++) 01377 Wq(q,q)=weights[q]; 01378 // get the inverse rest pose frame of the base to compute relative rest pose of end effectors 01379 // this is needed to handle the enforce parameter 01380 // ikscene->pchan[0] is the root channel of the tree 01381 // if it has no parent, then it's just the identify Frame 01382 float invBaseFrame[4][4]; 01383 pchan = ikscene->channels[0].pchan; 01384 if (pchan->parent) { 01385 // it has a parent, get the pose matrix from it 01386 float baseFrame[4][4]; 01387 pchan = pchan->parent; 01388 copy_m4_m4(baseFrame, pchan->bone->arm_mat); 01389 // move to the tail and scale to get rest pose of armature base 01390 copy_v3_v3(baseFrame[3], pchan->bone->arm_tail); 01391 invert_m4_m4(invBaseFrame, baseFrame); 01392 } else { 01393 unit_m4(invBaseFrame); 01394 } 01395 // finally add the constraint 01396 for (t=0; t<ikscene->targets.size(); t++) { 01397 IK_Target* iktarget = ikscene->targets[t]; 01398 iktarget->blscene = blscene; 01399 condata= (bKinematicConstraint*)iktarget->blenderConstraint->data; 01400 pchan = tree->pchan[iktarget->channel]; 01401 unsigned int controltype, bonecnt; 01402 double bonelen; 01403 float mat[4][4]; 01404 01405 // add the end effector 01406 // estimate the average bone length, used to clamp feedback error 01407 for (bonecnt=0, bonelen=0.f, a=iktarget->channel; a>=0; a=tree->parent[a], bonecnt++) 01408 bonelen += scale*tree->pchan[a]->bone->length; 01409 bonelen /= bonecnt; 01410 01411 // store the rest pose of the end effector to compute enforce target 01412 copy_m4_m4(mat, pchan->bone->arm_mat); 01413 copy_v3_v3(mat[3], pchan->bone->arm_tail); 01414 // get the rest pose relative to the armature base 01415 mul_m4_m4m4(iktarget->eeRest, mat, invBaseFrame); 01416 iktarget->eeBlend = (!ikscene->polarConstraint && condata->type==CONSTRAINT_IK_COPYPOSE) ? true : false; 01417 // use target_callback to make sure the initPose includes enforce coefficient 01418 target_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, iktarget); 01419 iktarget->target = new iTaSC::MovingFrame(initPose); 01420 iktarget->target->setCallback(target_callback, iktarget); 01421 ret = scene->addObject(iktarget->targetName, iktarget->target); 01422 if (!ret) 01423 break; 01424 01425 switch (condata->type) { 01426 case CONSTRAINT_IK_COPYPOSE: 01427 controltype = 0; 01428 if (condata->flag & CONSTRAINT_IK_ROT) { 01429 if (!(condata->flag & CONSTRAINT_IK_NO_ROT_X)) 01430 controltype |= iTaSC::CopyPose::CTL_ROTATIONX; 01431 if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Y)) 01432 controltype |= iTaSC::CopyPose::CTL_ROTATIONY; 01433 if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Z)) 01434 controltype |= iTaSC::CopyPose::CTL_ROTATIONZ; 01435 } 01436 if (condata->flag & CONSTRAINT_IK_POS) { 01437 if (!(condata->flag & CONSTRAINT_IK_NO_POS_X)) 01438 controltype |= iTaSC::CopyPose::CTL_POSITIONX; 01439 if (!(condata->flag & CONSTRAINT_IK_NO_POS_Y)) 01440 controltype |= iTaSC::CopyPose::CTL_POSITIONY; 01441 if (!(condata->flag & CONSTRAINT_IK_NO_POS_Z)) 01442 controltype |= iTaSC::CopyPose::CTL_POSITIONZ; 01443 } 01444 if (controltype) { 01445 iktarget->constraint = new iTaSC::CopyPose(controltype, controltype, bonelen); 01446 // set the gain 01447 if (controltype & iTaSC::CopyPose::CTL_POSITION) 01448 iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_POSITION, iTaSC::ACT_ALPHA, condata->weight); 01449 if (controltype & iTaSC::CopyPose::CTL_ROTATION) 01450 iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_ROTATION, iTaSC::ACT_ALPHA, condata->orientweight); 01451 iktarget->constraint->registerCallback(copypose_callback, iktarget); 01452 iktarget->errorCallback = copypose_error; 01453 iktarget->controlType = controltype; 01454 // add the constraint 01455 if (condata->flag & CONSTRAINT_IK_TARGETAXIS) 01456 ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, iktarget->targetName, armname, "", ikscene->channels[iktarget->channel].tail); 01457 else 01458 ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail); 01459 } 01460 break; 01461 case CONSTRAINT_IK_DISTANCE: 01462 iktarget->constraint = new iTaSC::Distance(bonelen); 01463 iktarget->constraint->setControlParameter(iTaSC::Distance::ID_DISTANCE, iTaSC::ACT_VALUE, condata->dist); 01464 iktarget->constraint->registerCallback(distance_callback, iktarget); 01465 iktarget->errorCallback = distance_error; 01466 // we can update the weight on each substep 01467 iktarget->constraint->substep(true); 01468 // add the constraint 01469 ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail); 01470 break; 01471 } 01472 if (!ret) 01473 break; 01474 } 01475 if (!ret || 01476 !scene->addCache(ikscene->cache) || 01477 !scene->addSolver(ikscene->solver) || 01478 !scene->initialize()) { 01479 delete ikscene; 01480 ikscene = NULL; 01481 } 01482 return ikscene; 01483 } 01484 01485 static void create_scene(Scene *scene, Object *ob) 01486 { 01487 bPoseChannel *pchan; 01488 01489 // create the IK scene 01490 for(pchan= (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan= (bPoseChannel *)pchan->next) { 01491 // by construction there is only one tree 01492 PoseTree *tree = (PoseTree*)pchan->iktree.first; 01493 if (tree) { 01494 IK_Data* ikdata = get_ikdata(ob->pose); 01495 // convert tree in iTaSC::Scene 01496 IK_Scene* ikscene = convert_tree(scene, ob, pchan); 01497 if (ikscene) { 01498 ikscene->next = ikdata->first; 01499 ikdata->first = ikscene; 01500 } 01501 // delete the trees once we are done 01502 while(tree) { 01503 BLI_remlink(&pchan->iktree, tree); 01504 BLI_freelistN(&tree->targets); 01505 if(tree->pchan) MEM_freeN(tree->pchan); 01506 if(tree->parent) MEM_freeN(tree->parent); 01507 if(tree->basis_change) MEM_freeN(tree->basis_change); 01508 MEM_freeN(tree); 01509 tree = (PoseTree*)pchan->iktree.first; 01510 } 01511 } 01512 } 01513 } 01514 01515 static void init_scene(Object *ob) 01516 { 01517 if (ob->pose->ikdata) { 01518 for(IK_Scene* scene = ((IK_Data*)ob->pose->ikdata)->first; 01519 scene != NULL; 01520 scene = scene->next) { 01521 scene->channels[0].pchan->flag |= POSE_IKTREE; 01522 } 01523 } 01524 } 01525 01526 static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, float ctime, float frtime) 01527 { 01528 int i; 01529 IK_Channel* ikchan; 01530 if (ikparam->flag & ITASC_SIMULATION) { 01531 for (i=0, ikchan=ikscene->channels; i<ikscene->numchan; i++, ++ikchan) { 01532 // In simulation mode we don't allow external contraint to change our bones, mark the channel done 01533 // also tell Blender that this channel is part of IK tree (cleared on each where_is_pose() 01534 ikchan->pchan->flag |= (POSE_DONE|POSE_CHAIN); 01535 ikchan->jointValid = 0; 01536 } 01537 } else { 01538 // in animation mode, we must get the bone position from action and constraints 01539 for(i=0, ikchan=ikscene->channels; i<ikscene->numchan; i++, ++ikchan) { 01540 if (!(ikchan->pchan->flag & POSE_DONE)) 01541 where_is_pose_bone(blscene, ikscene->blArmature, ikchan->pchan, ctime, 1); 01542 // tell blender that this channel was controlled by IK, it's cleared on each where_is_pose() 01543 ikchan->pchan->flag |= (POSE_DONE|POSE_CHAIN); 01544 ikchan->jointValid = 0; 01545 } 01546 } 01547 // only run execute the scene if at least one of our target is enabled 01548 for (i=ikscene->targets.size(); i > 0; --i) { 01549 IK_Target* iktarget = ikscene->targets[i-1]; 01550 if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF)) 01551 break; 01552 } 01553 if (i == 0 && ikscene->armature->getNrOfConstraints() == 0) 01554 // all constraint disabled 01555 return; 01556 01557 // compute timestep 01558 double timestamp = ctime * frtime + 2147483.648; 01559 double timestep = frtime; 01560 bool reiterate = (ikparam->flag & ITASC_REITERATION) ? true : false; 01561 int numstep = (ikparam->flag & ITASC_AUTO_STEP) ? 0 : ikparam->numstep; 01562 bool simulation = true; 01563 01564 if (ikparam->flag & ITASC_SIMULATION) { 01565 ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel); 01566 } 01567 else { 01568 // in animation mode we start from the pose after action and constraint 01569 convert_pose(ikscene); 01570 ikscene->armature->setJointArray(ikscene->jointArray); 01571 // and we don't handle velocity 01572 reiterate = true; 01573 simulation = false; 01574 // time is virtual, so take fixed value for velocity parameters (see itasc_update_param) 01575 // and choose 1s timestep to allow having velocity parameters in radiant 01576 timestep = 1.0; 01577 // use auto setup to let the solver test the variation of the joints 01578 numstep = 0; 01579 } 01580 01581 if (ikscene->cache && !reiterate && simulation) { 01582 iTaSC::CacheTS sts, cts; 01583 sts = cts = (iTaSC::CacheTS)(timestamp*1000.0+0.5); 01584 if (ikscene->cache->getPreviousCacheItem(ikscene->armature, 0, &cts) == NULL || cts == 0) { 01585 // the cache is empty before this time, reiterate 01586 if (ikparam->flag & ITASC_INITIAL_REITERATION) 01587 reiterate = true; 01588 } else { 01589 // can take the cache as a start point. 01590 sts -= cts; 01591 timestep = sts/1000.0; 01592 } 01593 } 01594 // don't cache if we are reiterating because we don't want to distroy the cache unnecessarily 01595 ikscene->scene->update(timestamp, timestep, numstep, false, !reiterate, simulation); 01596 if (reiterate) { 01597 // how many times do we reiterate? 01598 for (i=0; i<ikparam->numiter; i++) { 01599 if (ikscene->armature->getMaxJointChange() < ikparam->precision || 01600 ikscene->armature->getMaxEndEffectorChange() < ikparam->precision) 01601 break; 01602 ikscene->scene->update(timestamp, timestep, numstep, true, false, simulation); 01603 } 01604 if (simulation) { 01605 // one more fake iteration to cache 01606 ikscene->scene->update(timestamp, 0.0, 1, true, true, true); 01607 } 01608 } 01609 // compute constraint error 01610 for (i=ikscene->targets.size(); i > 0; --i) { 01611 IK_Target* iktarget = ikscene->targets[i-1]; 01612 if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF)) { 01613 unsigned int nvalues; 01614 const iTaSC::ConstraintValues* values; 01615 values = iktarget->constraint->getControlParameters(&nvalues); 01616 iktarget->errorCallback(values, nvalues, iktarget); 01617 } 01618 } 01619 // Apply result to bone: 01620 // walk the ikscene->channels 01621 // for each, get the Frame of the joint corresponding to the bone relative to its parent 01622 // combine the parent and the joint frame to get the frame relative to armature 01623 // a backward translation of the bone length gives the head 01624 // if TY, compute the scale as the ratio of the joint length with rest pose length 01625 iTaSC::Armature* arm = ikscene->armature; 01626 KDL::Frame frame; 01627 double q_rest[3], q[3]; 01628 const KDL::Joint* joint; 01629 const KDL::Frame* tip; 01630 bPoseChannel* pchan; 01631 float scale; 01632 float length; 01633 float yaxis[3]; 01634 for (i=0, ikchan=ikscene->channels; i<ikscene->numchan; ++i, ++ikchan) { 01635 if (i == 0) { 01636 if (!arm->getRelativeFrame(frame, ikchan->tail)) 01637 break; 01638 // this frame is relative to base, make it relative to object 01639 ikchan->frame = ikscene->baseFrame * frame; 01640 } 01641 else { 01642 if (!arm->getRelativeFrame(frame, ikchan->tail, ikscene->channels[ikchan->parent].tail)) 01643 break; 01644 // combine with parent frame to get frame relative to object 01645 ikchan->frame = ikscene->channels[ikchan->parent].frame * frame; 01646 } 01647 // ikchan->frame is the tail frame relative to object 01648 // get bone length 01649 if (!arm->getSegment(ikchan->tail, 3, joint, q_rest[0], q[0], tip)) 01650 break; 01651 if (joint->getType() == KDL::Joint::TransY) { 01652 // stretch bones have a TY joint, compute the scale 01653 scale = (float)(q[0]/q_rest[0]); 01654 // the length is the joint itself 01655 length = (float)q[0]; 01656 } 01657 else { 01658 scale = 1.0f; 01659 // for fixed bone, the length is in the tip (always along Y axis) 01660 length = tip->p(1); 01661 } 01662 // ready to compute the pose mat 01663 pchan = ikchan->pchan; 01664 // tail mat 01665 ikchan->frame.getValue(&pchan->pose_mat[0][0]); 01666 VECCOPY(pchan->pose_tail, pchan->pose_mat[3]); 01667 // shift to head 01668 VECCOPY(yaxis, pchan->pose_mat[1]); 01669 mul_v3_fl(yaxis, length); 01670 sub_v3_v3v3(pchan->pose_mat[3], pchan->pose_mat[3], yaxis); 01671 VECCOPY(pchan->pose_head, pchan->pose_mat[3]); 01672 // add scale 01673 mul_v3_fl(pchan->pose_mat[0], scale); 01674 mul_v3_fl(pchan->pose_mat[1], scale); 01675 mul_v3_fl(pchan->pose_mat[2], scale); 01676 } 01677 if (i<ikscene->numchan) { 01678 // big problem 01679 ; 01680 } 01681 } 01682 01683 //--------------------------------------------------- 01684 // plugin interface 01685 // 01686 void itasc_initialize_tree(struct Scene *scene, Object *ob, float ctime) 01687 { 01688 bPoseChannel *pchan; 01689 int count = 0; 01690 01691 if (ob->pose->ikdata != NULL && !(ob->pose->flag & POSE_WAS_REBUILT)) { 01692 init_scene(ob); 01693 return; 01694 } 01695 // first remove old scene 01696 itasc_clear_data(ob->pose); 01697 // we should handle all the constraint and mark them all disabled 01698 // for blender but we'll start with the IK constraint alone 01699 for(pchan= (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan= (bPoseChannel *)pchan->next) { 01700 if(pchan->constflag & PCHAN_HAS_IK) 01701 count += initialize_scene(ob, pchan); 01702 } 01703 // if at least one tree, create the scenes from the PoseTree stored in the channels 01704 if (count) 01705 create_scene(scene, ob); 01706 itasc_update_param(ob->pose); 01707 // make sure we don't rebuilt until the user changes something important 01708 ob->pose->flag &= ~POSE_WAS_REBUILT; 01709 } 01710 01711 void itasc_execute_tree(struct Scene *scene, struct Object *ob, struct bPoseChannel *pchan, float ctime) 01712 { 01713 if (ob->pose->ikdata) { 01714 IK_Data* ikdata = (IK_Data*)ob->pose->ikdata; 01715 bItasc* ikparam = (bItasc*) ob->pose->ikparam; 01716 // we need default parameters 01717 if (!ikparam) ikparam = &DefIKParam; 01718 01719 for (IK_Scene* ikscene = ikdata->first; ikscene; ikscene = ikscene->next) { 01720 if (ikscene->channels[0].pchan == pchan) { 01721 float timestep = scene->r.frs_sec_base/scene->r.frs_sec; 01722 if (ob->pose->flag & POSE_GAME_ENGINE) { 01723 timestep = ob->pose->ctime; 01724 // limit the timestep to avoid excessive number of iteration 01725 if (timestep > 0.2f) 01726 timestep = 0.2f; 01727 } 01728 execute_scene(scene, ikscene, ikparam, ctime, timestep); 01729 break; 01730 } 01731 } 01732 } 01733 } 01734 01735 void itasc_release_tree(struct Scene *scene, struct Object *ob, float ctime) 01736 { 01737 // not used for iTaSC 01738 } 01739 01740 void itasc_clear_data(struct bPose *pose) 01741 { 01742 if (pose->ikdata) { 01743 IK_Data* ikdata = (IK_Data*)pose->ikdata; 01744 for (IK_Scene* scene = ikdata->first; scene; scene = ikdata->first) { 01745 ikdata->first = scene->next; 01746 delete scene; 01747 } 01748 MEM_freeN(ikdata); 01749 pose->ikdata = NULL; 01750 } 01751 } 01752 01753 void itasc_clear_cache(struct bPose *pose) 01754 { 01755 if (pose->ikdata) { 01756 IK_Data* ikdata = (IK_Data*)pose->ikdata; 01757 for (IK_Scene* scene = ikdata->first; scene; scene = scene->next) { 01758 if (scene->cache) 01759 // clear all cache but leaving the timestamp 0 (=rest pose) 01760 scene->cache->clearCacheFrom(NULL, 1); 01761 } 01762 } 01763 } 01764 01765 void itasc_update_param(struct bPose *pose) 01766 { 01767 if (pose->ikdata && pose->ikparam) { 01768 IK_Data* ikdata = (IK_Data*)pose->ikdata; 01769 bItasc* ikparam = (bItasc*)pose->ikparam; 01770 for (IK_Scene* ikscene = ikdata->first; ikscene; ikscene = ikscene->next) { 01771 double armlength = ikscene->armature->getArmLength(); 01772 ikscene->solver->setParam(iTaSC::Solver::DLS_LAMBDA_MAX, ikparam->dampmax*armlength); 01773 ikscene->solver->setParam(iTaSC::Solver::DLS_EPSILON, ikparam->dampeps*armlength); 01774 if (ikparam->flag & ITASC_SIMULATION) { 01775 ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, ikparam->minstep); 01776 ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, ikparam->maxstep); 01777 ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel); 01778 ikscene->armature->setControlParameter(CONSTRAINT_ID_ALL, iTaSC::Armature::ID_JOINT, iTaSC::ACT_FEEDBACK, ikparam->feedback); 01779 } else { 01780 // in animation mode timestep is 1s by convention => 01781 // qmax becomes radiant and feedback becomes fraction of error gap corrected in one iteration 01782 ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, 1.0); 01783 ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, 1.0); 01784 ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, 0.52); 01785 ikscene->armature->setControlParameter(CONSTRAINT_ID_ALL, iTaSC::Armature::ID_JOINT, iTaSC::ACT_FEEDBACK, 0.8); 01786 } 01787 } 01788 } 01789 } 01790 01791 void itasc_test_constraint(struct Object *ob, struct bConstraint *cons) 01792 { 01793 struct bKinematicConstraint *data = (struct bKinematicConstraint *)cons->data; 01794 01795 /* only for IK constraint */ 01796 if (cons->type != CONSTRAINT_TYPE_KINEMATIC || data == NULL) 01797 return; 01798 01799 switch (data->type) { 01800 case CONSTRAINT_IK_COPYPOSE: 01801 case CONSTRAINT_IK_DISTANCE: 01802 /* cartesian space constraint */ 01803 break; 01804 } 01805 } 01806